#include <ctype.h>
#include <stdio.h>
#include "tt7rmlib.h"
#include <timing.h>
#include <userio.h>
#include <stdlib.h>

/*FUNCTION PROTOTYPES*/

int deploy(void);
int testfunc(void);
int turn_rov(char direction);
int move_rack(char direction);
int move_currmet(char direction);


/*********************************************/

main() {
  
   char inkey; 

   tt7init();
   printf("\nTattletale Model7 Initialized");

   while(1) {
     printf("\n\n\nRover System Main Menu V1.0");
     printf("\n\n  Deploy");
     printf("\n  Test functions");
     printf("\n  Reset");
     QueryChar("\nEnter Selection: ",0,"dtr",&inkey);
     switch(inkey){
       case 'd':
	 deploy();
	 break;
       case 't':
	 testfunc();
	 break;
       case 'r':
	 reset();
	 break;
     }
   }
} /*main*/

/*********************************************/

int deploy(void) {

  unsigned long traveltime, heading1, heading2, deploydelay;
  long starttime;
  int headingdiff;

  outport(3, SETBIT, POWER6);   /* Turn on sensor power */

  do {
    if(! QueryNum("\nEnter deployment delay time in seconds: ", "0", "%ld", &deploydelay))
      return -1;
  }  while (deploydelay < 0 || deploydelay > 1200);

  do {
    if(! QueryNum("\nEnter travel time in seconds: ", "0", "%ld", &traveltime))
      return -1;
  }  while (traveltime < 0 || traveltime > 60);

  do {
    if(! QueryNum("\nEnter first heading: ", "0", "%ld", &heading1))
      return -1;
  }  while (heading1 < 0 || heading1 > 359);

  do {
    if(! QueryNum("\nEnter second heading: ", "0", "%ld", &heading2))
      return -1;
  }  while (heading2 < 0 || heading2 > 359);

     /*** Start of Deployment ***/

  DelayMilliSecs(deploydelay*1000);
  printf("\nFirst Heading Turn");
  starttime = MilliSecs();

  for(;;) {
    headingdiff = heading1 - heading();

    if(headingdiff > 180) headingdiff -= 360;
      else if(headingdiff < (-180)) headingdiff += 360;

    if(headingdiff > 20) turn_rov('r');
      else if(headingdiff < (-20)) turn_rov('l');

    if( abs( headingdiff ) <= 20) break;
    if( labs(MilliSecs() - starttime) > 60000) {
      turn_rov('s');
      printf("First Turn Timeout");
      return -1;
    }
  }
  turn_rov('s');
  printf("\nTurn complete");

  DelayMilliSecs(500);
  turn_rov('f');
  DelayMilliSecs(traveltime*1000);
  turn_rov('s');

  printf("\nSecond Heading Turn");
  DelayMilliSecs(500);
  starttime = MilliSecs();

  for(;;) {
    headingdiff = heading2 - heading();

    if(headingdiff > 180) headingdiff -= 360;
    else if(headingdiff < (-180)) headingdiff += 360;

    if(headingdiff > 20) turn_rov('r');
      else if(headingdiff < (-20)) turn_rov('l');

    if( abs( headingdiff ) <= 20) break;
    if( labs(MilliSecs() - starttime) > 60000) {
      turn_rov('s');
      printf("Second Turn Timeout");
      return -1;
    }
  }
  turn_rov('s');
  printf("\nSecond Turn Complete");

  DelayMilliSecs(500);
  turn_rov('f');
  DelayMilliSecs(traveltime*1000);
  turn_rov('s');

  printf("\nRack Down");
  DelayMilliSecs(5000);
  move_rack('d');         /* Move rack down */
  for(;;) {
    if( !inport(LIMITDWN) ) break;
  }
  move_rack('s');

  printf("\nRack Up");
  DelayMilliSecs(5000);
  move_rack('u');          /* Move rack up */
  for(;;) {
    if( !inport(LIMITUP) ) break;
  }
  move_rack('s');

  outport(3, CLEARBIT, POWER6);  /* Turn off sensor power */
  return 0;
}

/*********************************************/

int testfunc(void) {

  char inkey;

  outport(3, SETBIT, POWER6);   /* Turn on sensor power for testing */

  printf("\n\n\nRover Test Function Menu");
  printf("\n\n  Drive system");
  printf("\n  Rack system");
  printf("\n  Current meter");
  printf("\n  Video system");
  printf("\n  Film Camera/Strobe");
  printf("\n  System status");
  printf("\n  Exit");
  QueryChar("\nEnter Selection: ",'E',"drcvfse",&inkey);

  switch(inkey){
    case 'd':  /*Drive Functions*/
      for(;;) {
      printf("\n  F = Forward");
      printf("\n  B = Backward");
      printf("\n  L = Left Turn");
      printf("\n  R = Right Turn");
      printf("\n  S = Stop");
      printf("\n  E = Exit to Main Menu");
	QueryChar("\nEnter Selection: ",0,"fblrse",&inkey);
	if (inkey != 'e') turn_rov(inkey);
	  else {
	    turn_rov('s');
	    break;
	  }
      }
    break;

    case 's':   /*System Parameters*/
      while( !(tt7kbhit()) ) {
	printf("\n\n\nBattery Voltage: %.2f volts", battvoltage());
	printf("\nSystem Current: %.3f amps", syscurrent());
	printf("\nGround Fault Voltage: %.2f volts", gfvoltage());
	printf("\n\nRover Tilt: X=%.1f, Y=%.1f degrees", tilt_x(), tilt_y());
	printf("\nRover Heading: %d degrees magnetic", heading());
	printf("\nRover Speed: %.1f cm/sec", speed());

	printf("\n\nRack Limitup: %d", inport(LIMITUP));
	printf("\nRack Limitdwn: %d", inport(LIMITDWN));
	printf("\nRackup Sensor: %d", inport(RACKUP));
	printf("\nPort Limitpad: %d", inport(LIMITPAD1));
	printf("\nStbd Limitpad: %d", inport(LIMITPAD2));
	printf("\nCM Limit: %d", inport(CMLIMIT));
	printf("\nCM Rotor: %d", inport(CMROTOR));
	DelayMilliSecs(3000);
      }
    break;

    case 'r':   /*Rack Functions*/
      for(;;) {
      printf("\n  U = Up");
      printf("\n  D = Down");
      printf("\n  S = Stop");
      printf("\n  E = Exit to Main Menu");
	QueryChar("\nEnter Selection: ",0,"udse",&inkey);
	if (inkey != 'e') move_rack(inkey);
	  else {
	    move_rack('s');
	    break;
	  }
      }
    break;

    case 'c':   /*Current Meter Functions*/
      for(;;) {
      printf("\n  U = Up");
      printf("\n  D = Down");
      printf("\n  S = Stop");
      printf("\n  E = Exit to Main Menu");
	QueryChar("\nEnter Selection: ",0,"udse",&inkey);
	if (inkey != 'e') move_currmet(inkey);
	  else {
	    move_currmet('s');
	    break;
	  }
      }
    break;
  }           /*End of Switch*/

  outport(3, CLEARBIT, POWER6);  /* Turn off sensor power */
  return(0);
}

/************************************************
   DRIVE FUNCTIONS
*************************************************/

int turn_rov(char direction) {

  switch(direction) {
    case 'f':
      outport(1, CLEARBIT, MOT1A+MOT1B+MOT2A+MOT2B);
      outport(1, SETBIT, MOT1A + MOT2A);           /* Turn on motor relays forward */
      break;
    case 'b':
      outport(1, CLEARBIT, MOT1A+MOT1B+MOT2A+MOT2B);
      outport(1, SETBIT, MOT1B + MOT2B);
      break;
    case 'l':
      outport(1, CLEARBIT, MOT1A+MOT1B+MOT2A+MOT2B);
      outport(1, SETBIT, MOT1B + MOT2A);
      break;
    case 'r':
      outport(1, CLEARBIT, MOT1A+MOT1B+MOT2A+MOT2B);
      outport(1, SETBIT, MOT1A + MOT2B);
      break;
    case 's':
      outport(1, CLEARBIT, MOT1A+MOT1B+MOT2A+MOT2B); /* Turn off both motor relays */
      break;
  }
  return(0);
}

/************************************************
	   RACK FUNCTIONS
************************************************/

int move_rack(char direction)  {

  switch(direction) {
    case 'u':
      outport(2, CLEARBIT, MOT1A+MOT1B);
      outport(2, SETBIT, MOT1A);  /* Turn rack motor on upward */
      break;
    case 'd':
      outport(2, CLEARBIT, MOT1A+MOT1B);
      outport(2, SETBIT, MOT1B);
      break;
    case 's':
      outport(2, CLEARBIT, MOT1A+MOT1B);  /* Turn off rack motor */
      break;
  }
  return(0);
}
/************************************************
	 CURRENT METER FUNCTIONS
************************************************/

int move_currmet(char direction)  {

  switch(direction) {
    case 'u':
      outport(2, CLEARBIT, MOT2A+MOT2B);
      outport(2, SETBIT, MOT2A);  /* Turn current meter motor on upward */
      break;
    case 'd':
      outport(2, CLEARBIT, MOT2A+MOT2B);
      outport(2, SETBIT, MOT2B);
      break;
    case 's':
      outport(2, CLEARBIT, MOT2A+MOT2B);  /* Turn off current meter motor */
      break;
  }
  return(0);
}