#include <iostream>
#include <iomanip>

#include <unistd.h>
#include <math.h>
#include <locale.h>
#include <array>
#include "main.hpp"
#include "AutomForArduino.cpp"

#include <prometheus/counter.h>
#include <prometheus/gauge.h>
#include <prometheus/histogram.h>
#include <prometheus/registry.h>
#include <prometheus/exposer.h>

#include <curl/curl.h>
#include <string>

#include <thread>
#include <atomic>
#include <queue>
#include <mutex>
#include <condition_variable>
#include <csignal>
#include <chrono>
#include <cstring>
#undef timeout
#include "mqtt/async_client.h"

#include <nlohmann/json.hpp>
using json = nlohmann::json;

// Constantes de fonctionnement
#define LEVEL_MIN 2
#define FLOW_PER_PUMP 150

/* Configuration MQTT */
const std::string ADDRESS = "tcp://rabbitmq:1883";
const std::string CLIENTID = "CppClientTP";
const std::string TOPIC = "geii/ordre/#";
const int QOS = 1;
const int CYCLE_MS = 100;

int etape = 0; // Étape du grafcet : début Automatique
int bp_mode, bp_mode_fm;
unsigned short pompe1, pompe2, pompe3, pompe4; // bouton des pompes 0 (arrêt) / 1 (marche)
unsigned short pompe1_old, pompe2_old, pompe3_old, pompe4_old;
unsigned short sensor_max, sensor_high, sensor_low, sensor_min;

float TankInitalValue = 7;

// Prometheus
// ************************************************************

// ************************************************************

class callback : public virtual mqtt::callback {
public:
  void message_arrived(mqtt::const_message_ptr msg) override {
    std::string payload = msg->to_string();

    if (payload == "p1") {
      pompe1 = !pompe1;
    } else if (payload == "p2") {
      pompe2 = !pompe2;
    } else if (payload == "p3") {
      pompe3 = !pompe3;
    } else if (payload == "p4") {
      pompe4 = !pompe4;
    }
  }
};

int main()
{
  /* Initialisation */
  InitPrometheus();

  ProcessInitIO();
  ProcessInitValues();

  mqtt::async_client client(ADDRESS, CLIENTID);
  callback cb;
  client.set_callback(cb);

  mqtt::connect_options connOpts;
  connOpts.set_clean_session(true);
  connOpts.set_user_name("admin");
  connOpts.set_password("geii2025");
  try {
    client.connect(connOpts)->wait();
    client.start_consuming();
    client.subscribe(TOPIC, QOS)->wait();
  } catch (const mqtt::exception &exc) {
    std::cerr << "Erreur MQTT: " << exc.what() << "\n";
    return 1;
  }

  while (1)
  {
    Process();

    sensor_min = digitalRead(IN_SENSOR_MIN);
    sensor_low = digitalRead(IN_SENSOR_LOW);
    sensor_high = digitalRead(IN_SENSOR_HIGH);
    sensor_max = digitalRead(IN_SENSOR_MAX);

    digitalWrite(OUT_PUMP_1, (pompe1 == 1));
    digitalWrite(OUT_PUMP_2, (pompe2 == 1));
    digitalWrite(OUT_PUMP_3, (pompe3 == 1));
    digitalWrite(OUT_PUMP_4, (pompe4 == 1));

    ProcessPrometheus();
    ProcessMQTT(&client);
    ProcessException();

    usleep(100000);
  }

  try {
    client.unsubscribe(TOPIC)->wait();
    client.stop_consuming();
    client.disconnect()->wait();
  } catch(const mqtt::exception &exc){
    std::cerr << "Erreur déconnexion MQTT: " << exc.what() << std::endl;
  }

  std::cout << "Fin du programme" << std::endl;
  return 0;
}

/**
 * Process
 */
void ProcessInitIO()
{
  pinMode(IN_SENSOR_MIN, IO_INPUT | DIGITAL);
  pinMode(IN_SENSOR_LOW, IO_INPUT | DIGITAL);
  pinMode(IN_SENSOR_HIGH, IO_INPUT | DIGITAL);
  pinMode(IN_SENSOR_MAX, IO_INPUT | DIGITAL);

  pinMode(IN_TANK_LEVEL, IO_INPUT | ANALOG);
  pinMode(IN_FLOW_OUT, IO_INPUT | ANALOG);
  pinMode(IN_FLOW_IN, IO_INPUT | ANALOG);
  pinMode(IN_FLOW_DIF, IO_INPUT | ANALOG);
  pinMode(IN_TANK_MIN, IO_INPUT | ANALOG);
  pinMode(IN_TANK_MAX, IO_INPUT | ANALOG);
  pinMode(IN_FLOW_CAP, IO_INPUT | ANALOG);
  pinMode(IN_FLOW_1, IO_INPUT | ANALOG);
  pinMode(IN_FLOW_2, IO_INPUT | ANALOG);
  pinMode(IN_FLOW_3, IO_INPUT | ANALOG);
  pinMode(IN_FLOW_4, IO_INPUT | ANALOG);

  pinMode(OUT_PUMP_1, IO_OUTPUT | DIGITAL);
  pinMode(OUT_PUMP_2, IO_OUTPUT | DIGITAL);
  pinMode(OUT_PUMP_3, IO_OUTPUT | DIGITAL);
  pinMode(OUT_PUMP_4, IO_OUTPUT | DIGITAL);

  pinMode(OUT_DISPLAY_MODE, IO_OUTPUT | DIGITAL);
  pinMode(OUT_DISPLAY_GRAFCET, IO_OUTPUT | DIGITAL);

  pinMode(OUT_LEVEL_MIN, IO_OUTPUT | ANALOG);
  pinMode(OUT_LEVEL_LOW, IO_OUTPUT | ANALOG);
  pinMode(OUT_LEVEL_HIGH, IO_OUTPUT | ANALOG);
  pinMode(OUT_LEVEL_MAX, IO_OUTPUT | ANALOG);
  pinMode(OUT_FLOW_PER_PUMP, IO_OUTPUT | ANALOG);
  pinMode(OUT_FLOW_OUT_AMPLITUDE, IO_OUTPUT | ANALOG);

  pinMode(OUT_BEEP, IO_OUTPUT | DIGITAL);

  _digital[OUT_PUMP_1].error = 30;
  _digital[OUT_PUMP_1].efficacite = 1.0;
  //_digital[OUT_PUMP_1].time = 4294967295; //UINT_MAX

  _digital[OUT_PUMP_2].error = 30;
  _digital[OUT_PUMP_2].efficacite = 0.72;
  //_digital[OUT_PUMP_2].time = 4294967295;

  _digital[OUT_PUMP_3].error = 10;
  _digital[OUT_PUMP_3].efficacite = 1.0;
  //_digital[OUT_PUMP_3].time = 4294967295;

  _digital[OUT_PUMP_4].error = 30;
  _digital[OUT_PUMP_4].efficacite = 1.0;
  //_digital[OUT_PUMP_4].time = 4294967295;
}

void ProcessInitValues()
{
  t_start = t_backup = millis();

  srand(time(NULL));

  _digital[IN_TANK_LEVEL].dvalue = _digital[IN_TANK_MAX].dvalue = _digital[IN_TANK_MIN].dvalue = TankInitalValue;

  _digital[OUT_FLOW_PER_PUMP].dvalue = FLOW_PER_PUMP;
  _digital[OUT_FLOW_OUT_AMPLITUDE].dvalue = 100.0;

  _digital[OUT_LEVEL_MIN].dvalue = LEVEL_MIN;
  _digital[OUT_LEVEL_LOW].dvalue = 6;
  _digital[OUT_LEVEL_HIGH].dvalue = 7;
  _digital[OUT_LEVEL_MAX].dvalue = 9.5;

  _digital[IN_FLOW_OUT].dvalue = 100.0;
}

/**
 * Fonctionnement des moteurs
 */
double ProcessMoteur(int i)
{
  double vitesse = 1.0;
  double t = _digital[i].time / 5000.0;

  if (_digital[i].ivalue)
  {
    if (_digital[i].time < 2500)
    {
      vitesse = 4 * pow(t, 3.0);
    }
    else if (_digital[i].time < 5000)
    {
      vitesse = 1.0 - pow(2 - 2 * t, 3) / 2.0;
    }
    else
    {
      vitesse = 1.0 + 1.0 / (_digital[i].error * 2.0) - rand() / (double)RAND_MAX / _digital[i].error;
    }
  }
  else
  {
    if (_digital[i].time < 2500)
    {
      vitesse = 1 - 4 * pow(t, 3.0);
    }
    else if (_digital[i].time < 5000)
    {
      vitesse = pow(2 - 2 * t, 3) / 2.0;
      //  vitesse = 1 - pow(t, 4.0);
    }
    else
    {
      vitesse = 0.0;
    }
  }

  return _digital[OUT_FLOW_PER_PUMP].dvalue * _digital[i].efficacite * vitesse;
}

void ProcessException()
{
  if (t_elapsed > 30) {
    _digital[OUT_PUMP_1].mode = 0;
  } else if (t_elapsed > 15) {
    _digital[IN_SENSOR_LOW].mode = 0;
  }
}

void Process()
{
  // *****
  unsigned long t = millis();
  t_elapsed = (t - t_start) / 1000.0;
  dt = (t - t_backup) / 1000.0;

  // ***** FLOW OUT
  if (_digital[IN_TANK_LEVEL].dvalue > 1.0)
  {
    //_digital[IN_FLOW_OUT].dvalue = SimulConsoSinusoidale(t);
    _digital[IN_FLOW_OUT].dvalue = SimulConsoBrown(_digital[IN_FLOW_OUT].dvalue);
  }
  else
  {
    if (_digital[IN_FLOW_CAP].dvalue == 0.0) {
      _digital[IN_FLOW_CAP].dvalue = _digital[IN_FLOW_OUT].dvalue;
    }
    _digital[IN_FLOW_OUT].dvalue = _digital[IN_FLOW_CAP].dvalue * _digital[IN_TANK_LEVEL].dvalue;
  }

  // ***** FLOW IN
  _digital[IN_FLOW_IN].dvalue = 0;
  for (int i = OUT_PUMP_1; i <= OUT_PUMP_4; i++)
  {
    _digital[i - 4].dvalue = ProcessMoteur(i);
    _digital[IN_FLOW_IN].dvalue += _digital[i - 4].dvalue;
  }

  _digital[IN_FLOW_DIF].dvalue = _digital[IN_FLOW_IN].dvalue - _digital[IN_FLOW_OUT].dvalue;

  // ***** TANK LEVEL
  _digital[IN_TANK_LEVEL].dvalue += (_digital[IN_FLOW_IN].dvalue - _digital[IN_FLOW_OUT].dvalue) / 1000.0 * dt;
  if (_digital[IN_TANK_LEVEL].dvalue > 10.0) {
    _digital[IN_TANK_LEVEL].dvalue = 10.0;
  }

  if (_digital[IN_TANK_LEVEL].dvalue > _digital[IN_TANK_MAX].dvalue) {
    _digital[IN_TANK_MAX].dvalue = _digital[IN_TANK_LEVEL].dvalue;
  }

  if (_digital[IN_TANK_LEVEL].dvalue < _digital[IN_TANK_MIN].dvalue) {
    _digital[IN_TANK_MIN].dvalue = _digital[IN_TANK_LEVEL].dvalue;
  }

  // **** SENSOR
  int test;

  test = (_digital[IN_TANK_LEVEL].dvalue > _digital[OUT_LEVEL_MIN].dvalue);
  if (_digital[IN_SENSOR_MIN].ivalue != test)
  {
    if (test == 0)
    {
      _digital[IN_SENSOR_MIN].nb += 1;
    }
    _digital[IN_SENSOR_MIN].ivalue = test;
  }

  test = _digital[IN_TANK_LEVEL].dvalue > _digital[OUT_LEVEL_LOW].dvalue && _digital[IN_SENSOR_LOW].mode & 0x01;
  if (_digital[IN_SENSOR_LOW].ivalue != test)
  {
    if (test == 0)
    {
      _digital[IN_SENSOR_LOW].nb += 1;
    }
    _digital[IN_SENSOR_LOW].ivalue = test;
  }

  test = _digital[IN_TANK_LEVEL].dvalue > _digital[OUT_LEVEL_MAX].dvalue;
  if (_digital[IN_SENSOR_MAX].ivalue != test)
  {
    if (test == 1)
    {
      _digital[IN_SENSOR_MAX].nb += 1;
    }
    _digital[IN_SENSOR_MAX].ivalue = test;
  }

  test = _digital[IN_TANK_LEVEL].dvalue > _digital[OUT_LEVEL_HIGH].dvalue;
  if (_digital[IN_SENSOR_HIGH].ivalue != test)
  {
    if (test == 1)
    {
      _digital[IN_SENSOR_HIGH].nb += 1;
    }
    _digital[IN_SENSOR_HIGH].ivalue = test;
  }

  t_backup = t;
}

double SimulConsoSinusoidale(long t)
{
  double alea = ((long)(t / 100.0) % 600) * 3 / 1800.0 * PI;
  //mvprintw(18, 0, "%ld %f", (long)(t / 100.0), alea);
  return 100 + cos(alea) * cos(alea) * _digital[OUT_FLOW_OUT_AMPLITUDE].dvalue;
}

// dt : Intervalle de temps
double SimulConsoBrown(double valeur_precedente)
{
  float mu = 0.01 * -((((int)t_elapsed / 30) % 2) * 2 - 1);  // Taux de croissance (1%)
  float sigma = 0.05;   // Volatilité (5%)

  // Nombre aléatoire compris dans [-1 +1]
  float rand_std_normal = ((double)rand() / RAND_MAX) * 2.0 - 1.0;

  // Calcule la variation logarithmique pour cette étape
  float drift = (mu - 0.5f * sigma * sigma) * dt;
  float diffusion = sigma * sqrt(dt) * rand_std_normal;

  return valeur_precedente * exp(drift + diffusion);
}

/**
 * Affichage dans la console
 */


/**
 * Prometheus
 */
void InitPrometheus()
{


}

void ProcessPrometheus()
{

}

void ProcessMQTT(mqtt::async_client* client)
{
  json obj = {
    {"entree", _digital[IN_FLOW_IN].dvalue},
    {"sortie", _digital[IN_FLOW_OUT].dvalue},
    {"tank", _digital[IN_TANK_LEVEL].dvalue},
    {"pompe1", _digital[IN_FLOW_1].dvalue},
    {"pompe2", _digital[IN_FLOW_2].dvalue},
    {"pompe3", _digital[IN_FLOW_3].dvalue},
    {"pompe4", _digital[IN_FLOW_4].dvalue},
    {"min",  _digital[IN_SENSOR_MIN].ivalue},
    {"low",  _digital[IN_SENSOR_LOW].ivalue},
    {"high",  _digital[IN_SENSOR_HIGH].ivalue},
    {"max",  _digital[IN_SENSOR_MAX].ivalue},
  };

  std::string payload = obj.dump();
  auto msg = mqtt::make_message("geii/telemetry", payload);
  msg->set_qos(1);
  client->publish(msg);
}