#include <StaticThreadController.h>
#include <Thread.h>
#include <ThreadController.h>

#include <Wire.h>
#include <Adafruit_ADS1015.h>
#include <HX711_ADC.h>

//ADS1015 constructor
Adafruit_ADS1015 ads(0x49);

//HX711 constructor (DT pin, SCK pin)
HX711_ADC LoadCell_Horizontal(3, 4);
HX711_ADC LoadCell_FrontalDireita(5, 6);
HX711_ADC LoadCell_FrontalEsquerda(7, 8);
HX711_ADC LoadCell_TraseiraDireita(9, 10);
HX711_ADC LoadCell_TraseiraEsquerda(11, 12);

class PitotThread: public Thread
{
public:
  float Voltage = 0.0;
  int16_t adc = 0;
  int16_t adc_port;

  void run(){
    adc = ads.readADC_SingleEnded(adc_port);
    Voltage = (adc * 0.1875)/1000;
    runned();
  }
};

class CellsThread: public Thread
{
public:
  float forca_horizontal;
  float forca_frontal_direita;
  float forca_frontal_esquerda;
  float forca_traseira_direita;
  float forca_traseira_esquerda;
  char inByte;

  void checkTare(){
    if (inByte == 't') {
      LoadCell_FrontalDireita.tareNoDelay();
      LoadCell_FrontalEsquerda.tareNoDelay();
      LoadCell_TraseiraDireita.tareNoDelay();
      LoadCell_TraseiraEsquerda.tareNoDelay();
    }

    //check if last tare operation is complete

    if (LoadCell_FrontalDireita.getTareStatus() == true) {
      Serial.println("Tare load cell 1 complete");
    }
    if (LoadCell_FrontalEsquerda.getTareStatus() == true) {
      Serial.println("Tare load cell 2 complete");
    }
    if (LoadCell_TraseiraDireita.getTareStatus() == true) {
      Serial.println("Tare load cell 3 complete");
    }
    if (LoadCell_TraseiraEsquerda.getTareStatus() == true) {
      Serial.println("Tare load cell 4 complete");
    }
  }

  void initialize(){
    LoadCell_FrontalDireita.begin();
    LoadCell_FrontalEsquerda.begin();
    LoadCell_TraseiraDireita.begin();
    LoadCell_TraseiraEsquerda.begin();

    long calibrationTime = 5000; // tare preciscion can be improved by adding a few seconds of stabilising time

    byte LoadCell_FrontalDireita_ready = 0;
    byte LoadCell_FrontalEsquerda_ready = 0;
    byte LoadCell_TraseiraDireita_ready = 0;
    byte LoadCell_TraseiraEsquerda_ready = 0;

    while ((LoadCell_FrontalDireita_ready + LoadCell_FrontalEsquerda_ready + LoadCell_TraseiraDireita_ready + LoadCell_TraseiraEsquerda_ready) < 4) { //run startup, stabilization and tare, both modules simultaniously
      if (!LoadCell_FrontalDireita_ready) LoadCell_FrontalDireita_ready = LoadCell_FrontalDireita.startMultiple(calibrationTime);
      if (!LoadCell_FrontalEsquerda_ready) LoadCell_FrontalEsquerda_ready = LoadCell_FrontalEsquerda.startMultiple(calibrationTime);
      if (!LoadCell_TraseiraDireita_ready) LoadCell_TraseiraDireita_ready = LoadCell_TraseiraDireita.startMultiple(calibrationTime);
      if (!LoadCell_TraseiraEsquerda_ready) LoadCell_TraseiraEsquerda_ready = LoadCell_TraseiraEsquerda.startMultiple(calibrationTime);
    }

    LoadCell_FrontalDireita.setCalFactor(744.0); // user set calibration factor (float)
    LoadCell_FrontalEsquerda.setCalFactor(744.0); // user set calibration factor (float)
    LoadCell_TraseiraDireita.setCalFactor(744.0); // user set calibration factor (float)
    LoadCell_TraseiraEsquerda.setCalFactor(744.0); // user set calibration factor (float)
    Serial.println("Startup + tare is complete");
  }

  void run(){
    LoadCell_FrontalDireita.update();
    LoadCell_FrontalEsquerda.update();
    LoadCell_TraseiraDireita.update();
    LoadCell_TraseiraEsquerda.update();

    forca_frontal_direita = LoadCell_FrontalDireita.getData();
    forca_frontal_esquerda = LoadCell_FrontalEsquerda.getData();
    forca_traseira_direita = LoadCell_TraseiraDireita.getData();
    forca_traseira_esquerda = LoadCell_TraseiraEsquerda.getData();

    if (Serial.available() > 0) {
      inByte = Serial.read();
    }

    runned();
  }
};

ThreadController controller = ThreadController();
PitotThread pitot0 = PitotThread();
PitotThread pitot1 = PitotThread();
PitotThread pitot2 = PitotThread();
PitotThread pitot3 = PitotThread();
CellsThread celulas_bancada = CellsThread();

void setup(){
  Serial.begin(9600);

  pitot0.adc_port = 0;
  pitot1.adc_port = 1;
  pitot2.adc_port = 2;
  pitot3.adc_port = 3;

  pitot0.setInterval(10);
  pitot1.setInterval(10);
  pitot2.setInterval(10);
  pitot3.setInterval(10);

  celulas_bancada.initialize();
  celulas_bancada.setInterval(10);

  controller.add(&pitot0);
  controller.add(&pitot1);
  controller.add(&pitot2);
  controller.add(&pitot3);
  controller.add(&celulas_bancada);

  ads.begin();
}

void loop(){

  controller.run();

//  Serial.print(50*pitot0.Voltage);
//  Serial.print("\t");
//
//  Serial.print(50*pitot1.Voltage);
//  Serial.print("\t");
//
//  Serial.print(50*pitot2.Voltage);
//  Serial.print("\t");
//
//  Serial.print(50*pitot3.Voltage);
//  Serial.print("\t");
//
//  Serial.print(celulas_bancada.forca_frontal_direita);
//  Serial.print("\t");
//
//  Serial.print(celulas_bancada.forca_frontal_esquerda);
//  Serial.print("\t");
//
//  Serial.print(celulas_bancada.forca_traseira_direita);
//  Serial.print("\t");
//
//  Serial.print(celulas_bancada.forca_traseira_esquerda);
//  Serial.print("\t");
//
//  Serial.println();

  Serial.print("!");

//  Serial.print("fh");
//  Serial.print("=");
//  Serial.print(cells.fh);
//  Serial.print(";");

  Serial.print("ffd");
  Serial.print("=");
  Serial.print(celulas_bancada.forca_frontal_direita);
  Serial.print(";");

  Serial.print("ffe");
  Serial.print("=");
  Serial.print(celulas_bancada.forca_frontal_esquerda);
  Serial.print(";");

  Serial.print("ftd");
  Serial.print("=");
  Serial.print(celulas_bancada.forca_traseira_direita);
  Serial.print(";");

  Serial.print("fte");
  Serial.print("=");
  Serial.print(celulas_bancada.forca_traseira_esquerda);
  Serial.print(";");

  Serial.print("pitot0");
  Serial.print("=");
  Serial.print(pitot0.Voltage);
  Serial.print(";");

  Serial.print("pitot1");
  Serial.print("=");
  Serial.print(pitot1.Voltage);
  Serial.print(";");

  Serial.print("pitot2");
  Serial.print("=");
  Serial.print(pitot2.Voltage);
  Serial.print(";");

  Serial.print("pitot3");
  Serial.print("=");
  Serial.print(pitot3.Voltage);
  Serial.print(";");

  Serial.println("@");
}