#include "main.hpp"
#include "autom.cpp"
#include <thread>
#include <atomic>

int marche_precedent = 0;
int marche_frontmontant = 0;

int arret_precedent = 1;
int arret_frontdescedant = 0;

int bouton_precedent[8];
int bouton_frontmontant[8];
int bouton_frontdescendant[8];

int b0_precedent = 0;
int b0_frontmontant = 0;
int b1_precedent = 0;
int b1_frontmontant = 0;
int b2_precedent = 0;
int b2_frontmontant = 0;
int b3_precedent = 0;
int b3_frontmontant = 0;
int b4_precedent = 0;
int b4_frontmontant = 0;
int b5_precedent = 0;
int b5_frontmontant = 0;
int b6_precedent = 0;
int b6_frontmontant = 0;
int b7_precedent = 0;
int b7_frontmontant = 0;

int cycle = 0;
long unsigned tempo = 0;
int compteur = 0;
int sens = 1;

int ci = 0;
int mode_auto = 0;
int mode_manu = 0;
int etape = 0;
int etape_2 = 0;

int c5_frontdescendant = 0;
int c5_precedent = 0;



int main()
{
  mqtt_open(&client);

  std::atomic<bool> running{true};

  while (running)
  {
    std::this_thread::sleep_for(std::chrono::seconds(1));
  }

  mqtt_close();
  return 0;
}

void process() {

  // Détection du front montant (état courant supérieur à l'état précédent)
  for(int i = 0; i < 8 ; i++) {
    bouton_frontmontant[i] = bouton[i] > bouton_precedent[i];
    bouton_frontdescendant[i] = bouton[i] < bouton_precedent[i];
  }

  marche_frontmontant = marche > marche_precedent;
  arret_frontdescedant = arret < arret_precedent;

  /*
  int etape_suivante = etape;
  int etape_suivante2 = etape_2;

  if (etape == 0 && b6_frontmontant == 1) {
    etape_suivante = 1;
  }

  if (etape == 1 && b6_frontmontant == 1) {
    etape_suivante = 0;
  }

  if (etape_2 == 0 && b7_frontmontant == 1)
  {
    etape_suivante2 = 1;
  }

  if (etape_2 == 1 && b7_frontmontant == 1)
  {
    etape_suivante2 = 0;
  }

  etape = etape_suivante;
  etape_2 = etape_suivante2;

  m0 = etape == 1;
  m1 = etape_2 == 1;
  */


  if (b6_frontmontant) {
    m0 = !m0;
  }

  if (b7_frontmontant) {
    m1 = !m1;
  }


  if (abs(p0 - v0) < 3) {
    m0 = m1 = 0;

  } else {

    if (p0 < v0)
    {
      m0 = 1;
      m1 = 0;
    }

    if (p0 > v0)
    {
      m0 = 0;
      m1 = 1;
    }
  }

  // Prêt pour un nouveau tour
  // L'état courant devient l'état précédent
  for (int i = 0; i < 8; i++) {
    bouton_precedent[i] = bouton[i];
  }
  marche_precedent = marche;
  arret_precedent = arret;
}