#include <EasyTransfer.h>
#include <Servo.h>
#include <Wire.h>
#include <TroykaIMU.h>

#define BETA 0.22
#define LEFT_MOTOR_DIR A1
#define LEFT_MOTOR_PWM 11
#define RIGHT_MOTOR_DIR A2
#define RIGHT_MOTOR_PWM 2
#define ROLL_SERVO_R 6
#define ROLL_SERVO_L 8
#define CAM_PITCH_SERVO 7
#define CAM_ROLL_SERVO 9

EasyTransfer ET;
Servo rollServoR;
Servo rollServoL;
Servo camPitchServo;
Servo camRollServo;
Madgwick filter;
Accelerometer accel;
Gyroscope gyro;
Compass compass;

struct RECEIVE_DATA_STRUCTURE {
  int throttle = 0;
  int pitch = 0;
  int yaw = 0;
  int camPitch = 0;
  int camRoll = 0;
};
RECEIVE_DATA_STRUCTURE mydata;

float gx, gy, gz, ax, ay, az, mx, my, mz;
float yaw, pitch, roll;
float fps = 100;
const double compassCalibrationBias[3] = {
  524.21,
  3352.214,
  -1402.236
};
const double compassCalibrationMatrix[3][3] = {
  {1.757, 0.04, -0.028},
  {0.008, 1.767, -0.016},
  { -0.018, 0.077, 1.782}
};

int Px = 90;
int Py = 90;
long AsixTime = 0;
long lastTimeConnect = 0;

void setup() {
  Serial.begin(115200);
  ET.begin(details(mydata), &Serial);
  pinMode(13, OUTPUT);
  rollServoR.attach(ROLL_SERVO_R);
  rollServoL.attach(ROLL_SERVO_L);
  accel.begin();
  gyro.begin();
  compass.begin();
  compass.calibrateMatrix(compassCalibrationMatrix, compassCalibrationBias);
  camPitchServo.attach(CAM_PITCH_SERVO);
  camRollServo.attach(CAM_ROLL_SERVO);
  AsixTime = millis();
  pinMode(LEFT_MOTOR_DIR, OUTPUT);
  pinMode(RIGHT_MOTOR_DIR, OUTPUT);
}

void loop() {
  if (ET.receiveData()) {
    if (millis() - lastTimeConnect > 1000) {
      Serial.write('G');
      lastTimeConnect = millis();
    }
    rollServoR.write(mydata.pitch);
    rollServoL.write(180 - mydata.pitch);
    digitalWrite(LEFT_MOTOR_DIR, HIGH);
    digitalWrite(RIGHT_MOTOR_DIR, HIGH);
    analogWrite(LEFT_MOTOR_PWM, constrain(mydata.throttle - mydata.yaw, 0, 255));
    analogWrite(RIGHT_MOTOR_PWM, constrain(mydata.throttle + mydata.yaw, 0, 255));
  }
  unsigned long startMillis = millis();
  accel.readGXYZ(&ax, &ay, &az);
  gyro.readRadPerSecXYZ(&gx, &gy, &gz);
  compass.readCalibrateGaussXYZ(&mx, &my, &mz);
  filter.setKoeff(fps, BETA);
  filter.update(gx, gy, gz, ax, ay, az, mx, my, mz);
  camPitchServo.write(constrain(mydata.camPitch + filter.getRollDeg(), 0, 180));
  camRollServo.write(constrain(mydata.camRoll - filter.getPitchDeg(), 0, 180));
  unsigned long deltaMillis = millis() - startMillis;
  fps = 1000 / deltaMillis;
}