#include <Arduino.h>
#include <Wire.h>
#include <HardwareSerial.h>

#define SRV_PWM_FREQ 50
#define SRV_PWM_RES_BITS 10
#define SRV_PWM_REFRESH_USEC 20000

#define TILT_SRV_CHAN 2
#define TILT_SRV_PIN 33
#define TILT_SRV_PWM_MIN 800 // corresp with 0 deg
#define TILT_SRV_PWM_DEFAULT TILT_SRV_PWM_MIN
#define TILT_SRV_PWM_MAX 2200 // corresp with 90 deg

#define CONFIG_FREQ_HB 3
#define CONFIG_FREQ_AGL 4
#define CONFIG_KP 1
#define CONFIG_KI 2
#define CONFIG_KD 3

#define SERIAL_BAUDRATE 115200
#define CMD_PARAMS_NUM 3

uint32_t timePassed = millis();
float elevActual;
float elevSetpoint;
uint16_t tiltPwm = TILT_SRV_PWM_DEFAULT;
struct {
  float Kp = 0.07f;
  float Ki = 0.03f;
  float Kd = 0.04f;
} configs;

int64_t strToInt64(String str) {
  uint8_t DIGITS_PER_PART = 9; // int32 max has >9 digits
  bool isNegative = str.charAt(0) == '-';
  if (isNegative) str = str.substring(1);
  uint8_t digitsLen = str.length();
  int64_t result = 0;
  
  for (int8_t e = digitsLen; e > 0; e -= DIGITS_PER_PART) {
    int8_t pos = (digitsLen - e) / DIGITS_PER_PART;
    int8_t s = e - DIGITS_PER_PART;
    if (s < 0) s = 0;

    int64_t partNum = str.substring(s, e).toInt();
    for (int8_t p = 0; p < pos; p++) partNum *= 1000000000;

    result += partNum;
  }

  if (isNegative) result *= -1;

  return result;
}

uint16_t usPwmToTicks(uint16_t usec) {
  return (uint16_t)((float)usec *
    (pow(2, SRV_PWM_RES_BITS) / (float)SRV_PWM_REFRESH_USEC));
}

void setTiltServoPwm(uint16_t pwmValue) {
  tiltPwm = pwmValue;
  #ifdef ESP32
  ledcWrite(TILT_SRV_CHAN, usPwmToTicks(pwmValue));
  #endif
}

void resetServosPosition() {
  setTiltServoPwm(TILT_SRV_PWM_DEFAULT);
}

void initServo() {
  // TODO: add support to ESP8266
  
  #ifdef ESP32
  // setup tilt servo
  ledcSetup(TILT_SRV_CHAN, SRV_PWM_FREQ, SRV_PWM_RES_BITS);
  ledcAttachPin(TILT_SRV_PIN, TILT_SRV_CHAN);
  #endif

  resetServosPosition();
}

void printStates() {
  static uint32_t prevTime;

  if (timePassed - prevTime >= 100) {
    // Serial.print("actl: ");
    // Serial.print(elevActual);
    // Serial.print("\tsetpo: ");
    // Serial.print(elevSetpoint);
    // Serial.print("\tpwm: ");
    // Serial.print(tiltPwm);
    // Serial.print("\tpid: ");
    // Serial.print(configs.Kp);
    // Serial.print(" ");
    // Serial.print(configs.Ki);
    // Serial.print(" ");
    // Serial.println(configs.Kd);

    prevTime = timePassed;
  }
}

void runCommand(String &cmd) {
  uint16_t cmdStrLen = cmd.length();
  char cmdType = '\0';
  int64_t cmdParams[CMD_PARAMS_NUM];
  String paramStrTemp = "";
  uint8_t paramIdx = 0;

  // fill array with the same initial value
  for(uint8_t i = 0; i < CMD_PARAMS_NUM; i++) {
    cmdParams[i] = INT64_MIN;
  }

  for (uint16_t i = 0; i < cmdStrLen && paramIdx < CMD_PARAMS_NUM; i++) {
    char c = cmd[i];
    if (i == 0) {
      cmdType = c;
      continue;
    }
    if (i == 1 && c == ',') continue;
    if ((c >= '0' && c <= '9') || c == '-') {
      paramStrTemp += c;
    }
    if (c == ',' || i == cmdStrLen-1) {
      cmdParams[paramIdx] = strToInt64(paramStrTemp);
      paramStrTemp = "";
      paramIdx++;
    }
  }

  int64_t &p1 = cmdParams[0];
  int64_t &p2 = cmdParams[1];
  int64_t &p3 = cmdParams[2];

  switch(cmdType) {
    case 'E': // set elevation setpoint angle (mode == SETPOINT_ANGLE)
      if (p1 >= 0 && p1 <= 90) {
        elevSetpoint = static_cast<float>(p1);
      }
      break;
    case 'T': // set servo tilt pwm value (mode == SERVO_PWM)
      if (p1 >= TILT_SRV_PWM_MIN && p1 <= TILT_SRV_PWM_MAX) {
        setTiltServoPwm(p1);
      }
      break;
    case 'C': // set config
      if (p1 == INT64_MIN || p2 == INT64_MIN) break;
      if (p1 >= CONFIG_KP && p1 <= CONFIG_KD) { // PID gains
        float newGain = static_cast<float>(p2) / pow(10, 3);
        if (p1 == CONFIG_KP) configs.Kp = newGain;
        if (p1 == CONFIG_KI) configs.Ki = newGain;
        if (p1 == CONFIG_KD) configs.Kd = newGain;
      }
      break;
  }
}

// read command from serial
void readSerial() {
  static bool isReceivingCmd = false;
  static String receivedCmd = "";

  if (Serial.available() > 0) {
    char rc = Serial.read();

    if (!isReceivingCmd && rc >= 'A' && rc <= 'Z') {
      isReceivingCmd = true;
    }

    if (isReceivingCmd) {
      if (rc != '\n') {
        receivedCmd += rc;
      } else {
        runCommand(receivedCmd);
        isReceivingCmd = false;
        receivedCmd = "";
      }
    }
  }
}

uint16_t updateInterval = 100;

void calculatePID() {
  static uint32_t prevTime;
  static float error, integralError, deltaError, prevError;

  if (timePassed - prevTime >= updateInterval) {
    error = elevSetpoint - elevActual;
    integralError += ((error + prevError) *
      ((float)updateInterval / 1000.0)) / 2;
    deltaError = abs(error - prevError);

    float proportional = configs.Kp * error;
    float integral = configs.Ki * integralError;
    float derivative = configs.Kd * deltaError;

    uint16_t output = elevActual + proportional + integral + derivative;
    output = constrain(output, 0, 90);

    Serial.print(elevActual);
    Serial.print(" -> ");
    Serial.print(elevSetpoint);
    Serial.print(" (");
    Serial.print(error);
    Serial.print(") | ");
    Serial.print(proportional);
    Serial.print(" ");
    Serial.print(integral);
    Serial.print(" ");
    Serial.print(derivative);
    Serial.print(" | gains: ");
    Serial.print(configs.Kp);
    Serial.print(" ");
    Serial.print(configs.Ki);
    Serial.print(" ");
    Serial.println(configs.Kd);

    uint16_t newPwm = map(output, 0, 90, TILT_SRV_PWM_MIN, TILT_SRV_PWM_MAX);
    setTiltServoPwm(newPwm);

    elevActual = output;

    prevError = error;
    prevTime = timePassed;
  }
}

void setup() {
  Serial.begin(SERIAL_BAUDRATE);
  Wire.begin();
  
  initServo();
}

void loop() {
  timePassed = millis();
  
  // don't run blocking codes here
  printStates();
  readSerial();
  calculatePID();

  delay(1); // prevent high processor usage
}