#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <freertos/queue.h>
#include <freertos/semphr.h>

// Define task handles
TaskHandle_t i2cTaskHandle, wifiTaskHandle, gpsTaskHandle, sdTaskHandle;

TaskHandle_t scheduler_task_handle;

// Define a queue handle for notifications
QueueHandle_t notificationQueue;

// Define shared state variables
struct WiFiState {
  bool performingOta;
  bool performingSync;
  bool performingMaintenance;
  bool syncApAvailable;
  bool maintApAvailable;
  bool otaApAvailable;
  // Add other WiFi-related state variables as needed
};

struct I2CState {
  // Add I2C-related state variables as needed
};

struct GPSTaskState {
  // Add GPS-related state variables as needed
};

struct SDTaskState {
  // Add SD-related state variables as needed
};

// Define job return codes
enum JobReturnCode {
  JOB_COMPLETE,
  JOB_RUNNING,
  JOB_QUISCENT,
};

// Define job function signature
typedef JobReturnCode (*JobFunction)(void*);

// Define job structure
struct Job {
  JobFunction function;
  TickType_t nextExecutionTime;
  bool allowDeepSleep;
  bool allowLightSleep;
};

// Define task-specific states
WiFiState wifiState;
I2CState i2cState;
GPSTaskState gpsState;
SDTaskState sdState;

// Initialize tasks
void initializeTasks() {
  // Create tasks and pass state variables
  xTaskCreate(i2cTask, "I2CTask", 2048, &i2cState, 3, &i2cTaskHandle);
  xTaskCreate(wifiTask, "WiFiTask", 2048, &wifiState, 3, &wifiTaskHandle);
  xTaskCreate(gpsTask, "GPSTask", 2048, &gpsState, 3, &gpsTaskHandle);
  xTaskCreate(sdTask, "SDTask", 2048, &sdState, 3, &sdTaskHandle);
}

// Scheduler main loop
void schedulerLoop(void* pvParameters) {
  while (true) {
    // Scheduler logic to determine the next job to run
    Job nextJob = getNextJob();

    // Dispatch the job to the corresponding task
    dispatchJob(nextJob);

    // Check the states of all tasks and set sleep mode accordingly
    setSleepModeBasedOnTaskStates();

    // If sleep mode is possible, set sleep and wait
    if (sleepModeIsPossible()) {
      enterSleepMode();
    } else {
      // If sleep is not possible, wait until the earliest task is able to execute
      waitForNextRunnableTask();
    }
  }
}

// Function to dispatch a job to the appropriate task
void dispatchJob(Job job) {
  
  if (taskCanAcceptJob(job)) {
    xTaskNotifyGive(taskHandle);  // Notify the task to run the job
  }
}

// I2C task
void i2cTask(void* pvParameters) {
  while (true) {
    ulTaskNotifyTake(pdTRUE, portMAX_DELAY);  // Wait for notification from scheduler

    // Execute I2C-related jobs and steps
    // ...

    vTaskDelay(1000 / portTICK_PERIOD_MS);  // Adjust as needed
  }
}

// WiFi task
void wifiTask(void* pvParameters) {
  while (true) {
    ulTaskNotifyTake(pdTRUE, portMAX_DELAY);  // Wait for notification from scheduler

    // Execute WiFi-related jobs and steps
    // ...

    vTaskDelay(1000 / portTICK_PERIOD_MS);  // Adjust as needed
  }
}

// GPS task
void gpsTask(void* pvParameters) {
  while (true) {
    ulTaskNotifyTake(pdTRUE, portMAX_DELAY);  // Wait for notification from scheduler

    // Execute GPS-related jobs and steps
    // ...

    vTaskDelay(1000 / portTICK_PERIOD_MS);  // Adjust as needed
  }
}

// SD task
void sdTask(void* pvParameters) {
  while (true) {
    ulTaskNotifyTake(pdTRUE, portMAX_DELAY);  // Wait for notification from scheduler

    // Execute SD-related jobs and steps
    // ...

    vTaskDelay(1000 / portTICK_PERIOD_MS);  // Adjust as needed
  }
}

// Function to get the task handle for a job
TaskHandle_t getTaskHandleForJob(Job job) {
  return job.handle;
}

// Function to check if a task can accept a new job
bool taskCanAcceptJob(Job job) {
  // Figure out
}

// Function to get the next job to run based on the schedule table
Job getNextJob() {
  Job nextJob;


  return nextJob;
}

// Function to set sleep mode based on the states of all tasks
void setSleepModeBasedOnTaskStates() {
  // Implement logic to set sleep mode based on task states
  // ...
}

// Function to check if sleep mode is possible
bool sleepModeIsPossible() {
  // Implement logic to check if sleep mode is possible
  // ...
}

// Function to enter sleep mode
void enterSleepMode() {
  // Implement logic to enter sleep mode
  // ...
}

// Function to wait for the next task to be runnable
void waitForNextRunnableTask() {
  // Implement logic to wait for the next task to be runnable
  // ...
}

// Example of a job function
JobReturnCode wifiStatusJob(void* pvParameters) {
  // Implement WiFi status job logic
  // ...

  // Example of setting the next execution time
  // job.nextExecutionTime = someTime;

  return JOB_COMPLETE;
}

// Other job functions...
void setup(){ 

}



void loop() {
  // Initialize tasks and scheduler
  initializeTasks();


  // Create the scheduler task
  xTaskCreate(schedulerLoop, "SchedulerTask", 4096, NULL, 3, &scheduler_task_handle);
 
}