#include <stdio.h>
#include <ctype.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "maxon.h"
#include "timer.h"
#include "uart1.h"
#include "eeprom.h"
#include "definitions.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];
unsigned short LastReadHigh = 0;   //upper 16 bits of the last ReadObject()
unsigned short LastReadOk   = 0;  //1 = last ReadObject() got a valid frame
unsigned short ReadQuiet = 0;   

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

extern uint8_t eePosition;



//------------------------------------------------------------------------------
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();
 U1STAbits.OERR = 0;
 vPutStrMaxon(payload,ArrayLength,1);
 RxStr(10,RxFifo,100,100);   //drain the ack frame, don't leave it in the FIFO
 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 short no_rx = 0;
 unsigned short hdr = 0xFFFF;
 unsigned long  err = 0;
 unsigned short retval = 0x0000;

 //pack data 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);

 ClearRxFifo();
 U1STAbits.OERR = 0;           //clear any buffer overruns

 vPutStrMaxon(payload,ArrayLength,1);

 //answer frame is 14 bytes: DLE STX op len err(4) data(4) crc(2)
 //read past it so nothing is left behind for the next transaction
 no_rx = RxStr(18,RxFifo,100,20);

 //printf("no_rx = %d, RxFifo: ",no_rx);
 //for (i=0; i<MAXSIZE; i++)printf("%02x ",RxFifo[i]);
 //printf("\n");

 //find the sync pattern - a stray byte can offset the whole frame
 for(i=0; (i+13) < MAXSIZE; i++)
  {
   if(RxFifo[i]==DLE && RxFifo[i+1]==STX){ hdr = i; break; }
  }

 if(hdr == 0xFFFF)
  {
   if(!ReadQuiet)printf("ReadObject 0x%04X:%02X - no frame (rx %d)\n",index,subindex,no_rx);
   LastReadHigh = 0;
   LastReadOk   = 0;
   return(0x0000);
  }

 //collapse doubled DLE bytes (maxon stuffs 0x90 inside the frame)
 {
  unsigned short s = hdr+2;
  unsigned short d = hdr+2;

  while(s < MAXSIZE)
   {
    RxFifo[d++] = RxFifo[s];
    if(RxFifo[s]==DLE && (s+1)<MAXSIZE && RxFifo[s+1]==DLE) s += 2;
    else s++;
   }
 }

 //error code, 4 bytes little endian
 err = ((unsigned long)RxFifo[hdr+7]<<24) |
       ((unsigned long)RxFifo[hdr+6]<<16) |
       ((unsigned long)RxFifo[hdr+5]<<8)  |
        (unsigned long)RxFifo[hdr+4];

 if(err) printf("ReadObject 0x%04X:%02X - error 0x%08lX\n",index,subindex,err);

 //data, 4 bytes little endian
 retval       = (RxFifo[hdr+9]<<8)  | RxFifo[hdr+8];
 LastReadHigh = (RxFifo[hdr+11]<<8) | RxFifo[hdr+10];
 LastReadOk   = (err == 0);

 return(retval);
}
//------------------------------------------------------------------------------
unsigned long ReadObject32(unsigned short index,unsigned char subindex)
{
 unsigned long lo;

 lo = (unsigned long)ReadObject(index,subindex);   //this also sets LastReadHigh
 return( ((unsigned long)LastReadHigh << 16) | lo );
}
//------------------------------------------------------------------------------
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);
 if(!LastReadOk) return(1);      //read failed - assume NOT home, keep looking
 return(GETBIT(val,2));
}
//------------------------------------------------------------------------------
unsigned short GetPosition(void)
{
 unsigned short val;
 
 val = ReadObject(GetPostitionIsIndex,GetPositionIsSubIndex);
 return(val);
}
//------------------------------------------------------------------------------
long GetPosition32(void)
{
 return( (long)ReadObject32(GetPostitionIsIndex,GetPositionIsSubIndex) );
}
//------------------------------------------------------------------------------
unsigned short MoveCntsRelative(long target)
{
 unsigned short sw = 0;
 int t;

 if(!IsDriveEnabled())
  {
   if(!EnableDrive(3000))
    {
     printf("MoveCntsRelative: drive would not enable\n");
     return(0);
    }
  }

 WriteObject(SetOperationModeIndex,SetOperationModeSubIndex,1,0);  //PPM
 msWait(20);

 WriteObject(TargetPosIndex,TargetPosSubIndex,                     //0x607A is INT32
             (unsigned short)( target        & 0xFFFF),
             (unsigned short)((target >> 16) & 0xFFFF));
 msWait(20);

 WriteObject(Controlword,0,0x000F,0);   //make sure bit4 starts low
 msWait(20);
 //printf("0x607A readback = %ld\n",(long)ReadObject32(TargetPosIndex,TargetPosSubIndex));

 //make sure any previous handshake has released
 WriteObject(Controlword,0,0x000F,0);
 t = 0;
 while(t < 500)
  {
   sw = ReadObject(Statusword,0x00);
   if(!GETBIT(sw,12)) break;
   msWait(20); t += 20;
  }
 if(t >= 500)
  {
   printf("Setpoint ack stuck high, SW = 0x%04X\n",sw);
   return(0);
  }

 //rising edge on bit4: new setpoint, change immediately, relative
 WriteObject(Controlword,0,0x007F,0);
 t = 0;
 while(t < 500)
  {
   sw = ReadObject(Statusword,0x00);
   if(GETBIT(sw,12)) break;
   msWait(20); t += 20;
  }
 if(t >= 500)
  {
   printf("No setpoint ack, SW = 0x%04X\n",sw);
   WriteObject(Controlword,0,0x000F,0);
   return(0);
  }

 WriteObject(Controlword,0,0x000F,0);   //clear bit4, completes handshake
 return(1);
}
//------------------------------------------------------------------------------
unsigned short Home(int ms)
{
 unsigned short val=0;
 int t=0;
 
 //check first to see if home
 val = CheckForHome();
 if(!val)return(0); //already there
 
 //begin motion
 MoveCntsRelative(-65535L);

 while(t<ms && val)
 {
  msWait(100);
  val = CheckForHome();
  t=t+100;
  //printf("t = %d, val = %d\n",t,val);
  printf("t = %d, AtTarget = %d, Pos = %ld, SW = 0x%04X\n",t,val,GetPosition32(),ReadObject(Statusword,0x00));
 }
 StopMotor();
 msWait(2000);
 MoveCntsRelative(-(long)eeRead(eHOME));
 msWait(2000);
 return(val);
}
//------------------------------------------------------------------------------
void StopMotor(void)
{
 
 WriteObject(Controlword,0,0x010F,0);   
    
 return;
}
//------------------------------------------------------------------------------
unsigned int sample (void)
{
    //int position=-9;
    long target = 0;
    unsigned short err_code = 0;
    unsigned short i=0;
    int try=0;
    
    //Check what position sampler wants to go to
    eePosition = eeRead(ePOS)+1;
    printf("MAXON will go to position %d\r\n",eePosition);
    write_eeprom();
    //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) //Couldnt find home, ABORT
    {
        err_code |= (1<<1); //set flag
        printf("FAILED TO HOME.  ABORTING!\n");
        printf("Sample %d, (err_code=%d)\r\n",eePosition,err_code);
        return(0);
    }
    
    //purge
    RunPump(eeRead(ePURGE),250);
    
    //try 1x to go to next sample position
     target = -(long)eePosition * (long)eeRead(eOFFSET);
    
    //begin motion
    printf("Moving to %ld cnts............................... \n",target);
    if(!MoveCntsRelative(target) || !WaitForMotion(15000))
     {
      err_code |= (1<<3);
      printf("MOVE FAILED - aborting sample\n");
      printf("Sample %d, (err_code=%d)\r\n",eePosition,err_code);
      return(0);
     }
    
    //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",eePosition,err_code);
    return(0);
}
//------------------------------------------------------------------------------
unsigned short WaitForMotion(int ms)
{
 unsigned short sw, val;
 int t = 0;

 while(t < ms)
  {
   msWait(100); t += 100;
   sw  = ReadObject(Statusword,0x00);
   val = GETBIT(sw,10);
   printf("t = %d, AtTarget = %d, Pos = %ld, SW = 0x%04X\n",t,val,GetPosition32(),sw);
   if(GETBIT(sw,3))
    {
     printf("FAULT during move, 0x603F = 0x%04X\n",ReadObject(0x603F,0x00));
     return(0);
    }
   if(val) return(1);
  }
 printf("WaitForMotion timeout\n");
 StopMotor();
 return(0);
}
//------------------------------------------------------------------------------
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;
  float pump_secs = 0;
 
  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*5) && 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");
  if (i>0) 
    {     
      pump_secs = i/5;
      printf("Pump ran for %5.2f seconds\n",pump_secs);
    }
  
  return (err_code);
}
//******************************************************************************
void PumpOn(void)
{ //turn on the pump and clear rev counter
 revs = 0;
 CNticks = 0;
 //WriteObject(SetDigOutIndex,SetDigOutSubIndex,0,HomeSensPower);
 GPIO_RE1_Set();
 return;
}
//******************************************************************************
void PumpOff(void)
{ //turn off the pump 
 //WriteObject(SetDigOutIndex,SetDigOutSubIndex,0,0);
 GPIO_RE1_Clear();
 printf("Pump completed %d revs + %d ticks\n",revs,CNticks);
 return;
}
//******************************************************************************
//------------------------------------------------------------------------------
// CiA402 statusword (0x6041) bits:
//  0 Ready to switch on   1 Switched on      2 Operation enabled
//  3 Fault               4 Voltage enabled   5 Quick stop
//  6 Switch on disabled  7 Warning          10 Target reached
//------------------------------------------------------------------------------
#define DS_NOT_READY        0
#define DS_SWITCH_ON_DIS    1
#define DS_READY_TO_SWITCH  2
#define DS_SWITCHED_ON      3
#define DS_OP_ENABLED       4
#define DS_QUICK_STOP       5
#define DS_FAULT_REACTION   6
#define DS_FAULT            7
#define DS_UNKNOWN          8

unsigned short DriveState(unsigned short *raw)
{
 unsigned short sw;

 sw = ReadObject(Statusword,0x00);
 if(raw) *raw = sw;

 if((sw & 0x004F) == 0x0040) return(DS_SWITCH_ON_DIS);
 if((sw & 0x004F) == 0x0000) return(DS_NOT_READY);
 if((sw & 0x004F) == 0x000F) return(DS_FAULT_REACTION);
 if((sw & 0x004F) == 0x0008) return(DS_FAULT);
 if((sw & 0x006F) == 0x0021) return(DS_READY_TO_SWITCH);
 if((sw & 0x006F) == 0x0023) return(DS_SWITCHED_ON);
 if((sw & 0x006F) == 0x0027) return(DS_OP_ENABLED);
 if((sw & 0x006F) == 0x0007) return(DS_QUICK_STOP);
 return(DS_UNKNOWN);
}

//------------------------------------------------------------------------------
unsigned short IsDriveEnabled(void)
{ //1 = power stage enabled (Operation Enable), 0 = anything else
 return( (ReadObject(Statusword,0x00) & 0x006F) == 0x0027 );
}

//------------------------------------------------------------------------------
void PrintDriveState(void)
{
 unsigned short sw = 0;
 unsigned short st;
 char *name;

 st = DriveState(&sw);
 switch(st)
 {
  case DS_NOT_READY:       name = "Not ready to switch on"; break;
  case DS_SWITCH_ON_DIS:   name = "Switch on disabled";     break;
  case DS_READY_TO_SWITCH: name = "Ready to switch on";     break;
  case DS_SWITCHED_ON:     name = "Switched on";            break;
  case DS_OP_ENABLED:      name = "OPERATION ENABLED";      break;
  case DS_QUICK_STOP:      name = "Quick stop active";      break;
  case DS_FAULT_REACTION:  name = "Fault reaction active";  break;
  case DS_FAULT:           name = "FAULT";                  break;
  default:                 name = "unknown/no reply";       break;
 }
 printf("Statusword = 0x%04X -> %s\n",sw,name);
 printf("  volt_en=%d quickstop=%d warning=%d target_reached=%d fault=%d\n",
        GETBIT(sw,4),GETBIT(sw,5),GETBIT(sw,7),GETBIT(sw,10),GETBIT(sw,3));
 if(st==DS_FAULT || st==DS_FAULT_REACTION)
   printf("  Error code 0x603F = 0x%04X\n",ReadObject(0x603F,0x00));
}
//------------------------------------------------------------------------------
unsigned short EnableDrive(int ms)
{ //returns 1 if drive reached Operation Enable, 0 otherwise
 unsigned short st;
 int t = 0;

 st = DriveState(0);
 if(st==DS_FAULT || st==DS_FAULT_REACTION)
 {
  printf("Drive faulted (0x603F=0x%04X), resetting...\n",ReadObject(0x603F,0x00));
  WriteObject(Controlword,0,0x0080,0);   //fault reset - bit 7
  msWait(100);
  WriteObject(Controlword,0,0x0000,0);   //clear it again (reset is edge triggered)
  msWait(100);
 }

 WriteObject(Controlword,0,0x0006,0);    //shutdown      -> Ready to switch on
 msWait(50);
 WriteObject(Controlword,0,0x000F,0);    //switch on + enable operation

 while(t < ms)
 {
  msWait(50); t += 50;
  st = DriveState(0);
  if(st==DS_OP_ENABLED){ printf("Drive enabled\n"); return(1); }
  if(st==DS_FAULT)     { printf("Enable FAILED, 0x603F=0x%04X\n",ReadObject(0x603F,0x00)); return(0); }
 }
 printf("Enable timed out - ");
 PrintDriveState();
 return(0);
}
//------------------------------------------------------------------------------
unsigned short WaitForEpos(int ms)
{
 int t = 0;
 unsigned short logic = 0;
 unsigned short sw = 0;
 unsigned short ok = 0;

 ReadQuiet = 1;
 
 printf("Waiting for EPOS4");
 while(t < ms)
 {
  logic = ReadObject(0x2200,0x02);       //internal valid logic supply
  sw    = ReadObject(Statusword,0x00);   //must look like a real CiA402 word

  if(logic == 1 && sw != 0x0000 && sw != 0xFFFF)
   {
    ok++;
    if(ok >= 2)
     {
      printf(" ready after %d ms, SW = 0x%04X, supply = %u (0.1V)\n",
             t,sw,ReadObject(0x2200,0x01));
      ReadQuiet = 0;
      return(1);
     }
   }
  else ok = 0;

  msWait(100);
  t += 100;
  if((t % 500) == 0) printf(".");
 }
 printf(" TIMEOUT - logic=%u, SW=0x%04X\n",logic,sw);
 ReadQuiet = 0;
 return(0);
}
//------------------------------------------------------------------------------
unsigned short RotateSampler(void)
{ //home if needed, rotate to position 15, then carry on round to home
  //returns 1 = completed OK, 0 = failed
 const unsigned short position = 15;
 long target;
 unsigned short rc;
 unsigned short try;

 //---- 1. home first if we are not sitting on the home switch
 if(CheckForHome())
  {
   printf("Not at home, homing............................\n");
   rc = 1; try = 0;
   while(rc && try < 3)
    {
     rc = Home(MAXHOMETIME);
     try++;
    }
   if(rc)
    {
     printf("RotateSampler: FAILED TO HOME\n");
     return(0);
    }
  }
 else printf("At home\n");

 //---- 2. rotate to the requested position
 target = -(long)position * (long)eeRead(eOFFSET);
 printf("Rotating to position %u (%ld cnts)............\n",position,target);

 if(!MoveCntsRelative(target))
  {
   printf("RotateSampler: move to position %u not accepted\n",position);
   return(0);
  }
 if(!WaitForMotion(MAXHOMETIME))
  {
   printf("RotateSampler: move to position %u did not finish\n",position);
   return(0);
  }
 printf("At position %u, Pos = %ld\n",position,GetPosition32());

 //---- 3. carry on round to home
 printf("Returning home.................................\n");
 rc = 1; try = 0;
 while(rc && try < 3)
  {
   rc = Home(MAXHOMETIME);
   try++;
  }
 if(rc)
  {
   printf("RotateSampler: FAILED TO RETURN HOME\n");
   return(0);
  }

 printf("RotateSampler complete, Pos = %ld\n",GetPosition32());
 return(1);
}
//------------------------------------------------------------------------------