/* ROVER CONTROL SOFTWARE VERSION 1.6 */

/*
   Simulation compiler: Turbo-C++ 1.01
   Cross-compiler: Aztec-C 5.2C
   Tattletale monitor: 1.32
	      ROM: 1.92
	      TPU: 1.14

Changes since v1.5:
--Benthic chambers powered up only when rack is down.
--Transponder could accidentally be turned off by "sleep" function;
  now corrected.
--Turn function data rate timer logic corrected.
--Turn timeout now drops through to next transit, regardless of heading.
--Transit timeout now drops through to next function.
--Current meter function uses time() function instead of TensMilliSecs()
  to avoid overflow on periods longer than 24 hours.
--Mission variables changed to type static.
--Central Controller looks for confirmation string from rack instruments
  before continuing with program.  Cycles power if no string detected.
--Designators 1 and 2 on benthic chambers, floodlights, and video systems
  changed to port and starboard.
--Benthic chamber calibration function disabled as the chamber doesn't seem
  to flush entirely unless the Rover moves first.

*/


#include <tt7.h>
#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>
#include <offload.h>
#include <serio.h>

#else                 /* For Turbo C */
#include <conio.h>
#include "tt7pclib.h"
#include "tt7pcio.h"
#endif

/************* DEFINITIONS ********************/

#define	CURRENT_HEADING		0
#define CURRENT_LOG		1

/************* FUNCTION PROTOTYPES ************/

int deploy(void);
int setup(void);
short currmet_cycle(int function, ulong log_time);

/************** MISSION DEFAULTS **************/

static ulong 	delay_time = 240,            /* Minutes */
		prime_heading_time = 60,
		cm_wait_time = 60,
		cm_data_rate = 10,
		dust_settle_time = 60,
		rack_down_time = 2280,
		video_on_time = 5,
		bc_cal_time = 60,
		transit_dist = 5,           /* Meters */
		max_sites = 5;

static short 	prime_heading_flag = 0;


/*************** MAIN ***********************/

main() {
  
  char menukey;
  short errcode;
  int t;
  time_t lt;

  tt7init();
  file_init();
  SetTickRate(TICK_RATE);
  printf("\nTick Rate = %ld", GetTickRate() );
  printf("\n\nTattletale Model 7 Initialized");

/********** Lower Current Meter **************/

  sensor_pwr_on();   /* Lower the current meter in case of reset */
  delaysecs(1);
  printf("\n\nCurrent meter down cycle");
  errcode = currmet_down();
  if(errcode){
    printf("\nCurrent meter error: %d", errcode);
  }
  else printf("\nCurrent meter down");


  while(1) {
    sensor_pwr_off();
    g_program_level = 1;     /* One ping on transponder */
    SerInFlush();

    printf("\n\n\nRover System Main Menu V1.6");
    printf("\n\n  1 -- Test Functions");
    printf("\n  2 -- Setup Deployment");
    printf("\n  3 -- Deploy");
    printf("\n  4 -- Data Files");
    printf("\n  5 -- Sleep");
    printf("\n  9 -- Reset");
    QueryChar("\nEnter Selection: ",'0',"1234589",&menukey);

    switch(menukey){
      case '1':
	testfunc();    /* In rovtest.c */
	break;

      case '2':
	setup();
	break;

      case '3':
	transponder_pwr_on();
	delaysecs(1);

	printf("\n\n\nDeployment delay time: %lu minutes.", delay_time);
	printf("\nTransit distance: %lu meters", transit_dist);
	printf("\nRack down time: %lu min.", rack_down_time);
	printf("\nMax. sites: %lu", max_sites);
	printf("\nVideo on time: %lu min.", video_on_time);
	printf("\nPrime heading time: %lu min.", prime_heading_time);
	printf("\nMaximum current wait time: %lu min.", cm_wait_time);
	printf("\nCurrent meter data rate: %lu min.", cm_data_rate);
	printf("\nBenthic Chamber cal time: %lu min.", bc_cal_time);
	lt = time(0);
	printf("\n\nClock reads: %s", asctime(localtime(&lt)));
	if(!QueryYesNo("\n\nDeploy function will erase data memory.\nDo you want to continue? ", FALSE) ) break;

	printf("\n\n\nRover will activate in: %lu minutes.", delay_time);
	printf("\n\nDisconnect comm. cable and deploy...");


	errcode = deploy();		   /* This is the mission */
	if(errcode) g_program_level = 1;

         /* Shut down procedure */
	stbd_flood_pwr_off();   /* Shut these off in case an error sent us here */
	port_flood_pwr_off();
	video(SHUT_DOWN);

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

	sensor_pwr_off();
	delaysecs(2);
	mem_to_disk();          /* Store any data in memory to disk */
	clear_auxports();       /* Make sure all power is off */
	transponder_pwr_on();   /* Turn the transponder on */
	delaysecs(5);	      /* Let transponder settle for 5 sec. */
	SerInFlush();

	while(1){
	  printf("\nMission complete, press any key next two seconds to exit...");
	  delaysecs(2);
	  if( SerByteAvail() ) break;
	  Sleep(0);
	  for(t=0;t<=116;t++){
	    Sleep(TICK_RATE);   /* 1 second */
	    transpond();
	  }
	}

	break;

      case '4':
	datafiles();
	break;

      case '5':          /* Sleep */
	clear_auxports();
	SerInFlush();
	Sleep(0);
	while(1){
	  printf("\nSleep mode, press any key next two seconds to wake up...");
	  delaysecs(2);
	  if( SerByteAvail() ) break;
	  Sleep(58*TICK_RATE);
	}
	break;

      case '9':
	do{
	  printf("\n\n  1 -- Reset system");
	  printf("\n  2 -- Disable Auto-Launch");
	  printf("\n  9 -- Exit");
	  QueryChar("\nEnter Selection: ",'9',"129",&menukey);
	  switch(menukey){

	    case '1':  /* Reset system */
#ifdef AZTEC_C
	      reset();
#else
	      exit(0);   /* Go back to DOS if Turbo C */
#endif
	      break;

	    case '2':  /* Disable Auto-Launch */
	      MonCmd("RUN 0");
	      printf("\n\nAuto-Launch disabled.  Monitor has control on next reset.");
	      break;
	  }

	} while(menukey != '9');

     }    /* End of switch */
  }       /* End of while */
}         /* End of main */

/*********************************************
	     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)) break;
	SetTimeTM(&t, NULL);
	break;

      case '2':  /* Mission Defaults */

	QueryNum("\nEnter deploy delay time (min): ", "%lu", "%lu", &delay_time);
	QueryNum("\nEnter travel distance in meters: ", "%lu", "%lu", &transit_dist);
	QueryNum("\nEnter current meter data rate (min): ", "%lu", "%lu", &cm_data_rate);
	QueryNum("\nEnter prime heading decision time (min): ", "%lu", "%lu", &prime_heading_time);
	QueryNum("\nEnter maximum current wait time (min): ", "%lu", "%lu", &cm_wait_time);
	QueryNum("\nEnter dust-settle time: ", "%lu", "%lu", &dust_settle_time);
	QueryNum("\nEnter rack down time (min): ", "%lu", "%lu", &rack_down_time);
	QueryNum("\nEnter number of sites: ", "%lu", "%lu", &max_sites);
	QueryNum("\nEnter video on time (min): ", "%lu", "%lu", &video_on_time);
//	QueryNum("\nEnter Benthic Chamber cal time (min): ", "%lu", "%lu", &bc_cal_time);

	break;

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

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

  } while(menukey != '9');

  return 0;
}


/*******************************************************
	      Deploy Function
********************************************************/

int deploy(void){
  short errcode, prime_heading;
  int site_count = 0, transit_ticks, x;
  unsigned long t;

  clear_disk_files();

  transit_ticks = (transit_dist * 100)/ODOCAL;   /* Convert to odometer ticks */

  /********** Deployment Delay with Transpond ********/

  rov_sleep(delay_time);

  /********** Raise Current Meter **********/

  printf("\n\nCurrent Meter Up Cycle");
  errcode = currmet_up();
  if(errcode){
    printf("\nCurrent meter error: %hd", errcode);
    return -1;                                           /* Fatal Error */
  }
  else printf("\nCurrent meter up");

  /* Bail out if we land on a steep slope */
  delaysecs(1);
  if( abs(tiltx_deg()) > 19 || abs(tilty_deg()) > 19 ){
    printf("\nExcessive tilt, exiting deployment");
    return -1; 						 /* Fatal Error */
  }

  g_program_level = 2;                  /* Two pings on transponder */

  /*********** Mission Loop ********************/

  site_count = 1;

  while(site_count <= max_sites){
    if(site_count == 3){
      transponder_pwr_off();        /* Shut down transponder to save power */
    }

    prime_heading = currmet_cycle(CURRENT_HEADING, 0);

    /* Short transit to dig us out of the hole */
    printf("\n\nFirst transit cycle");
    errcode = transit_rov(2*100/ODOCAL);  /* 2 meters */
    if( errcode ){
      printf("\nTransit error: %d", errcode);
    }
    else printf("\nFirst transit complete");

    /* Turn to the prime heading*/
    delaysecs(1);
    printf("\n\nTurn cycle");
    errcode = turn_rov(prime_heading);
    if( errcode ){
      printf("\nTurn error: %d", errcode);
    }
    else printf("\nTurn complete");

    /* Second transit */
    delaysecs(1);
    printf("\n\nSecond transit cycle");
    errcode = transit_rov(transit_ticks);
    if( errcode ){
      printf("\nTransit error: %d", errcode);
    }
    else printf("\nSecond transit complete");


  /*********** Wait for Dust to Settle **********/

    printf("\nDust settle time...");
    currmet_cycle(CURRENT_LOG, dust_settle_time);

  /**************** Video On *************/

    delaysecs(1);
    video(PWR_ON_STBD);
    video(PWR_ON_PORT);
    delaysecs(10);
    video(RECORD_STBD);
    delaysecs(1);
    video(RECORD_PORT);
    delaysecs(1);
    stbd_flood_pwr_on();
    port_flood_pwr_on();

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

    delaysecs(1);
    printf("\n\nRack Down Cycle");
    errcode = rack_down_cycle();
    if(errcode){
      switch(errcode){
	case (RACK_LIMITDWN_ERR):
	  printf("\nRack limit down error");       /* Continues anyway */
	  break;

	default:
	  printf("\nRack down error: %d", errcode);
	  return errcode;                           /* Fatal Error */
      }
    }
    else printf("\nRack Down");

    delaysecs(1);
    sensor_pwr_off();

/*********** Start Instruments **************/

    printf("\nStarting Instruments...");
    clear_inst_files();

    for(x=0; x<3; x++){
      stbd_bc_pwr_on();
      delaysecs(5);
      if( inst_start(STBD_BENCHAM) ){
	stbd_bc_pwr_off();
	printf("\nStbd benthic chamber not responding");
	rov_sleep(1);
      }
      else break;
    }

    for(x=0; x<5; x++){
      port_bc_pwr_on();
      delaysecs(5);
      if( inst_start(PORT_BENCHAM) ){
	port_bc_pwr_off();
	printf("\nPort benthic chamber not responding");
	rov_sleep(1);
      }
      else break;
    }

    setcomchan(AUX);
    TPuts("1");           /* Start syringe sampler */
    delaysecs(1);


/*********** Video ****************/

    printf("\nVideo on time");
    rov_sleep(video_on_time);

    stbd_flood_pwr_off();
    port_flood_pwr_off();
    video(SHUT_DOWN);

/************** Current Meter Data *************/

    g_program_level = 3;
    printf("\nRack down current meter data...");
    currmet_cycle(CURRENT_LOG, rack_down_time);       /* Stays here for rack_down_time */

/**************** Retreive Instrument Data ***************/

    inst_transfer_data(STBD_BENCHAM);
    printf("\nStbd benthic chamber data loaded");

    inst_transfer_data(PORT_BENCHAM);
    printf("\nPort benthic chamber data loaded");

    stbd_bc_pwr_off();
    port_bc_pwr_off();

/****************** Rack Up *****************/

    printf("\n\nRack Up Cycle");
    errcode = rack_up_cycle();
    if(errcode){
      switch(errcode){
	case (RACK_LIMITUP_ERR):
	  printf("\nRack limit up error");       /* Continues anyway */
	  break;

	default:
	  printf("\nRack up error: %d", errcode);
	  return errcode;                           /* Fatal Error */
      }
    }
    else printf("\nRack Up");

/******************* Benthic Chamber Cal Time ***************/

//    printf("\nBC cal time");

//    delaysecs(1);
//    bc1_pwr_on();
//    delaysecs(2);
//    bc2_pwr_on();
//    delaysecs(3);

//    setcomchan(BENCHAM1);
//    TPuts("\n");
//    delaysecs(5);
//    TPuts("5\n");            /* Start benthic chamber cal cycle */
//   delaysecs(1);

//    setcomchan(BENCHAM2);
//    TPuts("\n");
//    delaysecs(5);
//    TPuts("5\n");
//    delaysecs(1);

//    setcomchan(COMPASS);     /* Low power position */
//    currmet_cycle(CURRENT_LOG, bc_cal_time);       /* Stays here for bc_cal_time */

//    inst_transfer_data();
//    bc1_pwr_off();
//    bc2_pwr_off();

/****************** Store Data to Disk ***************/

    delaysecs(1);
    mem_to_disk();
    clear_rov_files();

    site_count++;

  }    /* End of site count WHILE loop */

  return 0;
}


/****************************************************
	  Current Meter Cycle

   The current meter cycle performs three functions.  First, it determines
   the "prime_heading" to be used in subsequent transits between sites.
   The use of a single heading prevents the Rover from traveling in circles.
   The Prime Heading section watches the current a predetermined time to
   find the direction of the strongest current.  This current direction
   is stored as the prime heading.  Second, the Current Wait section waits
   until the current is an acceptable direction before allowing the Rover
   to transit.  If the Current Wait Loop takes too long, the
   prime_heading_flag is reset and the Prime Heading Loop determines a new
   prime heading. Third, the current log section just logs the current and
   Rover system data. The g_heading variable is updated once while the Rover
   is stationary to avoid having to turn the sensor power on each time data.
   is stored.

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

short currmet_cycle(int function, ulong log_time){

  short prime_heading, heading_diff, max_curr_speed, max_curr_dir;
  ulong sample_time, ph_time, cw_time, data_rate;

  printf("\n\nCurrent Meter cycle...");
  sensor_pwr_on();
  delaysecs(5);
  g_heading = heading();
  sensor_pwr_off();

  data_rate = cm_data_rate * 60;  		/* Convert to seconds */
  g_cmrotor_ticks = 0;

  sample_time = time(NOW);   			/* Initialize timers */
  log_time = time(NOW) + (log_time * 60);
  ph_time =  time(NOW) + (prime_heading_time * 60);

  Sleep(0);

  while(1){
    cmrotor();             /* Check the rotor position */
    transpond();           /* Check the transponder */
    Sleep(TICK_RATE);   /* 1 second */

    if( time(NOW) >= sample_time ){
      sample_time += data_rate;    /* Figure next sample time */

      g_curr_speed = (g_cmrotor_ticks * CMROTOR_CAL)/(data_rate);  /* mm/sec */
      g_curr_dir = cmvane_deg();       /* Store cmvane relative heading */
      g_cmrotor_ticks = 0;                 /* Reset counters */
      store_data(CURRMET_DATA);            /* Store some data */

      if( g_curr_speed > max_curr_speed ){
	max_curr_speed = g_curr_speed;     /* Remember the highest speed */
	max_curr_dir = g_curr_dir;
      }

      if(function == CURRENT_HEADING){

	/* Prime Heading Section */
	if( prime_heading_flag == 0 && time(NOW) >= ph_time ){
	  prime_heading = ( g_heading + max_curr_dir ) % 360;  /* Prime Heading Calculation */
	  printf("\nPrime heading = %hd", prime_heading);
	  prime_heading_flag = 1;
	  cw_time = time(NOW) + (cm_wait_time * 60);  /* Set up timer for current wait */
	  printf("\nSwitching to current wait mode...");
	}

	/* Current Wait Section */
	else if( prime_heading_flag == 1 ){
	  heading_diff = prime_heading - ( (g_heading + cmvane_deg()) % 360 );
	  if(heading_diff > 180) heading_diff -= 360;
	  else if(heading_diff < (-180)) heading_diff += 360;

	  printf("\nHeading_diff = %hd", heading_diff);
	  printf("\nCurrent speed = %hd", g_curr_speed);

	  if( (abs(heading_diff) < 60) && g_curr_speed >= CURRENT_THRESHOLD ){
	    printf("\nCurrent's OK, lets go!");
	    return prime_heading;
	  }

	  else if( time(NOW) >= cw_time){
	    if( max_curr_speed < CURRENT_THRESHOLD ){
	      printf("\nNo detected current, moving along last prime heading");
	      return prime_heading;  /* Might be something wrong with rotor, go anyway */
	    }
	    else{
	      prime_heading_flag = 0;     /* Do the prime heading thing */
	      ph_time = (prime_heading_time * 60) + time(NOW);  /* Set up timer for prime heading */
	      printf("\nCurrent wait mode timeout, returning to prime heading mode");
	    }
	  }

	}     /* End of Current Wait section */
      }       /* End of Current Heading section */

      /* Current Log Section */
      else if( function == CURRENT_LOG && (time(NOW) > log_time) )
	return 0;

    }                   /* End of data_rate IF */
  }                     /* End of WHILE loop */
}                       /* End of currmet_cycle */