#include <Dynamixel2Arduino.h>
#define DXL_SERIAL   Serial1
#define DEBUG_SERIAL Serial
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN

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

Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);

//This namespace is required to use Control table item names
using namespace ControlTableItem;

long zero_offset = 0.0;

void setup() {
  // put your setup code here, to run once:
  
  // 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);
  // Get DYNAMIXEL information
  dxl.ping(DXL_ID);

  // Turn off torque when configuring items in EEPROM area
  dxl.torqueOff(DXL_ID);
  dxl.setOperatingMode(DXL_ID, OP_EXTENDED_POSITION);
  dxl.writeControlTableItem(RESOLUTION_DIVIDER, DXL_ID, 1);
  dxl.torqueOn(DXL_ID);

  // Limit the maximum velocity in Position Control Mode. Use 0 for Max speed
  dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 0);
  delay(500);


  // long current_position = dxl.getPresentPosition(DXL_ID);
  // zero_offset = current_position;
  // Serial.print("Zero_offset: ");
  // Serial.println(zero_offset);
  // delay(100);

  // Serial.print("Going to: ");
  // Serial.println(zero_offset);
  // dxl.setGoalPosition(DXL_ID, zero_offset); delay(2000);

  // current_position = dxl.getPresentPosition(DXL_ID);
  // Serial.print("now at: ");
  // Serial.println(current_position);
}



void loop() {

  // while(Serial.available() == 0) { }
  // String serial_string;
  // char char_input[6];  //char array buffer
  // int int_input[4];  //char array buffer
  // serial_string = Serial.readString();
  // // serial_string.toCharArray(char_input, 6);
  // long output = serial_string.toInt();

  // Serial.print("char array: ");
  // Serial.print(char_input[0]);
  // Serial.print(char_input[1]);
  // Serial.print(char_input[2]);
  // Serial.print(char_input[3]);
  // Serial.print(char_input[4]);
  // Serial.println(char_input[5]);

  // int_input[0] = char_input[0].toInt();
  // int_input[1] = char_input[0].toInt();
  // int_input[2] = char_input[0].toInt();
  // int_input[3] = char_input[0].toInt();

  // Serial.print("atoi char array: ");
  // Serial.print(atoi(char_input[0]));
  // Serial.print(atoi(char_input[1]));
  // Serial.print(atoi(char_input[2]));
  // Serial.print(atoi(char_input[3]));
  // Serial.print(atoi(char_input[4]));
  // Serial.println(atoi(char_input[5]));
  // // delay(10000000);

  // output = output + (char_input[1] - "0") * 1000;
  // output = output + (char_input[2] - "0") * 100;
  // output = output + (char_input[3] - "0") * 10;
  // output = output + (char_input[4] - "0") * 1;
  // if (char_input[0] == "-") {output = output * -1;}

  // Serial.print(" Input recieved: ");
  // Serial.println(output);
  long current_position = dxl.getPresentPosition(DXL_ID);
  
  // Serial.print(" Current: ");
  // Serial.print(current_position);
  // Serial.print(", target: ");
  // Serial.print(output);
  // Serial.println(", see you in 100ms");

  // dxl.setGoalPosition(DXL_ID, output);
  // delay(1000);
  
  // current_position = dxl.getPresentPosition(DXL_ID);

  // Serial.print("We moved, Now at: ");
  // Serial.println(current_position);
  // Serial.println();


  // put your main code here, to run repeatedly:
  
  // Please refer to e-Manual(http://emanual.robotis.com/docs/en/parts/interface/dynamixel_shield/) for available range of value. 
  // Set Goal Position in RAW value
  
  Serial.print("pos: ");
  Serial.println(current_position);
  delay(500);
  // int i_present_position = 0;
  // i_present_position = dxl.getPresentPosition(DXL_ID);
 
  
}