// PINS
#define SENSOR_DT_PIN 4
#define SENSOR_SCK_PIN 5
#define ESC_PIN 9
// LEAD CELL CALIBRATION
#define OFFSET 22
#define SCALE 2.565
// CONTINUOUS MODE
#define INTERVAL 500 // ms, interval to take measurements in continuous mode
// TEST PARAMETERS
#define STEPS 4 // number of steps to measure except 0 (e.g. 4 will measure at 0, 1250, 1500, 1750, 2000)
#define TRANSITION_INTERVAL 1000 // ms, time to wait after throttle is changet before take measurement (def 1000)
#define MEASURE_TIME 3000 // ms, time to capture data to calculate mean, be carefull with big numbers (def 3000)

#include <TimerMs.h>
TimerMs update_data(INTERVAL, true);
TimerMs test_wait_interval(TRANSITION_INTERVAL, false, true);
TimerMs test_capture_interval(MEASURE_TIME, false, true);

#include <Servo.h>
Servo esc;

// FIXME! Use this once arduino library is updated to 1.1
#include <GyverHX711.h> 
// #include "src/GyverHX711/GyverHX711.h"
GyverHX711 sensor(SENSOR_DT_PIN, SENSOR_SCK_PIN, HX_GAIN64_A);

// Variables
long thrust = 0;
long thrust_sum = 0;
long thrust_count = 0;
int throttle = 1000; // in ms 1000-2000
int offset = OFFSET;
float scale = SCALE;
int total_steps = STEPS;
int current_step = -1; // -2 - do nothing; -1 measure continuously; >=0 - do test


void setup() {
  esc.attach(ESC_PIN,1000,2000);
  esc.writeMicroseconds(1000);

  Serial.begin(9600);
  while (!Serial); // Wait for serial monitor
  
  Serial.println("konichiwaa! Trust_stand_for_serial_v1 by Varden");
  Serial.print("Thrust is measured each ");
  Serial.print(float(INTERVAL)/1000);
  if (INTERVAL < 2000){
    Serial.println(" second");
  } else {
    Serial.println(" seconds");
  }
  Serial.println("To set throttle send it in PWM (1000-2000)");
  Serial.println("Send word TEST to start test. NOTE! It will throttle to max");
  Serial.println("Send anything during test to interrupt it");
  Serial.println("At the and of test you will see results and no more measures will be done");
  Serial.println("Send throttle again to measure thrust again");
  Serial.println("Send STOP to stop contineous measurements");
}


void loop() {
  // handle input
  if (Serial.available()){
    String str = Serial.readString();
    if (current_step >= 0){
      stop_test();
      Serial.println("TEST interrupted!");
      return;
    }
    str.trim();
    int integer = str.toInt();
    if (str == "TEST"){
      start_test();
    } else if (integer >= 1000 && integer <= 2000){
      current_step = -1;
      set_throttle(integer);
    } else {
      Serial.print("Unknown command -> ");
      Serial.println(str);
    }
  }

  if (current_step == -1){
    if (update_data.tick()){
      get_thrust();
      print_values();
    }
  } else if (current_step >= 0) {
    perform_test();
  }
}


void start_test(){
  current_step = 0;
  test_wait_interval.start();
  Serial.println("Starting TEST!");
}


void perform_test(){
  // handle transition
  if (!test_capture_interval.active()){
    if (test_wait_interval.status()){
      return;
    } else {
      test_wait_interval.stop();
      test_capture_interval.start();
    }
  }

  // do measure
  if (sensor.available()) {
    get_thrust();
    thrust_sum += thrust;
    thrust_count++;
  }
  
  // when measure is done
  if (test_capture_interval.elapsed()) {
    Serial.print("Throttle = ");
    Serial.print(throttle);
    Serial.print("; Thrust_mean_raw = ");
    Serial.print(thrust_sum/thrust_count);
    Serial.print("; Thrust_mean = ");
    Serial.println(convert_thrust(thrust_sum/thrust_count));
    thrust_sum = 0;
    thrust_count = 0;
    test_capture_interval.stop();
    current_step++;
    if (current_step > total_steps) {
      stop_test();
    Serial.println("TEST completed!");
      return;
    }
    test_wait_interval.start();
    throttle = 1000 / total_steps * current_step + 1000;
    set_throttle(throttle);
    }
}


void stop_test(){
  set_throttle(1000);
  test_capture_interval.stop();
  test_wait_interval.stop();
  thrust_sum = 0;
  thrust_count = 0;
  current_step = -2;
}


void set_throttle(int thr){
  throttle = constrain(thr, 1000, 2000);
  esc.writeMicroseconds(throttle);
}


void get_thrust(){
  if (sensor.available()) {
    thrust = sensor.read()/1000;
  } else {
    thrust = 9999;
  }
}


int convert_thrust(int raw){
  return (raw + offset) * scale;
}


void print_values(){
  Serial.print("Throttle = ");
  Serial.print(throttle);
  Serial.print("; Thrust_raw = ");
  Serial.print(thrust);
  Serial.print("; Thrust = ");
  Serial.println(convert_thrust(thrust));
}