/* ROVER TEST FUNCTIONS */

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

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

#ifdef AZTEC_C
#include <userio.h>

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


/*********************************************
      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/Transponder");
    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*/
	tpurupt(ODOMETER, ON);
	g_odometer_ticks = 0;
	do{
	  printf("\n\n\nBattery Voltage: %.2f volts",
	    (float)get_ad_counts(BATTCHAN)/BATTCAL);
	  printf("\nSystem Current: %.3f amps",
	    (float)get_ad_counts(HPCURRCHAN)/HPCURRCAL);
	  printf("\nHeading: %d degrees magnetic", heading());
	  printf("\nDistance: %.1f cm", g_odometer_ticks * ODOCAL);

	  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');
	tpurupt(ODOMETER, OFF);
	break;


      case '2':   /*Rack Functions*/
	tpurupt(LEADSCREW1, ON);
	tpurupt(LEADSCREW2, ON);
	g_leadscrew1_ticks = 0;
	g_leadscrew2_ticks = 0;

	do{
	  printf("\n\n\nBattery Voltage: %.2f volts",
	    (float)get_ad_counts(BATTCHAN)/BATTCAL);
	  printf("\nSystem Current: %.3f amps",
	    (float)get_ad_counts(HPCURRCHAN)/HPCURRCAL);
	  printf("\nLeadscrew-1 Half-turns: %d", g_leadscrew1_ticks);
	  printf("\nLeadscrew-2 Half-turns: %d", g_leadscrew2_ticks);
	  if( rack_up() ) printf("\nRack Up!");

	  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');
	tpurupt(LEADSCREW1, OFF);
	tpurupt(LEADSCREW2, OFF);
	break;


      case '3':   /*Current Meter Functions*/
	tpurupt(CMMOT_SHAFT, ON);
	g_cmmot_shaft_ticks = 0;
	g_cmrotor_ticks = 0;

	do{
	  printf("\n\n\nCM Motor Shaft Turns: %d", g_cmmot_shaft_ticks/4);
	  printf("\nCM Vane Direction: %.1f degrees", cmvane_dir()/CMVANECAL);
	  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');
	tpurupt(CMMOT_SHAFT, OFF);
	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();
	      delaysecs(10);
	      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/Strobe/Transponder */
	do{
	  printf("\n\n\n1 -- Film Camera/Strobe");
	  printf("\n2 -- Transponder");
	  printf("\n9 -- Exit");
	  QueryChar("\n\nEnter Selection: ",'9',"129",&inkey);

	  switch(inkey){
	    case '1':    /* Film Camera/Strobe */
	      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(TRIGGERTIME);
		  filmcam_trig_off();
		  strobe_pwr_off();
		}
	      } while(inkey == 'y');
	    break;

	    case '2':              /* Transponder */
	      transponder_pwr_on();
	      tpurupt(TRANSPOND_IN, ON);
	      g_transpond_in = 0;
	      printf("\n\nPing transponder now. Hit any key to exit...");

	      while(1){
		if( g_transpond_in ){
		  printf("\nTransponder should ping again in 3 seconds");
		  delaysecs(3);
		  transpond_out();
		  g_transpond_in = 0;
		}

		if( kbhit() ){
		  SerInFlush();
		  break;
		}
	      }     /* End of while */

            tpurupt(TRANSPOND_IN, OFF);
	    transponder_pwr_off();
	    break;
	    }       /* End of switch */

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

      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++){

	    printf("  %.3f", (float)get_ad_counts(chan)/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", (float)get_ad_counts(BATTCHAN)/BATTCAL);
	printf("\nSystem Current: %.3f amps", (float)get_ad_counts(HPCURRCHAN)/HPCURRCAL);
	printf("\nGround Fault Voltage: %.2f volts", (float)get_ad_counts(GFVOLTCHAN)/GFVOLTCAL);
	printf("\n\nRover Tilt: X=%.1f, Y=%.1f degrees", ((float)get_ad_counts(TILTXCHAN)-2.5)/TILTXCAL,
	  ((float)get_ad_counts(TILTYCHAN)-2.5)/TILTYCAL);
	printf("\nRover Heading: %d degrees magnetic\n", heading());

	if( limit_up() ) printf("\nRack Limit Up");
	if( limit_dwn() ) printf("\nRack Limit Down");
	if( rack_up() ) printf("\nRack Up");
	if( limit_pad1() ) printf("\nPort Limitpad");
	if( limit_pad2() ) printf("\nStbd Limitpad");
	if( cm_limit() ) printf("\nCurrent Meter Limit");
	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);
}


/*********************************************************************
	    Test Deployment Function
*********************************************************************/

int testdeploy(void) {

  unsigned long transitdist = 0, heading1 = 0, heading2 = 0, deploydelay = 0,
		rack_down_time = 0;
  long starttime;
  int headingdiff, errcode;

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

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

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

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

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

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

  file_init();           /* Erase the old data files */
  delaysecs(deploydelay);
  sensor_pwr_on();
  delaysecs(5);	     /* Let compass settle */

/************Raise Current Meter**************/
/*
  printf("\nCurrent meter up cycle");

  if(errcode = currmet_up()){
    printf("\nCurrent meter error: %d", errcode);
    sensor_pwr_off();
    return errcode;
  }

  printf("\nCurrent meter up");
*/

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

  printf("\nFirst Heading Turn");
  if(errcode = turn_rov(heading1)){
    printf("First turn error: %d", errcode);
    sensor_pwr_off();
    return errcode;
  }
  printf("\nTurn complete");

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

  DelayMilliSecs(500);
  printf("\nFirst transit begins");
  if(errcode = transit_rov(transitdist)){
    printf("\nFirst transit error:", errcode);
    sensor_pwr_off();
    return errcode;
  }
  printf("\nFirst transit complete");

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

  DelayMilliSecs(500);
  printf("\nSecond Heading Turn");
  if( errcode = turn_rov(heading2) ){
    printf("\nSecond turn error %d", errcode);
    sensor_pwr_off();
    return errcode;
  }
  printf("\nSecond Turn Complete");

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

  DelayMilliSecs(500);
  printf("\nSecond transit begins");
  if( errcode = transit_rov(transitdist) ){
    printf("\nSecond transit error %d", errcode);
    sensor_pwr_off();
    return errcode;
  }
  printf("\nSecond transit complete");

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

  delaysecs(1);
  flood1_pwr_on();
  flood2_pwr_on();
  delaysecs(1);
  printf("\nRack Down");
  if(errcode = rack_down_cycle()){
    switch(errcode){
      case (RACK_LIMITDWN_ERR):
	printf("\nRack Limit Down Error");
	flood1_pwr_off();
	flood2_pwr_off();
	rack_up_cycle();
	break;

      default:
	printf("\nRack down error: %d", errcode);
	break;
    }
  }

  flood1_pwr_off();
  flood2_pwr_off();
  delaysecs(rack_down_time);

  printf("\nRack Up Cycle");
  if(errcode = rack_up_cycle()){
    printf("\nRack up error: %d", errcode);
  }

/************Lower Current Meter**************/
/*
  printf("\nCurrent meter down cycle");
  if(errcode = currmet_down()){
    printf("\nCurrent meter error: %d", errcode);
    sensor_pwr_off();
    return errcode;
  }

*/
  printf("\nEnd of mission");
  sensor_pwr_off();
  return 0;
}