/****************************************************************************/
/* 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\n"
#define UM "UM\n"
#define HOME "XQ##HOME()\n"
#define NEXT "XQ##Next()\n"
#define MO "MO\n"
#define PUMPON "OL[1]=1\n"
#define PUMPOFF "OL[1]=0\n"
#define PURGE 30 //Seconds to PURGE in HOME position
#define PUMP 70 //Seconds to fill bag
#define SV "SV\n"
#define UI "UI[1]\n"
#define MF "MF\n" //motor fault status



int elmo_UI_get (void)
{   //Get the UI[1] value from elmo
    // return i = -1 bad reading
    //            UI[1] value if success
    
    int i=-1;
    
    vPutStrU(UI,2);
    RxStr2(IBUF_SIZE, instr,10000,1000 );
    strncpy(instr,"\0",IBUF_SIZE);
    RxStr2(IBUF_SIZE, instr,10000,1000 );
    sscanf(instr,"%d;",&i);
    if(i<0)printf("!UI_get read fail\n");
    return(i);
}
//******************************************************************************
int elmo_UI_set (int pos)
{   //set UI[1] in elmo eeprom
    //returns i = -1 failure
    //            UI[1] updated value on success
    int i=-1;
    char outstr[12];
    
    if(pos > 15 || pos < 0)return(i); //bounds checking
    
    sprintf(outstr,"UI[1]=%d\n",pos);
    printf("%s",outstr);
    vPutStrU(outstr,2);
    RxStr2(IBUF_SIZE, instr,10000,1000 );
    strncpy(instr,"\0",IBUF_SIZE);
    RxStr2(IBUF_SIZE, instr,10000,1000 );
    
    //save
    elmo_SV();
    
    //verify new value
    i = elmo_UI_get();
     
    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;
    
    strncpy(instr,"\0",IBUF_SIZE);
    vPutStrU(MO,2);
    RxStr2(IBUF_SIZE, instr,10000,1000 );
    strncpy(instr,"\0",IBUF_SIZE);
    RxStr2(IBUF_SIZE, instr,10000,1000 );
    sscanf(instr,"%d;",&i);
    if(i<0)printf("!MO_get read fail\n");
    return(i);
}
//******************************************************************************
int elmo_MO_set (void)
{   //sets MO=0 (turns motor off)
    //return 0
    
    char outstr[12];
    
    strncpy(instr,"\0",IBUF_SIZE);
    sprintf(outstr,"MO=0\n");
    vPutStrU(outstr,2);
    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\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 = elmo_UI_get();
    printf("ELMO will go to position %d\n",position+1);
    eeWrite(position+1,ePOS);
    
    //get initial position
    printf("Initial cnts = %d\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\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_NEXT();
        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\n",mf_stat);
    }
     
    //save
    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\n",position+1,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");
  vPutStrU(HOME,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)
  {
      mo_code=elmo_MO_get();
      err_code=elmo_MF_get();
      cnts = elmo_PX();
      printf("MO=%d, MF=%lu, PX=%d\n",mo_code,err_code,cnts);
      DelaySecs ( 1 );
      //printf(".");
      i++;
      LATAbits.LATA0 ^= 1; //toggle LED every 1 second
  }
  LATAbits.LATA0 = 1; //leave LED on
  printf(" DONE with MO=%d, MF=%lu\n",mo_code,err_code);
  if(i>=58)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
    vPutStrU(CNTS,2);
    RxStr2(IBUF_SIZE, instr,10000,1000 );
    strncpy(instr,"\0",IBUF_SIZE);
    RxStr2(IBUF_SIZE, instr,10000,1000 );
    sscanf(instr,"%d;",&cnts);
    //printf("CNTS=%d\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\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\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\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\n",mo_code,err_code,cnts);
  
  return err_code;
}
//******************************************************************************
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\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_SV(void)
{ //save current elmo state to its eeprom
  // return 0
    
  int err_code = 0; 
  
  strncpy(instr,"\0",IBUF_SIZE);
  printf("Saving.... ");
  vPutStrU(SV,2);
  RxStr2(IBUF_SIZE, instr,10000,1000 );
  strncpy(instr,"\0",IBUF_SIZE);
  RxStr2(IBUF_SIZE, instr,10000,1000 );
  
  printf("DONE\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;
    
    vPutStrU(MF,2);
    RxStr2(IBUF_SIZE, instr,10000,1000 );
    strncpy(instr,"\0",IBUF_SIZE);
    RxStr2(IBUF_SIZE, instr,10000,1000 );
    sscanf(instr,"%ld;",&i);
    //printf("MF=%ld\n",i);
    return(i);
}
//******************************************************************************
void elmo_passthru(void)
{ CHAR ch=0;

    while(ch != '`')
    {
        if( ch=cGetCharU(1) )vPutCharU ( ch, 2 );
        if( ch=cGetCharU(2) )vPutCharU ( ch, 1 );
    }
}
//******************************************************************************
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\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\n",ch);  //echo
    
    if( (int)ch < 0   || (int)ch > MAX_PUMP_SECS)ch=PURGE;
    eeWrite((int)ch,ePURGE);
    
    return( (int)ch );
}
//******************************************************************************
void elmo_rotational_maint(void)
{ //rotate 360 degrees taking px readings
       //returns err_code bit:
        // 0 : MF on initial movement
        // 1 : Timed out on HOME
        // 2 : 
        // 3 : 
        // 4 : 
        // 5 : 
        // 6 : 
        // 7 :
    char outstr[12];
    int i=0;
    long unsigned int err_code=0;
    long unsigned int mf_stat=0;
    int cnts=0;
    int mo_code=0;
    
    //set mode to 2
    printf("UM=2\n");
    strncpy(instr,"\0",IBUF_SIZE);
    sprintf(outstr,"UM=2\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\n");
    strncpy(instr,"\0",IBUF_SIZE);
    sprintf(outstr,"JV=750\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\n");
    strncpy(instr,"\0",IBUF_SIZE);
    sprintf(outstr,"MO=1\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\n");
    strncpy(instr,"\0",IBUF_SIZE);
    sprintf(outstr,"BG\n");
    vPutStrU(outstr,2);
    RxStr2(IBUF_SIZE, instr,10000,1000 );
    strncpy(instr,"\0",IBUF_SIZE);
    RxStr2(IBUF_SIZE, instr,10000,1000 );
    
    //monitor rotation
      while((mf_stat==0) && i<10)
        {
            //printf("err_code=%d\n",err_code);
            DelaySecs ( 1 );
            mo_code=elmo_MO_get();
            mf_stat=elmo_MF_get();
            cnts = elmo_PX();
            printf("MO=%d, MF=%lu, PX=%d\n",mo_code,mf_stat,cnts);
            i++;
            LATAbits.LATA0 ^= 1; //toggle LED every 1 second
        }
    if(mf_stat) //there was a fault!
    {
       err_code |= 1; //set flag 
       printf("!MF=%lu\n",mf_stat);
    }
    //kill motion
    elmo_MO_set ();
    
    //send back to home position
    mf_stat = elmo_HOME();
        if(mf_stat) //there was a fault!
    {
       err_code |= (1<<1); //set flag 
       printf("!MF=%lu\n",mf_stat);
    }
    
    printf("Sample Rotation err_code=%lu\n",err_code);
    return;
}
//******************************************************************************