/*  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/(2RANGE)  */
#define RANGE 254
#define V_ANGLE 90

const double Z = RANGE;

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;

  // defaults
  port     = 0;
  baudrate = 9600;
  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);
      y_out = (int)((( (double)compute_Pitch( x_in, y_in ) + V_ANGLE/2 ) / V_ANGLE ) * Y_RES);
      printf("%d,%d\n", x_out, y_out );
      ssc_move( 0, x_in );
      ssc_move( 2, y_in );
    }
  }
}

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

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