// Code to store lookup table in PROGMEM to save RAM.
// Lookup table is used to determine correct PWM for given measured value mValue.
// Using a lookup table, is WAY faster than calculating a higher order polynomial.
// (Time is equal if the function is quadratic, but if it's 3rd or 4th order there
// is no comparison).

// Define lookup tables. Size needs to be 10x2, otherwise to function arguments 
// to read it need to be adjusted.

// Run simulation to determine voltage divider. Voltage should be around 3-3.3V.
// Also need to keep in mind the energy that is dipersed through the resistors.
// Maximum current (I) through a 1/4W resitor (R) is defined by sqr(1/(4*R))
// Maximum voltage (V) through a 1/4W resitor (R) is defined by sqr(R/4)

// For a sensor that reads between 40 and 245 ohms, a R1 of 160 ohms works great.
// Maximum allowed current through 160 ohm resistor is 39 mA. Maximum allowed
// through 245 ohm sensor is 32 mA. Maximum current through voltage divider is 25 mA.
// Maximum voltage at ESP32 pin is 3.0 Volts. For ESP32, linear region is between 0.15-2.5V

// When it comes to filtering sensor data, definetly add capacitor.
// Running average may not be enough, may require median filter, followed by low pass filter.

// Online simulator:
// https://www.falstad.com/circuit

/*
   5V
   |
   |
   |
  |-|
  | | R1
  |_|
   |
   |          _____
   ----------|_____|------ ESP32 pin
   |         R2, 10K
   |
  |-|
  |/| Variable resistor (Sensor)
  |_|
   |
   |
   |
  GND


*/

// To measure battery voltage/charging voltage:
// R1 = 1.5K
// Sensor (regular resistor or small pot to calibrate) = 330
// Add 10 uF capacitor accross 330 resistor

// If running out of PWM channels, look into khoih-prog/ESP32_PWM library.
// This library utilizes a single time to run 16 channels, however maximum
// PWM frequency is limited to 500Hz which should be fine for LEDs and 
// probably the gauges.


// First column - measured value
// Second column - PWM for analog gauge
// Third column - real value for digital gauge
// Fuel sensor range 40-245 ohms
// With 5V, R1 = 250 ohms (experimentally determined)
// Voltage range is 2.5V low - 0.75V full
// With 3.3V, R1 = 82 ohms
// Maximum current 30 mA through voltage divider
const int fuelL_LUT[10][3] PROGMEM = {
  {0, 0, 0},
  {500, 15, 0}, // reserve
  {800, 25, 12},
  {1200, 40, 25}, 
  {1600, 55, 38}, 
  {2000, 75, 50}, 
  {2400, 98, 63}, 
  {2800, 120, 75}, 
  {3200, 135, 87},
  {3800, 220, 100}
};

// If oil pressure above 15, gauge should show within normal range.
// If oil pressure lower than 10, it should show as low.
// Pressure sensor range 10-184 ohms; 80 PSI maximum of sender
// R1 = 180 ohms
// 26mA maximum current through voltage divider
const int oilP_LUT[10][3] PROGMEM = {
  {0, 0, 0},
  {200, 13, 10},  // Low
  {750, 27, 20},  // Acceptable
  {1200, 40, 30}, 
  {1600, 52, 40}, 
  {2000, 65, 50}, 
  {2400, 78, 60}, 
  {2800, 90, 70}, 
  {3200, 105, 80}, 
  {3800, 130, 100}
};

// Sensor value, PWM, real pressure
// No gauge movement till water is above 120 degrees
// Temperature sensor range 287-23 ohms; 250 degrees maximum of sensor
// R1 = 280 ohms
// 18mA maximum current through voltage divider
const int waterT_LUT[10][3] PROGMEM = {
  {0, 0, 0},
  {400, 0, 60},
  {800, 15, 120},  // gauge should start moving
  {1200, 20, 140}, 
  {1600, 30, 160}, 
  {2000, 40, 180},  // cold
  {2400, 70, 200},  // thermostat open
  {2800, 90, 220},  // perfect
  {3200, 175, 240}, // little warm
  {3800, 255, 260}  // overheating
};

// Dashboard lights controlled by headlight knob
// Knob position, PWM
// Slower increase at lower settings
const int dashB_LUT[10][2] PROGMEM = {
  {0, 0},
  {400, 50},
  {800, 60},
  {1200, 70}, 
  {1600, 80}, 
  {2000, 100}, 
  {2400, 120}, 
  {2800, 140}, 
  {3200, 190}, 
  {3800, 240}
};

// Speed, requested speedo motor revolutions
// 5 revolutions in 45 ms (3-4mph on gauge) is absolute minimum, need to add if statement to set limit
const int speed_LUT[10][2] PROGMEM = {
  {4, 5},   // 45 ms sample time
  {10, 16}, // 45 ms sample time; 7 pulses at 20ms
  {15, 10}, // 20ms
  {25, 18}, 
  {35, 25}, 
  {45, 33}, 
  {65, 47}, 
  {75, 54}, 
  {85, 62}, 
  {100, 74}
};

#include <CD74HC4067.h>

#define SAMPLES 150  // Number of samples for running average

class RunningAverage {
  private:
    int samples[SAMPLES];  // Array to hold past samples
    int index = 0;  // Index to keep track of the most recent sample
    int count = 0;  // Count of samples added

  public:
    // Add a new sample to the buffer and return the new average
    int addSample(int sample) {
      // Add new sample to the array
      samples[index] = sample;
      index = (index + 1) % SAMPLES;  // Increment the index
      if (count < SAMPLES) count++;  // Increment the count

      // Calculate the sum of the samples
      int sum = 0;
      for(int i = 0; i < count; i++) {
        sum += samples[i];
      }

      // Calculate and return the average
      return sum / count;
    }
};


RunningAverage fuelLevelAvg;
RunningAverage oilPressureAvg;
RunningAverage waterTempAvg;
RunningAverage brightnessPotAvg;
RunningAverage voltageAvg;
RunningAverage currSpeedAvg;

// For lookupTable function
int PWMvalue, realValue;

// For digital gauges
int realFuelL, realOilP, realWaterT, desiredRev;
float realV;
unsigned long sTimeFast, sTimeSlow;

// Measured values
int mValueF, mValueO, mValueW, mValueB, mValueV, curr_speed;

// Define pins
//byte fuelLin_pin  = 1;
byte fuelLon_pin  = 2;   // Sends voltage to fuel sender; prolongs sender life
//byte oilPin_pin   = 3;
//byte waterTin_pin = 4;
byte dashBin_pin  = 5;   // Move to other EPS32
byte lightSens_pin= 6;
//byte volt_in_pin  = 7;
byte rpm_in_pin   = 8;
byte speed_in_pin = 9;

byte fuelLout_pin   = 10;
byte oilPout_pin    = 11;
byte waterTout_pin  = 12;
byte dashBout_pin   = 12;   // Move to other ESP32
byte screenBout_pin = 13;
byte speed_out_pin  = 14;

// Pin that's connected to signal pin of multiplexer
const byte sigPin = 7;

// Set constructor pins
const byte addressPin0 = 18;
const byte addressPin1 = 17;
const byte addressPin2 = 16;
//const byte addressPin3 = 15; // setting this pin to -1 omits it. This leaves channels 0-7. Unused pins need to go to GND

// variable to keep track of the current channel
//int currentChannel = 0;

// Set number of channels to check (C0 to C15)
//byte maxChan = 4;

// create an instance of the CD74HC4067
CD74HC4067 test_mux(addressPin0, addressPin1, addressPin2, -1);


void setup() {
  Serial.begin(115200);
  Serial.println("Hello, ESP32-S3!");

  pinMode (addressPin0, OUTPUT);
  pinMode (addressPin1, OUTPUT);
  pinMode (addressPin2, OUTPUT);
  //pinMode (addressPin3, OUTPUT);

  // Set input pins
  //pinMode(fuelLin_pin, INPUT);
  //pinMode(oilPin_pin, INPUT);
  //pinMode(waterTin_pin, INPUT);
  pinMode(dashBin_pin, INPUT);
  pinMode(lightSens_pin, INPUT);
  //pinMode(volt_in_pin, INPUT);
  pinMode(rpm_in_pin, INPUT);
  pinMode(fuelLon_pin, OUTPUT); // Sends voltage to fuel sender; prolongs sender life


  // Setup PWM for output pins
  /*ledcSetup(0, 4000, 8);
  ledcAttachPin(fuelLout_pin, 0);

  ledcSetup(0, 4000, 8);
  ledcAttachPin(oilPout_pin, 1);

  ledcSetup(0, 4000, 8);
  ledcAttachPin(waterTout_pin, 2);

  // Move to other ESP32
  ledcSetup(0, 4000, 8);
  ledcAttachPin(dashBout_pin, 0);

  // Speedo drive motor settings
  ledcSetup(0, 4000, 8);
  ledcAttachPin(speed_out_pin, 3);
  */

  sTimeFast = millis();
  sTimeSlow = millis();
} // end void setup

void loop() {
  processAnalogData();
  delay(10);
} // end void loop


template <size_t N>
void lookupTable(const int Array[][N], int mValue, int numColumns) {
  for(int i = 0; i < 9; i++) {
    if(mValue >= pgm_read_word_near(&Array[i][0]) && mValue <= pgm_read_word_near(&Array[i+1][0])) {
      // Linear interpolation formula: y = y1 + ((x - x1) * (y2 - y1)) / (x2 - x1)
      PWMvalue = pgm_read_word_near(&Array[i][1]) + ((mValue - pgm_read_word_near(&Array[i][0])) * (pgm_read_word_near(&Array[i+1][1]) - pgm_read_word_near(&Array[i][1]))) / (pgm_read_word_near(&Array[i+1][0]) - pgm_read_word_near(&Array[i][0]));
      if(numColumns == 3) {
        realValue = pgm_read_word_near(&Array[i][2]) + ((mValue - pgm_read_word_near(&Array[i][0])) * (pgm_read_word_near(&Array[i+1][2]) - pgm_read_word_near(&Array[i][2]))) / (pgm_read_word_near(&Array[i+1][0]) - pgm_read_word_near(&Array[i][0]));
      }
      break;
    }
  }
} // end void lookupTable


// Function to process analog inputs
void processAnalogData(){
    test_mux.channel(0);
    delay(500);
    mValueO = analogRead(sigPin);
    //Serial.print("Channel 0, value: ");
    //Serial.println(mValueO);
    //delay(1000);
    int mValueO_avg = oilPressureAvg.addSample(mValueO);
    
    test_mux.channel(1);
    delay(500);
    mValueW = analogRead(sigPin);
    //Serial.print("Channel 1, value: ");
    //Serial.println(mValueW);
    //delay(1000);
    int mValueW_avg = waterTempAvg.addSample(mValueW);

    // Get voltage
    test_mux.channel(2);
    delay(500);
    mValueV = analogRead(sigPin);
    //Serial.print("Channel 2, value: ");
    //Serial.println(mValueV);
    //delay(1000);
    int mValueV_avg = voltageAvg.addSample(mValueV);
    
  // Gauges are update once a second
  if (millis()-sTimeFast > 1000){
    lookupTable(oilP_LUT, mValueO_avg, 3);
    ledcWrite(oilPout_pin, PWMvalue);
    realOilP = realValue;
    Serial.print("Oil Pressure: ");
    Serial.println(realOilP);

    lookupTable(waterT_LUT, mValueW_avg, 3);
    ledcWrite(waterTout_pin, PWMvalue);
    realWaterT = realValue;
    Serial.print("Water Temperature: ");
    Serial.println(realWaterT);

    realV = mValueV_avg*3.3*5.545/4095;
    Serial.print("Voltage: ");
    Serial.println(realV);
    
    // Get Mileage
    sTimeFast = millis();
  }

  // Every 5 seconds
  
    //digitalWrite(fuelLon_pin, HIGH);
  test_mux.channel(3);
  delay(500);
  mValueF = analogRead(sigPin);
  //Serial.print("Channel 3, value: ");
  //Serial.println(mValueF);
  //delay(1000);
  int mValueF_avg = fuelLevelAvg.addSample(mValueF);

  if (millis()-sTimeSlow > 5000){
    lookupTable(fuelL_LUT, mValueF_avg, 3);
    ledcWrite(fuelLout_pin, PWMvalue);
    realFuelL = realValue;
    Serial.print("Fuel level: ");
    Serial.println(realFuelL);

    //digitalWrite(fuelLon_pin, LOW);
    sTimeSlow = millis();
  }

  // Move this to other ESP32
  // Dash brightness (based on pot) is updated ASAP
  mValueB = analogRead(dashBin_pin);
  int mValueB_avg = brightnessPotAvg.addSample(mValueB);
  lookupTable(dashB_LUT, mValueB_avg, 2);
  ledcWrite(dashBout_pin, PWMvalue);

  // Move this to other ESP32
  // Screen brightness
  ledcWrite(screenBout_pin, PWMvalue);

  
  // Get Speed
  curr_speed = 55; // get curr_speed in mph
  int curr_speed_avg = currSpeedAvg.addSample(curr_speed);
  lookupTable(speed_LUT, curr_speed_avg, 2);
  desiredRev = realValue;
  ledcWrite(speed_out_pin, desiredRev); // Feed desiredRev into PID controller

  // Get RPM

} // end void processAnalogData