// src/bin/main.rs
#![no_std]
#![no_main]

use defmt::*;
use embassy_executor::Spawner;
use embassy_stm32::i2c::{Config, I2c};
use embassy_time::{Duration, Timer};
use {defmt_rtt as _, panic_probe as _};

#[embassy_executor::main]
async fn main(_spawner: Spawner) {
    info!("tititi");

    let p = embassy_stm32::init(Default::default());

    let mut i2c_cfg = Config::default();
    i2c_cfg.sda_pullup = true;
    i2c_cfg.scl_pullup = true;
    i2c_cfg.timeout = Duration::from_millis(200);

    let mut i2c = I2c::new_blocking(p.I2C3, p.PC0, p.PC1, i2c_cfg);
    let addr = 0x23;

    if let Err(e) = i2c.blocking_write(addr, &[0x01, 0x79]) {
        error!("Nepodarilo sa aktivovať CMM: {:?}", e);
    }

    loop {
        let mut raw = [0u8; 9];
        match i2c.blocking_write_read(addr, &[0x24], &mut raw) {
            Ok(_) => {
                let x = i32::from_be_bytes([raw[0], raw[1], raw[2], 0]) >> 8;
                let y = i32::from_be_bytes([raw[3], raw[4], raw[5], 0]) >> 8;
                let z = i32::from_be_bytes([raw[6], raw[7], raw[8], 0]) >> 8;
                info!("X: {}, Y: {}, Z: {}", x, y, z);
            }
            Err(e) => {
                error!("Chyba pri čítaní: {:?}", e);
            }
        }

        // Čítajte napríklad každých 100ms. Dáta v registroch budú vždy najaktuálnejšie dokončené meranie.
        Timer::after(Duration::from_millis(100)).await;
    }
}