#include "serialstepper2.h"


Status_t stepMotorInit(serialStepper_t* this){
  this->waiting       = 0;
  this->limitSwitches = 0;
  this->limitSerial   = 0;
  this->pingCounter   = 0;
  return OK;
} 


Status_t stepMotorSetSerial(serialStepper_t *this, char * port){
  if ((SerOpen(&(this->serial), port, 115200)) != OK) {
      printf("Unable to open serial port\n ");
      return CantOpen;
   } 
}



Status_t stepMotorListen(serialStepper_t *this) {
  char * reply = NULL;
  unsigned int repBytes;
  unsigned int i;
  
  SerRead(&(this->serial), &reply, &repBytes);      
  if (repBytes != 0) {
    for (i = 0; i < repBytes; i++) {
      switch(reply[i]) {
        case 'A':  SetBit(0, this->limitSwitches);   this->limitSerial++; break;
        case 'a':  ClearBit(0, this->limitSwitches); this->limitSerial++; break;
        case 'B':  SetBit(1, this->limitSwitches);   this->limitSerial++; break;
        case 'b':  ClearBit(1, this->limitSwitches); this->limitSerial++; break;
        case 'C':  SetBit(2, this->limitSwitches);   this->limitSerial++; break;
        case 'c':  ClearBit(2, this->limitSwitches); this->limitSerial++; break;
        case 'D':  SetBit(3, this->limitSwitches);   this->limitSerial++; break;
        case 'd':  ClearBit(3, this->limitSwitches); this->limitSerial++; break;
        case 'E':  SetBit(4, this->limitSwitches);   this->limitSerial++; break;
        case 'e':  ClearBit(4, this->limitSwitches); this->limitSerial++; break;
        case 'F':  SetBit(5, this->limitSwitches);   this->limitSerial++; break;
        case 'f':  ClearBit(5, this->limitSwitches); this->limitSerial++; break;
        case 'G':  SetBit(6, this->limitSwitches);   this->limitSerial++; break;
        case 'g':  ClearBit(6, this->limitSwitches); this->limitSerial++; break;
        case 'H':  SetBit(7, this->limitSwitches);   this->limitSerial++; break;
        case 'h':  ClearBit(7, this->limitSwitches); this->limitSerial++; break;
        case 'u':  this->pingCounter++;                                   break;
        case '.':  this->waiting = 0;                                     break;
      }        
    }
  }          
  return OK;
}

Status_t stepMotorQLimitUpdate(serialStepper_t *this) {
  return SerWrite(&(this->serial), "?", 1);
}

Status_t stepMotorQPing(serialStepper_t *this) {
  return SerWrite(&(this->serial), "U", 1);
}

Status_t finishStep(serialStepper_t *this) {
  // if we need to , wait for the last step to finish.
  
   while (this->waiting) {
      stepMotorListen(this);    
   } 
   return OK;              
}

Status_t stepMotorSync(serialStepper_t *this) {
  Status_t result; 
  
  finishStep(this); // way to check for errors....
   
  if (result = (SerWrite   (&(this->serial), "\r", 1)) == OK) {
    this->waiting = 1;
  }
  return result;
}


Status_t stepMotorSpd(serialStepper_t* this, unsigned int speed){
  char temp[16];
  sprintf(temp, "s%d.", speed);
  return SerWrite (&(this->serial), temp, strlen(temp));
}


Status_t stepMotorStep(serialStepper_t *this, unsigned int motor, int dir){
  unsigned char command;
  int temp;
  unsigned char cmdTable[] = "AaBbCcDdEeFf";
  
  if (dir == 0) return OK;   // if were not to move, then exit
  
  if (motor < 6) { 
    command = cmdTable[(motor*2)+((dir==(-1))?1:0)];
    return SerWriteNoWait  (&(this->serial), &command, 1);
  } else 
    return BadArg;
    
}


Status_t stepMotorFini(serialStepper_t* this){
  this->waiting = 0;
  return OK;
}