#![no_main]
#![no_std]

extern crate panic_itm;

use stm32h7xx_hal::hal::digital::v2::OutputPin;
use stm32h7xx_hal::{pac, prelude::*};

use cortex_m_rt::entry;

use cortex_m_log::println;
use cortex_m_log::{
    destination::Itm, printer::itm::InterruptSync as InterruptSyncItm,
};

/*
 * I2C Address of the I2C switch (TCA9548ARGER)
 */
const TCA9548ARGER_ADDR     :u8 = 0x72;

/*
 * Control register bit masks
 */
const CHANNEL_0             :u8 = 0x01;
const CHANNEL_1             :u8 = 0x02;
const CHANNEL_2             :u8 = 0x04;
const CHANNEL_3             :u8 = 0x08;
const CHANNEL_4             :u8 = 0x10;
const CHANNEL_5             :u8 = 0x20;
const CHANNEL_6             :u8 = 0x40;
const CHANNEL_7             :u8 = 0x80;

/*
 * I2C Address of slaves at different channels
 */
const TEMP_1_ADDR           :u8 = 0x48;
const TEMP_PRODUCT_ID_REG   :u8 = 0x07;

#[entry]
fn main() -> ! {
    let cp = cortex_m::Peripherals::take().unwrap();
    let dp = pac::Peripherals::take().unwrap();
    let mut log = InterruptSyncItm::new(Itm::new(cp.ITM));

    // Constrain and Freeze power
    // println!(log, "Setup PWR...                  ");
    let pwr = dp.PWR.constrain();
    let vos = pwr.freeze();

    // Constrain and Freeze clock
    // println!(log, "Setup RCC...                  ");
    let rcc = dp.RCC.constrain();
    let ccdr = rcc.sys_ck(100.mhz()).freeze(vos, &dp.SYSCFG);
    let gpiob = dp.GPIOB.split(ccdr.peripheral.GPIOB);
    let gpioe = dp.GPIOE.split(ccdr.peripheral.GPIOE);

    // Configure the SCL and the SDA pin for our I2C bus
    let scl = gpiob.pb8.into_alternate_af4().set_open_drain();
    let sda = gpiob.pb9.into_alternate_af4().set_open_drain();

    let mut i2c =
        dp.I2C1
            .i2c((scl, sda), 100.khz(), ccdr.peripheral.I2C1, &ccdr.clocks);

    // Setup delay
    let mut delay = cp.SYST.delay(ccdr.clocks);
    
    // Configure led
    let mut green = gpiob.pb0.into_push_pull_output();
    let mut yellow = gpioe.pe1.into_push_pull_output();
    let mut red = gpiob.pb14.into_push_pull_output();

    // I2C switch (TCA9548ARGER_ADDR): Only enable channel 5
    let mut tx1 :[u8, 1] = [CHANNEL_5];
    let mut rx1 :[u8; 1] = [0];

    loop {
        i2c.write(TCA9548ARGER_ADDR, &tx1);
        delay.delay_ms(10_u16);
    }

    // Read back the control value
    i2c.read(TCA9548ARGER_ADDR, &mut rx1).unwrap();

    // Match the control register content with the CHANNEL_5 mask
    match rx1[0] {
        CHANNEL_5 => yellow.set_high(),
        _ => yellow.set_low(),
    }.unwrap();

    loop {
        red.set_high().unwrap();
    }

    // delay.delay_ms(100_u16);

    // Temperature sensor (TEMP_1_ADDR): write TEMP_PRODUCT_ID_REG, and read its content (1 byte)
    let mut tx2 :[u8, 1] = TEMP_PRODUCT_ID_REG;
    let mut rx2 :[u8; 1] = [0];
    // i2c.write_read(TEMP_1_ADDR, &tx1.clone(), &mut rx).unwrap();
    i2c.write_read(TCA9548ARGER_ADDR, &tx2.clone(), &mut rx2).unwrap();

    // The ID should be 0xA1.
    match rx2[0] {
        0xA1 => green.set_high(),
        _ => green.set_low(),
    }.unwrap();

    loop {
        red.set_high().unwrap();
    }
}