/* "ROVLIB"  ROVER FUNCTION LIBRARY" */

#include "rovlib.h"

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

#ifdef AZTEC_C

#else
void reset(void){
  exit(0);
}
#endif

/********************************************************
	     Auxilliary Port Routines
********************************************************

	   Auxilliary Output Port Routine

In: Port number (1-4), 1 or 0 to set or clear bits in port, output
    byte (0 to FF).
Return: Void

The routine first checks to see if the port is the high byte
of the 16 bit port word.  If it is, it adds FF to the byte.
The routine then sets or clears bits depending upon the variable
"setclear".
**********************************************************/

void outauxport(short port, short setclear, unsigned short outdata) {

  static short int port1data, port2data;

  if(port==2 || port==4) outdata = outdata << 8;

  if(port==1 || port==2) {
    if(setclear) port1data = port1data | outdata;
      else port1data = port1data & ~outdata;

#ifdef AZTEC_C
    *IOPORT1 = port1data;  /* Write 16 bit word to OUT0-OUT15 */
    return;

#else
    printf("\nPort 1 and 2 data = %X", port1data); /* OUT0-OUT15 */
    return;
#endif
  }

  if(port==3 || port==4) {
    if(setclear) port2data = port2data | outdata;
      else port2data = port2data & ~outdata;

#ifdef AZTEC_C
    *IOPORT2 = port2data;  /* Write 16 bit word to OUT16-OUT31 */

#else
printf("\nPort 3 and 4 data = %X", port2data);   /* OUT16-OUT31 */
#endif
  }
  return;
}
/******************************************************/

void clear_auxports(void){

  *IOPORT1 = 0;
  *IOPORT2 = 0;
}


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

	 Auxilliary Input Port Routine

In: Number of port bit to be tested.
Return: Value of tested bit (0 or 1).

The routine clears all bits in "portword" except the
one set in "portbit" .  It then compares the two words:
if they are equal, then the bit of interest must have been a one.
*********************************************************/

int inauxport(unsigned short portbit) {

#ifdef AZTEC_C
  unsigned short portword;

  portword =  *IOPORT1;   /* Read 16 bit word from IN0-IN15 */
  portword = portword & portbit;
  if(portword == portbit) return 1;
    else return 0;

#else
  return 1;
#endif
}

/********************************************************
	    Analog Input Functions
********************************************************/

void setanachan(short chan) {
  if(chan/4){
    PSet(E,7);
    chan -= 4;
  }
  else PClear(E,7);

  if(chan/2){
    PSet(E,6);
    chan -= 2;
  }
  else PClear(E,6);

  if(chan) PSet(E,5);
    else PClear(E,5);
  return;
}

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

int get_ad_counts(chan) {

  setanachan(chan);
  DelayMilliSecs(10);
  return AtoDReadWord(0);
}


/**************************************
       RS-232 Input/Output Functions
***************************************/

void setcomchan(short chan) {

  if(chan/4) {
    PSet(D,5);
    chan -= 4;
  }
    else PClear(D,5);

  if(chan/2) {
    PSet(D,4);
    chan -= 2;
  }
    else PClear(D,4);

  if(chan) PSet(D,3);
    else PClear(D,3);
  return;
}

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

int TPUiorelay(char chan){

#ifdef AZTEC_C
  int inkey;

  setcomchan( (short)chan );
  while(1){
    if( SerByteAvail() ){
      inkey = (int)SerGetByte();
      if( inkey == 3 ) break;            /* Exit on Ctrl-C */
      TSerPutByte( TPUSERTX, inkey );
    }

    if( TSerByteAvail( TPUSERRX ) ){
      SerPutByte( (unsigned char)TSerGetByte( TPUSERRX ) );
    }
  }
  return 0;

#else
  printf( "\nTPUiorelay channel =  %d", chan );
  return 0;

#endif
}

/*******************************************/
  
int heading(void) {

#ifdef AZTEC_C
  char inbuf[20];
  char inchar;
  int i;

  setcomchan(7);
  TSerInFlush(13);
  TPuts("d1\r");

  for(i=0; i<=15; i++) {
    inchar = (char)TSerGetByteTimed(500);
    if( inchar == (-1) ) {
      printf("\nCompass communication timeout error");
      return -1;
    }
    else inbuf[i] = inchar;
  }
  inbuf[16] = '\0';

/*  printf("\n%s", inbuf); */

  return atoi(inbuf+3);

#else
  char headstring[4];
  int heading;

  printf("\nEnter Heading: ");
  gets(headstring);
  heading = atoi(headstring);
  printf("\nHeading: %d", heading);
  return heading;
#endif
}

/**************************************
	Input Capture Functions
***************************************/

int TPUGetPinES(short chan, short edge){

#ifdef AZTEC_C
  CHANPRIOR(chan, Disabled);
  FUNSEL(chan, DIO);
  PARAMRAM(chan)[CHANNEL_CONTROL] = InputChan | NoForceState | edge;
  HOSTSEQ(chan, UpdtOnTrans); /* need to enter transistion mode first ! */
  HOSTSERVREQ(chan, Initialize);
  CHANPRIOR(chan, LowPrior);
  while (HOSTSERVSTAT(chan))
    ;
  HOSTSEQ(chan, UpdtOnHSRInit);
  HOSTSERVREQ(chan, Initialize);
  while (HOSTSERVSTAT(chan))
    ;
  return ((PARAMRAM(chan)[PIN_LEVEL] & 0x8000) == 0x8000);

#else
  return;

#endif

}   /* TPUGetPinES() */


/************************************************
   DRIVE FUNCTIONS
*************************************************/

int move_rov(char direction) {

  switch(direction) {
    case 'f':
      outauxport(1, CLEARBIT, MOT1A+MOT1B+MOT2A+MOT2B);
      outauxport(1, SETBIT, MOT1A + MOT2A);           /* Turn on motor relays forward */
      break;
    case 'b':
      outauxport(1, CLEARBIT, MOT1A+MOT1B+MOT2A+MOT2B);
      outauxport(1, SETBIT, MOT1B + MOT2B);
      break;
    case 'l':
      outauxport(1, CLEARBIT, MOT1A+MOT1B+MOT2A+MOT2B);
      outauxport(1, SETBIT, MOT1B + MOT2A);
      break;
    case 'r':
      outauxport(1, CLEARBIT, MOT1A+MOT1B+MOT2A+MOT2B);
      outauxport(1, SETBIT, MOT1A + MOT2B);
      break;
    case 's':
      outauxport(1, CLEARBIT, MOT1A+MOT1B+MOT2A+MOT2B); /* Turn off both motor relays */
      break;
  }
  return(0);
}


/************************************************
	   RACK FUNCTIONS
************************************************/

int move_rack(char direction)  {

  switch(direction) {
    case 'u':
      outauxport(2, CLEARBIT, MOT1A+MOT1B);
      outauxport(2, SETBIT, MOT1A);  /* Turn rack motor on upward */
      break;
    case 'd':
      outauxport(2, CLEARBIT, MOT1A+MOT1B);
      outauxport(2, SETBIT, MOT1B);
      break;
    case 's':
      outauxport(2, CLEARBIT, MOT1A+MOT1B);  /* Turn off rack motor */
      break;
  }
  return(0);
}


/************************************************
	 CURRENT METER FUNCTIONS
************************************************/

int move_currmet(char direction)  {

  switch(direction) {
    case 'u':
      outauxport(2, CLEARBIT, MOT2A+MOT2B);
      outauxport(2, SETBIT, MOT2A);  /* Turn current meter motor on upward */
      break;
    case 'd':
      outauxport(2, CLEARBIT, MOT2A+MOT2B);
      outauxport(2, SETBIT, MOT2B);
      break;
    case 's':
      outauxport(2, CLEARBIT, MOT2A+MOT2B);  /* Turn off current meter motor */
      break;
  }
  return(0);
}

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

short cmvane(void){

#ifdef AZTEC_C
  short dir;

  cmvane_pwr_on();
  DelayMilliSecs(50);
  dir = get_ad_counts(CMVANECHAN);
  cmvane_pwr_off();
  return dir;

#else
  return 8190;   /* Should be 180 degrees */

#endif
}

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


/******************************************************
	     Mission Functions
******************************************************/


int transit_rov(int transit_dist){
  long start_time, data_store_time, filmcam_time;
  short errcode = 0;

  start_time = TensMilliSecs();    /* Initialize the timer variables */
  data_store_time = start_time;
  filmcam_time = start_time;

  strobe_pwr_on();
  move_rov('f');	         	/* Let's go! */
  tpurupt(ODOMETER, ON);		/* Odometer on */
  g_odometer_ticks = 0;                 /* Reset distance counter */
  SerInFlush();

  while(1){    /* Transit Loop */

          /* Time to store data? */
    if( timer(data_store_time, TRANSIT_DATA_RATE ) ){
      g_heading = heading();
      store_data(TRANSIT);
      data_store_time = TensMilliSecs();    /* Reset timer variable */
    }

             /* Too much time? */
    if( timer(start_time, TRANSIT_TIMEOUT) ){
      errcode = (TRANSIT_TIMEOUT_ERR);
      break;
    }

             /* Take a picture ? */
    if( timer(filmcam_time, FILMCAM_RATE) ){
      filmcam_trig_on();                     /* Trigger strobe/camera */
      DelayMilliSecs(TRIGGERTIME);
      filmcam_trig_off();
      filmcam_time = TensMilliSecs();;       /* Reset timer variable */
    }

           /* Far enough? */
    if( g_odometer_ticks > transit_dist ) break;   

	    /* Simulation function */
#ifdef __TURBOC__
    DelayMilliSecs(1000);
    g_odometer_ticks += 4;    /* Increment the odometer for simulation */
    printf("\nOdometer ticks = %d", g_odometer_ticks);
#endif

      /* Get us out of here if needed */
    if(SerByteAvail()) break;

  }  /* End of Transit Loop */

  strobe_pwr_off();
  tpurupt(ODOMETER, OFF);
  move_rov('s');         /* We're there--hopefully */
  store_data(errcode);  /* Store the errorcode just in case */
  return errcode;
}

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

int turn_rov(int new_heading){
  long starttime, data_store_time;
  short headingdiff, errcode = 0, turn_dir = 0;

  starttime = TensMilliSecs();


      /* Find and correct heading difference */
  g_heading = heading();
  headingdiff = new_heading - g_heading;
  if(headingdiff > 180) headingdiff -= 360;
  if(headingdiff < (-180)) headingdiff += 360;

      /* Decide which way to turn */
  if(headingdiff > 0){
    move_rov('r');
    turn_dir = TURN_RIGHT;
  }
  else if(headingdiff < 0){
    move_rov('l');
    turn_dir = TURN_LEFT;
  }

  DelayMilliSecs(500);        /* Let Compass Settle */

  while(1){                   /* Turn loop */
    g_heading = heading();
    store_data(turn_dir);
    data_store_time = TensMilliSecs();

     /* Find and correct heading difference */
    headingdiff = new_heading - g_heading;
    if(headingdiff > 180) headingdiff -= 360;
    else if(headingdiff < (-180)) headingdiff += 360;

      /* Are we there yet? */
    if(headingdiff <= 0 && turn_dir == TURN_RIGHT) break;                         /* We're there */
    if(headingdiff >= 0 && turn_dir == TURN_LEFT) break;
    if(headingdiff == 0) break;

	    /* Too much time? */
    if( timer( starttime, TURN_TIMEOUT ) ){
      errcode = (TURN_TIMEOUT_ERR);
      break;
    }
	    /* Hang out here awhile */
    while( timer(data_store_time, TURN_DATA_RATE));

  }   /* End of turn loop */

  move_rov('s');        /* End of turn */
  store_data(errcode);  /* Store error code just in case */
  return errcode;
}

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

int rack_down_cycle(void){
  int errcode = 0, last_leadscrew_ticks, rackloops;
  long starttime, data_store_time;

  SerInFlush();
  starttime = TensMilliSecs();     /* Start time for timeout */
  data_store_time = TensMilliSecs();
  tpurupt(LEADSCREW1, ON);         /* Turn on interrupts */
  tpurupt(LEADSCREW2, ON);
  g_leadscrew1_ticks = 0;
  g_leadscrew2_ticks = 0;
  last_leadscrew_ticks = 0;

  move_rack('d');         /* Move rack down */
  
  while(1){

		  /* Store some data? */
    if( timer(data_store_time, RACK_DATA_RATE) ){
      g_heading = heading();
      store_data(RACK_DOWN);
      data_store_time = TensMilliSecs();

	    /* Are the leadscrews turning ? */
      if( g_leadscrew1_ticks == last_leadscrew_ticks ){
	errcode = RACK_JAM_ERR;
	break;
      }
      else last_leadscrew_ticks = g_leadscrew1_ticks;
    }

#ifdef __TURBOC__
  g_leadscrew1_ticks++;
  g_leadscrew2_ticks++;
  DelayMilliSecs(2000);
#endif

         /* Watch for limit-switch */
      if( limit_dwn() ){                    
	errcode = (RACK_LIMITDWN_ERR);
	break;
      }
	       /* Leadscrew not turning? */
      if( abs(g_leadscrew1_ticks - g_leadscrew2_ticks) > LEADSCREW_DIFF_MAX ){
	errcode =  (RACK_LEADSCREW_ERR);
	break;
      }
	      /* Too much time? */
      if(timer(starttime, RACK_TIMEOUT)){
	errcode = (RACK_TIMEOUT_ERR);
	break;
      }
	    /* Watch the limit pads */
      if( limit_pad1() | limit_pad2() )	break;

	    /* Simulate rack down by pressing any key */
      if(kbhit()){
	SerInFlush();
	break;                
      }

  }         /* End of rack down main loop */

  move_rack('s');              /* Benthic chambers planted--hopefully */
  tpurupt(LEADSCREW1, OFF);
  tpurupt(LEADSCREW2, OFF);
  store_data(errcode);          /* Store error code just in case */
  return errcode;
}

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

int rack_up_cycle(void){
  int errcode = 0, last_leadscrew_ticks, rackloops;
  long starttime, data_store_time;

  SerInFlush();
  starttime = TensMilliSecs();    /* Start time in tens of seconds */
  data_store_time = TensMilliSecs();
  tpurupt(LEADSCREW1, ON);
  tpurupt(LEADSCREW2, ON);
  g_leadscrew1_ticks = 0;
  g_leadscrew2_ticks = 0;
  last_leadscrew_ticks = 0;

#ifdef __TURBOC__
  g_leadscrew1_ticks++;
#endif

  move_rack('u');                              /* Move rack up */

              /* Rack up loop */
  while(1){                                     

       /* Store data and check for rack jam */
    if( timer(data_store_time, RACK_DATA_RATE) ){
      g_heading = heading();
      store_data(RACK_UP);

	 /* Are the leadscrews not turning? */
      if( g_leadscrew1_ticks == last_leadscrew_ticks ){
	errcode = RACK_JAM_ERR;
	break;
      }
      else last_leadscrew_ticks = g_leadscrew1_ticks;
    }

#ifdef __TURBOC__
  g_leadscrew1_ticks++;
  g_leadscrew2_ticks++;
  DelayMilliSecs(2000);
#endif

	/* Watch for limit-switch */
      if( limit_up() ){
	errcode = (RACK_LIMITUP_ERR);
	break;
      }
	     /* Is a leadscrew not turning? */
      if( abs(g_leadscrew1_ticks - g_leadscrew2_ticks) > LEADSCREW_DIFF_MAX ){
	errcode = (RACK_LEADSCREW_ERR);
	break;
      }
	      /* Too much time? */
      if(timer(starttime, RACK_TIMEOUT)){
	errcode = (RACK_TIMEOUT_ERR);
	break;
      }
	    /* Is the rack up? */
      if( rack_up() ) break;

	    /* Simulate rack up by pressing any key */
      if(kbhit()){
	SerInFlush();
	break;                
      }


  }   /* End of rack up main loop */

  move_rack('s');                /* Rack is up -- hopefully */
  tpurupt(LEADSCREW1, OFF);
  tpurupt(LEADSCREW2, OFF);
  store_data(errcode);       /* Store error code just in case */
  return errcode;
}
/************************************************/

int currmet_up(void){
  int errcode = 0, last_cmmot_shaft_ticks = 0, cmmot_loops;
  long starttime, data_store_time;

  sensor_pwr_on();
  starttime = TensMilliSecs();     /* Start time for timeout */
  data_store_time = TensMilliSecs();
  tpurupt(CMMOT_SHAFT, ON);         /* Turn on interrupts */
  g_cmmot_shaft_ticks = 0;

#ifdef __TURBOC__
  g_cmmot_shaft_ticks++;
#endif

  move_currmet('u');
  SerInFlush();

	    /* Current meter up main loop */
  while(1){

       /* Store Data and Check for Jam Error */
    if( timer(data_store_time, CMMOT_DATA_RATE) ){
      g_heading = heading();
      data_store_time = TensMilliSecs();
      store_data(CURRMET_UP);

	    /* Are the leadscrews turning ? */
      if( g_cmmot_shaft_ticks == last_cmmot_shaft_ticks ){
	errcode = CMMOT_JAM_ERR;
	break;
      }
      else last_cmmot_shaft_ticks = g_cmmot_shaft_ticks;
      printf("\nCM motor shaft ticks %d", g_cmmot_shaft_ticks);


    }  /* End of sample if statement */

#ifdef __TURBOC__
      g_cmmot_shaft_ticks += 100;
#endif


	      /* Too much time? */
      if(timer(starttime, CMMOT_TIMEOUT)){
	errcode = CMMOT_TIMEOUT_ERR;
	break;
      }

	    /* Is the current meter up? */
      if( g_cmmot_shaft_ticks > CURRMET_UP_POS ) break;

       /* Get us out of here if needed */
      if( SerByteAvail() ) break;

  }   /* End of Current Meter up loop */

  move_currmet('s');              /* Current meter is up--hopefully */
  tpurupt(CMMOT_SHAFT, OFF);
  store_data(errcode);          /* Store error code just in case */

  return errcode;
}


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

int currmet_down(void){
  int errcode = 0, last_cmmot_shaft_ticks, cmmot_loops;
  long starttime, data_store_time;

  sensor_pwr_on();
  starttime = TensMilliSecs();     /* Start time for timeout */
  data_store_time = TensMilliSecs();
  tpurupt(CMMOT_SHAFT, ON);         /* Turn on interrupts */
  g_cmmot_shaft_ticks = 0;

#ifdef __TURBOC__
  g_cmmot_shaft_ticks++;
#endif

  move_currmet('d');         /* Move current meter down */
  SerInFlush();

	    /* Current meter down loop */
  while(1){

       /* Store Data and Check for Jam Error */
    if( timer(data_store_time, CMMOT_DATA_RATE) ){
      g_heading = heading();
      data_store_time = TensMilliSecs();
      store_data(CURRMET_DOWN);

	    /* Are the leadscrews turning ? */
      if( g_cmmot_shaft_ticks == last_cmmot_shaft_ticks ){
	errcode = CMMOT_JAM_ERR;
	break;
      }
      else last_cmmot_shaft_ticks = g_cmmot_shaft_ticks;
      printf("\nCM motor shaft ticks %d", g_cmmot_shaft_ticks);

#ifdef __TURBOC__
      g_cmmot_shaft_ticks++;
#endif

    }  /* End of sample if statement */

	      /* Too much time? */
      if(timer(starttime, CMMOT_TIMEOUT)){
	errcode = CMMOT_TIMEOUT_ERR;
	break;
      }

	    /* Is the current meter up? */
      if( g_cmmot_shaft_ticks > CURRMET_UP_POS ) break;

       /* Get us out of here in simulation */
      if( SerByteAvail() ) break;

  }   /* End of Current Meter up loop */


  move_currmet('s');              /* Current meter is down--hopefully */
  tpurupt(CMMOT_SHAFT, OFF);
  store_data(errcode);          /* Store error code just in case */

  return errcode;
}


/*********************************************
	    Misc. Functions
*********************************************/

int timer(long starttime, long maxtime){
  long timediff;

/*
*   starttime is absolute time from RTC in tens of milliseconds.
*   maxtime is the timeout period in tens of milliseconds.
*   function returns 1 upon timeout.
*/

  timediff = TensMilliSecs() - starttime;
  if(timediff < 0) timediff += 8.64e6;   /* Correct for rollover at midnight*/
  if(timediff >= maxtime) return 1;
    else return 0;
}

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

void delaysecs(unsigned long delay_time){

  DelayMilliSecs(delay_time*1000);
}

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

void sleep_with_break(unsigned long delay_time){
  unsigned long seconds;

  SerInFlush();
  Sleep(0);
  for(seconds = 0; seconds <= delay_time; seconds++){
    if(SerByteAvail()) break;
    Sleep(GetTickRate());    /* Sleep for a second */
  }
}

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

void transpond_out(void){
  int x;

  TPUSetPin(TRANSPOND_OUT, 0);
  for(x=0; x<10; x++);
  TPUSetPin(TRANSPOND_OUT, 1);

  return;
}