#include "speedSensorHelpers.h"
#include <Arduino.h>
#include "config.h"

// Speed Calibration Params
unsigned long lastTs = 0;
float avgSpeed = 0;
bool wheelInMotion = false;

void setupSpeedSensor(){
  pinMode(SPEED_SENSOR,INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(SPEED_SENSOR),speedISR,FALLING);
}

/*
 * While the sensor is away from the magnet on the wheel, the actual speed can not be detected.
 * It can be anything between 0 m/s and circumference/s
 */
void updateSpeedSensor(){
  if( wheelInMotion && ( (millis()-lastTs) > 1200) ){
    avgSpeed = 0;
    wheelInMotion = false;
  }
}

float getSpeedMs(){
  return avgSpeed;
}

void speedISR(){
    unsigned long currentTs = millis();
    
    if(!wheelInMotion){
      wheelInMotion = true;
      avgSpeed = MIN_SPEED;
    }
    else{
      unsigned int deltaT = currentTs - lastTs;
      if(deltaT > 0){
        float currentSpeed = (WHEEL_CIRCUMFERENCE*1000)/(float)deltaT;

        if(avgSpeed > 0){
          float sum = avgSpeed + currentSpeed;
          avgSpeed = sum/2.0;
        }
        else{
          avgSpeed = currentSpeed;
        }

    }
  }
  lastTs = currentTs;
}
