/* ROVER CONTROL SOFTWARE VERSION 1.4 */

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

   Note: New monitor required changing pin initialization in ROVINT.C
*/


#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);

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

unsigned long 	delay_time = 1,            /* Minutes */
		prime_heading_time = 2,
		max_curr_wait_time = 2,
		cm_data_rate = 1,
		rack_down_time = 1,
		video_on_time = 1,
		transit_dist = 2,           /* Meters */
		max_sites = 2;

short prime_heading_flag = 0;


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

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

  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.4");
    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  8 -- Test Deployment");
    printf("\n  9 -- Reset");
    QueryChar("\nEnter Selection: ",'9',"1234589",&menukey);

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

      case '2':
	setup();
	break;

      case '3':

	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.", max_curr_wait_time);
	printf("\nCurrent meter data rate: %lu min.", cm_data_rate);

	if(!QueryYesNo("\n\nDeploy function will erase data memory.\nDo you want to continue? ", FALSE) ) break;

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

        transponder_pwr_on();
	delaysecs(2);

	errcode = deploy();
	if(errcode) g_program_level = 1;

         /* Shut down procedure */
	flood1_pwr_off();  
	flood2_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 '8':
/*	testdeploy();     In rovtest.c */
	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", &max_curr_wait_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);

	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;
  unsigned long t;

  file_init();
  clear_disk_files();

  /* Convert deployment variables to values usable to library functions */
  transit_dist *= 100/ODOCAL;   /* Convert to odometer ticks */
  delay_time *= 60;             /* Convert to seconds */
  rack_down_time *= 60;
  video_on_time *= 60;

  cm_data_rate *= (60*100);        /* Convert to tens of Milliseconds */
  prime_heading_time *= (60*100);
  max_curr_wait_time *= (60*100);

  /********** 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;
  }
  else printf("\nCurrent meter up");

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

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

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

  site_count = 0;

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

    prime_heading = currmet_cycle(CURRENT_HEADING);

    /* 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);
      return errcode;                          /* Fatal Error */
    }
    else printf("\nFirst transit complete");

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

    /* Second transit */
    delaysecs(1);
    printf("\n\nSecond transit cycle");

    errcode = transit_rov(transit_dist);
    if( errcode ){
      printf("\nTransit error: %d", errcode);
      return errcode;                          /* Fatal Error */
    }
    else printf("\nSecond transit complete");

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

    delaysecs(1);
    video(PWR_ON_1);
    video(PWR_ON_2);
    delaysecs(5);
    video(RECORD_1);
    video(RECORD_2);
    delaysecs(1);
    flood1_pwr_on();
    flood2_pwr_on();

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

    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();

    printf("\nVideo on time");
    rov_sleep(video_on_time);
    flood1_pwr_off();
    flood2_pwr_off();
    video(SHUT_DOWN);

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

    g_program_level = 3;
    currmet_cycle(CURRENT_LOG);

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

    printf("\n\nRack Up Cycle");
    errcode = rack_up_cycle();
    if(errcode){
      printf("\nRack up error: %d", errcode);
      return errcode;                          /* Fatal Error */
    }
    else printf("\nRack Up");

    site_count++;

  }    /* End of site count WHILE loop */

  return 0;
}


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

short currmet_cycle(int function){

  short prime_heading, heading_diff, max_curr_speed, max_curr_dir;
  ulong cm_time, cm_log_time, cm_rotor_time;

/*
   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. I update the g_heading variable once while the Rover
   is stationary to avoid having to turn the sensor power on each time I
   store data.
*/

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

  g_cmrotor_ticks = 0;
  cm_time = TensMilliSecs();
  cm_log_time = cm_time;
  cm_rotor_time = cm_time;
  Sleep(0);

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

    if( timer(cm_rotor_time, cm_data_rate) ){
      g_curr_speed = ((10*g_cmrotor_ticks)/(cm_data_rate/100))*CMROTOR_CAL;
      g_curr_dir = cmvane_deg();       /* Store cmvane relative heading */
      g_cmrotor_ticks = 0;                 /* Reset counters */
      cm_rotor_time = TensMilliSecs();
      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 && timer(cm_time, prime_heading_time) ){
	  prime_heading = ( g_heading + max_curr_dir ) % 360;  /* Prime Heading Calculation */
	  printf("\nPrime heading = %hd", prime_heading);
	  prime_heading_flag = 1;
	  cm_time = TensMilliSecs();  /* 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;  /* Current's OK, lets go! */
	  }

	  if( timer(cm_time, max_curr_wait_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 */
	      cm_time = TensMilliSecs();  /* 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 && timer(cm_log_time, rack_down_time) )
	return 0;

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