/* ROVER CONTROL SOFTWARE VERSION 1.8 */

/*
   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.7:
--Sontek Argonaut current meter interface completed.

*/


#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 ********************/


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

int deploy(void);
int setup(void);
short cm_cycle();

/************** 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 = 12000,
		video_on_time = 2,
		time_lapse_interval = 720,
		transit_dist = 5,           /* Meters */
		max_sites = 21;

static short 	prime_heading_flag = 0, prime_heading = 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");

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

    printf("\n\n\nRover System Main Menu V1.8");
    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("\nDust settle time: %lu min.", dust_settle_time);
	printf("\nRack down time: %lu min.", rack_down_time);
	printf("\nVideo on time: %lu min.", video_on_time);
	printf("\nTime-lapse camera interval: %lu min.", time_lapse_interval);
	printf("\nMax. sites: %lu", max_sites);
	printf("\nPrime heading time: %lu min.", prime_heading_time);
	printf("\nMaximum current wait time: %lu min.", cm_wait_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);

	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(58L*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 transit distance (meters): ", "%lu", "%lu", &transit_dist);
	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 (min): ", "%lu", "%lu", &dust_settle_time);
	QueryNum("\nEnter rack down time (min): ", "%lu", "%lu", &rack_down_time);
	QueryNum("\nEnter video on time (min): ", "%lu", "%lu", &video_on_time);
	QueryNum("\nEnter time-lapse camera interval (min): ", "%lu", "%lu", &time_lapse_interval);
	QueryNum("\nEnter number of sites: ", "%lu", "%lu", &max_sites);
	break;

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

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

  } while(menukey != '9');

  return 0;
}


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

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

  /******** Initialize ***********/

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

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

  cm_start();			// Start the Argonaut
  rov_sleep(delay_time);	// Do nothing except transpond for a while
  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 = cm_cycle(CM_TIMEOUT);
    cm_stop();		   	// Protect Argonaut from glitches

    /* 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...");
    cm_start();				// Start Argonaut
    rov_sleep(dust_settle_time);

  /**************** Rack up video *************/

    printf("\nRack up video time...");
    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();

    rov_sleep(video_on_time);

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

/******************* 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);                // Sleep 1 min
      }
      else break;
    }

    for(x=0; x<3; 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;
    }

/*********** Rack down video ****************/

    printf("\nRack down video time...");
    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();

    rov_sleep(video_on_time);

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

/************** Rack Down Pictures *************/

    g_program_level = 3;
    printf("\nRack down picture time...");
    start_time = time(NOW);
    end_time = start_time + (rack_down_time * 60L);
    next_picture_time = start_time + (time_lapse_interval * 60L);
    filmcam(2);              	//Take 2 pictures

    while(1){
      rov_sleep(1);
      if(time(NOW) >= end_time) break;       	//Time's up
      if(time(NOW) >= next_picture_time){
	filmcam(2);                             //Take 2 pictures
	next_picture_time += time_lapse_interval * 60L;
      }
    }

/**************** 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");


/****************** 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 two 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
   is returned and the Rover moves anyway.

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

short cm_cycle(void){

  short heading_diff, max_curr_speed = 0, max_curr_dir;
  ulong ph_time, cw_time;

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

  ph_time =  time(NOW) + (prime_heading_time * 60);
  cw_time = time(NOW) + (cm_wait_time * 60);  /* Set up timer for current wait */

  Sleep(0);

  while(1){
    if(cm_data(CM_TIMEOUT)){     	// Get some data from the Argonaut
      printf("\nCurrent meter error");
      store_data(CM_DATA_ERR);
      return prime_heading;
    }
    store_data(CURRMET_DATA);

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

    /* Prime Heading Section */
    if( prime_heading_flag == 0 && time(NOW) >= ph_time ){
      prime_heading = (max_curr_dir + 180) % 360;  // Head opposite current direction
      printf("\nMax current speed = %hd", max_curr_speed);
      printf("\nPrime heading = %hd", prime_heading);
      prime_heading_flag = 1;
      cw_time = time(NOW) + (cm_wait_time * 60);  /* Reset timer for current wait */
      printf("\nSwitching to current wait mode...");
    }

    /* Current Wait Section */
    else if( prime_heading_flag == 1 ){
      heading_diff = prime_heading - ((g_curr_dir + 180)%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) < 45) && g_curr_speed >= CURRENT_THRESHOLD ){
	printf("\nCurrent's OK, lets go!");
	return prime_heading;
      }

      if( time(NOW) >= cw_time){
	printf("\nNo detected current, moving along last prime heading");
	store_data(CM_WAIT_ERR);
	return prime_heading;  /* Might be something wrong with cm, go anyway */
      }

    }    	// End of current wait section
  }           	// End of WHILE loop
}             	// End of cm_cycle