/**
 * Position control example.
 * This example controls a DC motor via a 2PWM-compatible driver hardware,
 * such as a L298N based driver.
 * The example is using a SC60228 magnetic position sensor to monitor the rotor position.
 * 
 * SimpleFOC's position mode is used to move the motor to specific positions (angles)
 * chosen by the user. Positive or negative values determine the direction of
 * rotation. The angle is expressed in radians.
 * 
 * SimpleFOC's commander object is used to enable serial control of the
 * desired angle. After connecting the serial console, type "M100" to
 * set the postion to 100 rad, or "M-2.5" to set it to negative 2.5
 * rad. 
 * Many other motor parameters can be set via the commander, please see 
 * our documentation for details on how to use it.
 */

#include <Arduino.h>

#include "ArduPID.h"


#define ELBOW_POT_PIN A1
#define SHOULDER_POT_PIN A0

#define SHOULDER_PWM_PIN 4
#define SHOULDER_FW_PIN 26
#define SHOULDER_BW_PIN 27
#define ELBOW_PWM_PIN 3
#define ELBOW_FW_PIN 25
#define ELBOW_BW_PIN 24


#define ELBOW_RANGE 300.0
#define SHOULDER_RANGE 180.0
#define FOREARM_LEN 11.8
#define UPPER_ARM_LEN 9
#define SHOULDER_ZERO 700
#define SHOULDER_PLAY 1
#define POT_MAX 300.0
const double SHOULDER_ANGLE_OFFSET = 0*DEG_TO_RAD; //SHOULDER_ZERO * POT_MAX / 1023.0 * DEG_TO_RAD - PI;
const double ELBOW_ANGLE_OFFSET = 30*DEG_TO_RAD;//30.0 * DEG_TO_RAD;


ArduPID shoulderController;
ArduPID elbowController;

double shoulderOutput;
double elbowOutput;
double p = 4000;
double i = 0;
double d = 0;

// Sensor object
// different sensor and encoder types are supported. Please choose the
// right type for your sensor. All the sensors are supported for use
// with DC motors, including the hardware-specific sensor drivers from
// our "SimpleFOC Drivers" library.
// This example uses a SPI absolute magnetic angle sensor of type Semiment SC60

float mapFloat(float x, float in_min, float in_max, float out_min, float out_max) {
  return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min;
}

// Sensor Implementation fuer die Potis
float readElbowAngle() {
  // Potentiometer auslesen
  // Winkel in Radiant zwischen 0 und 2PI zurueckgeben
  return mapFloat(analogRead(ELBOW_POT_PIN), 0, 1023, 0, DEG_TO_RAD * 300) + ELBOW_ANGLE_OFFSET;
}


// Sensor Implementation fuer die Potis
float readShoulderAngle() {
  // Potentiometer auslesen
  // Winkel in Radiant zwischen 0 und 2PI zurueckgeben
  // int value = analogRead(SHOULDER_POT_PIN);
  // Serial.println(value);
  return mapFloat(analogRead(SHOULDER_POT_PIN), 0, 1023, DEG_TO_RAD * 300, 0) + SHOULDER_ANGLE_OFFSET;
  // return mapFloat(value, 0, 1023, 0, DEG_TO_RAD * 300);
}


const double MAX_RPM = 3.0 * 60.0 * HALF_PI;

// float[] makeSteps(float new_angle) {
//   float now = shoulderMotor.shaft_angle;
//   float delta = now - new_angle;
//   int n = abs(delta) * RAD_TO_DEG / 3;
//   float steps[n] = {};
//   return;
// }

void onCoord(char* cmd) {
  char* y_str = strsep(*cmd, ' ');

  float x = atof(cmd);
  float y = atof(y_str);
  steps = atoi(y);

  direction[2] = x;
  direction[3] = y;

  float dx -= wristX;
  float dy -= wristY;

  //float factor = sqrt(sq(x) + sq(y));
  //int len = sqrt(sq(wristX-x)+sq(wristY-y));


  direction[0] = dx/steps;
  direction[1] = dy/steps;

  setTargetAngles(x-dx*(--n), y-dy*(n));
}


// Steuerungsfunktionen
int steps = 0;
double elbowAngle, elbowTarget, shoulderAngle, shoulderTarget, wristX, wristY;
double direction[6] = {0, 0, 0, 0, 0, 0};

void updateWristPos() {
  // 0 ist "rechts", 90 ist senkrecht, usw.
  double _shoulderAngle = shoulderAngle;
  // 180 ist mittig, gg. den Uhrzeigersinn, siehe geogebradatei posbestimmung
  double _elbowAngle = elbowAngle;
  wristX = cos(_shoulderAngle) * UPPER_ARM_LEN - cos(_shoulderAngle + _elbowAngle) * FOREARM_LEN;
  wristY = sin(_shoulderAngle) * UPPER_ARM_LEN - sin(_shoulderAngle + _elbowAngle) * FOREARM_LEN;
}

#define D0_MIN 4.0    // mind. Abstand zum Urspung
#define D0_MAX_TH 1.0 // mind. Abstand zur maximalen Auslegung

void setTargetAngles(float dX, float dY) {
  const double A1_SQ_PLUS_A2_SQ = sq(UPPER_ARM_LEN) + sq(FOREARM_LEN);
  const double TWO_A1_A2 = 2 * UPPER_ARM_LEN * FOREARM_LEN;
  const double MAX_R_SQ = sq(UPPER_ARM_LEN + FOREARM_LEN - D0_MAX_TH); // Etwas Fehlerspielraum
  double px, py, d0, q1, q2, cos_q2;

  px = dX;
  py = dY;

  // sicherstellen das die Position erreichbar ist
  // px und py müssen nicht angepasst werden, da sie nurnoch zusammen als Verhältnis vorkommen
  d0 = constrain(sq(px) + sq(py), D0_MIN, MAX_R_SQ);

  cos_q2 = (d0 - A1_SQ_PLUS_A2_SQ) / TWO_A1_A2;
  q2 = acos(cos_q2);
  
  // cos_q2 wird nicht mehr benoetigt, wir sparen uns eine weitere Variable
  cos_q2 = (UPPER_ARM_LEN + FOREARM_LEN * cos_q2);
  q1 = atan2(py, px) + atan2(FOREARM_LEN * sin(q2), cos_q2);

  shoulderTarget = q1;
  elbowTarget = q2;
}


void handleSerial() {
  // erstes Byte ist der Opcode:
  switch (Serial.read()) {
    case 'E':
      elbowTarget = Serial.parseFloat();
      Serial.print("elbowTarget => ");
      Serial.println(elbowTarget);
      break;
    case 'S':
      shoulderTarget = Serial.parseFloat();
      Serial.print("shoulderTarget => ");
      Serial.println(shoulderTarget);
      break;
    case 'C':
      /*float x = atof(x_str);
      float y = atof(y_str);


      Serial.print("x: ");
      Serial.print(x);
      Serial.print(" y: ");
      Serial.println(y);*/
      setTargetAngles(Serial.parseFloat(), Serial.parseFloat());
      Serial.print("eT: ");
      Serial.print(elbowTarget*RAD_TO_DEG);
      Serial.print(" sT: ");
      Serial.println(shoulderTarget*RAD_TO_DEG);

      break;
  }
}

/**
 * Setup function, in which you should intialize sensor, driver and motor,
 * and the serial communications and commander object.
 * Before calling the init() methods of these objects you can set relevant
 * parameters on them. 
 */
void setup() {
  // to use serial control we have to initialize the serial port
  Serial.begin(115200); // init serial communication
  // wait for serial connection - doesn't work with all hardware setups
  // depending on your application, you may not want to wait
  while (!Serial) {};   // wait for serial connection

  pinMode(ELBOW_POT_PIN, INPUT);
  pinMode(SHOULDER_POT_PIN, INPUT);
  pinMode(SHOULDER_PWM_PIN, OUTPUT);
  pinMode(SHOULDER_FW_PIN, OUTPUT);
  pinMode(SHOULDER_BW_PIN, OUTPUT);
  pinMode(ELBOW_PWM_PIN, OUTPUT);
  pinMode(SHOULDER_FW_PIN, OUTPUT);
  pinMode(ELBOW_BW_PIN, OUTPUT);
  // link driver to motor
  // link sensor to motor

  // set the target velocity to 0, we use the commander to set it later
  shoulderTarget = 80 * DEG_TO_RAD;
  elbowTarget = 180 * DEG_TO_RAD;

  shoulderAngle = readShoulderAngle();
  elbowAngle = readElbowAngle();

  elbowController.begin(&elbowAngle, &elbowOutput, &elbowTarget, p, i, d);
  shoulderController.begin(&shoulderAngle, &shoulderOutput, &shoulderTarget, p, i, d);
  elbowController.setOutputLimits(-255.0, 255.0);  
  shoulderController.setOutputLimits(-255.0, 255.0);
  elbowController.setWindUpLimits(-127, 127);
  shoulderController.setWindUpLimits(-127, 127);

  updateWristPos();

  Serial.println(atan2(10, 0));
  Serial.println("Initialization Complete");
}


int steps = 0;

void loop() {
  if (Serial.available() > 0) {
    handleSerial();
  }
  // call motor.move() once per iteration, ideally at a rate of 1kHz or more.
  // rates of more than 10kHz might need a delay, as the sensor may not be able to
  // update quickly enough (depends on sensor)

  
  shoulderAngle = readShoulderAngle();
  elbowAngle = readElbowAngle();

  shoulderController.compute();
  elbowController.compute();

  
  bool moveShoulder = abs(shoulderTarget - shoulderAngle) > 3 * DEG_TO_RAD;
  bool moveElbow = abs(elbowTarget - elbowAngle) > 3 * DEG_TO_RAD;
  analogWrite(SHOULDER_PWM_PIN, moveShoulder ? (int)abs(shoulderOutput) : 0);
  analogWrite(ELBOW_PWM_PIN, moveElbow ? abs((int)elbowOutput) : 0);
  digitalWrite(SHOULDER_FW_PIN, moveShoulder && shoulderOutput > 1);
  digitalWrite(SHOULDER_BW_PIN, moveShoulder && shoulderOutput <= 1);
  digitalWrite(ELBOW_FW_PIN, moveElbow && elbowOutput > 1);
  digitalWrite(ELBOW_BW_PIN, moveElbow && elbowOutput <= 1);

  /*float angle = motor.shaft_angle;
  if (abs(angle-motor.target) < 0.5 * DEG_TO_RAD ) {
    motor.move(angle);
  } else*/

  // call commander.run() once per loop iteration, it will process incoming commands

  steps++;

  if (steps % 10 == 0) {
    updateWristPos();
  }

  if (false && steps >= 100) {
    steps = 0;  
    Serial.print("X: ");
    Serial.print(wristX);
    Serial.print(" Y: ");
    Serial.print(wristY);
    Serial.print(" e: ");
    Serial.print(elbowAngle);
    Serial.print(" eT: ");
    Serial.print(elbowTarget);
    Serial.print(" s: ");
    Serial.print(shoulderAngle);
    Serial.print(" sT: ");
    Serial.println(shoulderTarget);
    Serial.print("sO: ");
    Serial.print(shoulderOutput);
    Serial.print(" eO: ");
    Serial.println(elbowOutput);
  }

  
}