use std::time::Instant;
use esp_idf_hal::delay::FreeRtos;
use esp_idf_hal::ledc::*;
use esp_idf_hal::peripherals::Peripherals;
use esp_idf_hal::prelude::*;

mod lcd;
mod gpio;

const TRIG: u8 = 10;
const ECHO: u8 = 2;

unsafe fn get_distance() -> i32 {
	gpio::write(TRIG, 0);
	FreeRtos::delay_us(2);
	gpio::write(TRIG, 1);
	FreeRtos::delay_us(10);
	gpio::write(TRIG, 0);
	while gpio::read(ECHO) == 0 {}
	let start = Instant::now();
	while gpio::read(ECHO) == 1 {}
	let duration: f32 = start.elapsed().as_micros() as f32;
	let distance: f32 = (duration * 0.034) / 2.0;
	return distance as i32;
}

fn main() -> anyhow::Result<()> {
	let peripherals = Peripherals::take().unwrap();

	let mut channel = LedcDriver::new(
			peripherals.ledc.channel0,
			LedcTimerDriver::new(
					peripherals.ledc.timer0,
					&config::TimerConfig::new().resolution(Resolution::Bits10).frequency(50.Hz().into()),
			)?,
			peripherals.pins.gpio7,
	)?;

	unsafe {
		gpio::output_enable(TRIG);
		lcd::init(16, 3, 255, 4, 8, 9, 18, 19);

		loop {
			let msg = "Distance: ";
			lcd::set_cursor(0, 0);
			for c in msg.chars() { 
				lcd::write(c as u8);
			}

			let distance = get_distance();
			let mut duty = 120.0 * distance as f32 / 400.0;

			if duty < 30.0 {
				duty = 30.0;
			}

			let _ = channel.set_duty(duty as u32);

			let dist_text = distance.to_string();
			lcd::set_cursor(10, 0);
			for c in dist_text.chars() { 
				lcd::write(c as u8);
			}

			let end = "     ";
			for c in end.chars() { 
				lcd::write(c as u8);
			}

			FreeRtos::delay_ms(100);
		}
	}
}