#include "Ifuzzy_set_intuitionistic.h"
#include "membership_intuitionistic.h"
#include "Ifuzzy_input_output.h"
#include "Ifuzzy_antecedent.h"
#include "Ifuzzy_rule.h"
#include "Ifuzzy.h"

// PINS
#define LDR A0
#define TRIGGER 4
#define ECHO 5
#define LED 6

FuzzyIntuitionistic* fuzzySystem;

// =========================
// PRINT TABLE REGLES
// =========================
void printRuleTable(FuzzyInput* input1, FuzzyInput* input2, FuzzyOutput* output, int** table) {
  Serial.println("\n=== TABLE DES REGLES ===");

  for (int i = 0; i < input1->count; i++) {
    for (int j = 0; j < input2->count; j++) {

      Serial.print("IF ");
      Serial.print(input1->sets[i]->name);
      Serial.print(" AND ");
      Serial.print(input2->sets[j]->name);
      Serial.print(" THEN ");

      int outIndex = table[i][j];
      Serial.println(output->sets[outIndex]->name);
    }
  }
}

// =========================
// DISTANCE
// =========================
double getDistance() {
  digitalWrite(TRIGGER, LOW);
  delayMicroseconds(5);
  digitalWrite(TRIGGER, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER, LOW);

  long duration = pulseIn(ECHO, HIGH);
  return duration * 0.034 / 2;
}

// =========================
// SETUP
// =========================
void setup() {

  Serial.begin(9600);

  pinMode(LDR, INPUT);
  pinMode(TRIGGER, OUTPUT);
  pinMode(ECHO, INPUT);
  pinMode(LED, OUTPUT);

  // =========================
  // FUZZY SETS
  // =========================

  static FuzzySetIntuitionistic small("Small",
    [](double x){ return triangular_mu(x, 0, 0, 120); }, 0);

  static FuzzySetIntuitionistic mid("Medium",
    [](double x){ return triangular_mu(x, 60, 120, 180); }, 0);

  static FuzzySetIntuitionistic big("Big",
    [](double x){ return triangular_mu(x, 120, 180, 240); }, 0);

  static FuzzySetIntuitionistic verybig("VeryBig",
    [](double x){ return triangular_mu(x, 180, 240, 400); }, 0);

  static FuzzySetIntuitionistic lowldr("Low",
    [](double x){ return triangular_mu(x, 0, 0, 100); }, 0);

  static FuzzySetIntuitionistic midldr("Medium",
    [](double x){ return triangular_mu(x, 60, 200, 700); }, 0);

  static FuzzySetIntuitionistic highldr("High",
    [](double x){ return triangular_mu(x, 400, 700, 1023); }, 0);

  static FuzzySetIntuitionistic off("Off",
    [](double x){ return triangular_mu(x, 0, 0, 0); }, 0);

  static FuzzySetIntuitionistic lowb("Low",
    [](double x){ return triangular_mu(x, 0, 25, 100); }, 0);

  static FuzzySetIntuitionistic midb("Medium",
    [](double x){ return triangular_mu(x, 55, 100, 200); }, 0);

  static FuzzySetIntuitionistic highb("High",
    [](double x){ return triangular_mu(x, 155, 230, 255); }, 0);

  // =========================
  // INPUT / OUTPUT
  // =========================

  static FuzzySetIntuitionistic* distSets[] = { &small, &mid, &big, &verybig };
  static FuzzySetIntuitionistic* lightSets[] = { &lowldr, &midldr, &highldr };
  static FuzzySetIntuitionistic* ledSets[] = { &off, &lowb, &midb, &highb };

  static FuzzyInput distInput(distSets, 4);
  static FuzzyInput lightInput(lightSets, 3);
  static FuzzyOutput ledOutput(ledSets, 4);

  // =========================
  // TABLE DES REGLES
  // =========================

  static int row0[] = {3, 2, 0};
  static int row1[] = {2, 2, 1};
  static int row2[] = {1, 1, 1};
  static int row3[] = {0, 0, 0};

  static int* ruleTable[] = { row0, row1, row2, row3 };

  // 👉 AFFICHAGE TABLE
  printRuleTable(&distInput, &lightInput, &ledOutput, ruleTable);

  fuzzySystem = new FuzzyIntuitionistic(
    &distInput,
    &lightInput,
    &ledOutput,
    ruleTable
  );

  Serial.println("\n=== SYSTEM READY ===");
}

// =========================
// LOOP
// =========================
void loop() {

  double dist = getDistance();
  double light = analogRead(LDR);

  if (dist < 0 || dist > 400 || light > 1023) return;

  fuzzySystem->infer(dist, light);
  double output = fuzzySystem->defuzzify();

  double mu = fuzzySystem->muFinal;
  double nu = fuzzySystem->nuFinal;
  double pi = 1.0 - mu - nu;

  if (pi < 0) pi = 0;

  analogWrite(LED, (int)output);

  Serial.println("\n=== RESULTATS ===");
  Serial.print("Distance: "); Serial.println(dist);
  Serial.print("Light: "); Serial.println(light);
  Serial.print("Output LED: "); Serial.println(output);

  Serial.println("--- INTUITIONISTIC ---");
  Serial.print("mu: "); Serial.println(mu, 4);
  Serial.print("nu: "); Serial.println(nu, 4);
  Serial.print("pi: "); Serial.println(pi, 4);

  delay(500);
}