/* 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, test_heading = 0;
  unsigned short portbit;
  long lasttime, last_menu_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("\nGround Fault Voltage: %.2f volts", gf_volt() );
	  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  H = Turn to heading");
	  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 */
	      if(inkey == 'h'){
		QueryNum("\nEnter heading (0-359): ", "%lu", "%lu", &test_heading);
		turn_rov(test_heading);
	      }
	      else move_rov(inkey);
	    }
	    if(inkey == 'e') break;
	  }
	} while(inkey != 'e');
	move_rov('s');
	tpurupt(ODOMETER, OFF);
	break;


      case '2':   /*Rack Functions*/
	tpurupt(STBD_LEADSCREW, ON);
	tpurupt(PORT_LEADSCREW, ON);
	g_stbd_leadscrew_ticks = 0;
	g_port_leadscrew_ticks = 0;

	do{
	  printf("\n\n\nBattery Voltage: %.2f volts", batt_volt() );
	  printf("\nSystem Current: %.3f amps", hp_current() );
	  printf("\nGround Fault Voltage: %.2f volts", gf_volt() );
	  printf("\nStbd Leadscrew Half-turns: %d", g_stbd_leadscrew_ticks);
	  printf("\nPort Leadscrew Half-turns: %d", g_port_leadscrew_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(STBD_LEADSCREW, OFF);
	tpurupt(PORT_LEADSCREW, OFF);
	break;


      case '3':
	currmet_pwr_on();
	delaysecs(1);
	printf("\n\n\nControl-C to exit...");
	TPUiorelay(CURRMET);  /* Talk to the Sontek Argonaut on COM 0 */
	currmet_pwr_off();
	break;

      case '4':  /* Video System */
	do{
	  printf("\n\n\n1 -- Power Stbd Video");
	  printf("\n2 -- Power Port Video");
	  printf("\n3 -- Record Stbd Video");
	  printf("\n4 -- Stop Stbd Video");
	  printf("\n5 -- Record Port Video");
	  printf("\n6 -- Stop Port Video");
	  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_STBD);
	      break;

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

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

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

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

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

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

	    case '8':  /* Power Floods */
	      stbd_flood_pwr_on();
	      port_flood_pwr_on();
	      delaysecs(10);
	      stbd_flood_pwr_off();
	      port_flood_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') filmcam(1);
	      } while(inkey == 'y');
	    break;

	    case '2':              /* Transponder */
	      g_program_level = 3;
	      sensor_pwr_off();
	      transponder_pwr_on();
	      delaysecs(2);
	      printf("\n\nPing transponder during next two minutes...");
	      rov_sleep(2);
	      transponder_pwr_off();
	      sensor_pwr_on();
	      g_program_level = 1;
	      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( stbd_limit_pad() ) printf("\nStbd Limitpad");
	if( port_limit_pad() ) printf("\nPort Limitpad");
	DelayMilliSecs(3000);
      }
      break;

    case '8':   /*Instrument/Compass*/
      stbd_bc_pwr_on();
      port_bc_pwr_on();
      mp_pwr_on();

      while(1){
	SerInFlush();

	printf("\n1 -- Stbd Benthic Chamber");
	printf("\n2 -- Port Benthic Chamber");
	printf("\n3 -- Microprofiler");
	printf("\n4 -- Not Assigned");
	printf("\n5 -- Stbd Video");
	printf("\n6 -- Port Video");
	printf("\n7 -- Compass");
	printf("\n9 -- Return to Test Menu");

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

      stbd_bc_pwr_off();
      port_bc_pwr_off();
      mp_pwr_off();
      break;

    } /*End of Switch*/

  } while(menukey != '9');

  sensor_pwr_off();
  return(0);
}