#include <stdio.h>
#include <ctype.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "crc.h"
#include "timer.h"
#include "uart1.h"
#include "eeprom.h"


#define MAXSIZE 32
#define ReadObjectOpCode 0x60
#define WriteObjectOpCode 0x68
#define NodeID 0x01
#define DLE 0x90
#define STX 0x02

unsigned short pDataArray[MAXSIZE];
unsigned short payload[MAXSIZE];
unsigned char retstr[MAXSIZE];

extern unsigned short revs;
extern unsigned short CNticks;
extern unsigned short AlarmCnt;//Global incremented in AlarmISR

#define GETBIT(var, bit)        (((var) >> (bit)) & 1)



//------------------------------------------------------------------------------
unsigned short WriteObject (unsigned short index,unsigned char subindex,unsigned short val1,unsigned short val2)
{
 unsigned short ArrayLength = 6;
 unsigned short CRC = 0;
 unsigned short len = 4;
 unsigned short i;

//packdata array for read


 pDataArray[0] = (len << 8) | WriteObjectOpCode;
 pDataArray[1] = ((index & 0x00FF)<<8) | NodeID;
 pDataArray[2] = (subindex << 8) | ((index & 0xFF00)>>8);
 pDataArray[3] = val1;
 pDataArray[4] = val2;
 pDataArray[5] = 0x0000; //CRC holder

 CRC = calc_CRC(ArrayLength);
 pDataArray[5] = CRC; //tack on the CRC
 pack_payload(ArrayLength);

 //printf("Payload: ");
 //for (i=0; i<ArrayLength+1; i++)printf("%04x ",payload[i]);
 //printf("\n");
 
 ClearRxFifo();
 vPutStrMaxon(payload,ArrayLength,2);
 //msWait(20); //wait 20ms for response
 //strncpy(retstr,"\0",MAXSIZE);
 //RxStr2(MAXSIZE, retstr,1000,100 );
 return(CRC);
}
//------------------------------------------------------------------------------
unsigned short ReadObject (unsigned short index,unsigned char subindex)
{
 unsigned short ArrayLength = 4;
 unsigned short CRC = 0;
 unsigned char len = 2;
 unsigned short i;
 //unsigned char test1[] = {0x90, 0x02, 0x00, 0x04, 0x00, 0x00,0x00,0x00,0x37, 0x04, 0x00, 0x00, 0xF1, 0x27};
 unsigned short retval = 9999;
// unsigned short no_chars = 0;
//packdata array for read


 pDataArray[0] = (len << 8) | ReadObjectOpCode;
 pDataArray[1] = ((index & 0x00FF)<<8) | NodeID;
 pDataArray[2] = (subindex << 8) | ((index & 0xFF00)>>8);
 pDataArray[3] = 0x0000;

 CRC = calc_CRC(ArrayLength);
 pDataArray[3] = CRC; //tack on the CRC
 pack_payload(ArrayLength);

 //printf("Payload: ");
 //for (i=0; i<ArrayLength+1; i++)printf("%04x ",payload[i]);
 //printf("\n");

 ClearRxFifo();
 vPutStrMaxon(payload,ArrayLength,2);
 msWait(30); //wait for response

 //for (i=0; i<MAXSIZE; i++)printf("%02x ",RxFifo[i]);
 //printf("\n");
 retval = (RxFifo[9]<<8) | RxFifo[8];
 //printf("RxFifo[9] = %02x, RxFifo[8] = %02x, retval = %04x",
 //        RxFifo[9], RxFifo[8], retval);

 return(retval);
}
//------------------------------------------------------------------------------
unsigned short calc_CRC(unsigned short ArrayLength)
{
 unsigned short shifter, c;
 unsigned short carry,i;
 unsigned short CRC = 0;

 //Calculate pDataArray Word by Word
 for(i=0; i < ArrayLength; i++)
 {
  shifter = 0x8000;
  c = pDataArray[i];
  do
  {
   carry = CRC & 0x8000;
   CRC <<= 1;
   if(c & shifter) CRC++;
   if(carry) CRC ^= 0x1021;
   shifter >>= 1;
  } while(shifter);
 }
 //printf("CRC = %04x\n",CRC);
 return(CRC);
}

//------------------------------------------------------------------------------
unsigned short pack_payload(unsigned short ArrayLength)
{
 unsigned short i;

 payload[0] = (DLE << 8) | STX; //Sata Link Escape starts syc
 //printf("payload[0] = %04x\n",payload[0]);
 for (i=1; i<ArrayLength+1; i++)
  {
   payload[i] = (pDataArray[i-1]<<8) | ((pDataArray[i-1] & 0xFF00)>>8);
   //printf("payload[%i] = %04x\n",i,payload[i]);
  }
 return(0);
}
//------------------------------------------------------------------------------
unsigned short CheckForMotion(void)
 {
  unsigned short AtTarget = 1; //default to being at target.  think its safer?
  //check status word

  AtTarget = ReadObject(Statusword,0x00);
  //printf("AtTarget = %x  ",AtTarget);
  AtTarget = GETBIT(AtTarget,10);
  //printf("BIT10 = %1d\n",AtTarget);
  return(AtTarget);
 }
//------------------------------------------------------------------------------
unsigned short CheckForHome(void)
{
 unsigned short val;
 
 val = ReadObject(GetDigInIndex,GetDigInSubIndex);
 val = GETBIT(val,2);
 //printf("CheckForHome = %d\n",val);
 return(val);
}
//------------------------------------------------------------------------------
unsigned short GetPosition(void)
{
 unsigned short val;
 
 val = ReadObject(GetPostitionIsIndex,GetPositionIsSubIndex);
 return(val);
}
//------------------------------------------------------------------------------
unsigned short MoveCntsRelative(unsigned short target)
{
 unsigned short pos = 0;
 
 printf("Enabling Drive..\n");
 //First make sure drive is enabled
 WriteObject(Controlword,0,0x06,0x00);
 WriteObject(Controlword,0,0x0F,0);
 
 printf("Enabling Profile Position Mode..\n");
 //Enable Profile Position Mode
 WriteObject(SetOperationModeIndex,SetOperationModeSubIndex,1,0);
 
 printf("Moving relative %d cnts..\n",target);
 //Set target position
 WriteObject(TargetPosIndex,TargetPosSubIndex,target,0);
 
 printf("Begin turning....\n");
 //Begin turning
 WriteObject(Controlword,0,0x7F,0);
 WriteObject(Controlword,0,0x0F,0);
 return(pos);
}
//------------------------------------------------------------------------------
unsigned short Home(int ms)
{
 unsigned short val=0;
 unsigned short t=0;
 
 //check first to see if home
 val = CheckForHome();
 if(!val)return(!val); //already there
 
 //begin motion
 MoveCntsRelative(0xFFFF);

 while(t<ms && val)
 {
  msWait(100);
  val = CheckForHome();
  t=t+100;
  //printf("t = %d, val = %d\n",t,val);
 }
 StopMotor();
 msWait(1000);
 MoveCntsRelative((unsigned short)eeRead(eHOME));
 msWait(2000);
 return(val);
}
//------------------------------------------------------------------------------
void StopMotor(void)
{
 
 WriteObject(Controlword,0,0x010F,0);   
    
 return;
}
//------------------------------------------------------------------------------
unsigned int sample (void)
{
    int position=-9;
    unsigned short target = 0;
    unsigned short err_code = 0;
    unsigned short i=0;
    int try=0;
    
    //Check what position sampler wants to go to
    position = eeRead(ePOS)+1;
    printf("ELMO will go to position %d\r\n",position);
    eeWrite(position,ePOS);
    //elmo_UI_set(1,position);
    
    //try 3x to get home
    try=0;i=1;
    while( (i) && (try<3))
    {
        printf("Homing...........................\n");
        i = Home(MAXHOMETIME);
        try++;
    }
    if(i!=0)err_code |= (1<<1); //set flag
    
    //purge
    RunPump(eeRead(ePURGE),250);
    
    //try 1x to go to next sample position
    target = position * eeRead(eOFFSET);
    //begin motion
    printf("Moving to %u cnts............................... \n",target);
    MoveCntsRelative(target);
    WaitForMotion(MAXHOMETIME);
    
    //run pump
    RunPump(eeRead(ePUMP),500);
    printf("Sampling complete, Homing................................\n");
    //try 3x to get home
    try=0;i=-1;
    while( (i!=0) && (try<3))
    {
        i = Home(MAXHOMETIME);
        try++;
    }
    if(i!=0)err_code |= (1<<5); //set flag


    printf("Sample %d, (err_code=%d)\r\n",position,err_code);
    return(0);
}
//------------------------------------------------------------------------------
unsigned short WaitForMotion(int ms)
{
 unsigned short val=0;
 unsigned short t=0;
 unsigned short pos = 0;
 
 //check first to see if home
 val = CheckForMotion();
 if(val)return(val); //already there
 

 while(t<ms && !val)
 {
  msWait(100);
  val = CheckForMotion();
  pos = GetPosition();
  t=t+100;
  printf("t = %d, AtTarget = %d, Pos = %d\n",t,val,pos);
 }
 StopMotor();
 msWait(1000);
 return(val);
}
//------------------------------------------------------------------------------
int RunPump(unsigned short revolutions,int secs)
{ //run the water pump for revolutions revs
  //secs= max seconds to try for that many revs
  //returns -1 = error
  //        int = seconds pumped
    
  int err_code = 0; 
  int i=-1;
 
  if(secs < 0)return(i);
  if(secs > MAX_PUMP_SECS)secs = MAX_PUMP_SECS; //bounds checking
  
  
  printf("Pumping for %d revs........ ",revolutions);
  //turn on pump
  //WriteObject(SetDigOutIndex,SetDigOutSubIndex,0,HomeSensPower);
  PumpOn(); //sets revs to 0
  
  i=0;
  while(i<secs && revs<revolutions)
  {
      //printf(".");
      msWait(200);
      //printf("Pump revs = %d\n",revs);
      //LATAbits.LATA0 ^= 1; //toggle LED every 1 second
      i++;
  }
  
  //turn off pump
  //WriteObject(SetDigOutIndex,SetDigOutSubIndex,0,0);
  PumpOff();
  printf("DONE\n");
  
  return (err_code);
}
//******************************************************************************
void PumpOn(void)
{ //turn on the pump and clear rev counter
 revs = 0;
 CNticks = 0;
 WriteObject(SetDigOutIndex,SetDigOutSubIndex,0,HomeSensPower);
 return;
}
//******************************************************************************
void PumpOff(void)
{ //turn off the pump 
 WriteObject(SetDigOutIndex,SetDigOutSubIndex,0,0);
 printf("Pump completed %d revs + %d ticks\n",revs,CNticks);
 return;
}
//******************************************************************************