/*  This is the telepointer control program
    it takes two integers on stdin, translates
    them to angular coordinates, and outputs them
    to the serial port specified to the SSC-II

    Written by Jan Grabski for the Telepointer project in ECE 496Y.
    
    Parts of this program are borrowed from James R. Bruce's public
    domain SSC-II test program.
    
    This program uses James R. Bruce's SSC-II control libraries,
    which are also public domain.
    
    Copyright 2000 Jan Grabski
    GPL rules apply.
*/


#include <stdio.h>
#include <math.h>
#include "telepoint.h"
#include "mini_ssc.h"

// constants used in the program for calculations

#define DEGREES 180.0/M_PI

// These are the X and Y resolutions of the video image
#define X_RES 254
#define Y_RES 254

/* RANGE is a computed value derived from the viewing
    angle of the camera. It is
    tan(viewing angle/2) = X_RES/(RANGE)  */
#define RANGE 488
#define V_ANGLE 47

double Z;

char *usage =
  "USAGE:\n"
  "  telepoint [-p<port number>] [-b<baud rate>] [-s<servo 0-31>]\n"
  "            [-m<min position 0-255] [-x<max position 0-255>]\n";

int main(int argc,char **argv)
{
  int port, baudrate, servo, min, max, delay, step;

  char *s;
  int i,n;
  int x_in, y_in;
  int x_out, y_out;

Z = 629;//(double)X_RES/(DEGREES*tan(V_ANGLE/2.0));
printf("RANGE IS %d\n",Z);
  // defaults
  port     = 0;
//  baudrate = 9600;
  baudrate = 2400; // the baud rate is selected between 2400
// for the small radios or 9600 for the faster radios,
// by a jumper on the servos board in the 8 inch dome to the wearer's right
  servo    = 0;

  // get parameter values
  for(i=0; i<argc; i++){
    s = argv[i];
    if((strlen(s) > 2) && s[0]=='-'){
      n = atoi(s + 2);
      switch(s[1]){
	case 'h':
	  printf(usage);
	  exit(0);
	  break;

        case 'p': port     = n; break;
        case 'b': baudrate = n; break;
      }
    }
  }
  
  // connect to the ssc
  if(!ssc_open(port,baudrate)){
  
    // enter infinite loop that translates pairs of screen coordinates
    // to pairs of telepointer coodinates.
    
    while(1){
      scanf("%d,%d", &x_in, &y_in);
      x_out = (int)((( (double)compute_Yaw( x_in, y_in ) + V_ANGLE/2 ) / V_ANGLE ) * X_RES)+4;
      y_out = (int)((( (double)compute_Pitch( x_in, y_in ) + V_ANGLE/2 ) / V_ANGLE ) * Y_RES)-22;
      printf("%d,%d\n", x_out, y_out );
      if(x_out > 254){
        printf("HIGH: %d, %d\n",x_in,y_in); 
	  }
      if (x_in == 999) ssc_move(4,0); // set pwm to narrowest
      else {
        ssc_move(4,254); // set pwm to widest (brightest)
        ssc_move( 0, x_out );
        ssc_move( 2, y_out );
      }
    }
  }
}

int compute_Pitch(int x, int y)
{
    double result;
    double X = (double)x - (double)((double)X_RES/2.0);
    double Y = (double)y - (double)((double)Y_RES/2.0);
    
    result = atan( Y / sqrt( pow(Z,2) + pow(X,2) ) );
    result = rint(DEGREES*result);
    return (int)result;
}

int compute_Yaw(int x, int y)
{
    double result;
    double X = (double)x - (double)((double)X_RES/2.0);
    double Y = (double)y - (double)((double)Y_RES/2.0);
    
    result = atan( X / Z );//sqrt( pow(Z,2) + pow(Y,2) ) );
    result = rint(DEGREES*result);
    return (int)result;
}