/****************************************************************************/
/* Copyright 2016 MBARI.                                                    */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/

#include <xc.h>
#include <stdio.h>
#include <string.h>
#include "uart1.h"
#include "elmo.h"
#include "timer.h"
#include "eeprom.h"

#define IBUF_SIZE 256
#define OBUF_SIZE 24
#define MAX_PUMP_SECS 150
char instr[IBUF_SIZE]; //input buffer

//Elmo commands
//Best I can tell command must be < 12 bytes for non interrupt comms
#define CNTS "PX\r"
#define UM "UM\r"
#define HOME "XQ##HOME()\r"
#define NEXT "XQ##Next()\r"
//#define ADV "XQ##Adv()\r"
#define ADV "XQ##Adv"
#define MO "MO\r"
#define PUMPON "OL[1]=1\r"
#define PUMPOFF "OL[1]=0\r"
#define PURGE 30 //Seconds to PURGE in HOME position
#define PUMP 70 //Seconds to fill bag
#define SV "SV\r"
#define UI "UI[1]\r"
#define MF "MF\r" //motor fault status
#define BUFFCLEAR "\r"


/*on board UI variables the elmo uses
	ui[1] = sample position
	ui[2] = current position cnts
	ui[3] = Homing offset cnts
	ui[4] = advance position cnts
	ui[5] = ms to wait for advancing
 */

int elmo_UI_get (int val)
{   //Get the UI[val] value from elmo
    // return i = -1 bad reading
    //            UI[val] value if success
    char outstr[12];
    int i=-1;
    
    ClearRxFifo();
    sprintf(outstr,"UI[%d]\r",val);
    //printf("Sending %s\n",outstr);
    vPutStrU(outstr,2);
    //i=RxStr2(IBUF_SIZE, instr,10000,1000 );
    //printf("rx'd %d chars: %s\n",i, instr);
    //strncpy(instr,"\0",IBUF_SIZE);
    //i=RxStr2(IBUF_SIZE, instr,10000,1000 );
    //printf("rx'd %d chars: %s\n",i, instr);
    
    //DelaySecs ( 3 );
    i=UI_parse(1000);
    if(i<0)printf("!UI_get read fail\n");
    return(i);
}
//******************************************************************************
int elmo_UI_set (int val, int pos)
{   //set UI[val]=pos in elmo eeprom
    //returns i = -1 failure
    //            UI[val] updated value on success
    int i=-1;
    char outstr[12];
    
    if(pos > 15 || pos < 0)return(i); //bounds checking
    
    sprintf(outstr,"UI[%d]=%d\r",val,pos);
    printf("SAVING %s\n",outstr);
    vPutStrU(outstr,2);
    UI_parse(1000);
    //RxStr2(IBUF_SIZE, instr,10000,1000 );
    //strncpy(instr,"\0",IBUF_SIZE);
    //RxStr2(IBUF_SIZE, instr,10000,1000 );
    
    //save
    elmo_SV();
    DelaySecs(2);
    
    //verify new value
    i = elmo_UI_get(val);
    printf("After save UI[%i] = %i\r\n", val, i);
     
    return(i);
}
//******************************************************************************
int elmo_MO_get (void)
{   //Get the current MO value from elmo (MO=motor on)
    //return i = -1 failure
    //         = 0 motor off
    //         = 1 motor on
    int i=-1;
    
    ClearRxFifo ();
    vPutStrU(MO,2);
    //RxStr2(IBUF_SIZE, instr,10000,1000 );
    //strncpy(instr,"\0",IBUF_SIZE);
    //RxStr2(IBUF_SIZE, instr,10000,1000 );
    i=UI_parse(1000);
    //sscanf(instr,"%d;",&i);
    if(i<0)printf("!MO_get read fail\r\n");
    return(i);
}
//******************************************************************************
int elmo_MO_set (void)
{   //sets MO=0 (turns motor off)
    //return 0
    
    char outstr[12];
    
    ClearRxFifo();
    sprintf(outstr,"MO=0\r");
    vPutStrU(outstr,2);
    UI_parse(1000);
    //RxStr2(IBUF_SIZE, instr,10000,1000 );
    //strncpy(instr,"\0",IBUF_SIZE);
    //RxStr2(IBUF_SIZE, instr,10000,1000 );
    
    //save
    elmo_SV();
      
    return(0);
}
//******************************************************************************
void elmo_sample (void)
{   //does a complete sample cycle
    //returns err_code bit:
        // 0 : MO=1 at start of sample run
        // 1 : Timed out on initial HOME
        // 2 : Motor fault first HOME try
        // 3 : Timed out going to NEXT position
        // 4 : Motor fault going to NEXT position
        // 5 : Timed out on final HOME
        // 6 : Motor fault final HOME try
        // 7 :
    
    int position=-9;
    unsigned int err_code = 0;
    int mo_stat=-1;
    unsigned long mf_stat=0;
    unsigned long i=0;
    int try=0;
    
    //first ensure we are off with MO=0
    mo_stat=elmo_MO_get();
    printf("MO=%d\r\n",mo_stat);
    
    if (mo_stat != 0) //If not zero then make zero
    {
        elmo_MO_set();
        err_code |= 1;
    }
    
    //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);
    
    //get initial position
    printf("Initial cnts = %d\r\n",elmo_PX());
    
    //try 3x to get home
    try=0;i=-1;
    while( (i!=0) && (try<3))
    {
        i = elmo_HOME();
        try++;
    }
    if(i!=0)err_code |= (1<<1); //set flag
    mf_stat = elmo_MF_get();
    if(mf_stat) //there was a fault!
    {
       err_code |= (1<<2); //set flag 
       printf("!MF=%lu\r\n",mf_stat);
    }
    
    //purge
    elmo_RunPump(eeRead(ePURGE));
    
    //try 1x to go to next sample position
    try=0;i=-1;
    while( (i!=0) && (try<1))
    {
        i = elmo_ADV();
        try++;
    }
    if(i!=0)err_code |= (1<<3); //set flag
    err_code = elmo_MF_get();
    if(mf_stat)
    {
      err_code |= (1<<4); //set flag  
      printf("!MF=%lu\r\n",mf_stat);
    }
     
    //save
    //elmo_UI_set(1,position);
    err_code = elmo_SV();
    
      //run pump
    elmo_RunPump(eeRead(ePUMP));
    
    //try 3x to get home
    try=0;i=-1;
    while( (i!=0) && (try<3))
    {
        i = elmo_HOME();
        try++;
    }
    if(i!=0)err_code |= (1<<5); //set flag
    mf_stat = elmo_MF_get();
    if(mf_stat) //there was a fault!
    {
       err_code |= (1<<6); //set flag 
       printf("!MF=%lu\n",mf_stat);
    }

    printf("Sample %d, (err_code=%d)\r\n",position,err_code);
    return;
}
//******************************************************************************
unsigned long elmo_HOME(void)
{ //issue XQ##HOME() to elmo.  Should go to the HOME position
  //returns err_code = 
    //  0  : success
    //  xx  : MF code failure
    
  unsigned long err_code = 0; 
  int mo_code = 1;
  int i=0;
  int cnts;
  
  printf("HOMING");
  ClearRxFifo();
  vPutStrU(HOME,2);
  
  msWait(100);
  
  
  while( (mo_code==1) && (err_code==0) && i<60)
  {
      mo_code=elmo_MO_get();
      err_code=elmo_MF_get();
      cnts = elmo_PX();
      printf("MO=%d, MF=%lu, PX=%d\r\n",mo_code,err_code,cnts);
      DelaySecs ( 1 );
      //printf(".");
      i++;
      //LATAbits.LATA0 ^= 1; //toggle LED every 1 second
  }
  //LATAbits.LATA0 = 1; //leave LED on
  DelaySecs (7);
  printf(" DONE with MO=%d, MF=%lu!\r\n",mo_code,err_code);
  if(i>=59)err_code=1;  //timed out
  
  return err_code;
}
//******************************************************************************
int elmo_PX(void)
{   //get the current motor position cnts
    //return cnts = -1 failure
    //            = cnts value
    int cnts=-1;
    
      //get px cnts
    ClearRxFifo();
    vPutStrU(CNTS,2);
    cnts = UI_parse(1000);
    //printf("CNTS=%d\r\n",cnts);
    
    return(cnts);
}
//******************************************************************************
unsigned long elmo_NEXT(void)
{ //issue XQ##Next() to elmo.  Should go to next sampling position
  // returns err_code =
       // 0 : success
       // 1 : timed out
    
  unsigned long err_code = 0;
  int mo_code=1;
  int cnts=0;
  int i=0;
  
  strncpy(instr,"\0",IBUF_SIZE);
  printf("Next\r\n");
  vPutStrU(NEXT,2);
  RxStr2(IBUF_SIZE, instr,10000,1000 );
  strncpy(instr,"\0",IBUF_SIZE);
  RxStr2(IBUF_SIZE, instr,10000,1000 );
  
  while((mo_code==1) && (err_code==0) && i<60)
  {
      //printf("err_code=%d\r\n",err_code);
      DelaySecs ( 1 );
      mo_code=elmo_MO_get();
      err_code=elmo_MF_get();
      cnts = elmo_PX();
      printf("MO=%d, MF=%lu, PX=%d\r\n",mo_code,err_code,cnts);
      i++;
      LATAbits.LATA0 ^= 1; //toggle LED every 1 second
  }
  LATAbits.LATA0 = 1; //leave LED on
  cnts = elmo_PX();
  printf(" DONE with MO=%d, MF=%lu, PX=%d\r\n",mo_code,err_code,cnts);
  
  return err_code;
}
//******************************************************************************
unsigned long elmo_ADV(void)
{ //issue XQ##Adv() to elmo.  Should go to next sampling position
  // returns MF code
    
  unsigned long err_code = 0;
  int mo_code=1;
  int mo_stat=1;
  int position = -9;
  int cnts=0;
  int i=0;
  char outstr[24];
  
  //first ensure we are off with MO=0
  mo_stat=elmo_MO_get();
  printf("MO=%d\r\n",mo_stat);
    
  if (mo_stat != 0) //If not zero then make zero
   {
        elmo_MO_set();
        err_code |= 1;
   }
    
  //Check what position sampler wants to go to
  position = eeRead(ePOS);
  printf("ELMO ADVANCING to pos %d\r\n",position);
 
  sprintf(outstr,"XQ##Adv(%d)\r",position);
  //eeWrite(position,ePOS);
   
  printf("Sending: %s\r\n",outstr);
  
  //get initial position
  printf("Initial cnts = %d\r\n",elmo_PX());

  
  ClearRxFifo();
  printf("ADVANCING...\r\n");
  vPutStrU(outstr,2);
  msWait(100);
  
  while((mo_code==1) && i<120)
  {
      //printf("err_code=%d\r\n",err_code);
      DelaySecs ( 1 );
      mo_code=elmo_MO_get();
      err_code=elmo_MF_get();
      cnts = elmo_PX();
      printf("MO=%d MF=%lu PX=%d\r\n",mo_code,err_code,cnts);
      i++;
      LATAbits.LATA0 ^= 1; //toggle LED every 1 second
  }
  LATAbits.LATA0 = 1; //leave LED on
  cnts = elmo_PX();

  printf("DONE ADVANCING UI[1]=%d MO=%d MF=%lu PX=%d\r\n",
                                elmo_UI_get(1),mo_code,err_code,elmo_PX());
  //printf("Sample %d, (err_code=%lu)\r\n",position,err_code);
  
  return err_code;
}
//******************************************************************************
int elmo_SV(void)
{ //save current elmo state to its eeprom
  // return 0
    
  int err_code = 0; 
  
  printf("Saving.... ");
  
  ClearRxFifo();
  vPutStrU(SV,2);
  UI_parse(2000);
  printf("DONE\r\n");
  
  return (err_code);
}
//******************************************************************************
unsigned long elmo_MF_get (void)
{   //get the last motor run fault value (0 means no fault)
    //return i = -1 failure
    //         = MF fault value
    
    unsigned long i=-1;
    
    ClearRxFifo();
    vPutStrU(MF,2);
    i=UI_parse(1000);
    //printf("MF=%ld\r\n",i);
    return(i);
}
//******************************************************************************
void elmo_passthru(void)
{ CHAR ch=0;
    IEC1bits.U2RXIE = 0;     // disable Receive Interrupt
    while(ch != '`')
    {
        if( ch=cGetCharU(1) )vPutCharU ( ch, 2 );
        if( ch=cGetCharU(2) )vPutCharU ( ch, 1 );
    }
    IEC1bits.U2RXIE = 1;     // Enable Receive Interrupt
}
//******************************************************************************
void elmo_rotational_maint(void)
{ //rotate 360 degrees taking px readings
   
    char outstr[12];
    int i=0;
    long unsigned int err_code=0;
    int cnts=0;
    int mo_code=0;
    
    //set mode to 2
    printf("UM=2\r\n");
    strncpy(instr,"\0",IBUF_SIZE);
    sprintf(outstr,"UM=2\r\n");
    vPutStrU(outstr,2);
    RxStr2(IBUF_SIZE, instr,10000,1000 );
    strncpy(instr,"\0",IBUF_SIZE);
    RxStr2(IBUF_SIZE, instr,10000,1000 );
    
    //set JV=750
    printf("JV=750\r\n");
    strncpy(instr,"\0",IBUF_SIZE);
    sprintf(outstr,"JV=750\r\n");
    vPutStrU(outstr,2);
    RxStr2(IBUF_SIZE, instr,10000,1000 );
    strncpy(instr,"\0",IBUF_SIZE);
    RxStr2(IBUF_SIZE, instr,10000,1000 );
    
    //set MO=1;
    printf("MO=1\r\n");
    strncpy(instr,"\0",IBUF_SIZE);
    sprintf(outstr,"MO=1\r\n");
    vPutStrU(outstr,2);
    RxStr2(IBUF_SIZE, instr,10000,1000 );
    strncpy(instr,"\0",IBUF_SIZE);
    RxStr2(IBUF_SIZE, instr,10000,1000 ); 
    
    //BG rotation
    printf("MO=1\r\n");
    strncpy(instr,"\0",IBUF_SIZE);
    sprintf(outstr,"BG\r\n");
    vPutStrU(outstr,2);
    RxStr2(IBUF_SIZE, instr,10000,1000 );
    strncpy(instr,"\0",IBUF_SIZE);
    RxStr2(IBUF_SIZE, instr,10000,1000 );
    
    //monitor rotation
      while((err_code==0) && i<10)
        {
            //printf("err_code=%d\r\n",err_code);
            DelaySecs ( 1 );
            mo_code=elmo_MO_get();
            err_code=elmo_MF_get();
            cnts = elmo_PX();
            printf("MO=%d, MF=%lu, PX=%d\r\n",mo_code,err_code,cnts);
            i++;
            LATAbits.LATA0 ^= 1; //toggle LED every 1 second
        }
    
    //kill motion
    elmo_MO_set ();
    
    //send back to home position
    elmo_HOME();
    printf("Rotation err_code=%lu\r\n",err_code);
    return;
}
//******************************************************************************
int elmo_RunPump(int secs)
{ //run the water pump for secs seconds
  //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
  
  strncpy(instr,"\0",IBUF_SIZE);
  printf("Pumping ");
  vPutStrU(PUMPON,2);
  RxStr2(IBUF_SIZE, instr,10000,1000 );
  strncpy(instr,"\0",IBUF_SIZE);
  RxStr2(IBUF_SIZE, instr,10000,1000 );
  
  i=0;
  while(i<secs)
  {
      //printf(".");
      DelaySecs ( 1 );
      //LATAbits.LATA0 ^= 1; //toggle LED every 1 second
      i++;
  }
  
  //LATAbits.LATA0 = 1; //leave LED on
  
  strncpy(instr,"\0",IBUF_SIZE);
  printf("DONE\r\n");
  vPutStrU(PUMPOFF,2);
  RxStr2(IBUF_SIZE, instr,10000,1000 );
  strncpy(instr,"\0",IBUF_SIZE);
  RxStr2(IBUF_SIZE, instr,10000,1000 );
  
  return (err_code);
}
//******************************************************************************
int elmo_change_pump_time(void)
{
    char ch=PUMP; //default to PUMP seconds 
    
    printf("Secs to pump:");     //prompt 
    while (!U1STAbits.URXDA); //wait for char
    ch = cGetCharU(1); //Read one character form UART1
    printf("%c\r\n",ch);  //echo
    
    if( (int)ch < 0   || (int)ch > MAX_PUMP_SECS)ch=PUMP;
    eeWrite((int)ch,ePUMP);
    
    return( (int)ch );
}
//******************************************************************************
int elmo_change_purge_time(void)
{
    char ch=PURGE; //default to PUMP seconds 
    
    printf("Secs to purge:");     //prompt 
    while (!U1STAbits.URXDA); //wait for char
    ch = cGetCharU(1); //Read one character form UART1
    printf("%c\r\n",ch);  //echo
    
    if( (int)ch < 0   || (int)ch > MAX_PUMP_SECS)ch=PURGE;
    eeWrite((int)ch,ePURGE);
    
    return( (int)ch );
}
//******************************************************************************