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

#define BUZZER_PIN 15      // Buzzer GPIO pin
#define SERVO_PIN 28       // Servo GPIO pin
#define IR_SENSOR_ADC_PIN 26 // IR sensor ADC pin
#define TRIG_PIN 3         // Ultrasonic sensor TRIG pin
#define ECHO_PIN 2         // Ultrasonic sensor ECHO pin

void setup_all() {
    // Initialize all peripherals
    stdio_init_all();
    buzzer_init(BUZZER_PIN);
    servo_init(SERVO_PIN);
    ir_sensor_init_adc(IR_SENSOR_ADC_PIN);
    lcd_init();
    hc_sr04_init(TRIG_PIN, ECHO_PIN);

    // Display initialization message on the LCD
    lcd_print("System Ready!");
    sleep_ms(2000); // Pause to show the message
    lcd_send_byte(LCD_CLEAR, true); // Clear the LCD for further use
}

int main() {
    setup_all();

    printf("Unified System Initialized\n");

    while (true) {
        // **1. Read and Display IR Sensor Data**
        uint16_t ir_raw_value = ir_sensor_read_adc();
        float ir_voltage = ir_sensor_get_voltage(ir_raw_value);
        char ir_message[32];
        sprintf(ir_message, "IR: %.2fV", ir_voltage);
        lcd_print(ir_message);
        printf("IR Sensor: %.2fV\n", ir_voltage);
        sleep_ms(1000);
        lcd_send_byte(LCD_CLEAR, true);

        // **2. Measure and Display Ultrasonic Sensor Distance**
        float distance = measure_distance(TRIG_PIN, ECHO_PIN);
        char us_message[32];
        if (distance < 0) {
            sprintf(us_message, "No Obj Detected");
            printf("Ultrasonic: Timeout! No object detected.\n");
        } else {
            sprintf(us_message, "Distance: %.2f cm", distance);
            printf("Ultrasonic: %.2f cm\n", distance);
        }
        lcd_print(us_message);
        sleep_ms(1000);
        lcd_send_byte(LCD_CLEAR, true);

        

        // **4. Play a Buzzer Tone**
        printf("Playing Buzzer Tone...\n");
        lcd_print("Playing Buzzer Tone...");
        play_tone(BUZZER_PIN, 1000, 500); // Play 1kHz tone for 500ms
        sleep_ms(500);

        // **5. Simple Buzzer ON/OFF**
        buzzer_on(BUZZER_PIN);
        sleep_ms(500);
        buzzer_off(BUZZER_PIN);

        // **3. Rotate Servo Motor**
        printf("Rotating Servo...\n");
        uint slice_num = pwm_gpio_to_slice_num(SERVO_PIN);
        rotate_servo(slice_num);

        // Repeat the loop
        printf("Cycle Completed\n");
    }

    return 0;
}