#include <stdlib.h>
#include <stdio.h>
#include <i86.h>
#include "Syslog.h"
#include "sv203.h"



#define CMDSTRSZ  20
#define RPLYSTRSZ 10
#define BRDCMDSZ 10
#define EEPROM_ADDR (0)

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

sv203::sv203(SerialDevice *dev, int board)
{
int i;

  _board = board;
  _dev = dev;
  _timeout = 1;

  _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);

  selectBoard();

  //read back current positions
  for (i = 0; i < 8; i++) {
    _pos[i] = readRam(50+i+1);
  }
  selectServo(1);

}

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

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


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

  protectedDelay(COMMANDDELAY); 
  _cmdstr[0] = 0x00;
  strcpy(_cmdstr,_boardcmd);
  strcat(_cmdstr,"\r");
  _dev->write(_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();
    _dev->write(_cmdstr, strlen(_cmdstr) );
    addr = readReply(3);
    if (addr != _board) printf("sv203::selectboard() failed\n");
  } while (addr != _board && tries++ < 10);
  if (addr == _board) {
    return 0;
  } else {
    return 1;
  }
}

int sv203::selectServo(int servo)
{
int tries=0;

  do {
    protectedDelay(COMMANDDELAY);
    constructCmd("SV",servo, "RR", 59);
    _dev->write(_cmdstr, strlen(_cmdstr) );
    protectedDelay(COMMANDDELAY);
    tcdrain(_dev->getFd());
  } while (readReply(1) != servo && tries++<10);

  if (tries==10) {
    return 1;
  } else {
    _currentServo = servo;
    return 0;
  }
}


int sv203::moveAbsolute(int pos)
{
int tries=0;

  protectedDelay(COMMANDDELAY);
  constructCmd("M",pos );
  _dev->write( _cmdstr, strlen(_cmdstr) );
  return 0;

  /*
  do {
    protectedDelay(COMMANDDELAY);
    printf("in move absolute\n");
    //    constructCmd("M",pos, "RR", _currentServo+50);
    constructCmd("M",pos );
    _dev->write( _cmdstr, strlen(_cmdstr) );
    protectedDelay(COMMANDDELAY);
    tcdrain(_dev->getFd());
  } while (readReply(3) != pos && tries++<10);

  if (tries==10) {
    return 1;
  } else {
    _pos[_currentServo] = pos;
    return 0;
  }
  */
}

int sv203::moveInc(int inc)
{
int tries=0;
int pos;

  pos = _pos[_currentServo]+inc;
  if (pos > 255) pos = 255;
  if (pos < 0) pos = 0;
  do {
    protectedDelay(COMMANDDELAY);
    constructCmd("I",inc, "RR", _currentServo+50);
    _dev->write( _cmdstr, strlen(_cmdstr) );
    protectedDelay(COMMANDDELAY);
    tcdrain(_dev->getFd());
  } while (readReply(3) != pos && tries++<10);

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

int sv203::pause(int millisec)
{
  delay(COMMANDDELAY);
  constructCmd("D",millisec);
  _dev->write(_cmdstr,strlen(_cmdstr));
  return 0;
}

int sv203::pinSet(int pin)
{
  delay(COMMANDDELAY);
  constructCmd("PS",pin);
  _dev->write(_cmdstr,strlen(_cmdstr));
  return 0;
}

int sv203::pinClear(int pin)
{
  delay(COMMANDDELAY);
  constructCmd("PC",pin);
  _dev->write(_cmdstr,strlen(_cmdstr));
  return 0;
}

int sv203::pinToggle(int pin)
{
  protectedDelay(COMMANDDELAY);
  constructCmd("PT",pin);
  _dev->write(_cmdstr,strlen(_cmdstr));
  return 0;
}

int sv203::readA2D(int port)
{
  delay(COMMANDDELAY);
  constructCmd("AD",port);
  _dev->write(_cmdstr,strlen(_cmdstr));
  return readReply(3);
}

int sv203::shiftIn()
{
  delay(COMMANDDELAY);
  constructCmd("SI");
  _dev->write(_cmdstr,strlen(_cmdstr));
  return 0;
}

int sv203::shiftOut(int bitmask)
{
  delay(COMMANDDELAY);
  constructCmd("SO",bitmask);
  _dev->write(_cmdstr,strlen(_cmdstr));
  return 0;
}

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

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

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

int sv203::eepromRead(int addr)
{
  delay(COMMANDDELAY);
  printf("in eepromRead\n");
  constructCmd("RE",addr);
  _dev->write(_cmdstr,strlen(_cmdstr));
  return readReply(3);
}

void sv203::readState(int *speed, int *temp)
{
int replies[3];

    protectedDelay(COMMANDDELAY);
    constructCmd("AD1AD2");
    _dev->write(_cmdstr, strlen(_cmdstr) );
    readReply(2, replies, 3);
    *speed = replies[0];    
    *temp = replies[1];
    //    _pos[_currentServo] = replies[2];
}

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

}


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

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

int sv203::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,_boardcmd);
  //  strcat(_cmdstr,cmd);
  strcpy(_cmdstr,cmd);
  strcat(_cmdstr,param1);
  strcat(_cmdstr,"  ");
  strcat(_cmdstr,param2);
  strcat(_cmdstr,"\r");
  return 0; 
}

int sv203::constructCmd(char* cmd1, int p1, char* cmd2, 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,_boardcmd);
  //  strcat(_cmdstr,cmd1);
  strcpy(_cmdstr,cmd1);
  strcat(_cmdstr,param1);
  strcat(_cmdstr,"  ");
  strcat(_cmdstr,cmd2);
  strcat(_cmdstr,param2);
  strcat(_cmdstr,"\r");
  return 0; 
}

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

  do {  
    if (timeout==True) {
      _dev->clearPort();
      protectedDelay(COMMANDDELAY);
      _dev->write( _cmdstr, strlen(_cmdstr) );
    }
    timeout = False;
    tcdrain(_dev->getFd());
    //read echo on half duplex serial port
    try {
      _dev->readUntil("\r",1000);
    }
    catch(...){
      //      printf("sv203::readReply() - echo failed %d\n", tries);
      timeout = True;
      break;
    }
    protectedDelay(COMMANDDELAY);
    try {
      bytes =  _dev->readUntil(_replystr,RPLYSTRSZ,"\r\n",
			       _timeout+numbytes/2);
    }
    catch(...) {
      //      printf("sv203::readReply - no response %d %s\n", tries, _cmdstr);
      timeout = True;
    }
  } while (timeout==True && tries++<10);

  if (timeout==True) {
    //    printf("sv203::ReadReply() - failed %d %s\n", tries, _cmdstr);
    //    throw sv203Exception("sv203::readReply failed to read reply str");
  }
  _replystr[bytes-2] = 0x00;
  //  printf("sv203::readReply(): - success %s\n %s\n", _cmdstr, _replystr);
  return  atoi(_replystr);
}

void sv203::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);
      _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",
				 _timeout+numReplies*numbytes/2);
      }
      catch(...) {
	timeout = True;
      }

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

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