#include <stdio.h>
#include <string.h>
#include "pico/stdlib.h"
#include "servo.h"
#include "keypad.h"
#include "lcd.h"
#include "hardware/pwm.h"
#include "ir_sensor.h"
#include "ultrasonic.h"


#define TRIG_PIN 3  // GPIO pin connected to TRIG
#define ECHO_PIN 2  // GPIO pin connected to ECHO
#define SERVO_PIN 28
#define BUZZER_PIN 15
#define IR_SENSOR_PIN 26

int main() {
    stdio_init_all();

    // Initialize the servo motor
    servo_init(SERVO_PIN);
    ir_sensor_init_adc(IR_SENSOR_PIN);
    printf("Ultrasonic Sensor Test\n");

    // Initialize the HC-SR04 sensor
    hc_sr04_init(TRIG_PIN, ECHO_PIN);
    // Initialize the keypad
    keypad_init();

    // Initialize the LCD
    lcd_init();
    lcd_print("Enter Password:");

    char password[6] = "12345"; // The correct password
    char input[6] = "";         // Input buffer
    int index = 0;              // Input index

    while (true) {
        char key = keypad_get_key();

        if (key != '\0') { // If a key is pressed
            if (key == 'A') { // Clear input
                index = 0;
                input[0] = '\0';
                lcd_send_byte(LCD_CLEAR, true);
                lcd_print("Enter Password:");
            } else if (key == 'B') { // Delete last character
                if (index > 0) {
                    index--;
                    input[index] = '\0';

                    lcd_send_byte(LCD_CLEAR, true);
                    lcd_print(input);
                }
            } else if (key == 'D') { // Done (check password)
                if (strcmp(input, password) == 0) { // Password matches
                    lcd_send_byte(LCD_CLEAR, true);
                    lcd_print("Access Granted!");
                    printf("Access Granted!\n");
                    play_accepted_sound(BUZZER_PIN);
                    // Rotate the servo motor
                    uint slice_num = pwm_gpio_to_slice_num(SERVO_PIN);
                    rotate_servo(slice_num);

                    // Reset for next input
                    sleep_ms(2000); // Wait for 2 seconds
                    lcd_send_byte(LCD_CLEAR, true);
                    lcd_print("Enter Password:");
                    index = 0;
                    input[0] = '\0';
                } else { // Password incorrect
                    lcd_send_byte(LCD_CLEAR, true);
                    lcd_print("Access Denied!");
                    printf("Access Denied!\n");
                    play_rejected_sound(BUZZER_PIN);
                    // Reset for next input
                    sleep_ms(2000); // Wait for 2 seconds
                    lcd_send_byte(LCD_CLEAR, true);
                    lcd_print("Enter Password:");
                    index = 0;
                    input[0] = '\0';
                }
            } else if (index < 5) { // Add key to input (max 5 characters)
                input[index++] = key;
                input[index] = '\0'; // Null-terminate the string

                lcd_send_byte(LCD_CLEAR, true);
                lcd_print(input);
            }
            uint16_t raw_value = ir_sensor_read_adc();
            float voltage = ir_sensor_get_voltage(raw_value);
            // uint16_t simulated_adc_value = 2048; // Half-scale value for testing
            // float voltage = (simulated_adc_value * 3.3f) / 4095.0f;
            // printf("Simulated IR Sensor Value: %u, Voltage: %.2fV\n", simulated_adc_value, voltage);


            // Display or use the IR sensor value without interrupting password entry
            if (voltage > 0.5) { // Example: Object detected if voltage > 0.5V
                printf("Object Detected! Voltage: %.2fV\n", voltage);
            } else {
                printf("No Object. Voltage: %.2fV\n", voltage);

            }
            
            float distance = measure_distance(TRIG_PIN, ECHO_PIN);

            // Check for timeout or print the distance
            if (distance >= 400.0) {
            printf("No Object Detected (Ultrasonic)\n");
            } else {
                printf("Object Detected! Distance: %.2f cm\n", distance);
            }



    sleep_ms(100); // Delay for debounce
        }

        sleep_ms(100); // Delay for debounce
    }

    return 0;
}