/* ROVER CONTROL SOFTWARE VERSION 1.1 */

#include <ctype.h>
#include <stdio.h>
#include <timing.h>
#include <stdlib.h>

#include "rovlib.h"
#include "rovint.h"
#include "rovsys.h"

#ifdef AZTEC_C
#include <userio.h>

#else
#include <conio.h>
#include "tt7pclib.h"
#include "tt7pcio.h"
#endif

/*FUNCTION PROTOTYPES*/

int deploy(void);
int testfunc(void);
int setup(void);

/* EXTERNAL VARIABLES */


/* MISC VARIABLES */



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

main() {
  
   char inkey; 

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

   while(1) {
     printf("\n\n\nRover System Main Menu V1.1");
     printf("\n\n  1 -- Deploy");
     printf("\n  2 -- Test functions");
     printf("\n  3 -- Setup");
     printf("\n  9 -- Reset");
     QueryChar("\nEnter Selection: ",0,"1239",&inkey);
     switch(inkey){
       case '1':
	 deploy();
	 break;
       case '2':
	 testfunc();
	 break;
       case '3':
	 setup();
	 break;
       case '9':
	 reset();
	 break;
     }
   }
} /*main*/

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

int deploy(void) {

  unsigned long transitdist, heading1, heading2, deploydelay, rack_down_time;
  long starttime;
  int headingdiff, errortype;

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

  do {
    if(! QueryNum("\nEnter travel distance in meters: ", "0", "%ld", &transitdist))
      return -1;
  }  while (transitdist < 0 || transitdist > 20);
  transitdist *= 100/ODOCAL;   /* Convert to odometer ticks */

  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);

  do {
    if(! QueryNum("\nEnter rack down time (sec): ", "0", "%ld", &rack_down_time))
      return -1;
  }  while (rack_down_time < 0 || rack_down_time > 18000);

/************ Start of Deployment *************/

  delaysecs(deploydelay);
  SENSOR_PWR_ON
  delaysecs(10);	     /* Let compass settle */

/************ First Turn ***********/

  printf("\nFirst Heading Turn");
  if(turn_rov(heading1)){
    printf("First Turn Timeout");
    SENSOR_PWR_OFF
    return -1;
  }
  printf("\nTurn complete");

/************** First Transit *******************/

  DelayMilliSecs(500);
  printf("\nFirst transit begins");
  if(transit_rov(transitdist)){
    printf("\nFirst Transit Timeout");
    SENSOR_PWR_OFF
    return -1;
  }
  printf("\nFirst transit complete");

/******************** Second Turn **********************/

  DelayMilliSecs(500);
  printf("\nSecond Heading Turn");
  if( turn_rov(heading2) ){
    printf("\nSecond Turn Timeout");
    SENSOR_PWR_OFF
    return -1;
  }
  printf("\nSecond Turn Complete");

/******************** Second Transit ***************/

  DelayMilliSecs(500);
  printf("\nSecond transit begins");
  if(transit_rov(transitdist)){
    printf("\nSecond Transit Timeout");
    SENSOR_PWR_OFF
    return -1;
  }
  printf("\nSecond transit complete");

/*********************Rack Cycle********************/

  delaysecs(1);
  FLOOD1_PWR_ON;
  FLOOD2_PWR_ON;
  delaysecs(1);
  printf("\nRack Down");
  if(errortype = rack_down_cycle()){
    switch(errortype){
      case (-1):
	printf("\nRack Limit Down Error");
	break;
      case (-2):
	printf("\nRack Leadscrew Error");
	break;
      case (-3):
	printf("\nRack Timeout");
	break;
    }
    move_rack('s');
    FLOOD1_PWR_OFF
    FLOOD2_PWR_OFF
    SENSOR_PWR_OFF
    return (-1);
  }

  FLOOD1_PWR_OFF
  FLOOD2_PWR_OFF
  delaysecs(rack_down_time);

  printf("\nRack Up");
  if(errortype = rack_up_cycle()){
    switch(errortype){
      case (-1):
	printf("\nRack Limit Up Error");
	break;
      case (-2):
	printf("\nRack Leadscrew Error");
	break;
      case (-3):
	printf("\nRack Timeout");
	break;
    }
  SENSOR_PWR_OFF
  return -1;
  }

  SENSOR_PWR_OFF;
  return 0;
}

/*********************************************
      TEST FUNCTION
*********************************************/

int testfunc(void) {

  char menukey, inkey;
  short chan;
  unsigned long channel;
  unsigned short portbit;
  long lasttime;

  SENSOR_PWR_ON            /* Turn on sensor power for testing */

  do{
    inkey = 0;

    printf("\n\n\nRover Test Function Menu");
    printf("\n\n  1 -- Drive system");
    printf("\n  2 -- Rack system");
    printf("\n  3 -- Current meter");
    printf("\n  4 -- Video system");
    printf("\n  5 -- Film Camera/Strobe");
    printf("\n  6 -- Port Levels");
    printf("\n  7 -- System Parameters");
    printf("\n  8 -- Instrument/Compass Setup");
    printf("\n  9 -- Exit");
    QueryChar("\nEnter Selection: ",'9',"123456789",&menukey);

    switch(menukey){
      case '1':  /*Drive Functions*/
	do{
	  printf("\n\n\nBattery Voltage: %.2f volts", battvoltage());
	  printf("\nSystem Current: %.3f amps", hpcurrent());
	  printf("\nHeading: %d degrees magnetic", heading());

	  printf("\n\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 Test Menu");
	  printf("\n\nEnter Selection: ");

	  lasttime = TensMilliSecs();
	  while( (TensMilliSecs()-300) <= lasttime ){
	    if( kbhit() ){
	      inkey = tolower( SerGetByte() );       /* Get command in lower case */
	      move_rov(inkey);
	    }
            if(inkey == 'e') break;
	  }
	} while(inkey != 'e');
	move_rov('s');
	break;


      case '2':   /*Rack Functions*/
	g_leadscrew1_ticks = 0;
	g_leadscrew2_ticks = 0;

	do{
	  printf("\n\n\nBattery Voltage: %.2f volts", battvoltage());
	  printf("\nSystem Current: %.3f amps", hpcurrent());
	  printf("\nLeadscrew-1 Half-turns: %d", g_leadscrew1_ticks);
	  printf("\nLeadscrew-2 Half-turns: %d", g_leadscrew2_ticks);

	  printf("\n\n  U = Up");
	  printf("\n  D = Down");
	  printf("\n  S = Stop");
	  printf("\n  E = Exit to Test Menu");
	  printf("\n\nEnter Selection: ");

	  lasttime = TensMilliSecs();

	  while( (TensMilliSecs()-300) <= lasttime ){
	    if( kbhit() ){
	      inkey = tolower( SerGetByte() );       /* Get command in lower case */
	      move_rack(inkey);
	    }
	    if(inkey == 'e') break;
	  }
	} while(inkey != 'e');
	move_rack('s');
	break;


      case '3':   /*Current Meter Functions*/
	g_cmmot_shaft_ticks = 0;
	g_cmrotor_ticks = 0;

	do{
	  printf("\n\n\nCM Motor Shaft Turns: %d", g_cmmot_shaft_ticks);
	  printf("\nCM Vane Direction: %d degrees", cmvane_dir());
	  printf("\nCM Rotor Turns: %d", g_cmrotor_ticks/2);
	  printf("\nCM Limit Status: %d", inauxport(CMLIMIT));

	  printf("\n\n  U = Up");
	  printf("\n  D = Down");
	  printf("\n  S = Stop");
	  printf("\n  E = Exit to Test Menu");
	  printf("\n\nEnter Selection: ");

	  lasttime = TensMilliSecs();
	  while( (TensMilliSecs()-300) <= lasttime ){
	    if( kbhit() ){
	      inkey = tolower( SerGetByte() );       /* Get command in lower case */
	      move_currmet(inkey);
	    }
	    if(inkey == 'e') break;
	  }
	} while(inkey != 'e');

	move_currmet('s');
	break;

      case '4':  /* Video System */
	do{
	  printf("\n\n\n1 -- Power Floods (10 sec)");
	  printf("\n2 -- Power Video 1");
	  printf("\n3 -- Power Video 2");
	  printf("\n4 -- Record Video 1");
	  printf("\n5 -- Stop Video 1");
	  printf("\n6 -- Record Video 2");
	  printf("\n7 -- Stop Video 2");
	  printf("\n8 -- Unpower Both Videos");
	  printf("\n9 -- Exit to Test Menu");
	  QueryChar("\n\nEnter Selection: ",'9',"123456789",&inkey);

	  switch(inkey){
	    case '1':  /* Power Floods */
	      FLOOD1_PWR_ON
	      FLOOD2_PWR_ON
	      DelayMilliSecs(10000);
	      FLOOD1_PWR_OFF
	      FLOOD2_PWR_OFF
	      break;

	    case '2':  /* Power Video 1 */
	      outauxport(3, SETBIT, POWER7);
	      break;

	    case '3':  /* Power Video 2 */
	      outauxport(3, SETBIT, POWER8);
	      break;

	    case '8':  /* Unpower Both Videos */
	      outauxport(3, CLEARBIT, POWER7 + POWER8);
	      break;


	  } /* End of switch */
	} while(inkey != '9');
	break;

      case '5':  /* Film Camera */
	do{
	  QueryChar("\n\n\nCycle Strobe and Camera",'N',"yn",&inkey);
	  if(inkey == 'y'){
	    STROBE_PWR_ON
	    delaysecs(5);

	    FILMCAM_TRIG_ON    /* Trigger strobe/camera */
	    DelayMilliSecs(TRIGGER_TIME);
	    FILMCAM_TRIG_OFF
	    STROBE_PWR_OFF
	  }
	} while(inkey == 'y');
	break;


      case '6':   /*Port Levels*/
	while( !(kbhit()) ) {

	  printf("\n\n\nAnalog Inputs");
	  printf("\nChannel");
	  for(chan=0; chan<=7; chan++){
	    printf("      %d", chan);
	  }
	  printf("\nVoltage  ");
	  for(chan=0; chan<=7; chan++){
	    setanachan(chan);
	    DelayMilliSecs(10);
	    printf("  %.3f", (float)(AtoDReadWord(0))/6552);
	  }

	  printf("\n\nInput Port Logic Levels");
	  printf("\nBit  ");
	  for(portbit=0; portbit<=7; portbit++){
	    printf("      %d", portbit);
	  }
	  printf("\nLevel");
	  for(portbit=0; portbit<=7; portbit++){
	    printf("      %d", inauxport(1<<portbit));
	  }
/*
	  printf("\n\nTPU Input Logic Levels");
	  printf("\nChannel");
	  for(chan=3; chan<=7; chan++){
	    printf("      %d", chan);
	  }
	  printf("      9");
	  printf("\nLevel  ");
	  for(chan=3; chan<=7; chan++){
	    printf("      %d", TPUGetPin(chan));
	  }
	  printf("      %d", TPUGetPin(9));
*/
	  DelayMilliSecs(3000);
      }
      break;

    case '7':  /*System Parameters*/
      while( !(kbhit()) ) {
	printf("\n\n\nBattery Voltage: %.2f volts", battvoltage());
	printf("\nSystem Current: %.3f amps", hpcurrent());
	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("\n\nRack Limitup: %d", inauxport(LIMITUP));
	printf("\nRack Limitdwn: %d", inauxport(LIMITDWN));
	printf("\nRackup Sensor: %d", inauxport(RACKUP));
	printf("\nPort Limitpad: %d", inauxport(LIMITPAD1));
	printf("\nStbd Limitpad: %d", inauxport(LIMITPAD2));
	printf("\nCM Limit: %d", inauxport(CMLIMIT));
	DelayMilliSecs(3000);
      }
      break;

    case '8':   /*Instrument/Compass*/
      do{
	printf("\n\n0 -- Aux Instruments");
	printf("\n1 -- Benthic Chamber 1");
	printf("\n2 -- Benthic Chamber 2");
	printf("\n3 -- Microprofiler");
	printf("\n4 -- Not Assigned");
	printf("\n5 -- Video 1");
	printf("\n6 -- Video 2");
	printf("\n7 -- Compass");
	printf("\n8 -- Return to Test Menu");

	QueryChar("\nEnter Selection: ",'8',"012345678",&inkey);
	printf("\nControl-C to Exit\n\n");
	TPUiorelay( inkey - 48 );  /* Convert ASCII to decimal */
      } while( inkey != '8' );
      break;

    } /*End of Switch*/

  } while(menukey != '9');

  SENSOR_PWR_OFF
  return(0);
}


/*********************************************
      SETUP FUNCTION
*********************************************/

int setup(void){
  char menukey;
  struct tm	t;

  do{
    printf("\n\n\nRover Setup Menu");
    printf("\n\n  1 -- Set Clock");
    printf("\n  2 -- Change Mission Defaults");
    printf("\n  3 -- Change System Defaults");
    printf("\n  4 -- Change Calibration Coefficients");
    printf("\n  9 -- Exit");
    QueryChar("\nEnter Selection: ",'9',"12349",&menukey);

    switch(menukey){
      case '1':  /*Set Clock*/
	printf("\nSet Current Date and Time (CMD-D from CrossCut)");
	if(! QueryDateTime("\n<CR> for default", TRUE, &t))
	  return;
	SetTimeTM(&t, NULL);
	break;

      case '2':  /* Mission Defaults */
	break;

      case '3':  /* System Defaults */
	break;

      case '4':  /* Calibration Coefficients */
	break;
    }

  } while(menukey != '9');

}