#include "Arduino.h"
#include "servo.h"

using namespace DYNAMIXEL;

const uint8_t DXL_ID = 1;
const float DXL_PROTOCOL_VERSION = 2.0;


dxl_servo servos[SERVO_COUNT];

Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);

void initServos()
{
  //Main Arm first Servo. Model 1020
  servos[0].id=11;
  servos[0].velocity=50;
  servos[0].mode=OP_CURRENT_BASED_POSITION;
  servos[0].current=10.0;
  servos[0].offset_angle=180;
  servos[0].min_angle=180-90;
  servos[0].max_angle=180+90;
  servos[0].orientation=1;
  servos[0].modelnumber=1020;

  //Main Arm second Servo. Model 1080
  servos[1].id=12;
  servos[1].velocity=75;
  servos[1].mode=OP_POSITION;
  servos[1].offset_angle=180;
  servos[1].min_angle=180-90;
  servos[1].max_angle=180+90;
  servos[1].orientation=1;
  servos[1].modelnumber=1080;

  //Main Arm third Servo. Model 1080
  servos[2].id=13;
  servos[2].velocity=100;
  servos[2].mode=OP_POSITION;
  servos[2].offset_angle=180;
  servos[2].min_angle=180-90;
  servos[2].max_angle=180+90;
  servos[2].orientation=1;
  servos[2].modelnumber=1080;

  
  for(uint8_t j=0;j<10;j++) { //Startup Wait
    Serial.print(j+1);Serial.println("/10");
    delay(200);
  }
  /*changeID(1,21);
  scanDynamixel(57600,DXL_PROTOCOL_VERSION,64);
  
  while(1){
    delay(100);
  }*/

  dxl.begin(57600);
  // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);

  uint8_t servoErrors=0;
  for (uint8_t i=0;i<SERVO_COUNT;i++) {
    
    // Get DYNAMIXEL information
    if (!dxl.ping(servos[i].id)){
      Serial.print("Warning: Servo "); Serial.print(i); Serial.print(" does not respond ");
      Serial.println();
      delay(2000);
      servoErrors++;
    }else{
      uint16_t servomodel=dxl.getModelNumber(servos[i].id);
      if (servos[i].modelnumber!=0 && servomodel!=servos[i].modelnumber) { //if modelnumber given, check if it matches
        Serial.print("Warning: Servo "); Serial.print(i); Serial.print(" with id "); Serial.print(servos[i].id); Serial.print(" with Model "); Serial.print(servomodel); Serial.print(" does not match "); Serial.print(servos[i].modelnumber);
        Serial.println();
        servoErrors++;
        delay(2000);
      }

      // Turn off torque when configuring items in EEPROM area
      dxl.torqueOff(servos[i].id);

      dxl.setOperatingMode(servos[i].id, servos[i].mode);
      if (servos[i].current>0 && servos[i].mode==OP_CURRENT_BASED_POSITION) {
        dxl.setGoalCurrent(servos[i].id, servos[i].current, UNIT_PERCENT);
      }

      dxl.writeControlTableItem(ControlTableItem::PROFILE_VELOCITY, servos[i].id, servos[i].velocity); //was 30
      dxl.writeControlTableItem(ControlTableItem::PROFILE_ACCELERATION, servos[i].id, 10);
      
      dxl.torqueOn(servos[i].id);
    }

    

    
  }

  
  if (servoErrors>0) {
    scanDynamixel(57600,DXL_PROTOCOL_VERSION,DXL_BROADCAST_ID);
    while(1){
      delay(100);
    }
  }
  
}


void loopServos(unsigned long millis) {
  
  static uint8_t servoReadI=0; //only one servo per function call
  static unsigned long last_readservo_update=0;

  static uint8_t servoUpdateI=0; //only one servo per function call
  static unsigned long last_servo_update=0;

  //Sequentially set servo angles
  if (millis-last_servo_update>SERVO_UPDATE_INTERVAL/SERVO_COUNT && millis-last_readservo_update>SERVO_MIN_COMMAND_TIME){
    last_servo_update=millis;
    
    dxl.setGoalPosition(servos[servoUpdateI].id, constrain((servos[servoUpdateI].offset_angle+servos[servoUpdateI].angle*servos[servoUpdateI].orientation) , servos[servoUpdateI].min_angle,servos[servoUpdateI].max_angle), UNIT_DEGREE);
    servoUpdateI++;
    servoUpdateI%=SERVO_COUNT;
  }

  //Sequentially read servo status
  if (millis-last_readservo_update>SERVO_READUPDATE_INTERVAL/SERVO_COUNT && millis-last_servo_update>SERVO_MIN_COMMAND_TIME){
    last_readservo_update=millis;
    
    servos[servoReadI].actual_angle = dxl.getPresentPosition(servos[servoReadI].id, UNIT_DEGREE);
    
    servoReadI++;
    servoReadI%=SERVO_COUNT;
  }
  
}

float getAverageServoPositionerror()
{
  float angleerror=0;
  for (uint8_t i=0;i<SERVO_COUNT;i++) {
    float current_error=servos[i].angle+servos[i].offset_angle - servos[i].actual_angle;
    
    current_error=abs(current_error);
    
    angleerror+=current_error;
  }
  angleerror/=SERVO_COUNT;
  return angleerror;
}


void printServoDebug() {
  for (uint8_t i=0;i<SERVO_COUNT;i++) {
    //Serial.print("Servo"); Serial.print(i);Serial.print(" : "); Serial.print(dxl.getPresentPosition(servos[i].id, UNIT_DEGREE)); Serial.print(" -> "); Serial.print(servos[i].offset_angle+servos[i].angle);
    Serial.print("Servo"); Serial.print(i);Serial.print(" : "); Serial.print(servos[i].actual_angle); Serial.print(" -> "); Serial.print(servos[i].offset_angle+servos[i].angle);
    Serial.println();
  }
  Serial.print("erroravg:"); Serial.print(getAverageServoPositionerror());
  Serial.println();
}


void scanDynamixel(int32_t baud, uint8_t protocol,uint8_t maxid) { 
  
  int8_t found_dynamixel = 0;
  dxl.setPortProtocolVersion((float)protocol);
  dxl.begin(baud);
  for(int id = 0; id < maxid; id++) { //highest ID is DXL_BROADCAST_ID
    //iterate until all ID in each buadrate is scanned.
    if(dxl.ping(id)) {
      DEBUG_SERIAL.print("ID : ");
      DEBUG_SERIAL.print(id);
      DEBUG_SERIAL.print(", Model Number: ");
      DEBUG_SERIAL.println(dxl.getModelNumber(id));
      found_dynamixel++;
    }
  }
}

void scanDynamixel() 
{
  #define MAX_BAUD  5
  const int32_t buad[MAX_BAUD] = {57600, 115200, 1000000, 2000000, 3000000};


  int8_t index = 0;
  int8_t found_dynamixel = 0;

    
  for(int8_t protocol = 1; protocol < 3; protocol++) {
    // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
    dxl.setPortProtocolVersion((float)protocol);
    DEBUG_SERIAL.print("SCAN PROTOCOL ");
    DEBUG_SERIAL.println(protocol);
    
    for(index = 0; index < MAX_BAUD; index++) {
      // Set Port baudrate.
      DEBUG_SERIAL.print("SCAN BAUDRATE ");
      DEBUG_SERIAL.println(buad[index]);
      dxl.begin(buad[index]);
      for(int id = 0; id < DXL_BROADCAST_ID; id++) {
        //iterate until all ID in each buadrate is scanned.
        if(dxl.ping(id)) {
          DEBUG_SERIAL.print("ID : ");
          DEBUG_SERIAL.print(id);
          DEBUG_SERIAL.print(", Model Number: ");
          DEBUG_SERIAL.println(dxl.getModelNumber(id));
          found_dynamixel++;
        }
      }
    }
  }
  
  DEBUG_SERIAL.print("Total ");
  DEBUG_SERIAL.print(found_dynamixel);
  DEBUG_SERIAL.println(" DYNAMIXEL(s) found!");
}



void changeID(uint8_t present_id, uint8_t new_id) {
  // put your setup code here, to run once:
  
  // For Uno, Nano, Mini, and Mega, use UART port of DYNAMIXEL Shield to debug.
  DEBUG_SERIAL.begin(115200);
  while(!DEBUG_SERIAL);
  
  // Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate.
  dxl.begin(57600);
  // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);

  DEBUG_SERIAL.print("PROTOCOL ");
  DEBUG_SERIAL.print(DXL_PROTOCOL_VERSION, 1);
  DEBUG_SERIAL.print(", ID ");
  DEBUG_SERIAL.print(present_id);
  DEBUG_SERIAL.print(": ");
  if(dxl.ping(present_id) == true) {
    DEBUG_SERIAL.print("ping succeeded!");
    DEBUG_SERIAL.print(", Model Number: ");
    DEBUG_SERIAL.println(dxl.getModelNumber(present_id));
    
    // Turn off torque when configuring items in EEPROM area
    dxl.torqueOff(present_id);
    
    // set a new ID for DYNAMIXEL. Do not use ID 200
    if(dxl.setID(present_id, new_id) == true){
      present_id = new_id;
      DEBUG_SERIAL.print("ID has been successfully changed to ");
      DEBUG_SERIAL.println(new_id);
    }else{
      DEBUG_SERIAL.print("Failed to change ID to ");
      DEBUG_SERIAL.println(new_id);
    }
  }
  else{
    DEBUG_SERIAL.println("ping failed!");
  }
}