#include <stdio.h>
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/gpio.h"

// Define the GPIO pin 
#define RIGHT_GREEN_BUTTON_GPIO_PIN 23    //For connection request sw
#define RED_BUTTON_GPIO_PIN 22      //For forward motion sw
#define BLUE_BUTTON_GPIO_PIN 21     //For backward motion sw
#define YELLOW_BUTTON_GPIO_PIN 15   //For left motion sw
#define GREY_BUTTON_GPIO_PIN 16     //For right motion sw
#define WHITE_BUTTON_GPIO_PIN 5     //For stop motion sw
#define LEFT_GREEN_BUTTON_GPIO_PIN 32 //For flashlight sw

#define FLASHLIGHT_LED_GPIO_PIN 33 //For flashlight led
#define CONNECTION_LED_GPIO_PIN 25 //For connection req. led
#define FORWARD_MOTION_LED_GPIO_PIN 26 //For forward motion led 
#define BACKWARD_MOTION_LED_GPIO_PIN 27 //For backward motion led
#define LEFT_MOTION_LED_GPIO_PIN 4 //For left motion led
#define RIGHT_MOTION_LED_GPIO_PIN 2 //For right motion led
#define STOP_MOTION_LED_GPIO_PIN 0  //For stop motion led

#define RS 12   // Register Select pin
#define E  13   // Enable pin
#define D4 14   // Data pin 4
#define D5 17   // Data pin 5
#define D6 18   // Data pin 6
#define D7 19   // Data pin 7

// LCD Commands
#define LCD_CLEAR 0x01
#define LCD_HOME 0x02
#define LCD_ENTRY_MODE 0x06
#define LCD_DISPLAY_ON 0x0C
#define LCD_FUNCTION_SET 0x28
#define LCD_SET_CURSOR 0x80


#define switch_debou_eff 250

int cmd_acpt_status = 0; //init, not accepting cmds from phone
int count = 0;
int flashlight_status = 0; //init, flashlight is Turn OFF

void io_init();
void lcd_send_command(uint8_t command);
void lcd_send_data(uint8_t data);
void lcd_init();
void lcd_print(const char* str);
void lcd_allclear();
void lcd_welcomeMESG();
void delay(int time);
void track_motion();


void app_main(void)
{

  io_init(); //Initilize the Switch
  lcd_init(); // Initialize the LCD
  
  lcd_welcomeMESG();

    while (1)
    {
        if (gpio_get_level(RIGHT_GREEN_BUTTON_GPIO_PIN) == 0) 
        {
          delay(switch_debou_eff);
          printf("Right side Green Button Pressed!\n\n\n");
          cmd_acpt_status = !cmd_acpt_status; //ready to accept cmds from phone 
          delay(50);
        }

        if (gpio_get_level(LEFT_GREEN_BUTTON_GPIO_PIN) == 0) 
        {
          delay(switch_debou_eff);
          printf("Left side Green Button Pressed!\n\n\n");
          flashlight_status = !flashlight_status; //turn on/off flashlight 
          delay(50);
        }

        if (cmd_acpt_status == 1)
        {
          if(count == 0)
          {
            lcd_allclear();
            lcd_print("Connected ");
            gpio_set_level(CONNECTION_LED_GPIO_PIN, 1); //turn ON connection req. led
            count++;
          }
          track_motion();
        }
        else
        {
          lcd_allclear();
          lcd_print("Disconnected ");
          gpio_set_level(CONNECTION_LED_GPIO_PIN, 0); //turn OFF connection req. led
          
          gpio_set_level(FORWARD_MOTION_LED_GPIO_PIN, 0); 
          gpio_set_level(BACKWARD_MOTION_LED_GPIO_PIN, 0);
          gpio_set_level(LEFT_MOTION_LED_GPIO_PIN, 0); 
          gpio_set_level(RIGHT_MOTION_LED_GPIO_PIN, 0); 
          gpio_set_level(STOP_MOTION_LED_GPIO_PIN, 0); 
          count = 0;
        }

        if (flashlight_status == 1)
        {
          gpio_set_level(FLASHLIGHT_LED_GPIO_PIN, 1); //turn ON flashlight
          // printf("Flashlight is turn: ON\n\n");
        }
        else
        {
          gpio_set_level(FLASHLIGHT_LED_GPIO_PIN, 0); //turn OFF flashlight
          // printf("Flashlight is turn: OFF\n\n");
        }


        // Add a small delay to avoid reading too fast
        vTaskDelay(500 / portTICK_PERIOD_MS);
    }
}


void track_motion()
{
  //switch init and pull mode
    gpio_set_direction(RED_BUTTON_GPIO_PIN, GPIO_MODE_INPUT);
    gpio_set_direction(BLUE_BUTTON_GPIO_PIN, GPIO_MODE_INPUT);
    gpio_set_direction(YELLOW_BUTTON_GPIO_PIN, GPIO_MODE_INPUT);
    gpio_set_direction(GREY_BUTTON_GPIO_PIN, GPIO_MODE_INPUT);
    gpio_set_direction(WHITE_BUTTON_GPIO_PIN, GPIO_MODE_INPUT);

    gpio_set_pull_mode(RED_BUTTON_GPIO_PIN, GPIO_PULLUP_ONLY);
    gpio_set_pull_mode(BLUE_BUTTON_GPIO_PIN, GPIO_PULLUP_ONLY);
    gpio_set_pull_mode(YELLOW_BUTTON_GPIO_PIN, GPIO_PULLUP_ONLY);
    gpio_set_pull_mode(GREY_BUTTON_GPIO_PIN, GPIO_PULLUP_ONLY);
    gpio_set_pull_mode(WHITE_BUTTON_GPIO_PIN, GPIO_PULLUP_ONLY);

    int red_button_state = gpio_get_level(RED_BUTTON_GPIO_PIN);
    int blue_button_state = gpio_get_level(BLUE_BUTTON_GPIO_PIN);
    int yellow_button_state = gpio_get_level(YELLOW_BUTTON_GPIO_PIN);
    int gery_button_state = gpio_get_level(GREY_BUTTON_GPIO_PIN);
    int white_button_state = gpio_get_level(WHITE_BUTTON_GPIO_PIN);

            if (red_button_state == 0)
            {
                delay(switch_debou_eff);
                printf("Red Button Pressed!\n");
                lcd_allclear();
                lcd_print("GO Forward ");
                gpio_set_level(FORWARD_MOTION_LED_GPIO_PIN, 1); 
                gpio_set_level(BACKWARD_MOTION_LED_GPIO_PIN, 0);
                gpio_set_level(LEFT_MOTION_LED_GPIO_PIN, 0); 
                gpio_set_level(RIGHT_MOTION_LED_GPIO_PIN, 0); 
                gpio_set_level(STOP_MOTION_LED_GPIO_PIN, 0); 
                printf("Going Forward!\n\n");
            }
            else if (blue_button_state == 0)
            {
                delay(switch_debou_eff);
                printf("Blue Button Pressed!\n");
                lcd_allclear();
                lcd_print("GO Backword ");
                gpio_set_level(FORWARD_MOTION_LED_GPIO_PIN, 0); 
                gpio_set_level(BACKWARD_MOTION_LED_GPIO_PIN, 1);
                gpio_set_level(LEFT_MOTION_LED_GPIO_PIN, 0); 
                gpio_set_level(RIGHT_MOTION_LED_GPIO_PIN, 0); 
                gpio_set_level(STOP_MOTION_LED_GPIO_PIN, 0); 
                printf("Going Backword!\n\n");
            }
            else if (yellow_button_state == 0)
            {
                delay(switch_debou_eff);
                printf("Yellow Button Pressed!\n");
                lcd_allclear();
                lcd_print("GO Left ");
                gpio_set_level(FORWARD_MOTION_LED_GPIO_PIN, 0); 
                gpio_set_level(BACKWARD_MOTION_LED_GPIO_PIN, 0);
                gpio_set_level(LEFT_MOTION_LED_GPIO_PIN, 1); 
                gpio_set_level(RIGHT_MOTION_LED_GPIO_PIN, 0); 
                gpio_set_level(STOP_MOTION_LED_GPIO_PIN, 0); 
                printf("Going Left!\n\n");
            }
            else if (gery_button_state == 0)
            {
                delay(switch_debou_eff);
                printf("Gery Button Pressed!\n");
                lcd_allclear();
                lcd_print("GO Right ");
                gpio_set_level(FORWARD_MOTION_LED_GPIO_PIN, 0); 
                gpio_set_level(BACKWARD_MOTION_LED_GPIO_PIN, 0);
                gpio_set_level(LEFT_MOTION_LED_GPIO_PIN, 0); 
                gpio_set_level(RIGHT_MOTION_LED_GPIO_PIN, 1); 
                gpio_set_level(STOP_MOTION_LED_GPIO_PIN, 0); 
                printf("Going Right!\n\n");
            }
            else if (white_button_state == 0)
            {
                delay(switch_debou_eff);
                printf("White Button Pressed!\n");
                lcd_allclear();
                lcd_print("STOP ");
                gpio_set_level(FORWARD_MOTION_LED_GPIO_PIN, 0); 
                gpio_set_level(BACKWARD_MOTION_LED_GPIO_PIN, 0);
                gpio_set_level(LEFT_MOTION_LED_GPIO_PIN, 0); 
                gpio_set_level(RIGHT_MOTION_LED_GPIO_PIN, 0); 
                gpio_set_level(STOP_MOTION_LED_GPIO_PIN, 1); 
                printf("STOPED!\n\n");
            }
}


void io_init()
{
    // Set the GPIO pin direction as input
    gpio_set_direction(RIGHT_GREEN_BUTTON_GPIO_PIN, GPIO_MODE_INPUT);
    gpio_set_direction(LEFT_GREEN_BUTTON_GPIO_PIN, GPIO_MODE_INPUT);

    // Enable the pull-up resistor on the button GPIO pin
    gpio_set_pull_mode(RIGHT_GREEN_BUTTON_GPIO_PIN, GPIO_PULLUP_ONLY);
    gpio_set_pull_mode(LEFT_GREEN_BUTTON_GPIO_PIN, GPIO_PULLUP_ONLY);

    // Set the GPIO pin direction as output
    gpio_set_direction(FLASHLIGHT_LED_GPIO_PIN, GPIO_MODE_OUTPUT);
    gpio_set_direction(CONNECTION_LED_GPIO_PIN, GPIO_MODE_OUTPUT);

    gpio_set_direction(FORWARD_MOTION_LED_GPIO_PIN, GPIO_MODE_OUTPUT); //red led
    gpio_set_direction(BACKWARD_MOTION_LED_GPIO_PIN, GPIO_MODE_OUTPUT); //blue led
    gpio_set_direction(LEFT_MOTION_LED_GPIO_PIN, GPIO_MODE_OUTPUT); //yellow led
    gpio_set_direction(RIGHT_MOTION_LED_GPIO_PIN, GPIO_MODE_OUTPUT); //red led
    gpio_set_direction(STOP_MOTION_LED_GPIO_PIN, GPIO_MODE_OUTPUT); //red led
}


void lcd_allclear()
{
  lcd_send_command(LCD_CLEAR);
  vTaskDelay(175/ portTICK_PERIOD_MS); // Wait after clearing
  // Print something new on the cleared screen
  lcd_send_command(LCD_SET_CURSOR); // Reset cursor to (0,0)
}


void lcd_welcomeMESG()
{
  lcd_print("Hello, ADMIN! ");
  delay(1000);
  lcd_allclear();
  lcd_print("Ready.....! ");
  printf("LCD Initialized\n"); // Debug info
  delay(500);
}

void lcd_send_command(uint8_t command) 
{
    gpio_set_level(RS, 0); // Command mode
    gpio_set_level(E, 1);
    gpio_set_level(D4, (command >> 4) & 0x01);
    gpio_set_level(D5, (command >> 5) & 0x01);
    gpio_set_level(D6, (command >> 6) & 0x01);
    gpio_set_level(D7, (command >> 7) & 0x01);
    gpio_set_level(E, 0);
    delay(1);
    
    gpio_set_level(E, 1);
    gpio_set_level(D4, command & 0x01);
    gpio_set_level(D5, (command >> 1) & 0x01);
    gpio_set_level(D6, (command >> 2) & 0x01);
    gpio_set_level(D7, (command >> 3) & 0x01);
    gpio_set_level(E, 0);
    delay(1);

    // Additional delay for the clear command
    if (command == LCD_CLEAR) {
        delay(2); // Increase to 2ms for clear
    }
}

void lcd_send_data(uint8_t data) 
{
    gpio_set_level(RS, 1); // Data mode
    gpio_set_level(E, 1);
    gpio_set_level(D4, (data >> 4) & 0x01);
    gpio_set_level(D5, (data >> 5) & 0x01);
    gpio_set_level(D6, (data >> 6) & 0x01);
    gpio_set_level(D7, (data >> 7) & 0x01);
    gpio_set_level(E, 0);
    delay(1);
    
    gpio_set_level(E, 1);
    gpio_set_level(D4, data & 0x01);
    gpio_set_level(D5, (data >> 1) & 0x01);
    gpio_set_level(D6, (data >> 2) & 0x01);
    gpio_set_level(D7, (data >> 3) & 0x01);
    gpio_set_level(E, 0);
    delay(1);
}

void lcd_init() 
{
    gpio_set_direction(RS, GPIO_MODE_OUTPUT);
    gpio_set_direction(E, GPIO_MODE_OUTPUT);
    gpio_set_direction(D4, GPIO_MODE_OUTPUT);
    gpio_set_direction(D5, GPIO_MODE_OUTPUT);
    gpio_set_direction(D6, GPIO_MODE_OUTPUT);
    gpio_set_direction(D7, GPIO_MODE_OUTPUT);

    delay(20); // Wait for LCD to power up
    lcd_send_command(0x33); // Initialization sequence
    delay(5); // Wait before next command
    lcd_send_command(0x32); // Set to 4-bit mode
    delay(5); // Wait before next command
    lcd_send_command(LCD_FUNCTION_SET); // 4-bit mode, 2-line, 5x8 dots
    delay(5); // Wait before next command
    lcd_send_command(LCD_DISPLAY_ON);    // Display ON
    delay(5); // Wait before next command
    lcd_send_command(LCD_CLEAR);          // Clear display
    delay(5); // Wait before next command
    lcd_send_command(LCD_ENTRY_MODE);     // Entry mode
}

void lcd_print(const char* str) 
{
    while(*str){
        lcd_send_data(*str++); // Use lcd_send_data for printing characters
    }
}

void delay(int time)
{
    vTaskDelay(time / portTICK_PERIOD_MS); // Wait before next command
}