#include <stdio.h>
#include <math.h>

#define imagerows 62
#define imagecolumns 82

/** factor to calculate x position from pixels to meters depending on
    y position (achieved by getting width in m of object that has
    width of screen at selected y positions. fact = val(m)/imagerows) */

float xfact[3][imagerows] =

/* camPos=MIDDLE (angle 150) */
{{0.0106, 0.0093, 0.0080, 0.0069, 0.0065, 0.0061, 0.0058, 0.0054,
  0.0050, 0.0048, 0.0045, 0.0043, 0.0040, 0.0039, 0.0039, 0.0038,
  0.0037, 0.0036, 0.0036, 0.0035, 0.0035, 0.0034, 0.0034, 0.0033,
  0.0033, 0.0032, 0.0031, 0.0031, 0.0030, 0.0030, 0.0029, 0.0029,
  0.0028, 0.0028, 0.0027, 0.0027, 0.0026, 0.0026, 0.0025, 0.0024,
  0.0024, 0.0023, 0.0023, 0.0023, 0.0022, 0.0022, 0.0022, 0.0021,
  0.0021, 0.0020, 0.0020, 0.0020, 0.0019, 0.0019, 0.0019, 0.0019,
  0.0018, 0.0018, 0.0018, 0.0018, 0.0017, 0.0017},

  /* camPos=UP (angle 225) */
 {0.0124, 0.0124, 0.0124, 0.0124, 0.0124, 0.0124, 0.0124, 0.0124,
  0.0124, 0.0124, 0.0124, 0.0124, 0.0124, 0.0124, 0.0124, 0.0124,
  0.0124, 0.0124, 0.0124, 0.0124, 0.0110, 0.0100, 0.0090, 0.0083,
  0.0075, 0.0068, 0.0060, 0.0057, 0.0055, 0.0053, 0.0051, 0.0050,
  0.0049, 0.0048, 0.0046, 0.0045, 0.0044, 0.0043, 0.0041, 0.0040,
  0.0039, 0.0038, 0.0037, 0.0036, 0.0035, 0.0034, 0.0034, 0.0033,
  0.0033, 0.0032, 0.0031, 0.0031, 0.0030, 0.0029, 0.0029, 0.0028,
  0.0027, 0.0027, 0.0026, 0.0025, 0.0024, 0.0023},
 
  /* camPos=DOWN (angle 0) */
 {0.0026, 0.0026, 0.0026, 0.0026, 0.0025, 0.0025, 0.0025, 0.0025,
  0.0024, 0.0024, 0.0024, 0.0023, 0.0023, 0.0023, 0.0023, 0.0022,
  0.0022, 0.0022, 0.0022, 0.0021, 0.0021, 0.0021, 0.0021, 0.0021,
  0.0020, 0.0020, 0.0019, 0.0019, 0.0019, 0.0019, 0.0018, 0.0018,
  0.0018, 0.0018, 0.0017, 0.0017, 0.0017, 0.0017, 0.0016, 0.0016,
  0.0016, 0.0015, 0.0015, 0.0015, 0.0015, 0.0014, 0.0000, 0.0000,
  0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000,
  0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000}};


/** factor to get y position in meters from y position in pixels */
float y2m[3][imagerows] =

 /* camPos=MIDDLE (angle 150) */
{{0.520, 0.460, 0.400, 0.340, 0.320, 0.300, 0.280, 0.260, 0.240,
  0.227, 0.215, 0.202, 0.190, 0.182, 0.174, 0.166, 0.158, 0.150,
  0.145, 0.140, 0.135, 0.130, 0.125, 0.121, 0.117, 0.113, 0.109,
  0.105, 0.101, 0.097, 0.094, 0.090, 0.087, 0.084, 0.081, 0.078,
  0.076, 0.073, 0.071, 0.068, 0.066, 0.063, 0.061, 0.059, 0.057,
  0.055, 0.053, 0.051, 0.049, 0.047, 0.045, 0.042, 0.040, 0.037,
  0.035, 0.033, 0.031, 0.029, 0.027, 0.025, 0.023, 0.021},
 
 /* camPos=UP (angle 255) */
 {0.990, 0.990, 0.990, 0.990, 0.990, 0.990, 0.990, 0.990, 0.990,
  0.990, 0.990, 0.990, 0.990, 0.990, 0.990, 0.900, 0.810, 0.750,
  0.690, 0.630, 0.570, 0.510, 0.450, 0.412, 0.375, 0.337, 0.300,
  0.287, 0.274, 0.261, 0.248, 0.235, 0.226, 0.217, 0.208, 0.200,
  0.192, 0.184, 0.176, 0.168, 0.160, 0.155, 0.150, 0.145, 0.140,
  0.135, 0.131, 0.127, 0.123, 0.119, 0.115, 0.111, 0.107, 0.103,
  0.100, 0.096, 0.092, 0.088, 0.084, 0.080, 0.076, 0.072},
 
/* camPos=DOWN (angle 0) */
 {0.095, 0.095, 0.092, 0.090, 0.087, 0.084, 0.081, 0.078, 0.075,
  0.072, 0.070, 0.067, 0.065, 0.063, 0.061, 0.059, 0.057, 0.055,
  0.053, 0.051, 0.049, 0.047, 0.045, 0.042, 0.040, 0.037, 0.035,
  0.033, 0.031, 0.029, 0.027, 0.025, 0.023, 0.021, 0.019, 0.017,
  0.015, 0.013, 0.012, 0.011, 0.010, 0.009, 0.008, 0.007, 0.006,
  0.005, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000,
  0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000}};


/** ball distance in meters depending on x and y position in pixels */
float ball_dist[3][imagerows / 2][imagecolumns];


void main()
{
  int i,x,y;
  /* set up table for ball distance */
  LCDPrintf("{\n");
  for (i = 0; i < 3; i++)
  {
    LCDPrintf("{\n");
    for(x = 0; x < imagerows / 2; x++)
    {
      LCDPrintf("{\n");
      for(y = 0; y < imagecolumns; y++)
      {
	ball_dist[i][x][y] = sqrt(y2m[i][x] * y2m[i][x] + x*xfact[i][x] * x*xfact[i][x]);
	LCDPrintf("%f, ", ball_dist[i][x][y]);
      }
      LCDPrintf("}, \n");
    }
    LCDPrintf("}, \n");
  }
  LCDPrintf("}, \n");
}