/* ROVER TEST FUNCTIONS */

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

#include "rovtest.h"
#include "rovlib.h"
#include "rovint.h"
#include "rovsys.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, i;
  unsigned long channel;
  unsigned short portbit;
  long lasttime, last_menu_time, last_cmrotor_time;

  sensor_pwr_on();            /* Turn on sensor power for testing */

  do{
    inkey = 0;
    SerInFlush();

    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", batt_volt() );
	  printf("\nSystem Current: %.3f amps", hp_current() );
	  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", batt_volt() );
	  printf("\nSystem Current: %.3f amps", hp_current() );
	  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: %d degrees", (int)cmvane_deg());
	  printf("\nCM Rotor Turns: %d", g_cmrotor_ticks/2);
	  if(cm_limit()) printf("\nCurrent meter down limit!");

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

          i = 0;
	  last_cmrotor_time = TensMilliSecs();

	  while(1){
	   if( timer(last_cmrotor_time, 100) ){
	     cmrotor();                    /* Check the current meter rotor */
	     i++;
	     last_cmrotor_time = TensMilliSecs();
	   }
	   if( i >= 3 ) break;  /* print the menu again */
	   if( kbhit() ){
	     inkey = tolower( SerGetByte() );  /* Get command in lower case */
	     if(inkey == 'e') break;           /* Get us out of here */
	     move_currmet(inkey);
	   }
	  }

	} while(inkey != 'e');     /* End of do/while loop */

	move_currmet('s');
	tpurupt(CMMOT_SHAFT, OFF);
	break;

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

	  switch(inkey){
	    case '1':  /* Power Video 1 */
	      video(PWR_ON_1);
	      break;

	    case '2':  /* Power Video 2 */
	      video(PWR_ON_2);
	      break;

	    case '3':  /* Record Video 1 */
	      video(RECORD_1);
	      break;

	    case '4':  /* Stop Video 1 */
	      video(STOP_1);
	      break;

	    case '5':  /* Record Video 2 */
	      video(RECORD_2);
	      break;

	    case '6':  /* Stop Video 2 */
	      video(STOP_2);
	      break;

	    case '7':  /* Shutdown Both Videos */
	      video(SHUT_DOWN);
	      break;

	    case '8':  /* Power Floods */
	      flood1_pwr_on();
	      flood2_pwr_on();
	      delaysecs(10);
	      flood1_pwr_off();
	      flood2_pwr_off();
	      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();
	      delaysecs(2);
	      printf("\n\nPing transponder now. Hit any key to exit...");

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

	      transponder_pwr_off();
	      break;
	    }       /* End of camera/strobe switch */

	}while(inkey != '9');
      break;        /* End of camera/strobe 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", batt_volt() );
	printf("\nHP System Current: %.3f amps", hp_current() );
	printf("\nLP System Current: %.3f amps", lp_current() );
	printf("\nGround Fault Voltage: %.2f volts", gf_volt() );
	printf("\n\nRover Tilt: X=%.1f, Y=%.1f degrees", tiltx_deg(), tilty_deg());
	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
*********************************************************************/