#include "inputHelpers.h"
#include <EEPROM.h>
#include "displayHelpers.h"

potiRange steeringRange = {
  100,  
  924, 
  512
};

potiRange brakingRange = {
  0,  
  1023, 
  0
};

void setupSensors(){
  EEPROM.begin();
  pinMode(STEERING_SENSOR, INPUT);
  pinMode(BRAKING_SENSOR, INPUT);
}

void printRange(String title,potiRange range){
  Serial.println(title);
  Serial.print("lowerL: ");Serial.println(range.lowerLimit);
  Serial.print("upperL: ");Serial.println(range.upperLimit);
  Serial.print("nullP : ");Serial.println(range.center);
}

void calibrate(){
  unsigned long startTime = millis();
  resetRange(steeringRange);
  displayStatusMsg("Calibrating...");
  delay(1000);
  displayMaxCalibrationMessage();
  while( (millis() - startTime) < CALIBRATION_DURATION){
    steeringRange = updateRange(analogRead(STEERING_SENSOR),steeringRange);
    delay(100);
    int remainingSeconds = ( CALIBRATION_DURATION - (millis() - startTime) )/1000;
    displayRemainingTime(remainingSeconds);
  }
  displayCenterCalibrationMessage();
  startTime = millis();
  while( (millis() - startTime) < 10000){
    delay(100);
    int remainingSeconds = ( 10000 - (millis() - startTime) )/1000;
    displayRemainingTime(remainingSeconds);
  }
  steeringRange.center = analogRead(STEERING_SENSOR);
  displayStatusMsg("Done");
  delay(2000);
  printRange("steeringRange",steeringRange);
  displayRange("Steering Range: ",steeringRange.upperLimit,steeringRange.lowerLimit,steeringRange.center);
  delay(10000);
  displayStatusMsg("Ready");
  Serial.println("ready");
  delay(2000);
  clearDisplay();
}

int getSteeringInput(){
  uint16_t analogValue = analogRead(STEERING_SENSOR);
  return toNormalizedValue(analogValue,steeringRange);
}
int getBrakingInput(){
  uint16_t analogValue = analogRead(BRAKING_SENSOR);
  return toNormalizedValueNoMidpoint(analogValue,brakingRange);
}

void resetRange(potiRange &range){
  range.lowerLimit = 1023;
  range.upperLimit = 0;
}

potiRange updateRange(uint16_t inputValue, potiRange range){
  if(inputValue < range.lowerLimit){
    range.lowerLimit = inputValue;
  }
  else if( inputValue > range.upperLimit){
    range.upperLimit = inputValue;
  }
  return range;
}

void storeRanges(){
  EEPROM.update(0,42);
  EEPROM.put(1,steeringRange);
  EEPROM.put(1+sizeof(steeringRange),brakingRange);
}

void loadRanges(){
  if(EEPROM.read(0) == 42){
    steeringRange = EEPROM.get(1,steeringRange);
    //brakingRange = EEPROM.get(1+sizeof(steeringRange),brakingRange);
  }
}

int toNormalizedValueNoMidpoint(uint16_t analogValue,potiRange range){
  Serial.print("Analog value: ");Serial.println(analogValue);
    if(analogValue < range.lowerLimit){
      return 0;
    }
    else if(analogValue > range.upperLimit){
      return 100;
    }

    unsigned long difference = (int)analogValue - range.lowerLimit;
    unsigned long positiveRange = range.upperLimit - range.lowerLimit;

    /*
    Serial.print("diference: ");Serial.println(difference);
    Serial.print("positiveRange: ");Serial.println(positiveRange);
    */
    unsigned long result = (100*difference)/positiveRange;
    /*Serial.print("Result: ");Serial.println(result);*/
    return result;
}

int toNormalizedValue(uint16_t analogValue,potiRange range){

    if(analogValue < range.lowerLimit){
      return -100;
    }
    else if(analogValue > range.upperLimit){
      return 100;
    }

    if(analogValue == range.center){
      return 0;
    }
    else if(analogValue < range.center){
      int difference = range.center - analogValue;
      int negativeRange = range.upperLimit - range.center;
      return (-100*difference)/negativeRange;
    }
    else{
      int difference = analogValue - range.center;
      int positiveRange = range.center - range.lowerLimit;
      return (100*difference)/positiveRange;
    }
}

unsigned long lastSteeringUpdate = 0;
int slotCtr = 0;

#define PWM_SLOTS 10
bool getSteeringPwmState(int currentValue, int maxValue){
    int level = (currentValue*PWM_SLOTS)/maxValue;
    bool outputState = (slotCtr < level);
    slotCtr++;
    slotCtr%=PWM_SLOTS;
    return outputState;
}

int getModifiedSteering(int input){
  int output = 0;
  int percentage = input;
  
  if(input < 0){
    percentage = -input;
  }

  bool pulseState = false;
  int steeringLevel = 0;

  if( (percentage > 2) && (percentage <= 15) ){
    steeringLevel = 1;
    pulseState = getSteeringPwmState(percentage-2, 13);     
  }
  else if( (percentage > 15) && (percentage <= 25) ){
    steeringLevel = 2;
    pulseState = false;
  }
  else if((percentage > 25) && (percentage <= 65)){
    steeringLevel = 2;
    pulseState = getSteeringPwmState(percentage-25,40);
  }
  else if(percentage > 65){
    steeringLevel = 2;
    pulseState = true;
  }

  switch(steeringLevel){
    case 0: output = 0;
            break;
    case 1: if(pulseState){
              output = GTA_STEERING_THRESHOLD+5;
            }
            break;
    case 2: if(pulseState){
              output = GTA_STEERING_MAX+5;
            }
            else{
              output = GTA_STEERING_THRESHOLD+5;
            } 
            break;
  }

  if(input < 0){
    return -output;
  }
  else{
    return output;
  }
    
}
