thunderborg_driver 0.2.0

use std::thread;
use std::time::Duration;

use rppal::i2c::I2c;

const THUNDERBORG_I2C_ADDR: u16 = 0x15;

const COMMAND_ANALOG_MAX: u16 = 0x3FF;
const VOLTAGE_PIN_MAX: f32 = 36.3;

const SET_LED_CMD: u8 = 5;
const GET_BATT_VOLTAGE_CMD: u8 = 21;
const SET_LEFT_MOTORS_PWR_FWD_CMD: u8 = 8;
const SET_LEFT_MOTORS_PWR_REV_CMD: u8 = 9; 
const SET_RIGHT_MOTORS_PWR_FWD_CMD: u8 = 11;
const SET_RIGHT_MOTORS_PWR_REV_CMD: u8 = 12;    
const ALL_MOTORS_OFF_CMD: u8 = 14;

const MAX_MOTOR_POWER: u8 = 242;

pub struct ThunderborgDriver
{
    i2c_conn: I2c, 
}

impl ThunderborgDriver
{
    pub fn new() -> Result<Self, rppal::i2c::Error>
    {
        let mut i2c = I2c::new()?;
        i2c.set_slave_address(THUNDERBORG_I2C_ADDR)?;

        Ok(Self{i2c_conn: i2c,})
    }

    pub fn drive_fwd_for_ms(&self, num_milliseconds: u64) -> Result<(), rppal::i2c::Error>
    {
        self.set_left_motors_pwr_fwd(MAX_MOTOR_POWER)?;
        self.set_right_motors_pwr_fwd(MAX_MOTOR_POWER)?;
        let sleep_time = Duration::from_millis(num_milliseconds);
        thread::sleep(sleep_time);
        self.motors_off()
    }

    pub fn drive_rev_for_ms(&self, num_milliseconds: u64) -> Result<(), rppal::i2c::Error>
    {
        self.set_left_motors_pwr_rev(MAX_MOTOR_POWER)?;
        self.set_right_motors_pwr_rev(MAX_MOTOR_POWER)?;
        let sleep_time = Duration::from_millis(num_milliseconds);
        thread::sleep(sleep_time);
        self.motors_off()
    }

    pub fn spin_left_for_ms(&self, num_milliseconds: u64) -> Result<(), rppal::i2c::Error>
    {
        self.set_left_motors_pwr_fwd(MAX_MOTOR_POWER)?;
        self.set_right_motors_pwr_rev(MAX_MOTOR_POWER)?;
        let sleep_time = Duration::from_millis(num_milliseconds);
        thread::sleep(sleep_time);
        self.motors_off()
    }

    pub fn spin_right_for_ms(&self, num_milliseconds: u64) -> Result<(), rppal::i2c::Error>
    {
        self.set_right_motors_pwr_fwd(MAX_MOTOR_POWER)?;
        self.set_left_motors_pwr_rev(MAX_MOTOR_POWER)?;
        let sleep_time = Duration::from_millis(num_milliseconds);
        thread::sleep(sleep_time);
        self.motors_off()
    }

    pub fn set_left_motors_pwr_fwd(&self, power_input: u8) -> Result<(), rppal::i2c::Error>
    {
        let mut set_power: [u8;1] = [0];

        if power_input > MAX_MOTOR_POWER
        {
            set_power[0] = MAX_MOTOR_POWER;
        }
        else
        {
            set_power[0] = power_input;
        }
        self.i2c_conn.block_write(SET_LEFT_MOTORS_PWR_FWD_CMD, &set_power)?;

        Ok(())
    }

    pub fn set_right_motors_pwr_fwd(&self, power_input: u8) -> Result<(), rppal::i2c::Error>
    {
        let mut set_power: [u8;1] = [0];

        if power_input > MAX_MOTOR_POWER
        {
            set_power[0] = MAX_MOTOR_POWER;
        }
        else
        {
            set_power[0] = power_input;
        }
        self.i2c_conn.block_write(SET_RIGHT_MOTORS_PWR_FWD_CMD, &set_power)?;

        Ok(())
    }

    pub fn set_left_motors_pwr_rev(&self, power_input: u8) -> Result<(), rppal::i2c::Error>
    {
        let mut set_power: [u8;1] = [0];

        if power_input > MAX_MOTOR_POWER
        {
            set_power[0] = MAX_MOTOR_POWER;
        }
        else
        {
            set_power[0] = power_input;
        }
        self.i2c_conn.block_write(SET_LEFT_MOTORS_PWR_REV_CMD, &set_power)?;

        Ok(())
    }

    pub fn set_right_motors_pwr_rev(&self, power_input: u8) -> Result<(), rppal::i2c::Error>
    {
        let mut set_power: [u8;1] = [0];

        if power_input > MAX_MOTOR_POWER
        {
            set_power[0] = MAX_MOTOR_POWER;
        }
        else
        {
            set_power[0] = power_input;
        }
        self.i2c_conn.block_write(SET_RIGHT_MOTORS_PWR_REV_CMD, &set_power)?;

        Ok(())
    }

    pub fn motors_off(&self) -> Result<(), rppal::i2c::Error>
    {
        //No data for this command
        let data: [u8;1] = [0];
        self.i2c_conn.block_write(ALL_MOTORS_OFF_CMD, &data)?;

        Ok(())
    }

    pub fn set_led_color(&self, red:u8, green: u8, blue: u8) -> Result<(), rppal::i2c::Error>
    {
        let set_rgb: [u8;3] = [red, green, blue];
        self.i2c_conn.block_write(SET_LED_CMD, &set_rgb)?;

        Ok(())
    }

    pub fn get_battery_voltage(&self) -> Result<f32, rppal::i2c::Error>
    {
        let mut read_buffer: [u8;4] = [0, 0, 0, 0];
        self.i2c_conn.block_read(GET_BATT_VOLTAGE_CMD, &mut read_buffer)?;
    
        //Have to convert u8 values to u32 so that we don't open up potential for left-shift overflow
        let v1: u32 = read_buffer[1].into();
        let v2: u32 = read_buffer[2].into();
        let raw = ((v1 << 8) + v2) as f32;

        let level = raw/(COMMAND_ANALOG_MAX as f32);
        let voltage = level*VOLTAGE_PIN_MAX;
        
        Ok(voltage)
    }    
}