use i2cdev::core::{I2CMessage, I2CTransfer};
use i2cdev::linux::{LinuxI2CDevice, LinuxI2CMessage};
use std::thread;
use std::time::Duration;

pub fn read_from_bus(
    i2c_device: &str,
    slave_address: u16,
    memory_address: u8,
    data_length: usize,
) -> Result<Vec<u8>, std::io::Error> {
    let mut data = vec![0; data_length];

    let mut dev = LinuxI2CDevice::new(i2c_device, slave_address)?;
    let mut msgs = [
        // Address to read from
        LinuxI2CMessage::write(&[memory_address]),
        // Read data into buffer
        LinuxI2CMessage::read(&mut data),
    ];

    match dev.transfer(&mut msgs) {
        Ok(_) => Ok(data),
        Err(e) => Err(e.into()),
    }
}

pub fn write_to_bus(
    i2c_device: &str,
    slave_address: u16,
    memory_address: u8,
    data: &[u8],
) -> Result<(), std::io::Error> {
    let mut dev = LinuxI2CDevice::new(i2c_device, slave_address)?;

    // Write 8 bytes at a time
    for (i, chunk) in data.chunks_exact(8).enumerate() {
        // Memory address
        let address = memory_address + (i as u8) * 8;
        let mut msgs = [
            // Address to write to
            LinuxI2CMessage::write(&[address]),
            // Write data to chip
            LinuxI2CMessage::write(chunk),
        ];

        dev.transfer(&mut msgs)?;

        thread::sleep(Duration::from_millis(20));
    }

    Ok(())
}