#include <SoftwareSerial.h>
#include <SabertoothSimplified.h>
// Include the required Wire library for I2C<br>#include
#include <Wire.h>

// RX on pin 17 (to S2), TX on pin 16 (to S1).
SoftwareSerial SWSerial(NOT_A_PIN, 16);
// Use SWSerial as the serial port.
SabertoothSimplified ST(SWSerial);

//////////////////ENCODER DATA//////////////////
unsigned int revolutions_L_rpm = 0;
unsigned int revolutions_R_rpm = 0;

int16_t x = 0;
int16_t y = 0;
////////////////////////////////////////////////

//////////////VARIABLES FOR ADJUST//////////////
int error = 0;
int kp = 12;
int adjusted = 0;
////////////////////////////////////////////////

////////////////////MOTORS//////////////////////
//Declare the arduino pins
int LEDg = 7;
int LEDr = 6;
int LEDy = 5;
int speedVar = 0;
int speedOne = 0;
int speedTwo = 0;
int power;
////////////////////END/////////////////////////

void setup() {
  //initlize the mode of the pins
  pinMode(LEDg,OUTPUT);
  pinMode(LEDr,OUTPUT);
  pinMode(LEDy,OUTPUT);
  //set the serial communication rate
  Serial.begin(9600);
  SWSerial.begin(9600);
  Wire.begin();
}

void loop()
{
  //check whether arduino is reciving signal or not
  if(Serial.available() > 0){
  char val = Serial.read() ;//reads the signal
  Serial.print("Recieved: ");
  Serial.println(val);

  switch(val){
  /*********Increase speed by 1 as long as e(triangle) is held*********/
  case 'a':
    forward();
  break;

  /*********Decrease speed by 1 as long as g(x) is held*********/
  case 'c':
    reverse();
  break;

    /*********Increase speed by 1 as long as e(triangle) is held*********/
  case 'd':
    turnLeft();
  break;

  /*********Decrease speed by 1 as long as g(x) is held*********/
  case 'b':
    turnRight();
  break;

  /*********Toggle when Circle is held for 5 seconds*********/
  case 'f':
    toggleSwitch(LEDy);
  break;

    /*********Toggle when Square is held for 5 seconds*********/
  case 'h':
    stopMotors();
  break;
  }
  Serial.print("sppedVar = ");
  Serial.print(speedVar);
  Serial.print("tleftSpeed: ");
  Serial.print(speedOne);
  Serial.print("trightSpeed: ");
  Serial.println(speedTwo);
  }

  Wire.requestFrom(9,4); // Request 4 bytes from slave arduino (9)
  byte a = Wire.read();
  Serial.print("a: ");
  Serial.print(a);
  byte b = Wire.read();
  Serial.print("  b: ");
  Serial.print(b);
  byte e = Wire.read();
  Serial.print(" --- e: ");
  Serial.print(e);
  byte f = Wire.read();
  Serial.print("  f: ");
  Serial.print(f);
  x = a;
  x = (x << 8) | b;
  Serial.print("tX: ");
  Serial.print(x);
  y = e;
  y = (y << 8) | f;
  Serial.print("tY: ");
  Serial.print(y);
  revolutions_L_rpm = x;
  revolutions_R_rpm = y;

  if ((revolutions_L_rpm != revolutions_R_rpm) && (speedVar != 0)){
    error = 0;
    error = revolutions_L_rpm - revolutions_R_rpm;
    adjusted = error/kp;
    Serial.print("Error: ");
    Serial.print(error);
    Serial.print("Error/kp: ");
    Serial.println(adjusted);

    if ((speedTwo < 20) && (speedTwo > -20)){
      speedTwo -= adjusted;
      power = speedTwo;
      ST.motor(2, -power);
      //delay(20);
    }
}

  // Print out rpm
  Serial.print("Left motor rps*100: ");
  Serial.print(revolutions_L_rpm);
  Serial.print(" ///// Right motor rps*100: ");
  Serial.println(revolutions_R_rpm);

    // Print out speed
  Serial.print("speedOne: ");
  Serial.print(speedOne);
  Serial.print("tspeedTwo: ");
  Serial.println(speedTwo);

  delay(1000);
}

// Include the required Wire library for I2C<br>#include <Wire.h>
#include <Wire.h>

// Checked for main program
volatile boolean counterReady;

// Internal to counting routine
unsigned int timerPeriod;
unsigned int timerTicks;
unsigned long overflowCount;


// The pin the encoder is connected
int encoder_in_L = 2;
int encoder_in_R = 3;

// The number of pulses per revolution
// depends on your index disc!!
unsigned int pulsesperturn = 16;

// The total number of revolutions
int16_t revolutions_L = 0;
int16_t revolutions_R = 0;

int16_t revolutions_L_rpm = 0;
int16_t revolutions_R_rpm = 0;

// Initialize the counter
int16_t pulses_L = 0;
int16_t pulses_R = 0;

byte myData[4];

// This function is called by the interrupt
void count_L() {
  pulses_L++;
}
void count_R() {
  pulses_R++;
}

void startCounting(unsigned int ms) {
  counterReady = false;     // time not up yet
  timerPeriod = ms;         // how many ms to count to
  timerTicks = 0;         // reset interrupt counter
  overflowCount = 0;      // no overflows yet

  // Reset timer 2
  TCCR2A = 0;
  TCCR2B = 0;

  // Timer 2 - gives us our 1 ms counting interval
  // 16 MHz clock (62.5 ns per tick) - prescaled by 128
  //  counter increments every 8 µs.
  // So we count 125 of them, giving exactly 1000 µs (1 ms)
  TCCR2A = bit (WGM21) ;   // CTC mode
  OCR2A  = 124;            // count up to 125  (zero relative!!!!)

  // Timer 2 - interrupt on match (ie. every 1 ms)
  TIMSK2 = bit (OCIE2A);   // enable Timer2 Interrupt

  TCNT2 = 0;          // set counter to zero

  // Reset prescalers
  GTCCR = bit (PSRASY);        // reset prescaler now
  // start Timer 2
  TCCR2B =  bit (CS20) | bit (CS22) ;  // prescaler of 128
}

ISR (TIMER2_COMPA_vect){
  // see if we have reached timing period
  if (++timerTicks < timerPeriod)
    return;

  TCCR2A = 0;    // stop timer 2
  TCCR2B = 0;
  TIMSK2 = 0;    // disable Timer2 Interrupt
  counterReady = true;
  if(counterReady){
    Serial.print("Pulses_L: ");
    Serial.print(pulses_L);
    Serial.print("  Pulses_R: ");
    Serial.println(pulses_R);

    // multiplying by 100 to get a greater difference to compare
    revolutions_L_rpm = (pulses_L * 100) / pulsesperturn;
    revolutions_R_rpm = (pulses_R * 100) / pulsesperturn;

    // Total revolutions
//    revolutions_L = revolutions_L + (pulses_L / pulsesperturn);
//    revolutions_R = revolutions_R + (pulses_R / pulsesperturn);

    pulses_L = 0;
    pulses_R = 0;
  }
}

void requestEvent() {
  myData[0] = (revolutions_L_rpm >> 8) & 0xFF;
  myData[1] = revolutions_L_rpm & 0xFF;
  myData[2] = (revolutions_R_rpm >> 8) & 0xFF;
  myData[3] = revolutions_R_rpm & 0xFF;

  Wire.write(myData, 4); //Sent 4 bytes to master
}

void setup() {
  Serial.begin(9600);
  pinMode(encoder_in_L, INPUT);
  pinMode(encoder_in_R, INPUT);
  attachInterrupt(0, count_L, RISING); //attachInterrupt(digitalPinToInterrupt(encoder_in_L, count_L, RISING);
  attachInterrupt(1, count_R, RISING); //attachInterrupt(digitalPinToInterrupt(encoder_in_R, count_R, RISING);
  // Start the I2C Bus as Slave on address 9
  Wire.begin(9);
  // Attach a function to trigger when something is received.
  Wire.onRequest(requestEvent);
}

void loop() {
  // stop Timer 0 interrupts from throwing the count out
  byte oldTCCR0A = TCCR0A;
  byte oldTCCR0B = TCCR0B;
  TCCR0A = 0;    // stop timer 0
  TCCR0B = 0;

  startCounting (1000);  // how many ms to count for

  while (!counterReady)
     { }  // loop until count over

  // Print out rpm
  Serial.print("Left motor rps: ");
  Serial.println(revolutions_L_rpm);
  Serial.print("Right motor rps: ");
  Serial.println(revolutions_R_rpm);

//  Print out revolutions
//  Serial.print("Left motor revolution count: ");
//  Serial.println(revolutions_L);
//  Serial.print("Right motor revolution count: ");
//  Serial.println(revolutions_R);


  // restart timer 0
  TCCR0A = oldTCCR0A;
  TCCR0B = oldTCCR0B;

  delay(200);
}