use esp_idf_hal::delay::FreeRtos;
use esp_idf_hal::gpio::*;
use esp_idf_hal::peripherals::Peripherals;

use esp_idf_hal::ledc::{config::TimerConfig, LedcDriver, LedcTimerDriver, Resolution};
use esp_idf_hal::prelude::*;

fn main() -> anyhow::Result<()> {
    esp_idf_sys::link_patches();

    // Take Peripherals
    let peripherals = Peripherals::take().unwrap();
    let mut led = PinDriver::output(peripherals.pins.gpio7)?;
    
    // Configure and Initialize LEDC Timer Drivers
    let timer_driver0 = LedcTimerDriver::new(
        peripherals.ledc.timer0,
        &TimerConfig::default()
            .frequency(50.Hz())
            .resolution(Resolution::Bits14),
    )
    .unwrap();

    let timer_driver1 = LedcTimerDriver::new(
        peripherals.ledc.timer1,
        &TimerConfig::default()
            .frequency(50.Hz())
            .resolution(Resolution::Bits14),
    )
    .unwrap();

    let timer_driver2 = LedcTimerDriver::new(
        peripherals.ledc.timer2,
        &TimerConfig::default()
            .frequency(50.Hz())
            .resolution(Resolution::Bits14),
    )
    .unwrap();

    let timer_driver3 = LedcTimerDriver::new(
        peripherals.ledc.timer3,
        &TimerConfig::default()
            .frequency(50.Hz())
            .resolution(Resolution::Bits14),
    )
    .unwrap();

    // Configure and Initialize LEDC Drivers
    let mut driver0 = LedcDriver::new(
        peripherals.ledc.channel0,
        timer_driver0,
        peripherals.pins.gpio19,
    )
    .unwrap();

    let mut driver1 = LedcDriver::new(
        peripherals.ledc.channel1,
        timer_driver1,
        peripherals.pins.gpio18,
    )
    .unwrap();

    let mut driver2 = LedcDriver::new(
        peripherals.ledc.channel2,
        timer_driver2,
        peripherals.pins.gpio8,
    )
    .unwrap();

    let mut driver3 = LedcDriver::new(
        peripherals.ledc.channel3,
        timer_driver3,
        peripherals.pins.gpio10,
    )
    .unwrap();

    // Get Max Duty and Calculate Upper and Lower Limits for Servo
    let max_duty = driver0.get_max_duty();
    println!("Max Duty {}", max_duty);
    let min_limit = max_duty * 25 / 1000;
    println!("Min Limit {}", min_limit);
    let max_limit = max_duty * 125 / 1000;
    println!("Max Limit {}", max_limit);

    // Define Starting Position
    driver0
        .set_duty(map(0, 0, 360, min_limit, max_limit))
        .unwrap();

    driver1
        .set_duty(map(90, 0, 360, min_limit, max_limit))
        .unwrap();

    driver2
        .set_duty(map(180, 0, 360, min_limit, max_limit))
        .unwrap();

    driver3
        .set_duty(map(270, 0, 360, min_limit, max_limit))
        .unwrap();
    // Give servo some time to update
    FreeRtos::delay_ms(500);

    loop {
        led.set_high()?;
        FreeRtos::delay_ms(500);
        led.set_low()?;

        // // Sweep from 0 degrees to 180 degrees
        // for angle in 0..30 {
        //     // Print Current Angle for visual verification
        //     println!("Current Angle {} Degrees", angle);
        //     // Set the desired duty cycle
        //     driver0
        //         .set_duty(map(angle, 0, 180, min_limit, max_limit))
        //         .unwrap();
        //     // Give servo some time to update
        //     FreeRtos::delay_ms(12);
        // }

        // led.set_low()?;

        // // Sweep from 180 degrees to 0 degrees
        // for angle in (0..30).rev() {
        //     // Print Current Angle for visual verification
        //     println!("Current Angle {} Degrees", angle);
        //     // Set the desired duty cycle
        //     driver0
        //         .set_duty(map(angle, 0, 180, min_limit, max_limit))
        //         .unwrap();
        //     // Give servo some time to update
        //     FreeRtos::delay_ms(12);
        // }
    }
}

// Function that maps one range to another
fn map(x: u32, in_min: u32, in_max: u32, out_min: u32, out_max: u32) -> u32 {
    (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
}

use embedded_hal::blocking::delay::DelayMs;
use esp_idf_svc::hal::{
    delay::FreeRtos,
    i2c::{I2cConfig, I2cDriver},
    peripherals::Peripherals,
    prelude::*,
};
use icm42670::{Address, Icm42670, PowerMode as imuPowerMode};
use shared_bus::BusManagerSimple;
use shtcx::{self, PowerMode as shtPowerMode};

// Goals of this exercise:
// - Part1: Instantiate i2c peripheral
// - Part1: Implement one sensor, print sensor values
// - Part2: Implement second sensor on same bus to solve an ownership problem

fn main() -> Result<()> {
    esp_idf_svc::sys::link_patches();

    let peripherals = Peripherals::take().unwrap();

    // 1. Instanciate the SDA and SCL pins, correct pins are in the training material.
    let sda = peripherals.pins.gpio10;
    let scl = peripherals.pins.gpio8;
    // 2. Instanciate the i2c peripheral
    let config = I2cConfig::new().baudrate(400.kHz().into());
    let i2c = I2cDriver::new(peripherals.i2c0, sda, scl, &config)?;

    // 3. Instantiate the bus manager, pass the i2c bus.
    let bus = BusManagerSimple::new(i2c);

    // 4. Create two proxies. Now, each sensor can have their own instance of a proxy i2c, which resolves the ownership problem.
    let proxy_1 = bus.acquire_i2c();
    let proxy_2 = bus.acquire_i2c();

    // 5. Change your previous code, so that one of the proxies is passed to the SHTC3, instead of the original i2c bus.
    let mut sht = shtcx::shtc3(proxy_1);

    // 6. Read and print the device ID.
    let device_id = sht.device_identifier().unwrap();
    println!("Device ID SHTC3: {:#02x}", device_id);

    // 7. Create an instance of ICM42670p sensor. Pass the second proxy and the sensor's address.
    let mut imu = Icm42670::new(proxy_2, Address::Primary).unwrap();

    // 8. Read the device's ID register and print the value.
    let device_id = imu.device_id().unwrap();
    println!("Device ID ICM42670p: {:#02x}", device_id);

    // 9. Start the ICM42670p in low noise mode.
    imu.set_power_mode(imuPowerMode::GyroLowNoise).unwrap();

    loop {
        // 10. Read gyro data
        let gyro_data = imu.gyro_norm().unwrap();
        sht.start_measurement(shtPowerMode::NormalMode).unwrap();
        FreeRtos.delay_ms(100u32);
        let measurement = sht.get_measurement_result().unwrap();

        // 11. Print all values
        println!(
            "TEMP: {:.2} °C | HUM: {:.2} % | GYRO: X= {:.2}  Y= {:.2}  Z= {:.2}",
            measurement.temperature.as_degrees_celsius(),
            measurement.humidity.as_percent(),
            gyro_data.x,
            gyro_data.y,
            gyro_data.z,
        );

        FreeRtos.delay_ms(500u32);
    }
}