/***********************************************************************/
/** @name global.c   

    Contains camera dependent lookup tables.  These will depend on the
    camera view angle and servo tilt mappings.  The current tables is
    calibrated against QuickCam color cameras.
    
    @author Thomas Braunl & Birgit Graf, UWA, 1998 */
/***********************************************************************/

/*@{*/

#include "global.h"
#include <math.h>

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

void
init_camtable(void)
{
  int row;
  /* Y factor */
  for (row = 0; row < imagerows; row++)
    {
      yfact[MIDDLE][row] = 0.00408362 - 0.000481069 * row     + 0.000288908 * pow(row, 1.1);
      yfact[UP][row]     = 0.00549862 + 11.0223 * pow(row,-2) - 0.358076 * pow(row,-1);
      yfact[DOWN][row]   = 0.00124624 - 0.0000444766 * row    + 0.0000252079 * pow(row,1.1);
    }

  /* X to M */
  for (row = 0; row < imagerows; row++)
    {
      x2m[MIDDLE][row] = 41.7242 - 4.66952 * row + 2.7729 * pow(row,1.1);
      x2m[UP][row]     = 13.9237 + 61687.6 * pow(row,-2.2) + 164.97 * pow(row,-1);
      x2m[DOWN][row]   = 11.8195 - 0.495745 * row + 0.244673 * pow(row,1.1);
    }
}

float yfact[3][imagerows];
#if 0

/* camPos=MIDDLE (angle 150) */
//0.00408362 - 0.000481069 * x + 0.000288908 * x^1.1
{{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.00549862 + 11.0223 * x^(-2) - 0.358076 * x^(-1)
 {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.00124624 - 0.0000444766 * x + 0.0000252079 * x^1.1
 {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}};
#endif

/** factor to get x position in meters from x position in pixels */
float x2m[3][imagerows];
#if 0
 /* camPos=MIDDLE (angle 150) */
//41.7242 - 4.66952 * x + 2.7729 * x^1.1
{{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) */
 //13.9237 + 61687.6 * x^(-2.2) + 164.97 * x^(-1)
 {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) */
 //11.8195 - 0.495745 * x + 0.244673 * x^1.1
 {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}};
#endif
/*@}*/

#if TEST
int
main(int argc, char *argv[])
{
  int row;
  init_camtable();
  LCDPrintf("\nY up\n");
  for (row = 0; row < imagerows; row++)
    LCDPrintf("%d %f\n", row, yfact[UP][row]);
  LCDPrintf("\nY middle\n");
  for (row = 0; row < imagerows; row++)
    LCDPrintf("%d %f\n", row, yfact[MIDDLE][row]);
  LCDPrintf("\nY down\n");
  for (row = 0; row < imagerows; row++)
    LCDPrintf("%d %f\n", row, yfact[DOWN][row]);

  LCDPrintf("\nX up\n");
  for (row = 0; row < imagerows; row++)
    LCDPrintf("%d %f\n", row, x2m[UP][row]);
  LCDPrintf("\nX middle\n");
  for (row = 0; row < imagerows; row++)
    LCDPrintf("%d %f\n", row, x2m[MIDDLE][row]);
  LCDPrintf("\nX down\n");
  for (row = 0; row < imagerows; row++)
    LCDPrintf("%d %f\n", row, x2m[DOWN][row]);
}
#endif