/*
  PINS USED:
  Sensor PINS USED:     +5, GND, 2, 3, 4, 5, 7, 8, 10, 11, 12
  The sensor pin names VCC, GND, 0, 1, 2, 3, 4,  5,  6,  7,  8
  NeoPixel LED Data pin 14
  Motor driver 6, 9, 13, 15
  Type L298N Motor driver. Enable pins are jumpered LOW
   6 //M1 Direction Control int 1 
   13 //M1 Direction Control int 2
   9 //M2 Direction Control int 3
   15 //M2 Direction Control int 4
*/
#include <QTRSensors.h>
#include <Adafruit_NeoPixel.h>
//#include <avr/power.h>

//////////Neo Pixal Leds///////////////
#define LEDPIN         2
#define NUMPIXELS      2  //How many NeoPixels
Adafruit_NeoPixel pixels = Adafruit_NeoPixel(NUMPIXELS, LEDPIN, NEO_RGB + NEO_KHZ400);

int BlinkTime = 300; // blink time
int CycleTime = 1;   // used to call for a blink toggle
int BlinkCycle = 1;  // used to alternate the LED on and off
int MyColor = 0; //it will hold the color setting
int red = 1;    //led color settings
int green = 2;  //led color settings
int blue = 3;   //led color settings
unsigned long previousMillis = 0;    // time keeping function

#define NUM_SENSORS   8     // number of sensors used
#define TIMEOUT       2500  // waits for 2500 microseconds for sensor outputs to go low
#define EMITTER_PIN   14     // emitter is controlled by digital pin 2

// sensors 0 through 7 are connected to digital pins 3 through 12, respectively
// I moved 6 and 9 for the motor driver
QTRSensorsRC qtrrc((unsigned char[]) {
  3, 4, 5, 7, 8, 10, 11, 12
}, NUM_SENSORS, TIMEOUT, EMITTER_PIN);

unsigned int sensorValues[NUM_SENSORS];

unsigned int sensors[8];
// unsigned int sensors[NUM_SENSORS];////////////problem
int position = 0;
int error = 0;
int m1Speed = 0;
int m2Speed = 0;
int motorSpeed = 0;

/////////////////////////////OUTPUT PINS////////////////
// Jumpered M1 Speed Control ENA
//Jumpered M2 Speed Control ENB
//maybe jump 2 pins to GND to reduce wire count?

int M1A = 6; //M1 Direction Control int 1
int M1B = 13; //M1 Direction Control int 2
int M2A = 9; //M2 Direction Control int 3
int M2B = 15;//M2 Direction Control int 4
boolean Freeze = 1; // pin high or low to stop motor????
///////////////////////PID TUNING//////////////
int lastError = 0;
float KP = 0.1;
float KD = 6;
int M1 = 0;  //base motor speeds
int M2 = 0;  //base motor speeds
int M1max = 255;  //max motor speeds
int M2max = 255;  //max motor speeds
int M1min = 0;  //max motor speeds
int M2min = 0;  //max motor speeds

void setup() {

  Serial.begin(115200);
  delay(500);
  Serial.println("hello");
  pinMode(13, OUTPUT);
  pinMode(M1A, OUTPUT); //M1 Speed Control int 1
  pinMode(M1B, OUTPUT); //M1 Direction Control int 2
  pinMode(M2A, OUTPUT); //M2 Speed Control int 3
  pinMode(M2B, OUTPUT); //M2 Direction Control int 4
  digitalWrite(M1A, Freeze); // stop the motor
  digitalWrite(M2A, Freeze); // stop the motor
  digitalWrite(M1B, HIGH);    // set the second directional pins LOW
  digitalWrite(M2B, HIGH);    // set the second directional pins LOW
  pixels.begin(); // This initializes the NeoPixel library.
  pixels.setPixelColor(0, pixels.Color(200, 0, 0)); // Moderately bright RED color.
  pixels.setPixelColor(1, pixels.Color(200, 0, 0)); // Moderately bright RED color.
  pixels.show();
  MyColor = red; // set led color to red

  for (int i = 0; i < 400; i++)  // make the calibration take about 10 seconds
  {
    TimeCheck();
    delay(5);
    qtrrc.calibrate();       // reads all sensors 10 times at 2500 us per read (i.e. ~25 ms per call)
  }

  // print the calibration minimum values measured when emitters were on

  for (int i = 0; i < NUM_SENSORS; i++)
  {
    Serial.print(qtrrc.calibratedMinimumOn[i]);
    Serial.print(' ');
  }
  Serial.println();

  // print the calibration maximum values measured when emitters were on
  for (int i = 0; i < NUM_SENSORS; i++)
  {
    Serial.print(qtrrc.calibratedMaximumOn[i]);
    Serial.print(' ');
  }
  Serial.println();
  /// Prepare to begin the line following we have finished calibration
  MyColor = blue; // set led color to blue
  for (int i = 0; i < 5; i++)  // make the calibration take about 10 seconds
  {
    TimeCheck();
    delay(1000);
  }
}

void loop()
{

  TimeCheck();  //Check to see if it is time to blink the LED

  position = qtrrc.readLine(sensors);

  /* Serial.print("position ");
    Serial.print(position);
    Serial.println(); */

  // compute our "error" from the line position
  // error is zero when the middle sensor is over the line,
  error = position - 3500;   /// using an 8 sensor array targeting middle of position 4 to 5

  /*   Serial.print("error ");
    Serial.print(error);
    Serial.println();*/

  // set the motor speed based on proportional and derivative PID terms
  motorSpeed = KP * error + KD * (error - lastError);
  lastError = error;
    /////////CHANGE THE - OR + BELOW DEPENDING ON HOW YOUR DRIVER WORKS
  m1Speed = M1 + motorSpeed; // M1 and M2 are base motor speeds.
  m2Speed = M2 - motorSpeed;
  /* Serial.print("m1Speed ");
     Serial.print(m1Speed);
    Serial.print(" ");
    Serial.print("m2Speed ");
    Serial.println(m2Speed);
    Serial.println(); */

  if (m1Speed < M1min)  //keep speeds to 0 or above
    m1Speed = M1min;
  if (m2Speed < M2min)
    m2Speed = M2min;
  if (m1Speed > M1max) //maximum allowed value
    m1Speed = M1max;
  if (m2Speed > M2max) //maximum allowed value
    m2Speed = M2max;
  // set motor speeds using the two motor speed variables above 255
/*  Serial.print("m1Speed ");
  Serial.print(m1Speed);
  Serial.print(" ");
  Serial.print("m2Speed ");
  Serial.println(m2Speed);
  Serial.println();  */

  forward();  // run the motors

}

void TimeCheck() {
  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis > BlinkTime) {
    // save the last time you blinked the LED
    previousMillis = currentMillis;

    BlinkIt();
  }
}
////////////////////////STOP//////////////
void stopit(void)
{
  digitalWrite(M1A, Freeze);  
  digitalWrite(M1B, LOW);
  digitalWrite(M2A, Freeze);
  digitalWrite(M2B, LOW);
  MyColor = red;
}

void BlinkIt() {
  if (BlinkCycle == 1) {
  // pixels.Color takes RGB values, from 0,0,0 up to 255,255,255
  for (int i = 0; i < NUMPIXELS; i++) {
      if (MyColor == red) {
        pixels.setPixelColor(i, pixels.Color(200, 0, 0)); // Moderately bright RED color.
      }
      if (MyColor == green) {
        pixels.setPixelColor(i, pixels.Color(0, 200, 0)); // Moderately bright GREEN color.
      }
      if (MyColor == blue) {
        pixels.setPixelColor(i, pixels.Color(0, 0, 200)); // Moderately bright BLUE color.
      }
      pixels.show(); // This sends the updated pixel color to the hardware.
    }
      }
  if (BlinkCycle == 2) {
  // pixels.Color takes RGB values, from 0,0,0 up to 255,255,255
  for (int i = 0; i < NUMPIXELS; i++) {
      pixels.setPixelColor(i, pixels.Color(0, 0, 0)); // Sets LED to off
      pixels.show(); // This sends the updated pixel color to the hardware.
    }
    BlinkCycle = 0;  // reset BlinkCycle
  }
  BlinkCycle++; // Set BlinkCycle to second mode
}


//////////////////////FORWARD////////
void forward()
{
  MyColor = green; // set led color to green
  TimeCheck();
  analogWrite(M1A, m1Speed);
  digitalWrite(M1B, HIGH);
  analogWrite(M2A, m2Speed);
  digitalWrite(M2B, HIGH);
}

/*
Motor driver is a TB6612FNG 1A Dual Motor Driver using Arduino UNO R3
Connections:
- Pin 3 ---> PWMA
- Pin 8 ---> AIN2
- Pin 9 ---> AIN1
- Pin 10 ---> STBY
- Pin 11 ---> BIN1
- Pin 12 ---> BIN2
- Pin 5 ---> PWMB

- Motor 1: A01 and A02
- Motor 2: B01 and B02
Sensor PINS USED: +5, GND, 1, 2, 4, 6, 7, 13, 14, 15, 16
The sensor pin names VCC, GND, 0, 1, 2, 3, 4, 5, 6, 7, 8
Arduino pin 0 is open at the moment
*/
#include 
#define NUM_SENSORS 8 // number of sensors used
#define TIMEOUT 2500 // waits for 2500 microseconds for sensor outputs to go low
#define EMITTER_PIN 0 // emitter is controlled by digital pin 2

//Define the Pins
// sensors 0 through 7 are connected to:
QTRSensorsRC qtrrc((unsigned char[]) {
1, 2, 4, 6, 7, 14, 15, 16
}, NUM_SENSORS, TIMEOUT, EMITTER_PIN);

int IndicatorLED = 13;

//Motor 1
int pinAIN1 = 9; //Direction
int pinAIN2 = 8; //Direction
int pinPWMA = 3; //Speed

//Motor 2
int pinBIN1 = 11; //Direction
int pinBIN2 = 12; //Direction
int pinPWMB = 5; //Speed

//Standby
int pinSTBY = 10;

unsigned int sensorValues[NUM_SENSORS];
unsigned int sensors[8];
int position = 0;
int error = 0;
int m1Speed = 0;
int m2Speed = 0;
int motorSpeed = 0;
unsigned long previousMillis = 0; // time keeping function
int BlinkTime = 300; // blink time
int CycleTime = 1; // used to call for a blink toggle
int BlinkCycle = 1; // used to alternate the LED on and off
///////////////////////PID TUNING//////////////
int lastError = 0;
float KP = 0.1;
float KD = 6;
int M1 = 100; //base motor speeds
int M2 = 100; //base motor speeds
int M1max = 100; //max motor speeds
int M2max = 100; //max motor speeds
int M1min = 0; //min motor speeds
int M2min = 0; //min motor speeds

//Constants to help remember the parameters
static boolean turnCW = 0; //for motorDrive function
static boolean turnCCW = 1; //for motorDrive function
static boolean motor1 = 0; //for motorDrive, motorStop, motorBrake functions
static boolean motor2 = 1; //for motorDrive, motorStop, motorBrake functions

void setup()
{
//Set the PIN Modes
pinMode(IndicatorLED, OUTPUT);
digitalWrite(IndicatorLED, HIGH);
pinMode(pinPWMA, OUTPUT);
pinMode(pinAIN1, OUTPUT);
pinMode(pinAIN2, OUTPUT);
pinMode(pinPWMB, OUTPUT);
pinMode(pinBIN1, OUTPUT);
pinMode(pinBIN2, OUTPUT);
pinMode(pinSTBY, OUTPUT);

for (int i = 0; i < 400; i++) // make the calibration take about 10 seconds
{
TimeCheck();
delay(5);
qtrrc.calibrate(); // reads all sensors 10 times at 2500 us per read (i.e. ~25 ms per call)
}

//we could print the calibration minimum values measured when emitters were on

for (int i = 0; i < NUM_SENSORS; i++)
{
int x = (qtrrc.calibratedMinimumOn[i]);
}

// we could print the calibration maximum values measured when emitters were on
for (int i = 0; i < NUM_SENSORS; i++)
{
int x = (qtrrc.calibratedMaximumOn[i]);
}

/// Prepare to begin the line following we have finished calibration
for (int i = 0; i < 5; i++) // make the LED blink for about 5 seconds
{
TimeCheck();
delay(1000);
}
}

void loop()
{
TimeCheck(); //Check to see if it is time to blink the LED

position = qtrrc.readLine(sensors);

// compute our "error" from the line position
// error is zero when the middle sensor is over the line,
error = position - 3500; /// using an 8 sensor array targeting middle of position 4 to 5

// set the motor speed based on proportional and derivative PID terms
motorSpeed = KP * error + KD * (error - lastError);
lastError = error;
/////////CHANGE THE - OR + BELOW DEPENDING ON HOW YOUR DRIVER WORKS
m1Speed = M1 - motorSpeed; // M1 and M2 are base motor speeds.
m2Speed = M2 + motorSpeed;

if (m1Speed < M1min) //keep speeds to 0 or above
m1Speed = M1min;
if (m2Speed < M2min) m2Speed = M2min; if (m1Speed > M1max) //maximum allowed value
m1Speed = M1max;
if (m2Speed > M2max) //maximum allowed value
m2Speed = M2max;
// set motor speeds using the two motor speed variables above 255

forward(); // run the motors based on error information
}

//////////////////////FORWARD////////
//Drive both motors CW, full speed
void forward()
{
TimeCheck();
motorDrive(motor1, turnCW, m1Speed);
motorDrive(motor2, turnCW, m2Speed);
}

/* //Apply Brakes, then into Standby
motorBrake(motor1);
motorBrake(motor2);
motorsStandby(); */

void TimeCheck() {
unsigned long currentMillis = millis();
if (currentMillis - previousMillis > BlinkTime) {
// save the last time you blinked the LED
previousMillis = currentMillis;

BlinkIt();
}
}

void motorDrive(boolean motorNumber, boolean motorDirection, int motorSpeed)
{
/*
This Drives a specified motor, in a specific direction, at a specified speed:
- motorNumber: motor1 or motor2 ---> Motor 1 or Motor 2
- motorDirection: turnCW or turnCCW ---> clockwise or counter-clockwise
- motorSpeed: 0 to 255 ---> 0 = stop / 255 = fast
*/

boolean pinIn1; //Relates to AIN1 or BIN1 (depending on the motor number specified)

//Specify the Direction to turn the motor
//Clockwise: AIN1/BIN1 = HIGH and AIN2/BIN2 = LOW
//Counter-Clockwise: AIN1/BIN1 = LOW and AIN2/BIN2 = HIGH
if (motorDirection == turnCW)
pinIn1 = HIGH;
else
pinIn1 = LOW;

//Select the motor to turn, and set the direction and the speed
if (motorNumber == motor1)
{
digitalWrite(pinAIN1, pinIn1);
digitalWrite(pinAIN2, !pinIn1); //This is the opposite of the AIN1
analogWrite(pinPWMA, motorSpeed);
}
else
{
digitalWrite(pinBIN1, pinIn1);
digitalWrite(pinBIN2, !pinIn1); //This is the opposite of the BIN1
analogWrite(pinPWMB, motorSpeed);
}

//Finally , make sure STBY is disabled - pull it HIGH
digitalWrite(pinSTBY, HIGH);
}

void motorBrake(boolean motorNumber)
{
/*
This "Short Brake"s the specified motor, by setting speed to zero
*/

if (motorNumber == motor1)
analogWrite(pinPWMA, 0);
else
analogWrite(pinPWMB, 0);
}

void motorStop(boolean motorNumber)
{
/*
This stops the specified motor by setting both IN pins to LOW
*/
if (motorNumber == motor1) {
digitalWrite(pinAIN1, LOW);
digitalWrite(pinAIN2, LOW);
}
else
{
digitalWrite(pinBIN1, LOW);
digitalWrite(pinBIN2, LOW);
}
}

void BlinkIt() {
if (BlinkCycle == 1) {
//Led On
digitalWrite(IndicatorLED, HIGH);
}
if (BlinkCycle == 2) {
//Led Off
digitalWrite(IndicatorLED, LOW);
BlinkCycle = 0; // reset BlinkCycle
}
BlinkCycle++; // Set BlinkCycle to second mode
}

#include 

// This example is designed for use with eight QTR-1RC sensors or the eight sensors of a
// QTR-8RC module.  These reflectance sensors should be connected to digital inputs 3 to 10.
// The QTR-8RC's emitter control pin (LEDON) can optionally be connected to digital pin 2,
// or you can leave it disconnected and change the EMITTER_PIN #define below from 2 to
// QTR_NO_EMITTER_PIN.

// The setup phase of this example calibrates the sensor for ten seconds and turns on
// the LED built in to the Arduino on pin 13 while calibration is going on.
// During this phase, you should expose each reflectance sensor to the lightest and
// darkest readings they will encounter.
// For example, if you are making a line follower, you should slide the sensors across the
// line during the calibration phase so that each sensor can get a reading of how dark the
// line is and how light the ground is.  Improper calibration will result in poor readings.
// If you want to skip the calibration phase, you can get the raw sensor readings
// (pulse times from 0 to 2500 us) by calling qtrrc.read(sensorValues) instead of
// qtrrc.readLine(sensorValues).

// The main loop of the example reads the calibrated sensor values and uses them to
// estimate the position of a line.  You can test this by taping a piece of 3/4" black
// electrical tape to a piece of white paper and sliding the sensor across it.  It
// prints the sensor values to the serial monitor as numbers from 0 (maximum reflectance)
// to 1000 (minimum reflectance) followed by the estimated location of the line as a number
// from 0 to 5000.  1000 means the line is directly under sensor 1, 2000 means directly
// under sensor 2, etc.  0 means the line is directly under sensor 0 or was last seen by
// sensor 0 before being lost.  5000 means the line is directly under sensor 5 or was
// last seen by sensor 5 before being lost.


#define NUM_SENSORS   8     // number of sensors used
#define TIMEOUT       2500  // waits for 2500 microseconds for sensor outputs to go low
#define EMITTER_PIN   2     // emitter is controlled by digital pin 2

// sensors 0 through 7 are connected to digital pins 3 through 10, respectively
QTRSensorsRC qtrrc((unsigned char[]) {
  3, 4, 5, 6, 7, 8, 9, 10
}, NUM_SENSORS, TIMEOUT, EMITTER_PIN);

unsigned int sensorValues[NUM_SENSORS];

int lastError = 0;
float KP = 0.1;
float KD = 5.0;
int M1 = 125;  //base motor speeds
int M2 = 125;  //base motor speeds

void setup() {
  delay(500);
  pinMode(13, OUTPUT);
  digitalWrite(13, HIGH);    // turn on Arduino's LED to indicate we are in calibration mode

  for (int i = 0; i < 400; i++)  // make the calibration take about 10 seconds
  {
    qtrrc.calibrate();       // reads all sensors 10 times at 2500 us per read (i.e. ~25 ms per call)
  }
  digitalWrite(13, LOW);     // turn off Arduino's LED to indicate we are through with calibration

  // print the calibration minimum values measured when emitters were on
  Serial.begin(9600);
  for (int i = 0; i < NUM_SENSORS; i++)
  {
    Serial.print(qtrrc.calibratedMinimumOn[i]);
    Serial.print(' ');
  }
  Serial.println();

  // print the calibration maximum values measured when emitters were on
  for (int i = 0; i < NUM_SENSORS; i++)
  {
    Serial.print(qtrrc.calibratedMaximumOn[i]);
    Serial.print(' ');
  }
  Serial.println();
  Serial.println();
  delay(1000);
}

void loop()
{
  unsigned int sensors[3];
  //unsigned int sensors[NUM_SENSORS];

  // get calibrated sensor values returned in the sensors array, along with the line position
  // position will range from 0 to 2000, with 1000 corresponding to the line over the middle
  // sensor
  int position = qtrrc.readLine(sensors);
  //int position = qtrrc.readLine(NUM_SENSORS);

  // compute our "error" from the line position.  We will make it so that the error is zero when
  // the middle sensor is over the line, because this is our goal.  Error will range from
  // -1000 to +1000.  If we have sensor 0 on the left and sensor 2 on the right,  a reading of
  // -1000 means that we see the line on the left and a reading of +1000 means we see the
  // line on the right.
  int error = position - 1000;
  //int error = position - 3000;

  // set the motor speed based on proportional and derivative PID terms
  // KP is the a floating-point proportional constant (maybe start with a value around 0.1)
  // KD is the floating-point derivative constant (maybe start with a value around 5)
  // note that when doing PID, it's very important you get your signs right, or else the
  // control loop will be unstable
  int motorSpeed = KP * error + KD * (error - lastError);
  lastError = error;

  // M1 and M2 are base motor speeds.  That is to say, they are the speeds the motors should
  // spin at if you are perfectly on the line with no error.  If your motors are well matched,
  // M1 and M2 will be equal.  When you start testing your PID loop, it might help to start with
  // small values for M1 and M2.  You can then increase the speed as you fine-tune your
  // PID constants KP and KD.
  int m1Speed = M1 + motorSpeed;
  int m2Speed = M2 - motorSpeed;

  // it might help to keep the speeds positive (this is optional)
  // note that you might want to add a similiar line to keep the speeds from exceeding
  // any maximum allowed value
  if (m1Speed < 0)
    m1Speed = 0;
  if (m2Speed < 0) m2Speed = 0; if (m1Speed > 255)
    m1Speed = 255;
  if (m2Speed > 255)
    m2Speed = 255;
  // set motor speeds using the two motor speed variables above 255


}

#include 

#define NUM_SENSORS   8     // number of sensors used
#define TIMEOUT       2500  // waits for 2500 microseconds for sensor outputs to go low
#define EMITTER_PIN   2     // emitter is controlled by digital pin 2

// sensors 0 through 7 are connected to digital pins 3 through 10, respectively
QTRSensorsRC qtrrc((unsigned char[]) {3, 4, 5, 6, 7, 8, 9, 10},
  NUM_SENSORS, TIMEOUT, EMITTER_PIN); 
unsigned int sensorValues[NUM_SENSORS];

int lastError = 0;
float KP = 0.1;
float KD = 5.0;
int M1 = 125;  //base motor speeds
int M2 = 125;  //base motor speeds

void setup() {
  // put your setup code here, to run once:

}

void loop()
{
  unsigned int sensors[3];
  // get calibrated sensor values returned in the sensors array, along with the line position
  // position will range from 0 to 2000, with 1000 corresponding to the line over the middle 
  // sensor.
  int position = qtrrc.readLine(sensors);
 
  // if all three sensors see very low reflectance, take some appropriate action for this 
  // situation.
  if (sensors[0] > 750 && sensors[1] > 750 && sensors[2] > 750)
  {
    // do something.  Maybe this means we're at the edge of a course or about to fall off 
    // a table, in which case, we might want to stop moving, back up, and turn around.
    return;
  }
 
  // compute our "error" from the line position.  We will make it so that the error is zero 
  // when the middle sensor is over the line, because this is our goal.  Error will range from
  // -1000 to +1000.  If we have sensor 0 on the left and sensor 2 on the right,  a reading of 
  // -1000 means that we see the line on the left and a reading of +1000 means we see the 
  // line on the right.
  int error = position - 1000;
 
  int leftMotorSpeed = 100;
  int rightMotorSpeed = 100;
  if (error < -500) // the line is on the left leftMotorSpeed = 0; // turn left if (error > 500)  // the line is on the right
    rightMotorSpeed = 0;  // turn right
 
  // set motor speeds using the two motor speed variables above
}