/*
circuit setup:
 wire pin 4 to the red wire of Parallax's QTI ir sensor. Black goes to ground, white to power.
 pin 6 goes to the forward wagonwheel
 pin5 goes to the backward wagonwheel
 circuit ground is connected to arduino ground through 0.01uF capacitor
 */
double desiredRpm = 200;
double duty = 255;  
// for wagon wheel motor control

// for reading serial
char val[3];

// for reading sensor and calculating rpm
unsigned long t1 = 0, t2 = 0, t0 = 0;
unsigned divisions = 2; // number of black divisions on motor
unsigned lastHigh = 0;
double circumference = 0, diameter = 2.64; // inches
int row = 4; // used for outputting to csv file

// pin definitions
int sensorPin = 4, forwardWagonWheel = 6, backwardWagonWheel = 5;

void setup(){
  circumference = 3.14159 * diameter;
  pinMode(forwardWagonWheel,OUTPUT);
  pinMode(backwardWagonWheel,OUTPUT);
  Serial.begin(9600);
  Serial.print("diameter,");
  Serial.print(diameter);
  Serial.println(",inches");
  Serial.print("circumference,");
  Serial.print(circumference);
  Serial.println(",inches");
  Serial.print("number of black divisions,");
  Serial.println(divisions);
  t2 = micros();
  pwmMotor(duty);
}

int readSerial(double duty){
  if (duty < 10){
    Serial.println("Please input a duty cycle");
  }
  while (duty < 30){ 
    if (Serial.available() > 2)
    {
      val[0] = Serial.read(); 
      val[1] = Serial.read();   
      val[2] = Serial.read();
      return atoi(val);
    }
  }
  return duty;
}

void pwmMotor(double d){
  Serial.print("Duty: ");
  Serial.println(d);
  analogWrite(forwardWagonWheel, d);
}

double readIRSensor(double rpm){
  double answer = 0;
  answer = RCtime(sensorPin);
  if ((answer < 30) && (lastHigh == 0)){
    t0 = t1;
    t1 = t2;
    t2 = micros();
    rpm = (double(1/double(divisions/2))/double(t2 - t0))*60000000;
    Serial.print("rpm ");
    Serial.println(rpm);
    lastHigh = 1;
  } 
  else if (answer > 30){
    lastHigh = 0;
  }
  return rpm;
}

long RCtime(int sensPin){
  long result = 0;
  pinMode(sensPin, OUTPUT);       // make pin OUTPUT
  digitalWrite(sensPin, HIGH);    // make pin HIGH to discharge capacitor - study the schematic
  delay(1);                       // wait a  ms to make sure cap is discharged

  pinMode(sensPin, INPUT);        // turn pin into an input and time till pin goes low
  digitalWrite(sensPin, LOW);     // turn pullups off - or it won't work
  while(digitalRead(sensPin)){    // wait for pin to go low
    result++;
  }
  return result;                   // report results   
}   

void loop(){
  //  /*
  double dutyCycle = duty;
  if (duty > 255 || duty < 0){
    duty = 255;
  } 
  else if(duty<10 && duty > 0){
    duty = readSerial(duty);
  }
  // */
  double rpm = 0;
  rpm = readIRSensor(rpm); 
  // /*
  if (millis() > 500){
    if (rpm > desiredRpm && rpm != 0.00 && duty > 0){
      duty = duty + (desiredRpm - rpm)/17;
      if (duty > 255){
        duty = 255;
      }
      Serial.print("Reducing duty cycle");
      Serial.println(duty);
    } 
    else if (rpm < desiredRpm  && rpm != 0.00 && duty < 255) {
      Serial.print("increasing duty cycle");
      Serial.println(duty);
      duty = duty + (desiredRpm - rpm)/17;
      if (duty < 0){
        duty = 0 ;
      }
    }  
    else{

      duty = dutyCycle; 
    }
  }
  if (duty != dutyCycle){
    pwmMotor(duty);
  }
  // */
}