#include <Servo.h>
#include <SoftwareSerial.h>
#include <PID_v1.h>

#define DISABLED_PIN 255
#define DATA_N 13

Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;

SoftwareSerial IMU(2, DISABLED_PIN); // RX and TX

double Roll_Setpoint, Roll_Input, Roll_Output;
double Pitch_Setpoint, Pitch_Input, Pitch_Output;
double Kp = 2, Ki = 0.2, Kd = 0.2;

PID Roll_PID(&Roll_Input, &Roll_Output, &Roll_Setpoint, Kp, Ki, Kd, REVERSE);
PID Pitch_PID(&Pitch_Input, &Pitch_Output, &Pitch_Setpoint, Kp, Ki, Kd, REVERSE);

void setup() {
  Serial.begin(9600);
  IMU.begin(9600); // Start serial to IMU

  setupServos();
  setupPID();

  Roll_Setpoint = 0;
  Pitch_Setpoint = 0;
}

void loop() {
  float euler[DATA_N];
  String dt = "";

  if (EBimuAsciiParser(euler, DATA_N)) {
    Roll_Input = euler[0];
    Pitch_Input = euler[1];

    computePID();

    int servo1Pos = map(Roll_Output, 0, 255, 0, 80);
    int servo2Pos = map(Pitch_Output, 0, 255, 0, 80);

    controlServos(servo1Pos, servo2Pos);

    dt = ("/*" + String(euler[2]) + "," + String(euler[1]) + "," + String(euler[0]) + "*/");
    delay(40);
    Serial.print(servo1Pos);
    Serial.println(dt);
  }
}

void setupServos() {
  servo1.attach(9);
  servo2.attach(10);
  servo3.attach(11);
  servo4.attach(12);

  servo1.write(80);
  servo2.write(80);
  servo3.write(80);
  servo4.write(80);
}

void setupPID() {
  Roll_PID.SetMode(AUTOMATIC);
  Pitch_PID.SetMode(AUTOMATIC);
  Roll_PID.SetOutputLimits(0, 255);
  Pitch_PID.SetOutputLimits(0, 255);
}

void computePID() {
  Roll_PID.Compute();
  Pitch_PID.Compute();
}

void controlServos(int servo1Pos, int servo2Pos) {
  if (Roll_Input < 0) {
    servo1.write(servo1Pos);
    servo3.write(80);
  } else {
    servo3.write(servo1Pos);
    servo1.write(80);
  }

  if (Pitch_Input > 0) {
    servo2.write(servo2Pos);
    servo4.write(80);
  } else {
    servo4.write(-servo1Pos);
    servo2.write(80);
  }
}

int EBimuAsciiParser(float *item, int number_of_item) {
  static char sbuf[SBUF_SIZE];
  static signed int sbuf_cnt = 0;

  int rbytes = IMU.available();
  for (int n = 0; n < rbytes; n++) {
    sbuf[sbuf_cnt] = IMU.read();

    if (sbuf[sbuf_cnt] == 0x0a) {
      char *addr = strtok(sbuf, ",");
      for (int i = 0; i < number_of_item; i++) {
        item[i] = atof(addr);
        addr = strtok(NULL, ",");
      }
      return 1;
    } else if (sbuf[sbuf_cnt] == '*') {
      sbuf_cnt = -1;
    }

    sbuf_cnt++;
    if (sbuf_cnt >= SBUF_SIZE) {
      sbuf_cnt = 0;
    }
  }
  return 0;
}