#include <vector>
#include <unordered_map>
#include <cmath>

// Main
bool Gear_Speed_Ready;
bool Gear_RPM_Ready;
uint8_t gear_speed;
uint16_t gear_rpm;
uint8_t Gear_PID;

// Global variables and constants
float actualRatio = 0.0f;
size_t closestIndex = 0;
constexpr int ratioArrayMax = 3;  // set to 3 for easy testing, always use odd for mode
constexpr int MAX_GEARS = 6;
constexpr float constsDeviation = 6.0f;
constexpr float lookupDeviation = 6.0f;

std::vector<float> ratioArray;
std::vector<float> constArray;

// Function prototypes
void gears();
void gearPrep();
void gearConsts(float currentRatio);
void gearLookup(float currentRatio);

void gears() {
  // Read input from serial monitor for gear_speed and gear_rpm
  Serial.println("Enter rpm and speed (e.g., 120 10):");
  while (Serial.available() == 0) {
    // Wait for input
  }
  String input = Serial.readStringUntil('\n');
  sscanf(input.c_str(), "%hu %hhu", &gear_rpm, &gear_speed);


  Gear_Speed_Ready = Gear_RPM_Ready = true; // Set flags to true after receiving input

  gearPrep();
  gearLookup(actualRatio);
}

void gearPrep() {
  if (!Gear_Speed_Ready || !Gear_RPM_Ready) {
    return;
  }
  
  if (gear_speed < 10 || gear_rpm == 0) {
    ratioArray.clear();
    Gear_PID = 0;
    
    Gear_Speed_Ready = Gear_RPM_Ready = false; // Reset flags after processing.
    return; // Exit the function after handling this condition.
  }

  float currentRatio = static_cast<float>(gear_rpm) / static_cast<float>(gear_speed);
  ratioArray.push_back(currentRatio);
  Gear_Speed_Ready = Gear_RPM_Ready = false; // Reset flags for the next measurement

  if (ratioArray.size() >= ratioArrayMax) {
    std::unordered_map<float, int> counts;
    int maxCount = 0;
    float maxRatio = 0.0f;

    for (float ratio : ratioArray) {
      int count = ++counts[ratio];
      if (count > maxCount) {
        maxCount = count;
        maxRatio = ratio;
      }
    }

    actualRatio = maxRatio; // Set the most frequently occurring ratio
    ratioArray.clear();
    gearConsts(actualRatio);
  }
}

void gearConsts(float currentRatio) {
    if (constArray.size() >= MAX_GEARS) {
        Serial.println("Maximum number of gears reached. Cannot add new gear ratio.");
        return;
    }

    // Check if constArray is empty, if so, add the current ratio as a new gear ratio
    if (constArray.empty()) {
        constArray.push_back(currentRatio);
        Serial.print("New gear ratio set: ");
        Serial.println(currentRatio);
        return; // Exit the function
    }

// Check if currentRatio is close to or larger than the last gear ratio
if (std::fabs(constArray.back() - currentRatio) <= constsDeviation) {
    Serial.println("Condition 1: Current ratio is close to the last gear ratio.");
    Serial.println(currentRatio);
    return; // Return without adding the ratio
}

// Check if currentRatio is larger than the last gear ratio
if (currentRatio > constArray.back()) {
    Serial.println("Condition 2: Current ratio is larger than the last gear ratio.");
    Serial.println(currentRatio);
    return; // Return without adding the ratio
}


    // Add the current ratio as a new gear ratio
    constArray.push_back(currentRatio);
    Serial.print("New gear ratio set: ");
    Serial.println(currentRatio);

    // Print the current gear ratios
    Serial.println("Current gear ratios:");
    for (size_t i = 0; i < constArray.size(); ++i) {
        Serial.print("Gear Ratio ");
        Serial.print(i + 1);
        Serial.print(": ");
        Serial.println(constArray[i]);
    }
}


void gearLookup(float currentRatio) {
  // Early exit if there are no gear ratios to compare against
  if (constArray.empty()) {
    // Serial.println("No gear ratios available for lookup.");
    return;
  }

  for (size_t i = 0; i < constArray.size(); ++i) {
    if (std::fabs(constArray[i] - currentRatio) <= lookupDeviation) {
      closestIndex = i + 1; // Shift to 1-based indexing at assignment
      Serial.print("Gear Matched: Gear ");
      Serial.println(closestIndex);
      Gear_PID = i + 1;
      return;
    }
  }


  Serial.println("No closest gear ratio found.");
}

void setup() {
  // Initialize serial communication
  Serial.begin(9600);
}

void loop() {
  gears(); // Call the function to test with serial input
}