#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Bounce2.h>
#include <SPI.h>
#include <Wire.h>

#define N
#define R 33
#define L 25
#define gearKnob 32
#define Throttle 34
#define gas 2

#define pushStart 5
int startTrack = 1; //will use to track how many times it has turned off and on
bool pushState = true;

#define joysticklevel 35

#define screen_width 128
#define screen_height 64
Adafruit_SSD1306 display(screen_width, screen_height);

SemaphoreHandle_t displaySemaphore;
SemaphoreHandle_t gasSemaphore;
SemaphoreHandle_t throttleSemaphore;
SemaphoreHandle_t gearSemaphore;

int speed;
float gasLevel;
float adjSpeed;
char gearText[5][1 + 1] = {"P", "R", "N", "D", "L"};
// Task prototypes
void taskScreen(void *pvParameters);
void taskThrottle(void *pvParameters);
void taskGas(void *pvParameters);
void taskGear(void *pvParameters);

void setup() {
  Serial.begin(9600);
  
  // Initialize semaphores
  displaySemaphore = xSemaphoreCreateMutex();
  gasSemaphore = xSemaphoreCreateMutex();
  throttleSemaphore = xSemaphoreCreateMutex();
  gearSemaphore = xSemaphoreCreateMutex();
  
  // Initialize OLED display
  display.begin(SSD1306_SWITCHCAPVCC, 0x3c);
  
  // Create the tasks--------------------------------------------------------
  xTaskCreate(taskScreen, "ScreenTask", 5000, NULL, 1, NULL);
  xTaskCreate(taskThrottle, "ThrottleTask", 1000, NULL, 2, NULL);
  xTaskCreate(taskGas, "GasTask", 1000, NULL, 3, NULL);
  xTaskCreate(taskGear, "GearTask", 1000, NULL, 4, NULL);

 // pinMode(pushStart, INPUT_PULLUP);// going to put this here to keep it easy
  attachInterrupt(pushStart, Ext_INT1_ISR, RISING); // bad name should do find and delete
}

void loop() {
  vTaskDelay(1000); // Delay loop to apparently free CPU time but idk if that is true
}

//start mode ----------------------------------------------------------------
void IRAM_ATTR Ext_INT1_ISR()
{
  startTrack ++;
  if(startTrack > 10)
  {
    display.setTextSize(2);
        display.setTextColor(SSD1306_WHITE);
        display.setCursor(18, 16);
        display.print("Change Oil");//very sloppy need to fix
        display.display();
  }
  if(startTrack % 2)
  {
    pushState = false;
  }
  else
{
    pushState = false;
}
Serial.println("pushstart");
}

// works on screen

void taskScreen(void *pvParameters) {
  for (;;) {
    // get semaphore to access display
    if (xSemaphoreTake(displaySemaphore, portMAX_DELAY) == pdTRUE) {
      // Update OLED display with speed, gas level, and gear
      display.clearDisplay();

      if(pushState = true)
      {

      display.setTextSize(2);
      display.setTextColor(SSD1306_WHITE);
      display.setCursor(0, 0);
      display.print(speedControl());

      display.setTextSize(2);
      display.setCursor(0, 50);
      display.print(gasLevel);

      display.setTextSize(2);
      display.setCursor(0, 25);
      display.print(gearText[getCurrentGear()]);

      display.display();
      }
      else //maybe the worst way to do this becuase the program is still running :(
       {
        display.setTextSize(2);
        display.setTextColor(SSD1306_WHITE);
        display.setCursor(18, 16);
        display.print("Push To    Start");//very sloppy need to fix
        display.display();
       }
      // Release semaphore
      xSemaphoreGive(displaySemaphore);
    }
    vTaskDelay(100); // Delay to control task rate
  }
}

void taskThrottle(void *pvParameters) {
  for (;;) {
    // Read throttle input
    int throttleValue = analogRead(Throttle) / 10;
    
    // Acquire semaphore to update speed
    if (xSemaphoreTake(throttleSemaphore, portMAX_DELAY) == pdTRUE) {
      speed = throttleValue;
      xSemaphoreGive(throttleSemaphore); // Release semaphore
    }
    vTaskDelay(100); // Delay to control task rate
  }
}

// This function reads the gas level input and updates the gasLevel variable
void taskGas(void *pvParameters) {
  for (;;) {
    // Read gas level input
    float gasValue = analogRead(gas);
    
    // Acquire semaphore to update gas level
    if (xSemaphoreTake(gasSemaphore, portMAX_DELAY) == pdTRUE) {
      gasLevel = gasValue; // Need to complete the math on this
      xSemaphoreGive(gasSemaphore); // Release semaphore
    }
    vTaskDelay(100); // Delay to control task rate
  }
}

// This function determines the current gear based on the position of the gear knob
void taskGear(void *pvParameters) {
  for (;;) {
    // Acquire semaphore to get current gear
    if (xSemaphoreTake(gearSemaphore, portMAX_DELAY) == pdTRUE) {
      int currentGear = getCurrentGear();
      xSemaphoreGive(gearSemaphore); // Release semaphore
      
      // Print current gear to serial monitor
      switch (currentGear) {
        case 1:
          Serial.println("Car in Neutral");
          break;
        case 2:
          Serial.println("Car in Reverse");
          break;
        case 3:
          Serial.println("Car in Low");
          break;
        case 4:
          Serial.println("Car is in Drive");
          break;
        default:
          Serial.println("Car is in Park");
          break;
      }
    }
    vTaskDelay(100); // Delay to control task rate
  }
}

// This function determines the current gear based on the position of the gear knob
int getCurrentGear() {
  int gear;
  int gearKnobValue = analogRead(gearKnob);
  
  if (gearKnobValue < 819)
    gear = 0;
  else if (gearKnobValue < 1638)
    gear = 1;
  else if (gearKnobValue < 2457)
    gear = 2;
  else if (gearKnobValue < 3276)
    gear = 3;
  else
    gear = 4;

  return gear;
}

// this reads the analog input and adjusts the speed based on what gear the car is in
int speedControl()
{
  //going to use an else if tree do reduce process time.
  int adjSpeed;

  speed = analogRead(Throttle)/40; //this reads speed all we need to do is add math lol

  if(getCurrentGear() == 1 && speed > 30) //should change to a switch to improve performance
    adjSpeed = 30;
  else if(getCurrentGear() == 0 || getCurrentGear() == 2)// P/N
    adjSpeed = 0;
  else if(getCurrentGear() == 4 && speed > 50) //drive
    adjSpeed = 50;
  else
    adjSpeed = speed;

  return adjSpeed;
  
}




//we are using this to control the actual speed of the car independent of the gas peddle

/*1. The gas peddle input is a potentiometer input with a voltage range of 0 – 5 volts
to the controller.

done

2. The controller interprets the gas peddle input as a request to set the vehicle’s
velocity in MPH where 5 volts is interpreted as 100 MPH.

need to add the math but as good as done

3. The fuel tank input is also a potentiometer with a 0-3.3 volts input.

need to switch it over to 3.3v

4. The fuel tank holds 18 Gallons of fuel when full, where 3.3 represents a full tank.

not added

5. The vehicle’s automatic transmission can handle the following,
a. Neutral – 0 MPH allowed
b. Low – up to 30 MPH allowed
c. Drive – up to 100 MHP allowed
d. Reverse – up to 50 MPH allowed

added 

6. There is key start to the vehicle, represented by a momentary push button.

not added but i would like to replace it with an rfid in the future

7. Pressing the Start button a 2nd time on the fixture will cause the vehicle to
decelerate and stop.

not added

8. When the vehicle is started there is a display that shows
a. Amount of fuel in the vehicle
b. The velocity of the vehicle
c. The gear the transmission is in

the display is set up but have not yet added gears

9. If the controller detects a Stop while the vehicle is moving the display should
show a deceleration and a waring message that the vehicle was turned off while
in motion.

not added

10. The display should display a waring message if the fuel drops below ¼ of a tank.

not added

*/