temporary switch from SPI to I2C, more math, logic analyzer

// motor-controller::core0
#[embassy_executor::task]
pub async fn i2c_task(
    mut device: I2cSlave<'static, crate::i2c::I2cPeripheral>,
    mut sms: SmDriverBatch,
) {
    info!("Spawned core0 executor and telemetry task!");
    
    let mut write_buffer = [0u8; 2];
    loop {
        match device.listen(&mut write_buffer).await {
            // Ignore erronous partial write err arising from two-byte write
            Ok(i2c_slave::Command::Write(_)) | Err(i2c_slave::Error::PartialWrite(_)) => {
                info!("recieved the following command: {:08b}", write_buffer);
                write_dshot(&mut sms, write_buffer).await;
            },
            Ok(_) => warn!("Received erroneous i2c instruction!"),
            Err(error) => error!("Error in reading I2C: Error {}", error)
        }
    }
}

// motor-controller::i2c
macro_rules! define_i2c_config {
    (
        peripheral: $i2c_peripheral:ty,
        scl_pin: $scl_pin:ty,
        sda_pin: $sda_pin:ty,
        addr: $addr:expr,
        general_call: $general_call:expr,
        scl_pullup: $scl_pullup:expr,
        sda_pullup: $sda_pullup:expr,
    ) => {
        // Asserts that the types of the given SLC pin, SDA, and I2C Peripheral are valid
        assert_impl!($scl_pin: SclPinTrait<$i2c_peripheral>);
        assert_impl!($sda_pin: SdaPinTrait<$i2c_peripheral>);

        pub type I2cPeripheral = $i2c_peripheral;

        /// Gets the correct peripherals based on configured I2C
        #[macro_export]
        macro_rules! get_i2c_peripherals {
            ($peripherals:ident) => {
                pastey::paste! { ($peripherals.[<$i2c_peripheral>], $peripherals.[<$scl_pin>], $peripherals.[<$sda_pin>]) }
            }
        }
        
        /// Binds the i2c interrupt corresponding to the provided `i2c_peripheral`
        #[macro_export]
        macro_rules! bind_i2c_interrupt {
            () => {
                pastey::paste! {
                    bind_interrupts!(struct I2cIrq {
                        [<$i2c_peripheral _IRQ>] => embassy_rp::i2c::InterruptHandler<$i2c_peripheral>;
                    });
                } 
            }
        }

        /// Initilizes a new [`i2c_slave::Config`] object given the config values set in config module
        pub fn new() -> i2c_slave::Config {
            let mut config = i2c_slave::Config::default();
            config.addr = $addr;
            config.general_call = $general_call;
            config.scl_pullup = $scl_pullup;
            config.sda_pullup = $sda_pullup;

            config
        }
    };
}

define_i2c_config! {
    peripheral: I2C0,
    scl_pin: PIN_5,
    sda_pin: PIN_4,
    addr: 0x60,
    general_call: false,
    scl_pullup: false,
    sda_pullup: false,
}

#[instrument]
pub async fn interface_handler() {
    info!("Started I2C interface handler!");
    let i2c = config::i2c::new()
        .unwrap_or_else(|err| panic!("Failed to initalize I2C peripheral! Error: {err}"));

    // Small overhead of atmoics when using only one sensor, but we'll always use more in normal operation; changing isn't worth additional code complexity.
    let i2c_refcell = AtomicCell::new(i2c);

    // setup of other I2C peripherals here...

    let mut motor_controller_handle = AtomicDevice::new(&i2c_refcell);

    let write_buffer: [u8; 2];

    if let Some(frame) = DShotFrame::<StandardDShotVariant>::from_throttle(512, true) {    
        write_buffer = frame.inner().to_le_bytes();
        info!("Fake frame to write: Raw: [{:08b}, {:08b}]", write_buffer[0], write_buffer[1]);
    } else {
        panic!("Failed to construct DShot command frame! Throttle exceeded maximum value!");
    }

    // let write_buffer = [0xFF, 0x00, 0xFF];
    loop {
        if let Err(err) = motor_controller_handle.write(config::motor_controller::I2C_ADDR, &write_buffer) {
            match err {
                AtomicError::Busy => error!("Driver requirments not met for i2c synchronization!"),
                AtomicError::Other(i2c_err) => error!("I2c error in motor controller write! Error {i2c_err}")
            }
        }
        // get data from other i2c sensors here...
    }
}

// motor-controller::dshot
fn set_sm_config<'d, PIO: Instance, const SM: usize> (
    sm: &mut StateMachine<'d, PIO, SM>,
    prg: &embassy_rp::pio::LoadedProgram<'d, PIO>,
    pin: &Pin<'d, PIO>
) {
    let mut config = pio::Config::<PIO>::default();

    // 🚫 Nothing present

    // ✅ Forgot to tell the specific state state machine to use the program, 
    // not just loading into the PIO block memory.
    config.use_program(prg, &[]); 

    config.clock_divider = PIO_CLOCK_DIVIDER;
    
    config.set_set_pins(&[pin]);
    config.set_out_pins(&[pin]);

    sm.set_config(&config);
}

// motor-controller::core0
async fn write_dshot(sms: &mut SmDriverBatch, buffer: [u8; 2]) {
    let first_byte = buffer[0];

    if let Ok(command) = DShotCommand::try_from(first_byte) {
        // Handle as command
        for_each_driver!(sms, |driver| {
            driver.write_command(command, true).await.unwrap_or_else(|err| {
                error!("Error while writing DShot command to PIOs. Error: {}", err);
            });
        });
    } else {
        // Handle as throttle
        let raw = u16::from_le_bytes([buffer[0], buffer[1]]);
        
        // 🚫 Subtracted 48 from the raw, so.
        // checked_sub() subtracts the function argument from the value it is called on
        // Its not a wrapping function like checked_sub(raw - 48)
        // So this would just always yield 48
        let Some(throttle) = raw.checked_sub(raw - 48) else {
            error!("Invalid raw value: {}", raw);
            return;
        };

        // ✅ Should instead just supply 48 to the function argument
        let Some(throttle) = raw.checked_sub(48) else {
            error!("Invalid raw value: {}", raw);
            return;
        };

        for_each_driver!(sms, |driver| {
            driver.write_throttle(throttle, true).await.unwrap_or_else(|err| {
                error!("Error while writing Dshot throttle to PIOs. Error {}", err);
            });
        });
    }
}

pub fn generate_blink_program() -> Program<4> {
    let mut a = Assembler::new();
    
    let mut loop_label = a.label();
    
    a.bind(&mut loop_label);
    a.set(SetDestination::PINDIRS, 1);           // Set pin to OUTPUT
    a.set_with_delay(SetDestination::PINS, 1, 31); // Set HIGH, delay 32 cycles
    a.set_with_delay(SetDestination::PINS, 0, 31); // Set LOW, delay 32 cycles  
    a.jmp(pio::JmpCondition::Always, &mut loop_label); // Loop forever
    
    a.assemble_program()
}

pub fn test_pio_blink<'d, P: embassy_rp::pio::PioPin>(
    pio0: &mut Pio<'d, PIO0>,
    pin: embassy_rp::Peri<'d, P>
) {
    let program = generate_blink_program();
    
    let prg = pio0.common.load_program(&program);
    
    let pio_pin = pio0.common.make_pio_pin(pin);
    
    let mut config = pio::Config::<PIO0>::default();
    
    config.clock_divider = 128u8.into();
    
    config.set_set_pins(&[&pio_pin]);
    
    config.use_program(&prg, &[]);
    
    pio0.sm0.set_config(&config);

    pio0.sm0.set_enable(true);
}