#define SAMPLE_RATE 50
#define BAUD_RATE 9600
#define BUFFER_SIZE 128
#include <Wire.h>

#define MPUaddr0 0x68    //addr used if pin AD0 is set to 0 (left unconnected)
#define MPUaddr1 0x69   

 //addr used if pin AD0 is set to 1 (wired to VDD)
#include <util/atomic.h> // For the ATOMIC_BLOCK macro
#include "Wire.h"

// n20 control pins

int PWM[6] = {2, 3, 4, 5, 6, 7}; 
int ENCA[6] = {8, 9, 10, 11, 12, 13}; 
int ENCB[6] = {22, 23, 24, 25, 26, 27}; 
int IN2[6] = {28, 29, 30, 31, 32, 33}; 
int IN1[6] = {34, 35, 36, 37, 38, 39}; 
int limitSwitch[6] = {40, 41, 42, 43, 44, 45};


unsigned long timer = 0;

float AccelX, AccelY, AccelZ = 0;
float accelXangle, accelYangle = 0;
float pitch, roll = 0;



volatile long posi[6] = {0, 0, 0, 0, 0, 0}; // specify posi as volatile: https://www.arduino.cc/reference/en/language/variables/variable-scope-qualifiers/volatile/
long prevT = 0;
float eprev[6] = {0, 0, 0, 0, 0, 0};
float eintegral[6] = {0, 0, 0, 0, 0, 0};

long base = 1810;

long target[6] = {0, 0, 0, 0, 0, 0};
float theta[6] = {20, 30, 40, 50, 60, 70};

int activeMotors[6] = {0, 1, 1, 1, 1, 0};

float angleX;

float r1 = 39.682;
float r2 = 35;
float s = 86;

float kp = 3;
float kd = 0.05;
float ki = 0;

float getM(float t){
  float rad = t * 3.1415/180;

  // applying inverse kinematic formulae
  float u = (r2*(1-cos(rad)) + r1*(sin(rad))) / s;  
  float c = sqrt(1 - u * u);
  float m = s*(1 - c) - r1*(1 - cos(rad)) + r2*(sin(rad));

  // divide distance by pitch to get number of rots
  float rots = m / 8;

  // rots in encoder units
  return (base * rots);
}

void readEncoder(){
  for(int i = 0; i < 6; i++){
    if(activeMotors[i] == 0){
      continue;
    }
    long b = digitalRead(ENCB[i]);
    if(b > 0){
      posi[i]++;
    }
    else{
      posi[i]--;
    }
  }

}

void setup() {
  Serial.begin(BAUD_RATE);
  Serial.println("Hello world");

  for(int i = 0; i < 6; i++){ 
    if(activeMotors[i] == 0){
      continue;
    }
    pinMode(ENCA[i],INPUT);
    pinMode(ENCB[i],INPUT);
    
    pinMode(PWM[i],OUTPUT);
    pinMode(IN1[i],OUTPUT);
    pinMode(IN2[i],OUTPUT);

    attachInterrupt(digitalPinToInterrupt(ENCA[i]),readEncoder,RISING);
  }


  for(int i = 0; i < 6; i++){
    if(activeMotors[i] == 0){
      continue;
    }
    setMotor(1,200,PWM[i],IN1[i],IN2[i]);
  }
  int flag[6] = {0, 0, 0, 0, 0, 0};

  while(1){
    for(int i = 0; i < 6; i++){
      Serial.print(digitalRead(limitSwitch[i]));
      Serial.print("\t");
      Serial.print(flag[i]);
      Serial.print("\t");
      if(activeMotors[i] == 0){
        continue;
      }
      if(digitalRead(limitSwitch[i]) == 1){
        setMotor(0,0,PWM[i],IN1[i],IN2[i]);
        flag[i] = 1;
      }
    }
    Serial.println();
    int mainFlag = 1;
    for(int i = 0; i < 6; i++){
      if(activeMotors[i] != flag[i]){
        mainFlag = 0;
      }
    }
    if(mainFlag == 1){
      break;
    }

    // continue;
  }

  for(int i = 0; i < 6; i++){
    if(activeMotors[i] == 0){
      continue;
    }
    setMotor(-1,160,PWM[i],IN1[i],IN2[i]);
  }
  int flag2[6] = {0, 0, 0, 0, 0, 0};

  while(1){
    for(int i = 0; i < 6; i++){
      Serial.print(digitalRead(limitSwitch[i]));
      Serial.print("\t");
      Serial.print(flag2[i]);
      Serial.print("\t");
      if(activeMotors[i] == 0){
        continue;
      }
      if(digitalRead(limitSwitch[i]) == 0){
        setMotor(0,0,PWM[i],IN1[i],IN2[i]);
        flag2[i] = 1;
      }
    }
    Serial.println();
    int mainFlag = 1;
    for(int i = 0; i < 6; i++){
      if(activeMotors[i] != flag2[i]){
        mainFlag = 0;
      }
    }
    if(mainFlag == 1){
      break;
    }

    // continue;
  }

  for(int i = 0; i < 6; i++){
    if(activeMotors[i] == 0){
      continue;
    }
    target[i] = getM(theta[i]);
  }

  Serial.println("Setup Complete!!");
  delay(2000);

}

void loop() {
	static unsigned long past = 0;
	unsigned long present = micros();
	unsigned long interval = present - past;
	past = present;

	static long timer = 0;
	timer -= interval;

	if(timer < 0) {
		timer += 1000000 / SAMPLE_RATE;
		Serial.print("    ");

    long currT = micros();
    float deltaT = ((float) (currT - prevT))/( 1.0e6 );
    prevT = currT;

    long pos[6] = {0, 0, 0, 0, 0, 0}; 
    ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
      for(int i = 0; i < 6; i++){
        if(activeMotors[i] == 0){
          continue;
        }
        pos[i] = posi[i];
      }
    }
    
    long error[6], dedt[6];
    
    for(int i = 0; i < 6; i++){
      if(activeMotors[i] == 0){
        continue; 
      }
      error[i] = pos[i] - target[i];
      dedt[i] = (error[i]-eprev[i])/(deltaT);
      eintegral[i] = eintegral[i] + error[i]*deltaT;

      float u = kp*error[i] + kd*dedt[i] + ki*eintegral[i];

      float pwr = fabs(u);
      if( pwr > 90 ){
        pwr = 90;
      }

      int dir = 1;
      if(u<0){
        dir = -1;
      }

      setMotor(dir,pwr,PWM[i],IN1[i],IN2[i]);
      eprev[i] = error[i];

      Serial.print(target[i]);
      Serial.print(" ");
      Serial.print(pos[0]);
      Serial.print(" ");
    }

    timer = millis();  
    
      
    readAccel(16384.0);     //read XYZ Accel data from registers 0x3B to 0x40 
    Serial.println(accelXangle);
  
	}
}


void setMotor(int dir, int pwmVal, int pwm, int in1, int in2){
  analogWrite(pwm,pwmVal);
  if(dir == 1){
    analogWrite(in1,1);
    analogWrite(in2,0);
  }
  else if(dir == -1){
    analogWrite(in1,0);
    analogWrite(in2,1);
  }
  else{
    analogWrite(in1,0);
    analogWrite(in2,0);
  }  
}

void setAccelSensitivity(uint8_t g){
  //Config AFS_SEL[1:0] bits 4 and 3 in register 0x1C
  //0x00: +/-2g (default)
  //0x08: +/-4g
  //0x10: +/-8g
  //0x18: +/-16g

  Wire.beginTransmission(MPUaddr0);   //initialize comm with MPU @ 0x68
  Wire.write(0x1C);                   //write to register 0x1C
  Wire.write(g);                      //setting bit 7 to 1 resets all internal registers to default values
  Wire.endTransmission();             //end comm
}

void resetMPU(void){
  Wire.beginTransmission(MPUaddr0);   //initialize comm with MPU @ 0x68
  Wire.write(0x6B);                   //write to register 0x6B
  Wire.write(0x00);                   //reset all internal registers to default values
  Wire.endTransmission();             //end comm
  delay(100);
}

void readAccel(float accelDivisor){
  //NOTE: as you increase the accelerometer's range, the resolution decreases
  //+/-2g, use divisor of 16384 (14-bit resolution)
  //+/-4g, use divisor of 8192 (13-bit resolution)
  //+/-8g, use divisor of 4096 (12-bit resolution)
  //+/-16g, use divisor of 2048 (11-bit resolution)
  
  Wire.beginTransmission(MPUaddr0);
  Wire.write(0x3B);
  Wire.endTransmission();
  Wire.requestFrom(MPUaddr0, 6);    //read 6 consecutive registers starting at 0x3B
  if (Wire.available() >= 6){
    int16_t temp0 = Wire.read() << 8;   //read upper byte of X
    int16_t temp1 = Wire.read();        //read lower byte of X
    AccelX = (float) (temp0 | temp1);
    AccelX = AccelX / accelDivisor;
    
    temp0 = Wire.read() << 8;           //read upper byte of Y
    temp1 = Wire.read();                //read lower byte of Y
    AccelY = (float) (temp0 | temp1);
    AccelY = AccelY / accelDivisor;
    
    temp0 = Wire.read() << 8;           //read upper byte of Z
    temp1 = Wire.read();                //read lower byte of Z
    AccelZ = (float) (temp0 | temp1);
    AccelZ = AccelZ / accelDivisor;
        
  }
  //You can only calculate roll and pitch from accelerometer data
  accelXangle = (atan2(AccelY, AccelZ)) * 180 / PI;                                   //calculate roll
  accelYangle = (atan2(-AccelX, sqrt(pow(AccelY, 2) + pow(AccelZ, 2)))) * 180 / PI;   //calculate pitch

}