#include "pico/stdlib.h"
#include "hardware/pwm.h"
#include "hardware/gpio.h"

#define PIR_SENSOR_PIN 17
#define LED1_PIN 20

#define TRIG_PIN 18
#define ECHO_PIN 19

#define PWM_LED_PIN 16

// Function to measure distance
float measure_distance_cm() {
    gpio_put(TRIG_PIN, 0);
    sleep_us(2);
    gpio_put(TRIG_PIN, 1);
    sleep_us(10);
    gpio_put(TRIG_PIN, 0);

    // Wait for echo high
    while (gpio_get(ECHO_PIN) == 0);
    absolute_time_t start = get_absolute_time();

    // Wait for echo low
    while (gpio_get(ECHO_PIN) == 1);
    absolute_time_t end = get_absolute_time();

    int64_t pulse_time = absolute_time_diff_us(start, end);
    float distance = pulse_time / 58.0f;  // Convert to cm

    return distance;
}

// Convert distance to PWM duty (0–100%)
uint16_t distance_to_pwm(float distance_cm) {
    const float min_dist = 2.0f;
    const float max_dist = 100.0f;

    if (distance_cm < min_dist) distance_cm = min_dist;
    if (distance_cm > max_dist) distance_cm = max_dist;

    float brightness = 1.0f - ((distance_cm - min_dist) / (max_dist - min_dist));
    return (uint16_t)(brightness * 65535);
}

int main() {
    stdio_init_all();

    // PIR Setup
    gpio_init(PIR_SENSOR_PIN);
    gpio_set_dir(PIR_SENSOR_PIN, GPIO_IN);
    gpio_pull_down(PIR_SENSOR_PIN);

    gpio_init(LED1_PIN);
    gpio_set_dir(LED1_PIN, GPIO_OUT);

    // Ultrasonic Setup
    gpio_init(TRIG_PIN);
    gpio_set_dir(TRIG_PIN, GPIO_OUT);
    gpio_put(TRIG_PIN, 0);

    gpio_init(ECHO_PIN);
    gpio_set_dir(ECHO_PIN, GPIO_IN);

    // PWM LED2 Setup
    gpio_set_function(PWM_LED_PIN, GPIO_FUNC_PWM);
    uint slice = pwm_gpio_to_slice_num(PWM_LED_PIN);
    pwm_set_wrap(slice, 65535);
    pwm_set_enabled(slice, true);

    while (true) {
        // PIR sensor logic
        bool motion = gpio_get(PIR_SENSOR_PIN);
        gpio_put(LED1_PIN, motion);

        // Distance & brightness control
        float dist = measure_distance_cm();
        uint16_t duty = distance_to_pwm(dist);
        pwm_set_gpio_level(PWM_LED_PIN, duty);

        sleep_ms(200);
    }
}