#include <SCServo.h>
#include <math.h>
CFSCL st;
#define Gripper_id 1
#define ARM_SERVO_POS_RANGE 4096
#define ST_TORQUE_MAX 1000
#define ST_TORQUE_MIN 50
struct ServoFeedback {
  bool status;
  int pos;
  int speed;
  int load;
  float voltage;
  float current;
  float temper;
  byte mode;
};
ServoFeedback Gripper;

double ang2deg(double inputAng) {
  return (inputAng / 180) * M_PI;
}

double calculatePosByRad(double radInput) {
  return round((radInput / (2 * M_PI)) * ARM_SERVO_POS_RANGE);
}

void Gripper_JointCtrlRad(double inputAng, u16 speedInput, u8 accInput,u16 torque) {
  st.ServoMode(Gripper_id);
  speedInput = abs(speedInput);
  accInput = abs(accInput);
  double radInput=ang2deg(inputAng);
  s16 computePos = calculatePosByRad(radInput) + 1024;
  s16 Pos = constrain(computePos, 2047, 3150);//90-180
    st.WritePosEx(Gripper_id, Pos, speedInput, accInput,torque);
}

void Gripper_open(u16 speedInput, u8 accInput,u16 torque)
{st.ServoMode(Gripper_id);
  st.WritePosEx(Gripper_id, 2047, speedInput, accInput,torque);
}

void Gripper_close(u16 speedInput, u8 accInput,u16 torque)
{st.ServoMode(Gripper_id);
  st.WritePosEx(Gripper_id, 3150, speedInput, accInput,torque);
}

void Gripper_getFeedBack(){
   if(st.FeedBack(Gripper_id)!=-1) {
    Gripper.status = true;
  	Gripper.pos = st.ReadPos(Gripper_id);
    Gripper.speed = st.ReadSpeed(Gripper_id);
    Gripper.load = st.ReadLoad(Gripper_id);
    Gripper.voltage = st.ReadVoltage(Gripper_id);
    Gripper.current = st.ReadCurrent(Gripper_id);
    Gripper.temper = st.ReadTemper(Gripper_id);
    Gripper.mode = st.ReadMode(Gripper_id);
  } else{
    Gripper.status = false;
    if(InfoPrint == 1){Serial.print("Servo ID:");Serial.print(Gripper_id);
                  Serial.println(" status: failed.");}
  }
}
// feedback info in json.
void Gripper_infoFeedback() {
  jsonInfoHttp.clear();
  jsonInfoHttp["T"] = 1051;
  jsonInfoHttp["pos"] = Gripper.pos;
  jsonInfoHttp["speed"] = Gripper.speed;
  jsonInfoHttp["load"] =  Gripper.load;
  jsonInfoHttp["voltage"] = Gripper.voltage;
  jsonInfoHttp["current"] = Gripper.current;
  jsonInfoHttp["temper"] = Gripper.temper;
  jsonInfoHttp["mode"] = Gripper.mode;

  String getInfoJsonString;
  serializeJson(jsonInfoHttp, getInfoJsonString);
  Serial.println(getInfoJsonString);
}

void servoInit(){
  Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
  st.pSerial = &Serial1;
  while(!Serial1) {}

}
void Gripper_baseTorqueCtrl(int inputTorque) {
  st.unLockEprom(Gripper_id);
 st.writeWord(Gripper_id, RSBL_ST_TORQUE_LIMIT_L, constrain(inputTorque, ST_TORQUE_MIN, ST_TORQUE_MAX));
  st.LockEprom(Gripper_id);
}

void Gripper_dynamicAdaptation(byte inputM, int inputG) {
  if (inputM == 0) {
    Gripper_baseTorqueCtrl(ST_TORQUE_MAX);
  } else if (inputM == 1) {
    Gripper_baseTorqueCtrl(inputG);
  }
}
// ctrl the torque lock of a servo.
// input the servo ID and command: 1-on : produce torque.
//                                 0-off: release torque.
void servoTorqueCtrl(byte servoID, u8 enableCMD){
  st.EnableTorque(servoID, enableCMD);
}

void setMiddle(byte servoID){
    st.CalibrationOfs(servoID);
}
void setEleMode(byte servoID, s16 Torque ){
    st.EleMode(servoID);
    Torque = constrain(Torque, -200, 200);
    st.WriteEle(servoID,Torque);
}
