// Channels are locked to these pins:
// Ch0=Pin2, Ch1=Pin3, Ch2=Pin4, Ch3=Pin5, Ch4=Pin6, Ch5=Pin7, Ch6=Pin8, Ch7=Pin9, Ch8=Pin10, Ch9=Pin11
// Ch10=Pin12, Ch11=Pin13, Ch12=PinA0, Ch13=PinA1, Ch14=PinA2, Ch15=PinA3, Ch16=PinA4, Ch17=PinA5, Ch18=Pin0, Ch19=Pin1
//
// Serial commands:
// # = Servo channel
// P = Pulse width in us
// p = Pulse width in 1/16 us
// S = Speed in us per second
// s = Speed in 1/16 us per second
// T = Time in ms
// PO = Pulse offset in us. -2500 to 2500 in us. Used to trim servo position.
// Po = Pulse offset in 1/16us -40000 to 40000 in 1/16 us
// I = Invert servo movements.
// N = Non-invert servo movements.
// Q = Query movement. Return "." if no servo moves and "+" if there are any servos moving.
// QP = Query servo pulse width. Return 20 bytes where each is from 50 to 250 in 10us resolution. 
//      So you need to multiply byte by 10 to get pulse width in us. First byte is servo 0 and last byte is servo 20.
// <cr> = Carrage return. ASCII value 13. Used to end command.
//
// Examples:
// #0 P1500 T1000<cr>                        - Move Servo 0 to 1500us in 1 second.
// #0 p24000 T1000<cr>                       - Move Servo 0 to 1500us in 1 second.
// #0 p40000 s1600<cr>                       - Move Servo 0 to 2500us in 100us/s speed
// #0 p40000 S100<cr>                        - Move Servo 0 to 2500us in 100us/s speed
// #0 P1000 #1 P2000 T2000<cr>               - Move Servo 0 and servo at the samt time from current pos to 1000us and 2000us in 2 second.
// #0 P2400 S100<cr>                         - Move servo 0 to 2400us at speed 100us/s
// #0 P1000 #1 P1200 S500 #2 P1400 T1000<cr> - Move servo 0, 1 and 2 at the same time, but the one that takes longes S500 or T1000 will be used.
// #0 PO100 #1 PO-100<cr>                    - Will set 100 us offset to servo 0 and -100 us ofset to servo 1
// #0 Po1600 #1 Po-1600<cr>                  - Will set 100 us offset to servo 0 and -100 us ofset to servo 1
// #0 I<cr>                                  - Will set servo 0 to move inverted from standard
// #0 N<cr>                                  - Will set servo 0 back to move non-inverted
// Q<cr>                                     - Will return "." if no servo moves and "+" if there are any servos moving
// QP<cr>                                    - Will retur 18 bytes (each 20ms apart) for position of servos 0 to 17


#include "FR_SVBServo.h" 


FR_SVBServo::FR_SVBServo(SoftwareSerial* toServo)
{
  _toServo=toServo;
  lowerPW=520;
  upperPW=2480;
  init();
}

FR_SVBServo::FR_SVBServo(SoftwareSerial* toServo,int minPW,int maxPW)
{
  _toServo=toServo;
  lowerPW=minPW;
  upperPW=maxPW;
  init();
}

void FR_SVBServo::init()
{
  // _toServo=SoftwareSerial(7,8); // RX, TX
  _toServo->begin(115200);
}

boolean FR_SVBServo::servoActive()
{
  _toServo->write(81);
  _toServo->write(13);
  while(_toServo->available()==0);
//    Serial.println("W");
  if(_toServo->read()=='.')
    return false;
  return true;
}

void FR_SVBServo::moveServo(byte num,byte degree,int stime)
{
  if(degree!=oldDegree[num])
  {
    char msg[20];
    sprintf(msg,"#%d P%d T%d",servo2ch[num],degree2time(degree),stime);
    _toServo->write(msg);
    _toServo->write(13);
    oldDegree[num]=degree;
//    Serial.println(msg);
  }
}

void FR_SVBServo::moveServoArray(byte *degree,int stime,byte starti,byte endi)
{
  String temp;
  char tmp[20];
  byte first=1;
  for(int i=starti;i<=endi;i++)
  {
    if(degree[i]!=oldDegree[i])
    {
      if(first!=1) temp+=" ";
      else first=0;

      sprintf(tmp,"#%d P%d",servo2ch[i],degree2time(degree[i]));
      temp+=tmp;

      oldDegree[i]=degree[i];
    }
  }
  char msg[temp.length()];
  for(int i=0;i<temp.length();i++)
    msg[i]=temp.charAt(i);
  sprintf(tmp," T%d",stime);
  _toServo->write(msg);
  _toServo->write(tmp);
  _toServo->write(13);
//  Serial.println(msg);
}

byte FR_SVBServo::readDegree(byte num)
{
  _toServo->write("QP");
  _toServo->write(13);
  while(_toServo->available()>=18);
//    Serial.println("W");
  for(int i=0;i<num-1;i++)
    _toServo->read();

  return _toServo->read();
}

void FR_SVBServo::readAllDegree(byte *retAngle)
{
  _toServo->write("QP");
  _toServo->write(13);
  while(_toServo->available()>=18);
//    Serial.println("W");
  for(int i=0;i<18;i++)
    retAngle[i]=_toServo->read();
}

int FR_SVBServo::degree2time(byte degree)
{
  return map(degree,0,180,lowerPW,upperPW);
}