/* "ROVLIB"  ROVER FUNCTION LIBRARY" */
#include <stdlib.h>
#include "rovlib.h"
#include "rovsys.h"
#include "rovint.h"

#ifdef AZTEC_C    /* Include if compiler is Aztec C */
#include <sim.h>
#include <pins.h>
#include <atod.h>
#include <tpu.h>
#include <tt7.h>
#include <timing.h>
#include <excepts.h>
#include <serio.h>
#include "tserial.h"
#define IOPORT1   ((uspv) (0xE00000))
#define IOPORT2   ((uspv) (0xE00002))

#else
#include "tt7pclib.h"
#include "conio.h"
#endif



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

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

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

	 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;
}

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

float battvoltage(void) {

  setanachan(0);
  DelayMilliSecs(20);
  return AtoDReadWord(0)*BATTVOLTCAL;
}

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

float hpcurrent(void) {

  setanachan(1);
  DelayMilliSecs(20);
  return AtoDReadWord(0)*HPCURRCAL;
}

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

float lpcurrent(void) {

  setanachan(2);
  DelayMilliSecs(20);
  return AtoDReadWord(0)*LPCURRCAL;
}

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

float gfvoltage(void) {

  setanachan(3);
  DelayMilliSecs(20);
  return AtoDReadWord(0)*GFVOLTCAL;
}

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

float tilt_x(void) {

  setanachan(4);
  DelayMilliSecs(20);
  return (AtoDReadWord(0)-16380) / TILTXCAL;
}

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

float tilt_y(void) {

  setanachan(5);
  DelayMilliSecs(20);
  return (AtoDReadWord(0)-16380) / TILTYCAL;
}

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

int cmvane_dir(void){

#ifdef AZTEC_C
  int dir;

  setanachan(6);
  TPUSetPin(CMVANE_PWR, 1);
  DelayMilliSecs(10);
  dir = AtoDReadWord(0)/CMVANECAL;
  TPUSetPin(CMVANE_PWR, 0);

  return dir;

#else
  return 180;

#endif
}

/**************************************
       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( TPU_SER_TX, inkey );
    }

    if( TSerByteAvail( TPU_SER_RX ) ){
      SerPutByte( (unsigned char)TSerGetByte( TPU_SER_RX ) );
    }
  }
  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);
}

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

int turn_rov(int new_heading){
  long starttime;
  int headingdiff, head, errcode = 0;

  starttime = TensMilliSecs()/100;
  headingdiff = new_heading - heading();
  if(headingdiff > 180) headingdiff -= 360;
  if(headingdiff < (-180)) headingdiff += 360;

  if(headingdiff > 0){
    move_rov('r');
    DelayMilliSecs(500);
    while(1){
      head = heading();
      headingdiff = new_heading - head;
      if(headingdiff > 180) headingdiff -= 360;
      else if(headingdiff < (-180)) headingdiff += 360;

      printf("\nr,head %d headingdiff %d", head, headingdiff);
      if(headingdiff <= 0) break;
      if( timeout( starttime, TURN_TIMEOUT ) ){
	errcode = (-1);
	break;
      }
    }
  }
  else if(headingdiff < 0){
    move_rov('l');
    DelayMilliSecs(500);
    while(1){
      head = heading();
      headingdiff = new_heading - head;
      if(headingdiff > 180) headingdiff -= 360;
      else if(headingdiff < (-180)) headingdiff += 360;

      printf("\nl,head %d headingdiff %d", head, headingdiff);
      if(headingdiff >= 0) break;
      if( timeout( starttime, TURN_TIMEOUT ) ){
	errcode = (-1);
	break;
      }
    }
  }
  move_rov('s');
  return errcode;
}

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

int transit_rov(int transit_dist){
  long starttime;
  int filmtime = 0;

  starttime = TensMilliSecs()/100;
  g_odometer_ticks = 0;        /* Reset distance counter */
  STROBE_PWR_ON
  move_rov('f');

  while(1){                                      /* Transit Loop */
    if( g_odometer_ticks > transit_dist ) break;   /* Far enough? */
    if( timeout(starttime, TRANSIT_TIMEOUT)) {    /* Too much time? */
	move_rov('s');
	STROBE_PWR_OFF
	return -1;
    }
    DelayMilliSecs(1000);
    filmtime++;
    if( filmtime >= 5 ){
      FILMCAM_TRIG_ON    /* Trigger strobe/camera */
      DelayMilliSecs(TRIGGER_TIME);
      FILMCAM_TRIG_OFF
      filmtime = 0;
    }

#ifdef __TURBOC__
    check_int();       /* Simulate odometer interrupt by pressing 'o' key */
#endif
  }

  STROBE_PWR_OFF
  move_rov('s');    /* We're there! */
  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);
}

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

int rack_down_cycle(void){
  int errcode = 0;
  long starttime;

  starttime = TensMilliSecs()/100;
  g_leadscrew1_ticks = 0;
  g_leadscrew2_ticks = 0;

  move_rack('d');         /* Move rack down */

  while(1){                           /* Rack down loop */
    if( !inauxport(LIMITDWN) ){          /* Watch for limit-switch */
      errcode = (-1);
      break;
    }
    if( abs(g_leadscrew1_ticks - g_leadscrew2_ticks) > 1 ){   /* Leadscrew not turning? */
      errcode =  (-2);
      break;
    }
    if(timeout(starttime, RACK_TIMEOUT)){
      errcode = (-3);
      break;
    }
    if( !inauxport(LIMITPAD1) ) break;   /* Watch limit pads */
    if( !inauxport(LIMITPAD1) ) break;

#ifdef __TURBOC__
    if(kbhit()){
      getch();
      break;         /* Simulate limitpad by pressing any key */
    }
#endif

  }
  move_rack('s');      /* Benthic chambers planted! */
  return errcode;
}

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

int rack_up_cycle(void){
  int errcode = 0;
  long starttime;

  starttime = TensMilliSecs()/100;    /* Start time in seconds */
  g_leadscrew1_ticks = 0;
  g_leadscrew2_ticks = 0;
  move_rack('u');         /* Move rack up */

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

    if( !inauxport(LIMITUP) ){          /* Watch for limit-switch */
      errcode = (-1);
      break;
    }
    if( abs(g_leadscrew1_ticks - g_leadscrew2_ticks) > 1 ){   /* Leadscrew not turning? */
      errcode = (-2);
      break;
    }
    if(timeout(starttime, RACK_TIMEOUT)){
      errcode = (-3);
      break;
    }

    if( !inauxport(RACKUP) ) break;      /* This is where we want to stop */
    if( !inauxport(LIMITPAD1) ) break;   /* Watch limit pads for manual stop*/
    if( !inauxport(LIMITPAD2) ) break;

#ifdef __TURBOC__
    if(kbhit()){
      getch();
      break;         /* Simulate limitpad by pressing any key */
    }
#endif

  }
  move_rack('s');      /* Benthic chambers planted! */
  return errcode;
}
/************************************************
	 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);
}

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

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

  timediff = TensMilliSecs()/100 - starttime;
  if(timediff < 0) timediff += 86400;
  if(timediff >= maxtime) return 1;
    else return 0;
}

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

void delaysecs(unsigned long delaytime){

  DelayMilliSecs(delaytime*1000);
}

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