#include <Servo.h> //Include servo library
//Global parameters
const int INTERRUPT_PIN = 2;
boolean inputValue = false;
int selection = 1;

//Keeping track of pulses
int count = 0;
unsigned long lastPulse = micros();
unsigned long timeBetweenPulses = 0;

//Servo control
Servo servo;
Servo ESC;
int servoMicroseconds = 1350; //Corresponds to straight forward
int escMicroseconds = 1500; //Default valuie

//Encoder parameters
const float MAGNET_SPACING_ANGLE = 2*3.14/3;
float radPerSecond = 0;
float speed = 0;

long lastTime = 0;;


void setup() {
  pinMode(INTERRUPT_PIN,INPUT_PULLUP); //Set pin 2 to input, enable internal pull-up resistor(instead of using external pull-up resitor)
  servo.attach(9);
  servo.writeMicroseconds(servoMicroseconds); //Point wheels straight ahead, not the middle point of the servo
  ESC.attach(10);
  ESC.writeMicroseconds(escMicroseconds); //Start by sending signal at 1500 us
  attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN),pulseRegistered,FALLING); //Trigger interrupt on falling for encoder
  Serial.begin(115200); //Begin serial at baud rate of 115200
}

void loop() {

  //Print information to serial monitor once per second
  long currTime = millis();
  if(currTime-lastTime>1000) {
    Serial.print("\n\n\n\n\n\n\n\n"); // "`Clear" Serial monitor
    Serial.print("Number of pulses counted: ");
    Serial.println(count);

    //Print Servo values
    Serial.print("Servo value:" );
    Serial.print(servoMicroseconds);
    Serial.print(",   ESC value:" );
    Serial.print(escMicroseconds);
    Serial.println(" us");

    //Print speed
    Serial.print("Current speed estimate: ");
    Serial.print(speed);
    Serial.print(" cm/s,    "); 
    Serial.print(radPerSecond);
    Serial.print(" rad/s, ");
    Serial.print(radPerSecond/2/3.14);
    Serial.println(" rotations/s");
    lastTime = currTime;
    count = 0; //Uncomment to get pulses per second(or rather about every second)

    if(inputValue) {
      Serial.println("Enter desired value in ms:");
    } else {
      Serial.println("Select which output to control: 1 - ESC, 2 - Servo");
    }
  }

  if(Serial.available() != 0) {   //read if possible
    int input = Serial.parseInt();
    if(!inputValue) {
      selection = input;
      inputValue = true;
    } else if(selection == 1) {
      escMicroseconds = input;
      ESC.writeMicroseconds(escMicroseconds);
      inputValue = false;
    } else if(selection == 2) {
      servoMicroseconds = input;
      servo.writeMicroseconds(servoMicroseconds);
      inputValue = false;
    } else {
      inputValue = false; 
    }
    while(Serial.available() > 0) { //"Flush" Serial buffer 
      char t = Serial.read();
    }
  }
}

//Interrupt for encoder, calculates speed
void pulseRegistered() { 
  count++;
  unsigned long currTime = micros(); //Get time since last pulse
  timeBetweenPulses = currTime - lastPulse;
  lastPulse = currTime;
  radPerSecond = 2*(3.14/3)/(float)timeBetweenPulses*1000000; //Angle between magets is 2 pi/3 radians, calculated for rad/s
  speed = 3*radPerSecond;                                     //Radius of wheel is approximately 3 cm, speed is cm/s
}