#include "main.h" // Include the main header file

void app_main()
{
    lcd_i2c_handle_t display = { 
        .address = 0x27, // I2C address of the LCD
        .num = I2C_NUM_1, // I2C port number
        .backlight = 1, // Backlight on
        .size = DISPLAY_16X02};
    ;
    i2c_init();

    setup_gpio();

    lcd_i2c_init(&display);
    vTaskDelay(500 / portTICK_PERIOD_MS);

    int index = 0;
    char input[5] = {0}; // extra byte for null-terminator

    while (1)
    {
        for (int i = 0; i <= 20; i++)
        {
            if (i <= 20)
            {

                lcd_i2c_cursor_set(&display, 0, 0); // Move the cursor to beginning of first line
                lcd_i2c_print(&display, "Enter Code:"); 
                vTaskDelay(1000 / portTICK_PERIOD_MS);
                const char correctCode[] = "8329"; // predefined correct code
 
                char key = scan_keypad(); // scan keypad for key press
                if (key != '\0') // key pressed
                {
                    input[index++] = key; // store key in input array and increment index
                    for (int i = 0; i < index; i++)
                    {
                        
                        lcd_i2c_cursor_set(&display, 11 + i, 0); // Move the cursor to necessary position after "Enter Code:"
                        lcd_i2c_print(&display, "*");            // Print asterisk for each entered digit
                        fflush(stdout);                          // Ensure immediate output  
                        if (index == SEQUENCE_LENGTH)
                        {
                            input[index] = '\0'; // null-terminate string before comparing
                            if (strcmp(input, correctCode) == 0)
                            {
                                lcd_i2c_write(&display, 0, CLEAR_DISPLAY);
                                lcd_i2c_cursor_set(&display, 0, 0);
                                lcd_i2c_print(&display, "Access Granted");

                                gpio_set_level(leds[0], 1);      // turn on green LED
                                vTaskDelay(pdMS_TO_TICKS(5000)); // keep LED on for 5 seconds
                                gpio_set_level(leds[0], 0);      // turn off green LED
                                lcd_i2c_write(&display, 0, CLEAR_DISPLAY);
                                vTaskDelay(10 / portTICK_PERIOD_MS); // Short delay for clear to take effect
                                for (int i = 0; i <= 2; i++)
                                {
                                    ESP_ERROR_CHECK(i2c_send_data_block(MPU6050_PWR_MGMT_1, 0x00));
                                    vTaskDelay(100 / portTICK_PERIOD_MS);
                                    uint8_t data[14];
                                    ESP_ERROR_CHECK(i2c_read_bytes(MPU6050_ACCEL_XOUT_H, data, sizeof(data))); // read 14 bytes of data starting from ACCEL_XOUT_H

                                    int16_t ax = (data[0] << 8) | data[1];   // accel x
                                    int16_t ay = (data[2] << 8) | data[3];   // accel y
                                    int16_t az = (data[4] << 8) | data[5];   // accel z
                                    int16_t temp = (data[6] << 8) | data[7]; // temperature
                                    int16_t gx = (data[8] << 8) | data[9];   // gyro x
                                    int16_t gy = (data[10] << 8) | data[11]; // gyro y
                                    int16_t gz = (data[12] << 8) | data[13]; // gyro z
                                    float tempC = (temp / 340.0 + 36.53);

                                    vTaskDelay(50 / portTICK_PERIOD_MS); // Short delay before printing

                                    printf("Accel: %d %d %d\n", ax, ay, az);
                                    printf("Gyro: %d %d %d\n", gx, gy, gz);
                                    printf("Temp: %.2f C\n", tempC);
                                    vTaskDelay(3000 / portTICK_PERIOD_MS);

                                    vTaskDelay(30 / portTICK_PERIOD_MS); // Short delay for clear to take effect

                                    lcd_i2c_cursor_set(&display, 0, 0); // Move the cursor to beginning of first line
                                    lcd_i2c_print(&display, "Accel: %d", ax);

                                    lcd_i2c_cursor_set(&display, 0, 1); // Move to second line
                                    lcd_i2c_print(&display, "%d %d", ay, az);
                                    printf("here 1");

                                    vTaskDelay(3000 / portTICK_PERIOD_MS);
                                    lcd_i2c_write(&display, 0, CLEAR_DISPLAY); // Clear display
                                    vTaskDelay(30 / portTICK_PERIOD_MS);

                                    lcd_i2c_cursor_set(&display, 0, 0); // Move the cursor to beginning of first line
                                    lcd_i2c_print(&display, "TempC: %.2fC", tempC);
                                    vTaskDelay(3000 / portTICK_PERIOD_MS);
                                    lcd_i2c_write(&display, 0, CLEAR_DISPLAY); // Clear display
                                    vTaskDelay(30 / portTICK_PERIOD_MS);
                                    lcd_i2c_cursor_set(&display, 0, 0); // Move the cursor to beginning of first line
                                    lcd_i2c_print(&display, "Gyro:%d", gx / 10);
                                    printf("here 2");

                                    lcd_i2c_cursor_set(&display, 0, 1);
                                    lcd_i2c_print(&display, "%d:%d", gy / 10, gz / 10); // display gyro y and z divided by 10 to fit the screen
                                    vTaskDelay(3000 / portTICK_PERIOD_MS);
                                    lcd_i2c_write(&display, 0, CLEAR_DISPLAY); // Clear display
                                    vTaskDelay(30 / portTICK_PERIOD_MS);
                                    lcd_i2c_cursor_set(&display, 0, 0); // Move the cursor to beginning of first line
                                    lcd_i2c_print(&display, "                "); // clear line
                                    lcd_i2c_cursor_set(&display, 0, 1); // Move the cursor to beginning of second line
                                    lcd_i2c_print(&display, "                "); // clear line
                                    lcd_i2c_cursor_set(&display, 0, 0);
                                    lcd_i2c_print(&display, "TempC: %.2fC", tempC); 
                                    vTaskDelay(3000 / portTICK_PERIOD_MS);
                                    lcd_i2c_write(&display, 0, CLEAR_DISPLAY); // Clear display
                                    vTaskDelay(30 / portTICK_PERIOD_MS);
                                    printf("here 3");
                                }
                                vTaskDelay(10 / portTICK_PERIOD_MS); // Short delay for clear to take effect
                                break;
                            }
                            else
                            {
                                lcd_i2c_write(&display, 0, CLEAR_DISPLAY); // Clear display
                                lcd_i2c_cursor_set(&display, 0, 0); // Move the cursor to beginning of first line
                                lcd_i2c_print(&display, "Access denied"); 
                                gpio_set_level(leds[1], 1);      // turn on red LED
                                vTaskDelay(pdMS_TO_TICKS(2000)); // keep LED on for 2 seconds
                                gpio_set_level(leds[1], 0);      // turn off red LED
                                lcd_i2c_write(&display, 0, CLEAR_DISPLAY); // Clear display
                                vTaskDelay(10 / portTICK_PERIOD_MS); // Short delay for clear to take effect
                                break;                               // exit the while loop after incorrect code
                            }
                        }
                        key = 0;                      // set to remove possible overflow
                        if (index >= SEQUENCE_LENGTH) // reset index when it exceeds max length
                        {
                            index = 0; // reset index
                        }
                    }
                    vTaskDelay(250 / portTICK_PERIOD_MS);
                }
                else // no key pressed
                {
                    break; // exit for loop if no key pressed
                    printf("hello");
                }
            }
        }
    }
}