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

#include "gm.h"

static wii_pt bounds[2][4];
static int    g_calibrated[4];

double get_dist( wii_pt *p1, wii_pt *p2 ) {
  double d = (p1->x - p2->x)*(p1->x - p2->x) +
             (p1->y - p2->y)*(p1->y - p2->y);
  return sqrt(d);
}

/* This call stores information about the expected
   rectangle for each wii
*/
void wii_calibrate( int wii_id, wii_pt *coords ) {
  double max_dist = 0.0;
  int pt1 = 0, pt2 = 0, i, j;

  /* Points with maximal distance span a rectangle */
  for( i=0; i<4; ++i )
    for( j=1; j<4; ++j ) { 
      double dist = get_dist( coords+i, coords+j );
      if( dist > max_dist ) {
        pt1 = i; pt2 = j; 
        max_dist = dist;
      }
    }

  if( coords[pt1].x > coords[pt2].x ) {
    bounds[0][wii_id] = coords[pt1];
    bounds[1][wii_id] = coords[pt2];
  } else {
    bounds[1][wii_id] = coords[pt1];
    bounds[0][wii_id] = coords[pt2];
  }

  g_calibrated[wii_id] = 1;
}

int wii_point_to_led( int wii_id, wii_pt *coords ) {
  // Ick nenn die immer so.
  const double x0 = bounds[0][wii_id].x;
  const double y0 = bounds[0][wii_id].y;
  const double x1 = bounds[1][wii_id].x;
  const double y1 = bounds[1][wii_id].y;
  const double xp = coords->x;
  const double yp = coords->y;

//printf( "%lf:%lf ", xp, yp );

  // Where m is slope of orthogonal line
  // And n is square of diagonal of bounding box
  double m = (x1-x0)*(xp-x0)+(y1-y0)*(yp-y0);
  double n = (x1-x0)*(x1-x0)+(y1-y0)*(y1-y0);

  // To find out distance of coord to our base line
  double q = (xp-x0)*(y1-y0)-(yp-y0)*(x1-x0);

//printf( "%lf %lf %lf\n", m, n, q );

  double offs_on_line = m/n;
  double dist_to_line = q/n;

  // If wii is not calibrated, we cannot detect point
  if( !g_calibrated[wii_id] )
    return -1;

  // Point is too far away to be considered a match
  if( ( dist_to_line > 0.1 ) || ( dist_to_line < -0.1 ) ) {
//    puts( "too far " );
    return -1;
  }

//  printf( "%1.3lf\n", offs_on_line );

  // Check, which segment our line hits
  if( ( offs_on_line > (-1.0/6.0) ) && ( offs_on_line <= (1.0/6.0) ) )
    return 0;
  if( ( offs_on_line > ( 1.0/6.0) ) && ( offs_on_line <= (3.0/6.0) ) )
    return 1;
  if( ( offs_on_line > ( 3.0/6.0) ) && ( offs_on_line <= (5.0/6.0) ) )
    return 2;
  if( ( offs_on_line > ( 5.0/6.0) ) && ( offs_on_line <= (7.0/6.0) ) )
    return 3;

  // No match!
  return -1;
}