#include <Servo.h>

// HARDWARE PINS
#define motBase  2
#define motBoom  3
#define motJib   4
#define motRJib  5
#define motWrPan 6
#define motWrTlt 7

// SERIAL SLOTS
#define axiBase  0
#define axiBoom  1
#define axiJib   2
#define axiRJib  3
#define axiWrPan 4
#define axiWrTlt 5

// array of motors
const int motors[] = { motBase , motBoom, motJib, motRJib, motWrPan, motWrTlt };
// length of motor array;
const int motLength = 6;


//
// Axis Class, a mechanical joint
//
class Axis
{
public: 
  Axis() {
    m_servo = Servo();
    maneuver( 90 ); // home
    m_lastUpdate = millis();
  }

  int  angle()         { return m_angle;      }
  void attach(int pin) { m_servo.attach(pin); }
  
  void update() {
    if((millis() - m_lastUpdate) > m_interval)  // time to update
    {
      m_lastUpdate = millis();
      m_angle = m_servo.read(); 
      
      if (m_angle < m_target) {
        m_angle += m_increment;
        if (m_angle > m_target) {   // overshoot
          m_angle = m_target;
        }
      } 
      else if (m_angle > m_target) {
        m_angle -= m_increment;
        if (m_angle < m_target) {
          m_angle = m_target;       // overshoot
        }
      }
      m_servo.write(m_angle);                   // write the new angle
    }
  }

  void maneuver(int target, int interval = 15, int increment = 1) {
    m_target = constrain(target, 0, 180);
    m_interval = max(interval, 1);
    m_increment = constrain(increment, 1, 180);
  }

private:
  Servo m_servo;              // the servo
  int   m_angle {0};          // current servo position
  int   m_target {0};         // target servo position 
  int   m_increment {0};      // increment to move for each interval
  int   m_interval {0};       // interval between updates
  unsigned long m_lastUpdate; // last update of position
}; // Axis


//
// Robot Class, a mechanical unit
//
class Robot
{
public:
  Robot() {
    for (int i = 0; i < motLength; i++) {
      m_axis[i] = new Axis();
      m_axis[i]->update();
    }
  };

  Axis *axis(int n) { return m_axis[n]; }
  
  void  sequence()  {
    for (int i = 0; i < motLength; i++) {
      m_axis[i]->update();
      m_position[i] = m_axis[i]->angle();
    }
  } // sequence

  String position() {
    String s = "{";
    for (int i = 0; i < motLength; i++) {
      String v = String((uint8_t)m_axis[i]->angle(), HEX);
      if (v.length() < 2) {
        v = '0' + v;
      }
      s = String(s + v);
      if (i < motLength-1) {
        s = String(s + ",");
      }
    }
    return String(s + "}");
  } // position

private:
  Axis *m_axis[motLength];
  int   m_position[motLength] {};
  
}; // Robot
Robot r;


//
//
//
int nibble2number(int n) {
  switch (n) {
  case '0':
    return 0;
  case '1':
    return 1;
  case '2':
    return 2;
  case '3':
    return 3;
  case '4':
    return 4;
  case '5':
    return 5;
  case '6':
    return 6;
  case '7':
    return 7;
  case '8':
    return 8;
  case '9':
    return 9;
  case 'a':
    return 10;
  case 'A':
    return 10;
  case 'b':
    return 11;
  case 'B':
    return 11;
  case 'c':
    return 12;
  case 'C':
    return 12;
  case 'd':
    return 13;
  case 'D':
    return 13;
  case 'e':
    return 14;
  case 'E':
    return 14;
  case 'f':
    return 15;
  case 'F':
    return 15;
  default:
    return -1;
  }
}


//
//  read command from serial port
//
void readCommand() {
  bool inMessage = false;
  int i = 0;
  while (Serial.available()) {
    delay(10);          // small delay to allow input buffer to fill
    
    int c = Serial.read();
    if ( c = -1 ) {     // no bytes read
      break;    
    }
    if (!isAscii(c)) {
      break;            // only ready ASCII
    }

    // start a message
    if (!inMessage) {
      if (c == '{') {   // message start delimiter
        inMessage = true;
      }
      continue;
    }

    // in a message
    if (c == '}') {     // message end delimiter
      inMessage = false;
      break;          
    }
    if (c == ',') {     // value seperator
      i++;
      if (i >= motLength) {
        inMessage = false;
        break;
      }
      continue;
    }

    // read a hex value
    int Msb = c;
    if (!isHexadecimalDigit(Msb)) {
      inMessage = false;
      break;
    }
    while(!Serial.available()) { // Lsb isn't in buffer yer
      delay(3); // todo: make this less bad
    }
    int Lsb = Serial.read();
    if (!isHexadecimalDigit(Lsb)) {
      inMessage = false;
      break;
    }

    // move the axis
    int v = ( Msb << 4 ) | Lsb;
    r.axis(i)->maneuver(v);

    // next
    continue;
  }
  
} // readCommand


//
// ARDUINO Setup
//
void setup() {
  // init hardware
  for (int i = 0; i < motLength; i++) {
    r.axis(i)->attach(motors[i]);
  }
  
  // home isnt' default of 90
  r.axis(axiBoom)->maneuver(140);
  r.axis(axiJib)->maneuver(10);
  r.axis(axiRJib)->maneuver(95);
  
  // turn on the serial port
  Serial.begin(9600);
}


//
// ARDUINO Loop
//
void loop() {
  // read from serial port
  readCommand();
 
  // sequence the arm.
  r.sequence();

  // write current position info to serial port
  if (Serial) {
    Serial.println(r.position());
  }
  delay(15);
}