#![no_std]
#![no_main]
#![feature(type_alias_impl_trait)]

use core::sync::atomic::{AtomicU16, AtomicU32, Ordering};

use defmt::*;
use embassy_executor::Spawner;
//use embassy_futures::join::join;
use embassy_futures::select::*;
use embassy_rp::adc::{self, Adc};
use embassy_rp::peripherals::*;
use embassy_time::{Duration, Timer};
use embassy_rp::gpio::{Input, Level, Output, Pull, Flex};
use embassy_rp::i2c;
use embassy_rp::interrupt;
//use embedded_hal_async::i2c::I2c;
use embassy_embedded_hal::SetConfig;
use embassy_rp::uart::{self, Parity};
use embassy_rp::pwm::{self, Pwm};
use {defmt_rtt as _, panic_probe as _};
use heapless::String;

use heizung::i2c_scan::{self, ScanResultForBus};
use heizung::modbus_server::{ModbusServer, ModbusRegisters, ModbusErrorCode};
use heizung::rs485::RS485;

#[embassy_executor::task]
async fn i2c_task(mut i2c_peri: embassy_rp::peripherals::I2C0,
        mut scl: embassy_rp::peripherals::PIN_13, mut sda: embassy_rp::peripherals::PIN_12,
        mut i2c_irq: embassy_rp::interrupt::I2C0_IRQ, i2c_config: i2c::Config) {
    let mut prev_scan_result = ScanResultForBus::new();
    loop {
        if true {
            let _i2c = i2c::I2c::new_async(&mut i2c_peri, &mut scl, &mut sda, &mut i2c_irq, i2c_config);
        }

        let scan_result = ScanResultForBus::scan(&mut scl, &mut sda).await;
        if prev_scan_result != scan_result {
            prev_scan_result = scan_result;
            const MSG_LEN : usize = i2c_scan::SCAN_RESULT_STRING_LENGTH;
            let mut msg = String::<MSG_LEN>::new();
            match scan_result.format(&mut msg) {
                Ok(()) => info!("I2C scan result:\n{}", msg),
                Err(()) => info!("I2C scan result: too long for our buffer!"),
            }
        }

        Timer::after(Duration::from_secs(1)).await;
    }
}

#[embassy_executor::task]
async fn beeper_task(pwm_channel: embassy_rp::peripherals::PWM_CH3, beeper_pin: embassy_rp::peripherals::PIN_6) {
    let mut c: pwm::Config = Default::default();
    c.top = 0x8000;
    c.compare_a = 8;
    c.divider = fixed::FixedU16::from_num(20);
    let mut pwm = Pwm::new_output_a(pwm_channel, beeper_pin, c.clone());

    let mut toggle = false;
    loop {
        info!("PWM: duty={}/32768, divider={}", c.compare_a, c.divider.to_bits());
        Timer::after(Duration::from_millis(200)).await;
        //c.compare_a = c.compare_a.rotate_left(4);
        c.compare_a = if toggle { c.top/2 } else { 0 };
        //toggle = !toggle;
        c.divider -= fixed::FixedU16::from_num(0.1);
        if c.divider < 1 {
            c.divider = fixed::FixedU16::from_num(20);
        }
        toggle = true;
        pwm.set_config(&c);
    }
}

#[embassy_executor::task]
async fn uart_task(this: RS485<ModbusServer<ModBusRegs<'static>>>) {
    this.run_task().await;
}

struct AdcData {
    current1: AtomicU16,
    current2: AtomicU16,
    analog_in1_raw: AtomicU16,
    analog_in1_millivolt: AtomicU16,
    vcc: AtomicU16,
}

fn convert_to_celsius(raw_temp: u16) -> f32 {
    // According to chapter 4.9.5. Temperature Sensor in RP2040 datasheet
    27.0 - (raw_temp as f32 * 3.0522 / 4096.0 - 0.706) / 0.001721 as f32
}

#[embassy_executor::task]
async fn adc_task(shared_data: &'static AdcData, adc: ADC, en_measure_current: Output<'static, PIN_3>,
    mut analog_in1: PIN_26, mut current2: PIN_27, mut current1: PIN_28, mut measure_vcc: PIN_29) {
    let irq = interrupt::take!(ADC_IRQ_FIFO);
    let mut adc = Adc::new(adc, irq, adc::Config::default());

    loop {
        let level = adc.read(&mut analog_in1).await;
        shared_data.analog_in1_raw.store(level, Ordering::Release);
        let value = 3.05*level as f32/4096. * (10.+2.*5.1)/5.1;
        shared_data.analog_in1_millivolt.store((value * 1000.) as u16, Ordering::Release);
        info!("ADC analog_in1: {} -> {}", level, value);

        let level = adc.read(&mut current2).await;
        info!("ADC current2: {}", level);
        let level = adc.read(&mut current1).await;
        info!("ADC current1: {}", level);

        let level = adc.read(&mut measure_vcc).await;
        let vcc = 3.05 * level as f32 / 4096. * (100. + 10.)/10.;
        info!("ADC measure_vcc: {} -> {}", level, vcc);

        let temp = adc.read_temperature().await;
        info!("Temp: {} -> {} degrees", temp, convert_to_celsius(temp));
        Timer::after(Duration::from_secs(1)).await;
    }
}
struct ModBusRegs<'a> {
    led_g: Output<'a, PIN_5>,
    button_boot2: Input<'a, PIN_11>,
    reed3: Input<'a, PIN_19>,
    reed4: Input<'a, PIN_20>,
    reed2: Input<'a, PIN_21>,
    reed1: Input<'a, PIN_22>,
    adc_data: &'a AdcData,
}

impl<'a> ModbusRegisters for ModBusRegs<'a> {
    fn read_discrete_input(self: &mut Self, addr: u16) -> Result<bool, ModbusErrorCode> {
        match addr {
            0 => Ok(self.reed1.is_high()),
            1 => Ok(self.reed2.is_high()),
            2 => Ok(self.reed3.is_high()),
            3 => Ok(self.reed4.is_high()),
            4 => Ok(self.button_boot2.is_low()),
            _ => Err(ModbusErrorCode::IllegalDataAddress),
        }
    }

    fn read_holding_register(self: &mut Self, addr: u16) -> Result<u16, ModbusErrorCode> {
        if addr == 1 {
            return Ok(42)
        }
        Err(ModbusErrorCode::IllegalDataAddress)
    }

    fn read_input_register(self: &mut Self, addr: u16) -> Result<u16, ModbusErrorCode> {
        match addr {
            0 => Ok(42),
            1 => Ok(self.adc_data.analog_in1_raw.load(Ordering::Relaxed)),
            2 => Ok(self.adc_data.analog_in1_millivolt.load(Ordering::Relaxed)),
            _ => Err(ModbusErrorCode::IllegalDataAddress)
        }
    }

    fn write_coil(self: &mut Self, addr: u16, value: bool) -> Result<(), ModbusErrorCode> {
        match addr {
            0 => { self.led_g.set_level(Level::from(value)); Ok(()) },
            _ => Err(ModbusErrorCode::IllegalDataAddress),
        }
    }

    fn write_register(self: &mut Self, addr: u16, value: u16) -> Result<u16, ModbusErrorCode> {
        Err(ModbusErrorCode::IllegalDataAddress)
    }
}

#[embassy_executor::main]
async fn main(spawner: Spawner) {
    let p = embassy_rp::init(Default::default());
    info!("starting");

    // pinout:
    let mut _drive2 = Output::new(p.PIN_0, Level::Low);
    let mut _drive1 = Output::new(p.PIN_1, Level::Low);
    //let LED_W = p.PIN_2;
    let _ws2812_display = p.PIN_2;
    //let LED_Y = p.PIN_3;
    let en_measure_current = Output::new(p.PIN_3, Level::Low);
    let mut led_b = Output::new(p.PIN_4, Level::Low);
    let led_g = Output::new(p.PIN_5, Level::Low);
    let led_r = p.PIN_6;
    let _matrix_in1 = Input::new(p.PIN_7, Pull::Up);
    let _sbu1 = p.PIN_8;
    let _sbu2 = p.PIN_9;
    let _vbus_det = p.PIN_10;
    let button_boot2 = Input::new(p.PIN_11, Pull::Up);
    let sda = p.PIN_12;
    let scl = p.PIN_13;
    let _ws2811_red_in = Input::new(p.PIN_14, Pull::None);
    let tx_en = p.PIN_15;
    let tx = p.PIN_16;
    let rx = p.PIN_17;
    let _matrix_in2 = Input::new(p.PIN_18, Pull::Up);
    let reed3 = Input::new(p.PIN_19, Pull::Up);
    let reed4 = Input::new(p.PIN_20, Pull::Up);
    let reed2 = Input::new(p.PIN_21, Pull::Up);
    let reed1 = Input::new(p.PIN_22, Pull::Up);
    let _matrix_in3 = Input::new(p.PIN_23, Pull::Up);
    let _digout1 = p.PIN_24;
    let _digout2 = p.PIN_25;
    let analog_in1 = p.PIN_26;
    let current2 = p.PIN_27;
    let current1 = p.PIN_28;
    let measure_vcc = p.PIN_29;

    let i2c_irq = interrupt::take!(I2C0_IRQ);
    let mut i2c_config = i2c::Config::default();
    i2c_config.frequency = 400_000;
    unwrap!(spawner.spawn(i2c_task(p.I2C0, scl, sda, i2c_irq, i2c_config)));

    static ADC_DATA: AdcData = AdcData {
        current1: AtomicU16::new(0), current2: AtomicU16::new(0), analog_in1_raw: AtomicU16::new(0),
        analog_in1_millivolt: AtomicU16::new(0), vcc: AtomicU16::new(0)
    };
    unwrap!(spawner.spawn(adc_task(&ADC_DATA, p.ADC, en_measure_current, analog_in1, current2, current1, measure_vcc)));

    // use 19200 baud in 8E1 mode - not great but it's the Modbus default
    let mut uart_config = uart::Config::default();
    uart_config.baudrate = 19200;
    uart_config.parity = Parity::ParityEven;
    let rs485 = RS485::new(
        p.UART0, rx, tx, tx_en, interrupt::take!(UART0_IRQ), p.DMA_CH1, uart_config,
        p.PIO0, p.DMA_CH0, p.DMA_CH2,
        ModbusServer::new(ModBusRegs {
            led_g,
            button_boot2,
            reed1, reed2, reed3, reed4,
            adc_data: &ADC_DATA,
        }),
    );
    unwrap!(spawner.spawn(uart_task(rs485)));

    if false {
        unwrap!(spawner.spawn(beeper_task(p.PWM_CH3, led_r)));
    }

    loop {
        led_b.set_high();
        Timer::after(Duration::from_secs(1)).await;

        led_b.set_low();
        Timer::after(Duration::from_secs(1)).await;
    }
}