// main board controller
// all forces are in newtons and times in milliseconds

#include "types.h"
#include <HX711_ADC.h>
#include <ESC.h>

#define TOTAL_FORCE_THRESHOLD 300 // minimum sum force required
#define SENSOR_FORCE_THRESHOLD 100 // minimum force required on each sensor
#define FRONT_CALIBRATION_FACTOR 4 //
#define REAR_CALIBRATION_FACTOR 4.7  //
#define STABILISING_TIME 2000 // time at startup to intialize and tare sensors

#define UPDATE_INTERVAL 50 // time between each write to the ESC
#define SERIAL_BAUD 9600

#define SERVO_MIN_DEGREES 0
#define SERVO_MAX_DEGREES 179
#define SERVO_MIN_PULSE_WIDTH 1000
#define SERVO_MAX_PULSE_WIDTH 2000
#define SERVO_PIN 2
#define LED 12

#define REVERSE_THROTTLE 0
#define FULL_THROTTLE 179
#define BRAKE_THROTTLE 89
#define NEUTRAL_THROTTLE 30

#define TARE_BUTTON_PIN 8

//HX711 constructor (dout pin, sck pin)
HX711_ADC FrontSensor(4, 5); 
HX711_ADC RearSensor(6, 7);
Servo motor;

unsigned long t;
int last_TARE_BUTTON_PIN_state = HIGH;

InterpPair Throttle[5] = 
{
    {300, 10.02},
    {700, 89.542},
    {800, 126.452}, 
    {900, 171.453}, 
    {1500, 225.123}
};    


void setup() {
   motor.attach( SERVO_PIN, SERVO_MIN_PULSE_WIDTH,
        SERVO_MAX_PULSE_WIDTH );
    motor.write(90);
  Serial.begin(SERIAL_BAUD);

  FrontSensor.begin();
  RearSensor.begin();
  byte FrontSensor_rdy = 0;
  byte RearSensor_rdy = 0;
  
  while ((FrontSensor_rdy + RearSensor_rdy) < 2) { 
    //run startup, stabilization and tare, both modules simultaniously
    if (!FrontSensor_rdy) FrontSensor_rdy = FrontSensor.startMultiple(STABILISING_TIME);
    if (!RearSensor_rdy) RearSensor_rdy = RearSensor.startMultiple(STABILISING_TIME);
  }
  FrontSensor.setCalFactor(FRONT_CALIBRATION_FACTOR);
  RearSensor.setCalFactor(REAR_CALIBRATION_FACTOR);
  /*
  Serial.println("Startup + tare is complete");
  Serial.println(LookupInterp(Throttle, 5, 280.0));
  Serial.println(LookupInterp(Throttle, 5, 868.3242));
  Serial.println(LookupInterp(Throttle, 5, 1234.222));
  Serial.println(LookupInterp(Throttle, 5, 1600.0));
  pinMode(LED_BUILTIN, OUTPUT);*/
    pinMode( TARE_BUTTON_PIN, INPUT );
    digitalWrite( TARE_BUTTON_PIN, HIGH ); // Activate pullups
}

void loop() {
 
  int cur_TARE_BUTTON_PIN_state = digitalRead( TARE_BUTTON_PIN );
  if( cur_TARE_BUTTON_PIN_state != last_TARE_BUTTON_PIN_state ) {
        // Change state when we're moving from high to low (let off button)
        if( HIGH == cur_TARE_BUTTON_PIN_state ) 
            FrontSensor.tare();
            RearSensor.tare();
    }
    last_TARE_BUTTON_PIN_state = cur_TARE_BUTTON_PIN_state;
  FrontSensor.update();
  RearSensor.update();
  //takes a reading from each sensor and stores it 
  
  if (millis() > t + UPDATE_INTERVAL) 
  {
    float frontForce = FrontSensor.getData();
    float backForce = RearSensor.getData();
    Serial.print (frontForce);
    Serial.print(" ");
    Serial.print(backForce);
    Serial.print(" ");
        
 bool standingOnBoard = (frontForce + backForce) >= TOTAL_FORCE_THRESHOLD && frontForce >= SENSOR_FORCE_THRESHOLD && backForce >= SENSOR_FORCE_THRESHOLD;
    Serial.print(1000.0);
    Serial.print(" ");
    if(standingOnBoard)
    {
      float balanceScore = (frontForce-backForce)/(frontForce+backForce);
      Serial.println(1000*balanceScore);
      
      if(balanceScore > 0)
      {
        float throttle = balanceScore * 50 + 90;
        motor.write(constrain(throttle, 90, 180));
      }
      else
      {
        motor.write(90);
      }
      //float throttle = LookupInterp(throttle, 5, balanceScore)
      //go or brake
    }
    else
    {
      motor.write(90);
      //brake
      Serial.println(0);
    }
    t = millis();
  } 
}

float LookupInterp(InterpPair* t, int n, float input)
{
    int i;
    if(input < t[0].in) return t[0].out; // if the input is below the range, clamp the output value
    for(i = 0; i < n-1; i++)
    {
        if (t[i].in <= input && t[i+1].in >= input)
        {
            float diffInput = input - t[i].in;
            float diffOutput = t[i+1].in - t[i].in;
            return t[i].out + (t[i+1].out - t[i].out) * diffInput / diffOutput;
        }
    }
    return t[n-1].out; // if the input is above the range, clamp the output value
}