#include <Wire.h>
#include <MPU6050.h>
#include <MadgwickAHRS.h>
#include <A4988.h>

#define DEBUG 0

#define MOTOR_STEPS 200
#define DIR_R 8
#define STP_R 9
#define DIR_L 10
#define STP_L 11

#define MPU6050_PWR_MGMT_1 0x6B
#define MPU_ADDRESS 0x68

Madgwick MadgwickFilter;
A4988 stepperR(MOTOR_STEPS, DIR_R, STP_R);
A4989 stepperL(MOTOR_STEPS, DIR_L, STP_R);

float roll = 0;
float pitch = 0;
float yaw = 0;

void motor_init(void);
void mpu_init(void);
void mortor_main(void);
void mpu_main(void);

void setup() {
  //stepping motor init
  stepper_R.begin(10, 30);
  stepper_L.begin(10, 30);

  mpu_init();
}

void loop() {
  mpu_main();

  Serial.print(roll); Serial.println("");
  stepper_R.rotate(roll * 0.1);
  stepper_L.rotate(roll * -0.1);
}

void motor_init(void){}

void mpu_init(void){
  Wire.begin();
  Serial.begin(115200);

  Wire.beginTransmission(MPU_ADDRESS);
  Wire.write(MPU6050_PWR_MGMT_1);
  Wire.write(0x00);
  Wire.endTransmission();

  MadgwickFilter.begin(100);
}

void motor_main(void){}

void mpu_main(void){
  Wire.beginTransmission(0x68);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(0x68, 14, true);
  while (Wire.available() < 14);
  int16_t axRaw, ayRaw, azRaw, gxRaw, gyRaw, gzRaw, Temperature;

  axRaw = Wire.read() << 8 | Wire.read();
  ayRaw = Wire.read() << 8 | Wire.read();
  azRaw = Wire.read() << 8 | Wire.read();
  Temperature = Wire.read() << 8 | Wire.read();
  gxRaw = Wire.read() << 8 | Wire.read();
  gyRaw = Wire.read() << 8 | Wire.read();
  gzRaw = Wire.read() << 8 | Wire.read();

  // convert acceleration (G) to divide the acceleration value by the resolution
  float acc_x = axRaw / 16384.0;  //FS_SEL_0 16,384 LSB / g
  float acc_y = ayRaw / 16384.0;
  float acc_z = azRaw / 16384.0;

  // convert angular velocity (deg/sec) to devide the angular velocity by the resolution
  float gyro_x = gxRaw / 131.0;  // degree / sec
  float gyro_y = gyRaw / 131.0;
  float gyro_z = gzRaw / 131.0;
  
  MadgwickFilter.updateIMU(gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z);
  roll  = MadgwickFilter.getRoll();
  pitch = MadgwickFilter.getPitch();
  yaw   = MadgwickFilter.getYaw();
}