//int acclx = 0;
int accly = 1;
int acclz = 2;
int gyrox = 3;
//int gyroy = 4;
//int gyroxhires = 5;

//int voltsacclx;
int voltsaccly;
int voltsacclz;
int voltsgyrox;
//int voltsgyroxhires;
//int voltsgyroy;

//float gacclx;
//float gacclx2;
float gaccly;
float gaccly2;
float gacclz;
float gacclz2;

int ggyrox;
//int ggyroxhires;
//int ggyroy;
//int ggyrox2;
//int ggyrox2hires;
//int ggyroy2;
float GYRO_X_INIT_VAL = 423.0;
//--------------------------------------
const float INIT_ANGLE = -0.04; //balance offset

const int ANGLE_BUFFER_LENGTH = 1;    
//-------------------------------------
const float SumErrMax = 10;
const float SumErrMin = -10;

//Tuning Knobs
const int analogInPin1 = 0;  // Analog input pin that the potentiometer is attached to
const int analogInPin2 = 4;  // Analog input pin that the potentiometer is attached to
const int analogInPin3 = 5;  // Analog input pin that the potentiometer is attached to

int sensorValue1 = 0;        // value read from the pot
int sensorValue2 = 0;        // value read from the pot
int sensorValue3 = 0;        // value read from the pot
int outputValue1 = 0;        // value output to the P
int outputValue2 = 0;        // value output to the I
int outputValue3 = 0;        // value output to the D

//STEERING POT
const int steering = 7;
int steeringValue = 0;
float turnRate = 0;

// <editor-fold defaultstate="collapsed" desc="Global Variables">

int VxAcc = 0, VyAcc = 0, VzAcc = 0;
int VxGyro = 0, VzGyro = 0;
float degX = 0.0f, degY = 0.0f, degZ = 0.0f;

float throttle = 0.0;
float leftthrottle = 0.0;
float softStartCounter = 0;
float leftsoftStartCounter = 0;

float accAngle = 0.0, currAngle = 0.0, prevAngle = 0.0, elapsedTimeSec=0.0;
unsigned long prevTimeStamp = 0, currTimeStamp = 0, elapsedTime = 0;

boolean isFirstTime = true;

float angleBuffer[ANGLE_BUFFER_LENGTH];
int angleBufferIndex = 0;

boolean MotorOff = false;

float curErr = 0, prevErr = 0, SumErr = 0;
float integralTerm = 0, derivativeTerm = 0;
float Cn = 0;
float LeftCn = 0;

//float Kp = 0.0, Ki = 0.0,  Kd = 0.0;  //hard code pid numbers instead of pot


//MOTOR SETUP
const int bhi = 8;    //on 5v
const int ahi = 7;    //on 5v
const int ali = 6;    //reverse pwm
const int bli = 5;    //forward pwm
const int dis = 4;    //disable 5v=yes=coast

// Left MOTOR SETUP
const int lbhi = 13;    //on 5v
const int lahi = 12;    //on 5v
const int lali = 11;    //reverse pwm
const int lbli = 10;    //forward pwm
const int ldis = 9;    //disable 5v=yes=coast

enum WheelDirection {
    DIR_FORWARD,
    DIR_BACKWARD,
    DIR_STOP
};

enum LeftWheelDirection {
    LeftDIR_FORWARD,
    LeftDIR_BACKWARD,
    LeftDIR_STOP
};

// CALIBRATE GYRO

float CalGyro() {

    float val = 0.0;

    delay(500);

    for (int i = 0; i < 25; i++) {

        val += analogRead(gyrox);

        delay(10);

      }

    val /= 25.0;

    
    return val;

}

float GetAvgAngle() {

    int count = 0;

    float a = 0;

 

    for (int i = 0; i < ANGLE_BUFFER_LENGTH; i++) {

        if (angleBuffer[i] != 0) {

            count++;

            a += angleBuffer[i];

        }

    }
    
    if (count > 0)

        return a / float(count);

    else
    
        return 0.0;

}

float GetAccAngle() {

    float r = sqrt((float) gacclz2 * (float) gacclz2 + (float) gaccly2 * (float) gaccly2);

    accAngle = (float) gacclz2 / r; //approximates sine
    
 
    return accAngle;

}

float EstimateAngle() {

    currTimeStamp = micros();

    elapsedTime = currTimeStamp - prevTimeStamp;

    prevTimeStamp = currTimeStamp;

 

    elapsedTimeSec = (float) elapsedTime / 1e6f;

 

    float angle = GetAccAngle();

    if (isFirstTime) {

        currAngle = angle;

        isFirstTime = false;

    } else {

        VxGyro = analogRead(gyrox);

        degX = (float) (VxGyro - GYRO_X_INIT_VAL) * 3300.0f / 512.0f / 180.0f * (float) elapsedTimeSec;
        degX = degX * - 1.0f;    // Invert Gyro Reading
        currAngle = 0.995 * (prevAngle + degX) + 0.005 * angle;

    }

    prevAngle = currAngle;

 
    if (MotorOff) { //for debugging purposes

        Serial.print("\tcurrAngleAvg=");

        Serial.print(GetAvgAngle(), 4);

        Serial.print("\tcurrAngle=");

        Serial.print(currAngle, 4);

        Serial.print("\taccAngle=");

        Serial.print(accAngle);

        Serial.print("\tgyroAngle=");

        Serial.print(degX + prevAngle);    //* 2864.788975 = degrees

        Serial.print("\tVxGyro=");

        Serial.print(VxGyro);

        Serial.print("\telapsedTime=");

        Serial.print(elapsedTime);

        Serial.print("\tthrottle=");

        Serial.println(throttle);

    }

    return currAngle;
    }

// MOTOR DRIVE
void MoveWheels(WheelDirection (dir), int speed) {

    if (MotorOff) return;

    if (speed > 255) speed = 255;

    else if (speed < 0) speed = 0;
    
    if (dir == DIR_STOP) {

        digitalWrite(ahi, HIGH);      //set ahi and bhi osmc 
        digitalWrite(bhi, HIGH);
        digitalWrite(dis, LOW);
        digitalWrite(ali, LOW);
        digitalWrite(bli, LOW);

    } else if (dir == DIR_FORWARD) {

        digitalWrite(ahi, HIGH);      //set ahi and bhi osmc 
        digitalWrite(bhi, HIGH);
        digitalWrite(dis, LOW);
        digitalWrite(ali, LOW);
        analogWrite(bli, speed);


    } else if (dir == DIR_BACKWARD) {

        digitalWrite(ahi, HIGH);      //set ahi and bhi osmc 
        digitalWrite(bhi, HIGH);
        digitalWrite(dis, LOW);
        analogWrite(ali, speed);
        digitalWrite(bli, LOW);
    }

}
// MOTOR DRIVE LEFT
void LeftMoveWheels(LeftWheelDirection (leftdir), int (leftspeed)) {

    if (MotorOff) return;

    if (leftspeed > 255) leftspeed = 255;

    else if (leftspeed < 0) leftspeed = 0;
    

    if (leftdir == LeftDIR_STOP) {

        digitalWrite(lahi, HIGH);      //set ahi and bhi osmc 
        digitalWrite(lbhi, HIGH);
        digitalWrite(ldis, LOW);
        digitalWrite(lali, LOW);
        digitalWrite(lbli, LOW);

    } else if (leftdir == LeftDIR_FORWARD) {

        digitalWrite(lahi, HIGH);      //set ahi and bhi osmc 
        digitalWrite(lbhi, HIGH);
        digitalWrite(ldis, LOW);
        digitalWrite(lali, LOW);
        analogWrite(lbli, leftspeed);


    } else if (leftdir == LeftDIR_BACKWARD) {

        digitalWrite(lahi, HIGH);      //set ahi and bhi osmc 
        digitalWrite(lbhi, HIGH);
        digitalWrite(ldis, LOW);
        analogWrite(lali, leftspeed);
        digitalWrite(lbli, LOW);
    }

}


void setup()
{
  Serial.begin(19200);
 // pinMode(acclx, INPUT);
  pinMode(accly, INPUT);
  pinMode(acclz, INPUT);
  pinMode(gyrox, INPUT);
 // pinMode(gyroy, INPUT);
  
  analogReference (EXTERNAL);    //gyro and acl use 3.3vref, connect 3.3v to vref
 
 GYRO_X_INIT_VAL = CalGyro();    //gyro calibrates once on startup hold still for 5 seconds
 
        pinMode(ahi, OUTPUT);        //enable motor control pins
        pinMode(bhi, OUTPUT);
        pinMode(dis, OUTPUT);
        pinMode(ali, OUTPUT);
        pinMode(bli, OUTPUT);
        
        pinMode(lahi, OUTPUT);        //enable left motor control pins
        pinMode(lbhi, OUTPUT);
        pinMode(ldis, OUTPUT);
        pinMode(lali, OUTPUT);
        pinMode(lbli, OUTPUT);
        
        digitalWrite(ahi, HIGH);      //set motor to stop 
        digitalWrite(bhi, HIGH);
        digitalWrite(dis, LOW);
        digitalWrite(ali, LOW);
        digitalWrite(bli, LOW);
        
        digitalWrite(lahi, HIGH);      //set left motor to stop 
        digitalWrite(lbhi, HIGH);
        digitalWrite(ldis, LOW);
        digitalWrite(lali, LOW);
        digitalWrite(lbli, LOW);
  
 
}

void loop()
{

// READ ACCELEROMETERS AND GYRO  
// X ACCELEROMETER
//voltsacclx = analogRead (0);
//gacclx = voltsacclx * 3.3 / 1024;  
//gacclx2 = (gacclx - 1.382) / .3;

// Y ACCELEROMETER
voltsaccly = analogRead (1);
gaccly = voltsaccly * 3.3 / 1024;  
gaccly2 = (gaccly - 1.368) / .3;

// Z ACCELEROMETER
voltsacclz = analogRead (2);
gacclz = voltsacclz * 3.3 / 1024;  
gacclz2 = (gacclz - 1.460) / .3;

// GYRO X
//voltsgyrox = analogRead (3);
//ggyrox = voltsgyrox * 3.3 / 1024;  
//ggyrox2 = (ggyrox - 1.344) / .002;

//voltsgyroxhires = analogRead (5);
//ggyroxhires = voltsgyroxhires * 3.3 / 1024;  
//ggyrox2hires = (ggyroxhires - 1.34)  / .0091;


// GYRO Y
//voltsgyroy = analogRead (4);
//ggyroy = voltsgyroy * 3.3 / 1024;  
//ggyroy2 = (ggyroy - 1.286) / .002;

// read the tuning knob value:
  sensorValue1 = analogRead (analogInPin1);
  sensorValue2 = analogRead (analogInPin2);
  sensorValue3 = analogRead (analogInPin3);
 
  
  // map tuning knob to the range of the analog out:
  outputValue1 = map(sensorValue1, 0, 1023, 0, 2000);    //P Knob
  outputValue2 = map(sensorValue2, 0, 1023, 0, 20000);    //I Knob
  outputValue3 = map(sensorValue3, 0, 1023, 0, 500);    //D Knob
  
  //Steering
  
  steeringValue = analogRead (steering);
  turnRate = (steeringValue - 509) * .1;          //turnRate = ( pot - pot offset ) * turnRateGain
  

//PID CALCULATION
    
    //Get Estimated Angle
    float a = EstimateAngle() - INIT_ANGLE;

 
    //Buffer the angle get an average
    angleBuffer[angleBufferIndex] = a;

    angleBufferIndex = (angleBufferIndex + 1) % ANGLE_BUFFER_LENGTH;

    float ang = GetAvgAngle();

 
    //CURRENT ERROR
    curErr = ang - INIT_ANGLE; //error   

    SumErr += curErr;

    if (SumErr > SumErrMax) SumErr = SumErrMax;

    else if (SumErr < SumErrMin) SumErr = SumErrMin;

    // Ki*SumE/(Kp*Fs*X)
    
    // INTEGRAL TERM
    integralTerm = SumErr * elapsedTimeSec * float(outputValue2) / float(outputValue1);
    
    // DERIVATIVE TERM
    derivativeTerm = curErr - prevErr;

  //  if(derivativeTerm > 0.1) derivativeTerm = 0.1;

   // else if (derivativeTerm < -0.1) derivativeTerm = -0.1;

    // Kd(curErr-prevErr)*Ts/(Kp*X)
    
    derivativeTerm = derivativeTerm * 10000 * float(outputValue3) * elapsedTimeSec / float(outputValue1);

  //  if(derivativeTerm > 120) derivativeTerm = 120;

  //  else if (derivativeTerm < -120) derivativeTerm = -120;

    //PID CALC
    Cn = ((curErr + integralTerm + derivativeTerm) * float(outputValue1)) - turnRate;
    LeftCn = ((curErr + integralTerm + derivativeTerm) * float(outputValue1)) + turnRate;

    //MOTOR DRIVE

    WheelDirection dir;

    if (Cn > 0) dir = DIR_BACKWARD;

    else if (Cn < -0) dir = DIR_FORWARD;

    else dir = DIR_STOP;

      throttle = abs(Cn);
      
      if ( softStartCounter < 1000 ) {    //Start the motor slow on power up
      
        softStartCounter++;
        throttle *= ( softStartCounter / 1000 );
    }

    if (abs(ang) > 0.7)  MoveWheels(DIR_STOP, 0); //if angle too large to correct, stop motor       

    else MoveWheels(dir, throttle);
    
    //LEFT MOTOR DRIVE
    
    LeftWheelDirection leftdir;

  
    if (LeftCn > 0) leftdir = LeftDIR_BACKWARD;

  
     else if (LeftCn < -0) leftdir = LeftDIR_FORWARD;

    else leftdir = LeftDIR_STOP;

      leftthrottle = abs(LeftCn);
      
      if ( leftsoftStartCounter < 1000 ) {          //Start the motor slow on power up
      
       leftsoftStartCounter++;
        leftthrottle *= ( leftsoftStartCounter / 1000 );
    }
    
    if (abs(ang) > 0.7)  LeftMoveWheels(LeftDIR_STOP, 0); //if angle too large to correct, stop motor       

    else LeftMoveWheels(leftdir, leftthrottle);
    
    prevErr = curErr;
    
//  SERIAL PRINTS FOR DEBUGGING    
    
//  Serial.print (accAngle);
//  Serial.print(",");
//  Serial.print (prevAngle + degX);
 // Serial.print(",");
//Serial.print (currAngle);
//Serial.print (voltsacclx);
//Serial.print(",");
//Serial.print (gacclx);
//Serial.print(",");
//Serial.print (gacclx2);
//Serial.print (",");
//Serial.print (voltsaccly);
//Serial.print(",");
//Serial.print (gaccly);
//Serial.print(",");
//Serial.print (gaccly2);
//Serial.print (",");
//Serial.print (voltsacclz);
//Serial.print(",");
//Serial.print (gacclz);
//Serial.print(",");
//Serial.print (gacclz2);
//Serial.print (",");
//Serial.print (voltsgyroy);
//Serial.print(",");
//Serial.print (ggyroy);
//Serial.print(",");
//Serial.print (ggyrox2 );

//Serial.print (voltsgyrox);
//Serial.println("");
//Serial.print (ggyrox);
//Serial.print(",");
//Serial.print (ggyroy2);
/*
//Tuning Knobs  P=653 I=3431 D=34
//osmc1: P=621 I=13919 D=0 Best
//osmc2: P=707 I=11925 D=5 Third
//osmc3: P=437 I=11612 D=196 Second
Serial.print("\t P = ");      
Serial.print(outputValue1);   
Serial.print("\t I = ");      
Serial.print(outputValue2);   
Serial.print("\t D = ");      
Serial.print(outputValue3);
Serial.println("");
*/
/*
   Serial.print(throttle);
   Serial.print(",");
   Serial.print(curErr);
   Serial.print(","); 
   Serial.print(prevErr);
   Serial.print(",");
   Serial.print(derivativeTerm *100);
   Serial.print(",");
   Serial.print(SumErr);
   Serial.print(",");
   Serial.print(integralTerm);
   Serial.print(",");
   Serial.print(Cn);
 //  Serial.print(",");
  
*/
/*  
  Serial.print("\t sumerr=");
  Serial.print(SumErr);
   Serial.print("\t integral=");
   Serial.print(integralTerm);
   Serial.println("");
  *//* Serial.print(derivativeTerm);
   Serial.print(",");
   Serial.print(elapsedTimeSec);
   Serial.print(",");
   Serial.print(outputValue3);
   Serial.print(",");
   Serial.print(outputValue1);
   Serial.println("");
   */
//Serial.print(elapsedTime);
 // Serial.println("");

 
}