/*************************************************************************************************************************************************
 * Nano test TBIB and slave servo driver
 *
 *************************************************************************************************************************************************/

#include <Wire.h>
#include "Adafruit_PWMServoDriver.h"
#include "TBIB_Slave.h"

#define USMIN  600 // minimum pulse length (in uS)
#define USMAX  2400 // maximum pulse length (in uS)
#define SERVO_FREQ 50 // 50 Hz updates

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

const int NumServos = 8;

const int InitialPosition[NumServos] = { // pulse len in microseconds
 (124L) * (USMAX - USMIN) / 255 + USMIN,
 (121L) * (USMAX - USMIN) / 255 + USMIN,
 (129L) * (USMAX - USMIN) / 255 + USMIN,
 ( 90L) * (USMAX - USMIN) / 255 + USMIN,
 (128L) * (USMAX - USMIN) / 255 + USMIN,
 (128L) * (USMAX - USMIN) / 255 + USMIN,
 (128L) * (USMAX - USMIN) / 255 + USMIN,
 (128L) * (USMAX - USMIN) / 255 + USMIN};

int CurrentPosition[NumServos]; // pulse len in 0.1 microseconds
int TargetPosition[NumServos]; // pulse len in 0.1 microseconds
int Velocity[NumServos]; // increment of CurrentPosition per VelPeriod in 0.1 microseconds
const int VelPeriod = 20; // interval in mS between position increments
bool AtTargets = true;

byte MaximumVelocity[NumServos] = {100,100,100,100,100,100,100,100}; // max pulse increment in 0.1 microseconds

//-----------------------------------------------------------
// recvByte()
//  wait for a byte from the PC
//-----------------------------------------------------------
byte recvByte() {
  while (! Serial.available()) ;
  byte b = Serial.read();
  return b;
}

//-----------------------------------------------------------
// recvCmd()
//   get command byte from the PC and act on it
//     byte '0'..'7': select servo 0..7
//       byte i: set servo to i; i is 0..255; output is USMIN..USMAX
//-----------------------------------------------------------
void recvCmd() {
  if (Serial.available() > 0) {
    byte b = Serial.read();
    if ((b >= '0') && (b <= '7')) {
      int i = ((long)recvByte()) * (USMAX - USMIN) / 255 + USMIN;
      //Serial.println(i);
//      pwm.writeMicroseconds(b - '0', i);
      SetTargetPosition(b - '0', i);
    } else
    if (b == 'g') {
      GetPWMserial();
    }
  }
}

//-----------------------------------------------------------
// recvTBIBmsg() 
//-----------------------------------------------------------
void recvTBIBmsg(byte msg, byte *data) {
  switch (msg) {
    case msgPWMs:
      GetPWMtbib();
      break;
    case msgSetServo:
      Serial.print("move servo "); Serial.print(data[0]); Serial.print(" = "); Serial.println(data[1]);  
      int i = ((long)data[1]) * (USMAX - USMIN) / 255 + USMIN;
      SetTargetPosition(data[0], i);
      break;    
  }
}

//-----------------------------------------------------------
// getPWM16()
//   reads the current PWM pulse length from the PCA9685 (in clock counts)
//   (the version in the Adafruit driver doesn't work)
//-----------------------------------------------------------

uint8_t getPWMreg(uint8_t reg) {
  Wire.beginTransmission(PCA9685_I2C_ADDRESS);
  Wire.write(reg);
  Wire.endTransmission();
  Wire.requestFrom(PCA9685_I2C_ADDRESS, 1);
  return Wire.read();
}

uint16_t getPWM16(uint8_t num) {
  int ON = getPWMreg(PCA9685_LED0_ON_H + 4 * num);
  ON <<= 8;
  ON += getPWMreg(PCA9685_LED0_ON_L + 4 * num);

  int OFF = getPWMreg(PCA9685_LED0_OFF_H + 4 * num);
  OFF <<= 8;
  OFF += getPWMreg(PCA9685_LED0_OFF_L + 4 * num);
  
  return OFF - ON;
}

//-----------------------------------------------------------
// GetPWMserial()
//-----------------------------------------------------------
void GetPWMserial()
{
  for (byte servo = 0; servo < NumServos; servo++) {
    Serial.print(servo);
    Serial.print(" ");
    Serial.println(getPWM16(servo));
  }
}

//-----------------------------------------------------------
// GetPWMtbib()
//-----------------------------------------------------------
void GetPWMtbib()
{
  Serial.print("GetPWMtbib "); for (byte servo = 0; servo < NumServos; servo++) {Serial.print(" "); Serial.print(getPWM16(servo));} Serial.println();

  StartMsgToMaster(msgPWMs);
  for (byte servo = 0; servo < NumServos; servo++) {
    int i = getPWM16(servo);
    SendByteToMaster(lowByte(i));
    SendByteToMaster(highByte(i));
  }
  EndMsgToMaster();
}

//-----------------------------------------------------------
// SoftStart()
//-----------------------------------------------------------
void SoftStart()
{
  pwm.setPWMFreq(SERVO_FREQ);  

  for (byte servo = 0; servo < NumServos; servo++) 
    pwm.setPin(servo,0);
    
  for (byte servo = 0; servo < NumServos; servo++) {
    pwm.writeMicroseconds(servo, InitialPosition[servo]);
    delay(500);
  }
}

//-----------------------------------------------------------
// MoveServos() 
//   change servo position so they're closer to their target
//-----------------------------------------------------------
void MoveServos() {
  static unsigned long previousMillis = 0;
  bool b = true;
  if (millis() - previousMillis < VelPeriod) 
    return;
  previousMillis = millis();

  for (byte servo = 0; servo < NumServos; servo++) 
    if (Velocity[servo]) {
      CurrentPosition[servo] += Velocity[servo];
      if (((Velocity[servo] > 0) && (CurrentPosition[servo] >= TargetPosition[servo])) ||
          ((Velocity[servo] < 0) && (CurrentPosition[servo] <= TargetPosition[servo]))) {
        CurrentPosition[servo] = TargetPosition[servo];
        Velocity[servo] = 0;
      }
      pwm.writeMicroseconds(servo, CurrentPosition[servo]/10);
      b = false;
    }

  if (b && !AtTargets) {
    AtTargets = true;
    SendMsgToMaster(msgAtTargets); // all servos are at their targets
  }    
}

//-----------------------------------------------------------
// SetTargetPosition()
//   set servo target position and Velocity
//   calculate Velocity so all servos reach tagrgets at the same time
//-----------------------------------------------------------

void SetTargetPosition(uint8_t servo, int pos) {
  pos *= 10;

  if (CurrentPosition[servo] != pos) {
    TargetPosition[servo] = pos;
    AtTargets = false;
//Serial.println();

    int max_servo_move_time = 1;
    for (byte servo = 0; servo < NumServos; servo++) 
      max_servo_move_time = max(max_servo_move_time,abs(CurrentPosition[servo]-TargetPosition[servo])/MaximumVelocity[servo]);
    
    for (byte servo = 0; servo < NumServos; servo++) {
      Velocity[servo] = constrain(abs(CurrentPosition[servo]-TargetPosition[servo]) / max_servo_move_time, 1, MaximumVelocity[servo]);
      if (TargetPosition[servo] < CurrentPosition[servo])
        Velocity[servo] = -Velocity[servo];
//Serial.print("SetTarget "); Serial.print(servo); Serial.print(" = "); Serial.print(TargetPosition[servo]); Serial.print(", "); Serial.println(Velocity[servo]);
    }    
  }
}

//-----------------------------------------------------------
// setup() 
//-----------------------------------------------------------
void setup() 
{
  Serial.begin(115200);
  Serial.println();
  Serial.println("Booting...");

  SetTBIBpins();

  pwm.begin();
  pwm.setOscillatorFrequency(27000000);
  pwm.setPWMFreq(SERVO_FREQ);  // Analog servos run at ~50 Hz updates
  delay(10);
  SoftStart();
  for (byte servo = 0; servo < NumServos; servo++) {
    CurrentPosition[servo] = InitialPosition[servo]*10;
    Velocity[servo] = 0;
    pwm.writeMicroseconds(servo, CurrentPosition[servo]/10);
  }

  Serial.println("Initialised");
}

//-----------------------------------------------------------
// loop() 
//-----------------------------------------------------------
void loop() {
  recvCmd();
  PollTBIB();
  MoveServos();
}