#include <stdio.h>
#include <stdlib.h>
#include <i86.h>
#include "SerialDevice.h"
#include "Syslog.h"
#include "stp100.h"




#define CMDSTRSZ  20
#define RPLYSTRSZ 10
#define BRDCMDSZ 10

//////////////////////////
// constructor


void stp100::mywrite(char *data, int nbytes)
{
int i;
 for (i = 0; i < nbytes; i++) {
   _dev->write(data++,1); delay(1);
 }
}

static void protectedDelay(int ms)
{
int unelapsed;
    unelapsed = ms;
    do {
      unelapsed = delay(unelapsed);
    } while (unelapsed != 0);
}

stp100::stp100(SerialDevice *dev, int board)
{
  _board = board;
  _dev = dev;
  _timeout = 5;
  _offset = 0;

  _dev->raw();
  // twenty is more than needed
  _cmdstr = new char[CMDSTRSZ];
  _replystr = new char[RPLYSTRSZ];
 
  // set up board command substring
  char buffer[BRDCMDSZ];
  _boardcmd = new char[BRDCMDSZ];
  itoa(board,buffer,BRDCMDSZ);
  strcpy(_boardcmd,"BD");
  strcat(_boardcmd,buffer);
}


/////////////////////////
// finalizer

stp100::~stp100()
{
  delete [] _cmdstr;
  delete [] _replystr;
  delete [] _boardcmd;
}


int stp100::selectBoard()
{
int addr, board;
int tries = 0;
static char param[10];


  // start with board select command
  protectedDelay(COMMANDDELAY);
  _cmdstr[0] = 0x00;
  strcpy(_cmdstr,_boardcmd);
  strcat(_cmdstr,"\r");
  mywrite(_cmdstr, strlen(_cmdstr));
  tcdrain(_dev->getFd());

  _cmdstr[0] = 0x00;
  strcpy(_cmdstr,_boardcmd);
  strcat(_cmdstr, "RE");
  param[0] = 0x00; 
  itoa(EEPROM_ADDR ,param, 10);
  strcat(_cmdstr,param);
  strcat(_cmdstr,"\r");

  do {
    protectedDelay(COMMANDDELAY);
    _dev->clearPort();
    mywrite(_cmdstr, strlen(_cmdstr));
    try {
      addr = readReply(3);
    }
    catch(...){};
  } while (addr != _board && tries++ < 10);

  if (addr == _board) {
    return 0;
  } else {
    return 1;
  }
}

int stp100::moveImmediate(int abspos)
{
int tries=0;
int pos;

  protectedDelay(COMMANDDELAY);
  constructCmd("MI",abspos);
  _dev->write( _cmdstr, strlen(_cmdstr) );
  return 0;
/*
  do {

    try {
      pos = readReply(10);
    }
    catch(...){};
  } while (pos != abspos && tries++<10);

  if (tries==10) {
    return 1;
  } else {
    _currentSteps = abspos;
    return 0;
  }
*/
}

int stp100::moveCued(int abspos)
{
int tries=0;

  constructCmd("MC",abspos);

  do {
    protectedDelay(COMMANDDELAY);
    _dev->write( _cmdstr, strlen(_cmdstr) );
  } while (readDest() != abspos && tries++<10);

  if (tries==10) {
    return 1;
  } else {
    _currentSteps = abspos;
    return 0;
  }
}

int stp100::incrementImmediate(int inc)
{
  protectedDelay(COMMANDDELAY);
  constructCmd("II",inc);
  _dev->write( _cmdstr, strlen(_cmdstr) );
  return 0;
}

int stp100::incrementCued(int inc)
{
  protectedDelay(COMMANDDELAY);
  constructCmd("IC",inc);
  _dev->write( _cmdstr, strlen(_cmdstr) );
  return 0;
}

int stp100::cue()
{
  protectedDelay(COMMANDDELAY);
  constructCmd("CU");
  _dev->write( _cmdstr, strlen(_cmdstr) );
  return 0;
}

int stp100::pinSet(int pinnum)
{
  protectedDelay(COMMANDDELAY);
  constructCmd("PS",pinnum);
  _dev->write( _cmdstr, strlen(_cmdstr) );
  return 0;
}

int stp100::pinClear(int pinnum)
{
  protectedDelay(COMMANDDELAY);
  constructCmd("PC",pinnum);
  _dev->write( _cmdstr, strlen(_cmdstr) );
  return 0;
}


int stp100::readPin(int pinnum)
{
  protectedDelay(COMMANDDELAY);
  constructCmd("RP",pinnum);
  _dev->write( _cmdstr, strlen(_cmdstr) );
  return readReply(1);
}

int stp100::readA2D(int chan)
{
  _dev->clearPort();
  protectedDelay(COMMANDDELAY);
  constructCmd("AD",chan);
  mywrite(_cmdstr, strlen(_cmdstr));
  //  _dev->write( _cmdstr, strlen(_cmdstr) );
  return  readReply(3);
}

int stp100::writeRam(int addr, int val)
{
  protectedDelay(COMMANDDELAY);
  constructCmd("WR",addr,val);
  _dev->write( _cmdstr, strlen(_cmdstr) );
  return 0;
}

int stp100::readRam(int addr)
{
  protectedDelay(COMMANDDELAY);
  constructCmd("RR",addr);
  _dev->write( _cmdstr, strlen(_cmdstr) );
  return readReply(3);;
}

int stp100::eepromWrite(int addr, int val)
{
  protectedDelay(COMMANDDELAY);
  constructCmd("WE",addr,val);
  _dev->write( _cmdstr, strlen(_cmdstr) );
  return 0;
}

int stp100::eepromRead(int addr)
{
  protectedDelay(COMMANDDELAY);
  constructCmd("RE",addr);
  _dev->write( _cmdstr, strlen(_cmdstr) );
  return readReply(3);
}

int stp100::writeSysSet()
{
  protectedDelay(COMMANDDELAY);
  constructCmd("WSS");
  _dev->write( _cmdstr, strlen(_cmdstr) );
  return 0;
}

int stp100::readPos()
{
  int val = 0;
  _dev->clearPort();
  protectedDelay(COMMANDDELAY);
  constructCmd("RC");
  mywrite(_cmdstr, strlen(_cmdstr));
  //  _dev->write( _cmdstr, strlen(_cmdstr) );
  val = readReply(10);  
  return val;
}


int stp100::readDest()
{
int val;

  _dev->clearPort();
  protectedDelay(COMMANDDELAY);
  constructCmd("RD");
  _dev->write( _cmdstr, strlen(_cmdstr) );
  try {
    val = readReply(10);
  }
  catch(...) {
    throw stp100Exception("stp100::readDest failed to return value");
  }
  return val;
}

int stp100::readDelta()
{
  protectedDelay(COMMANDDELAY);
  constructCmd("RT");
  _dev->write( _cmdstr, strlen(_cmdstr) );
  return readReply(10);
}

//fix this to verify that the home command went through
int stp100::home(int pos)
{
  protectedDelay(COMMANDDELAY);
  constructCmd("HM",pos);
  _dev->write( _cmdstr, strlen(_cmdstr) );
  _currentSteps = pos;
  return 0;
}

int stp100::haltSmooth()
{
  protectedDelay(COMMANDDELAY);
  constructCmd("HO");
  _dev->write( _cmdstr, strlen(_cmdstr) );
  return 0;
}

int stp100::haltImmediate()
{
  protectedDelay(COMMANDDELAY);
  constructCmd("HI");
  _dev->write( _cmdstr, strlen(_cmdstr) );
  return 0;
}

int stp100::turnClockwise()
{
  protectedDelay(COMMANDDELAY);
  constructCmd("H+");
  _dev->write( _cmdstr, strlen(_cmdstr) );
  return 0;
}

int stp100::turnCounterClockwise()
{
  protectedDelay(COMMANDDELAY);
  constructCmd("H-");
  _dev->write( _cmdstr, strlen(_cmdstr) );
  return 0;
}

int stp100::provideHoldingTorque(Boolean hold)
{
  protectedDelay(COMMANDDELAY);
  if(hold)
    constructCmd("SP");
  else
    constructCmd("S0");

  _dev->write( _cmdstr, strlen(_cmdstr) );
  return 0;
}

int stp100::setStepMode(int mode)
{
  protectedDelay(COMMANDDELAY);
  switch (mode){
    
  case STEPHALF: 
    constructCmd("SH");
    break;
  case STEPFULL:
    constructCmd("SF");
    break;
  case STEPWAVE:
    constructCmd("SW");
     break;
  default :
    return -1;
  }
  mywrite(_cmdstr, strlen(_cmdstr));
  //  _dev->write( _cmdstr, strlen(_cmdstr) );
  return 0;
}

int stp100::setDelay(int del)
{
  protectedDelay(COMMANDDELAY);
  constructCmd("SD",del);
  mywrite(_cmdstr, strlen(_cmdstr));
  //  _dev->write( _cmdstr, strlen(_cmdstr) );
  return 0;
}

int stp100::readDelay()
{
  protectedDelay(COMMANDDELAY);
  constructCmd("RSD");
  mywrite(_cmdstr, strlen(_cmdstr));
  //  _dev->write( _cmdstr, strlen(_cmdstr) );
  return readReply(10);
}

int stp100::setAccelFactor(int accelfactor)
{
  protectedDelay(COMMANDDELAY);
  constructCmd("SA",accelfactor);
  _dev->write( _cmdstr, strlen(_cmdstr) );
  return 0;
}

int stp100::readAccelFactor()
{
  protectedDelay(COMMANDDELAY);
  constructCmd("RSA");
  _dev->write( _cmdstr, strlen(_cmdstr) );
  return readReply(10);
}

int stp100::setMinStepDelayFactor(int del)
{
  protectedDelay(COMMANDDELAY);
  constructCmd("SM",del);
  mywrite(_cmdstr, strlen(_cmdstr));
  //  _dev->write( _cmdstr, strlen(_cmdstr) );
  return 0;
}

int stp100::readMinStepDelayFactor()
{
  protectedDelay(COMMANDDELAY);
  constructCmd("RSM");
  mywrite(_cmdstr, strlen(_cmdstr));
  //  _dev->write( _cmdstr, strlen(_cmdstr) );
  return readReply(10);
}

void stp100::readState()
{
int replies[4];

    protectedDelay(COMMANDDELAY);
    constructCmd("AD1AD2RCRDRE0");
    _dev->write(_cmdstr, strlen(_cmdstr) );
    readReply(5, replies, 10);
    _Pot1 = replies[0] - _Pot1Home;
    _AD2 = replies[1];
    _currentSteps = replies[2];    
    _goalSteps = replies[3];
    if (replies[4] != _board) {
      printf("wierd address\n");
      throw stp100Exception("wierd address\n");
    }
}

int stp100::constructCmd(char* cmd)
{
  _cmdstr[0] = 0x00;
  // append the action command
  strcpy(_cmdstr,cmd);
  // append the carrige return
  strcat(_cmdstr,"\r"); 
  return 0;

}


int stp100::constructCmd(char* cmd, int p1)
{
  _cmdstr[0] = 0x00;
  static char param1[10];
  param1[0] = 0x00; 
  itoa(p1,param1,10);

  // append the action command
  strcpy(_cmdstr,cmd);
  // append the parameter
  strcat(_cmdstr,param1);
  // append the carrige return
  strcat(_cmdstr,"\r");
  return 0;
}

int stp100::constructCmd(char* cmd, int p1, int p2)
{
  _cmdstr[0] = 0x00;
  static char param1[10];
  static char param2[10];
  param1[0] = 0x00; 
  param2[0] = 0x00;
  itoa(p1,param1,10);
  itoa(p2,param2,10);
  strcpy(_cmdstr,cmd);
  strcat(_cmdstr,param1);
  strcat(_cmdstr,"  ");
  strcat(_cmdstr,param2);
  strcat(_cmdstr,"\r");
  return 0; 
}

int stp100::constructCmd(char* cmd1, int p1, char* cmd2)
{
  _cmdstr[0] = 0x00;
  static char param1[10];
  param1[0] = 0x00; 
  itoa(p1,param1,10);
  strcpy(_cmdstr,cmd1);
  strcat(_cmdstr,param1);
  strcat(_cmdstr,"  ");
  strcat(_cmdstr,cmd2);
  strcat(_cmdstr,"\r");
  return 0; 
}

int stp100::readReply(int numbytes)
{
Boolean timeout=False;
int bytes = 0;
int tries = 0;
int unelapsed;

  do {  
    if (timeout==True) {
      _dev->clearPort();
      mywrite(_cmdstr, strlen(_cmdstr));
      //      _dev->write( _cmdstr, strlen(_cmdstr) );
    }
    timeout = False;
    tcdrain(_dev->getFd());
    try {
      _dev->readUntil("\r",1000);
    }
    catch(...) {
      timeout = True;
      break;
    }
    protectedDelay(COMMANDDELAY);
    try {
      bytes =  _dev->readUntil(_replystr,RPLYSTRSZ,"\r\n",
			       _timeout+numbytes/2);
    }
    catch(...) {
      printf("timeout %d - %s\n", tries, _cmdstr);
      timeout = True;
    }
  } while (timeout==True && tries++<10);

  if (timeout==True) {
    printf("timeout happenedint readReply()\n");
    return -1;
  } else {
    _replystr[bytes-2] = 0x00;
    return  atoi(_replystr);
  }
}

void stp100::readReply(int numReplies, int vals[], int numbytes)
{
Boolean timeout=False;
int bytes = 0;
int tries = 0;
int unelapsed;
int i;

  do {  
    if (timeout==True) {
      _dev->clearPort();
      protectedDelay(COMMANDDELAY);
      mywrite(_cmdstr, strlen(_cmdstr));
      //      _dev->write( _cmdstr, strlen(_cmdstr) );
    }
    timeout = False;
    tcdrain(_dev->getFd());
    try {
      _dev->readUntil("\r",1000);
    }
    catch(...) {
      timeout = True;
      break;
    }
    protectedDelay(COMMANDDELAY*numReplies);
    for (i = 0; i < numReplies; i++) {
      try {
	bytes =  _dev->readUntil(_replystr,RPLYSTRSZ,"\r\n",
				 2*_timeout+numbytes/2);
      }
      catch(...) {
	timeout = True;
      }

      if (timeout==True) {
	printf("timeout happened, str is %s\n i is %d\n", _cmdstr, i);
	break;
      }
      _replystr[bytes-2] = 0x00;
      vals[i] = atoi(_replystr);
    }
  }  while (timeout==True && tries++<10);

  if (timeout == True) {
    printf("timeout happened in stp100::readReply(int int*)\n");
    //    throw stp100Exception("stp100::readReply(int int*)  failed to read reply str");
  }
}