#include <movingAvg.h>
unsigned long test_time = 0;
const int DATA_PIN = A7;
const int BUTTON_PIN = 2;
boolean isStreaming = true;
int f = 200;
int w = 0;
unsigned long time_step = 5000;

const double TEN_BIT_SCALAR = 0.3516; // 360/1024
int homePosition;
double displayPosition;

// Rate variables
unsigned long lastTime = 0;
unsigned long currTime = 0;
int lastReading = 0;
int currReading = 0;
double deltaTime = 1;
double deltaReading = 0;
unsigned long buttonTimer = 0;
int i = 0;

double rate;

// Long Press Variables
const long LONG_PRESS = 250;
boolean isInPositionMode = true;
movingAvg myRate(10);

void rateMode(){
  deltaTime = currTime - lastTime; // Find change in time
  deltaReading = currReading - lastReading; // Find change in position
  deltaReading += (deltaReading < -512) ? (1024) : (0);
  deltaReading -= (deltaReading > 512) ? (1024) : (0);
  
  // Rate is change in position divided by change in time (multiply by 10^6 to convert microseconds to seconds)
  rate = TEN_BIT_SCALAR * deltaReading * 1000000 / deltaTime;
 
  myRate.reading(rate);
  rate = myRate.getAvg();
  
  if(isStreaming && !(isInPositionMode)){
    Serial.println(rate);
  }
  
}

void positionMode(){
  displayPosition = TEN_BIT_SCALAR * (double)(currReading);
  displayPosition += (displayPosition < 0) ? (360) : (0);
  displayPosition -= 180;
  if(isStreaming && isInPositionMode){
    Serial.println(displayPosition);
  }
}

void handleButton(){
  // If button was pressed
  if(digitalRead(BUTTON_PIN) == HIGH){ 
    buttonTimer = millis();
    while(digitalRead(BUTTON_PIN) == HIGH){   
    }
    if(millis() - buttonTimer > LONG_PRESS){ // Detected a long press, swap modes
      isInPositionMode = !isInPositionMode;
    }
    else{ // Short press occured, grab current position to reset home position regardless of mode
      homePosition = analogRead(DATA_PIN);
      homePosition += 512;
    }
    // LED on if we're in position mode, off if we're in rate mode
    digitalWrite(LED_BUILTIN, isInPositionMode);
  }
}

void handleSerial(){
  if(Serial.available() > 1 && Serial.find('@')){
    char readChar = Serial.read();
    switch(readChar){
      case 'h': // Set current position to be zero
        homePosition = analogRead(DATA_PIN);
        homePosition += 512;
        break;
      case 'w': // Change wait time between cycles (in microseconds)
        w = Serial.readStringUntil('!').toInt();
        if(w > 0 && w < 1000000){
          time_step = w;
        }
        break;
      case 't': // Toggle mode
        isInPositionMode = !isInPositionMode;
        // LED on if we're in position mode, off if we're in rate mode
        digitalWrite(LED_BUILTIN, isInPositionMode);
        break;
      case 'p': // Set to position mode
        isInPositionMode = true;
        // LED on if we're in position mode, off if we're in rate mode
        digitalWrite(LED_BUILTIN, isInPositionMode);
        break;
      case 'r': // Set to rate mode
        isInPositionMode = false;
        // LED on if we're in position mode, off if we're in rate mode
        digitalWrite(LED_BUILTIN, isInPositionMode);
        break;
      case 'P': // Print position
        Serial.println(displayPosition);
        break;
      case 'R': // Print rate
        Serial.println(rate);
        break;
      case 'v': // Print value based on mode
        if(isInPositionMode){
          Serial.println(displayPosition);
        }
        else{
          Serial.println(rate);
        }
        break;
      case 's': // Enable Streaming
        isStreaming = true;
        break;
      case 'd': // Disable Streaming
        isStreaming = false;
        break;
      case 'x': // Print raw value 
        Serial.println(analogRead(DATA_PIN));
        break;
    }
  }
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  pinMode(BUTTON_PIN, INPUT);
  pinMode(DATA_PIN, INPUT);
  pinMode(LED_BUILTIN, OUTPUT);
  homePosition = analogRead(DATA_PIN); // Capture home position
  homePosition += 512; // Offset home position to middle value
  lastTime = micros();
  lastReading = analogRead(DATA_PIN);
  // LED on if we're in position mode, off if we're in rate mode
  digitalWrite(LED_BUILTIN, isInPositionMode);
  myRate.begin();
}

void loop() {  
    currTime = micros();
    currReading = (analogRead(DATA_PIN) - homePosition + 1024) % 1024;

    // Switch mode after long button press, zero position after short button press
    handleButton();
    // Find the position
    positionMode();
    // Find the rate
    rateMode();
    // Handle commands from the PC
    handleSerial();
    // Reset vars for next cycle
    lastTime = currTime;
    lastReading = currReading;
    delayMicroseconds(time_step);
 
}