//Import
#include "GY_85.h"
#include <Wire.h>


//Declare library objects
GY_85 IMU;


//GPIO PIN MAPPING
const byte  M1_ENC_A        = 34;
const byte  M1_ENC_B        = 35;
const byte  M2_ENC_A        = 32;
const byte  M2_ENC_B        = 33;
const byte  M1_A            = 18;
const byte  M1_B            = 19;
const byte  M2_A            = 16;
const byte  M2_B            = 17;
const byte  IMU_I2C_SCL     = 26;
const byte  IMU_I2C_SDA     = 27;


//System variables
unsigned long tNow          = micros();


//Motor variables
const int   PWM_CYCLE       = 12000;
const byte  PWM_RESOLUTION  = 12;

//Encoders variables
long int        m1Raw, m1RawLast;
long int        m2Raw, m2RawLast;
volatile bool   M1_A_state;
volatile bool   M1_B_state;
volatile bool   M2_A_state;
volatile bool   M2_B_state;


//Interrupt routines
//  CW =  INCREASE
//  CCW = DECREASE


void IRAM_ATTR m1_A_changed() {
  M1_A_state = digitalRead(M1_ENC_A);
  M1_B_state = digitalRead(M1_ENC_B);

  //Rising
  if (M1_A_state == HIGH) {
    if (M1_B_state == HIGH) {
      m1Raw = m1Raw - 1;
    }
    else if (M1_B_state == LOW) {
      m1Raw = m1Raw + 1;
    }
  }

  //Falling
  else if (M1_A_state == LOW) {
    if (M1_B_state == HIGH) {
      m1Raw = m1Raw + 1;
    }
    else if (M1_B_state == LOW) {
      m1Raw = m1Raw - 1;
    }
  }
}


void IRAM_ATTR m1_B_changed() {
  M1_A_state = digitalRead(M1_ENC_A);
  M1_B_state = digitalRead(M1_ENC_B);

  //Rising
  if (M1_B_state == HIGH) {
    if (M1_A_state == HIGH) {
      m1Raw = m1Raw + 1;
    }
    else if (M1_A_state == LOW) {
      m1Raw = m1Raw - 1;
    }
  }

  //Falling
  else if (M1_B_state == LOW) {
    if (M1_A_state == HIGH) {
      m1Raw = m1Raw - 1;
    }
    else if (M1_A_state == LOW) {
      m1Raw = m1Raw + 1;
    }
  }
}

void IRAM_ATTR m2_A_changed() {
  M2_A_state = digitalRead(M2_ENC_A);
  M2_B_state = digitalRead(M2_ENC_B);

  //Rising
  if (M2_A_state == HIGH) {
    if (M2_B_state == HIGH) {
      m2Raw = m2Raw - 1;
    }
    else if (M2_B_state == LOW) {
      m2Raw = m2Raw + 1;
    }
  }

  //Falling
  else if (M2_A_state == LOW) {
    if (M2_B_state == HIGH) {
      m2Raw = m2Raw + 1;
    }
    else if (M2_B_state == LOW) {
      m2Raw = m2Raw - 1;
    }
  }
}


void IRAM_ATTR m2_B_changed() {
  M2_A_state = digitalRead(M2_ENC_A);
  M2_B_state = digitalRead(M2_ENC_B);

  //Rising
  if (M2_B_state == HIGH) {
    if (M2_A_state == HIGH) {
      m2Raw = m2Raw + 1;
    }
    else if (M2_A_state == LOW) {
      m2Raw = m2Raw - 1;
    }
  }

  //Falling
  else if (M2_B_state == LOW) {
    if (M2_A_state == HIGH) {
      m2Raw = m2Raw - 1;
    }
    else if (M2_A_state == LOW) {
      m2Raw = m2Raw + 1;
    }
  }
}


void setup() {
  //Initialize serial
  Serial.begin(57600);
  delay(10);

  //Initialice I2C
  Wire.begin(IMU_I2C_SCL, IMU_I2C_SDA);
  delay(10);

  //Initialize IMU
  IMU.init();
  delay(10);

  //Initialize encoder interrupts
  pinMode(M1_ENC_A, INPUT_PULLUP);
  pinMode(M1_ENC_B, INPUT_PULLUP);
  pinMode(M2_ENC_A, INPUT_PULLUP);
  pinMode(M2_ENC_B, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(M1_ENC_A), m1_A_changed, CHANGE);
  attachInterrupt(digitalPinToInterrupt(M1_ENC_B), m1_B_changed, CHANGE);
  attachInterrupt(digitalPinToInterrupt(M2_ENC_A), m2_A_changed, CHANGE);
  attachInterrupt(digitalPinToInterrupt(M2_ENC_B), m2_B_changed, CHANGE);

  //Initialize encoders
  m1Raw = 0;
  m1RawLast = 100;
  m2Raw = 0;
  m2RawLast = 100;

  // Initialize PWM channels
  // channels 0-15, resolution 1-16 bits, freq limits depend on resolution
  // ledcSetup(uint8_t channel, uint32_t freq, uint8_t resolution_bits);

  ledcAttachPin(M1_A, 1);
  ledcAttachPin(M1_B, 2);
  ledcAttachPin(M2_A, 3);
  ledcAttachPin(M2_B, 4);

  ledcSetup(1, PWM_CYCLE, PWM_RESOLUTION);
  ledcSetup(2, PWM_CYCLE, PWM_RESOLUTION);
  ledcSetup(3, PWM_CYCLE, PWM_RESOLUTION);
  ledcSetup(4, PWM_CYCLE, PWM_RESOLUTION);

  Serial.println("Reference,Actual,SpeedCommand");

}

void loop() {
  //Update system variables
  tNow = micros();

  ////  //Only print encoder value if value changed since last print
  //  if (m1Raw != m1RawLast) {
  //    Serial.println(m1Raw);
  //    m1RawLast = m1Raw;
  //  }

  //Sense
  readIMU();


  //Think


  //Act
  motorControl();


  //Delay
  delay(5);             // only read every 0,5 seconds, 10ms for 100Hz, 20ms for 50Hz
}