#include "tt7rmlib.h"
#include "rovcal.h"
#include <sim.h>
#include <pins.h>
#include <atod.h>
#include <tpu.h>
#include <tt7.h>
#include <timing.h>
#include <excepts.h>
#include "tserial.h"
#include <stdlib.h>

#define IOPORT1   ((uspv) (0xE00000))
#define IOPORT2   ((uspv) (0xE00002))

/**********************************************
       Initialization Functions
**********************************************/

int tt7init(void){

  static ExcCFrame	framebuf;
  short		result;

/*
** DEFINE CHIP SELECT FOR MEMORY MAPPING INTO 0xE00000
*/
   *CSBAR7 = 0xE000 | SZ2K; /* 2K @ E00000, E00002, E00004, E00006 */
   *CSOR7 = 
		M_MDSYNC   &   CLR   /* 0 = async, 1 = synchronous      */
	|       M_BYTEU    &   SET   /* upper byte                      */
	|       M_BYTEL    &   SET   /* lower byte                      */
	|       M_CSWRITE  &   SET   /* write                           */
	|       M_CSREAD   &   SET   /* read                            */
	|       M_DSSTRB   &   SET   /* 0 = address, 1 = data strobe    */
	|       M_WAIT0              /* wait states                     */
	|       M_SPCS     &   SET   /* supervisor                      */
	|       M_SPCU     &   SET   /* user                            */
	|       M_AVEC     &   CLR   /* 0 = ext rupt, 1 = autovector    */
   ;


   *IOPORT1 = 0;   /* Clear Aux. I/O ports */
   *IOPORT2 = 0;

   PConfOutp(E,0); /* Configure port E */
   PClear(E,0);
   PConfOutp(E,1);
   PClear(E,1);
   PConfOutp(E,5);
   PConfOutp(E,6);
   PConfOutp(E,7);
   PConfOutp(D,3);  /* Configure port D */
   PConfOutp(D,4);
   PConfOutp(D,5);

/* Setup TPU Channels and Interrupts */

/*
   InstallHandler(TPUHandler, TPU_INT_VECTOR + TPU_CHAN, OS_VECTOR_BASE,
	&framebuf);


   for(tpuchan=0, tpuchan <= 4, tpuchan++) TPUGetPin(tpuchan);
   for(tpuchan=5, tpuchan <=11, tpuchan++) TPUSetPin(tpuchan, 0);
*/

/*
**	OPEN THE TPU SERIAL DRIVERS
*/
	if ((result = TSerOpen(13, INP, 100, 9600, 'N', 8, 1)) != tsOK)
		{
		printf(" Failed TSerOpen %d \n", result);
		return;
		}

	if ((result = TSerOpen(12, OUTP, 100, 9600, 'N', 8, 1)) != tsOK)
		{
		printf(" Failed TSerOpen %d \n", result);
		return;
		}

} /*tt7init()*/


/********************************************************
	     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 outport(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;

    *IOPORT1 = port1data;  /* Write 16 bit word to OUT0-OUT15 */
    return;
  }

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

    *IOPORT2 = port2data;  /* Write 16 bit word to OUT16-OUT31 */
  }
  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 the input word except the
one of interest.  It then compares the two words: if they are
equal, then the bit of interest must have been a one.
*********************************************************/

int inport(unsigned short portbit) {

  unsigned short portword;

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

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

float battvoltage(void) {

  PClear(E,5);
  PClear(E,6);
  PClear(E,7);
  DelayMilliSecs(20);
  return AtoDReadWord(0)*BATTVOLTCAL;
}

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

float syscurrent(void) {

  PSet(E,5);
  PClear(E,6);
  PClear(E,7);
  DelayMilliSecs(20);
  return AtoDReadWord(0)*SYSCURRCAL;
}

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

float gfvoltage(void) {

  PClear(E,5);
  PSet(E,6);
  PClear(E,7);
  DelayMilliSecs(20);
  return AtoDReadWord(0)*GFVOLTCAL;
}

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

float tilt_x(void) {

  PClear(E,5);
  PClear(E,6);
  PSet(E,7);
  DelayMilliSecs(20);
  return ((AtoDReadWord(0)-16380) * TILTXCAL);
}

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

float tilt_y(void) {

  PSet(E,5);
  PClear(E,6);
  PSet(E,7);
  DelayMilliSecs(20);
  return ((AtoDReadWord(0)-16380) * TILTYCAL);
}

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

int tt7kbhit(void) {
  return (int) SerByteAvail();
}

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

int heading(void) {

  char inbuf[20];
  char inchar;
  int i;

  PSet(D,3);
  PSet(D,4);
  PSet(D,5);
  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);
}

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

float speed(void) {
  return 10.1;
}