use core::mem::size_of;
use field_offset::offset_of;
use stm32l4xx_hal::prelude::_embedded_hal_blocking_i2c_Write;
use tock_registers::{
    interfaces::{ReadWriteable, Readable, Writeable},
    register_bitfields, register_structs,
    registers::InMemoryRegister,
};

const MAX_BRIGHTNESS: u32 = 4096;

register_bitfields![
    u8,
    MODE_1 [
        ALLCALL 0,
        SUB3 1,
        SUB2 2,
        SUB1 3,
        SLEEP 4,
        AI 5,
        EXTCLK 6,
        RESTART 7,
    ],
    MODE_2 [
        OUTNE OFFSET(0) NUMBITS(2) [],
        OUTDRV OFFSET(2) NUMBITS(1) [],
        OCH OFFSET(3) NUMBITS(1) [],
        INVRT OFFSET(4) NUMBITS(1) [],
    ],
];

register_bitfields![
    u32,
    LED_CTRL [
        On OFFSET(0) NUMBITS(12) [],
        On_Full OFFSET(12) NUMBITS(1) [],
        Off OFFSET(16) NUMBITS(12) [],
        Off_Full OFFSET(28) NUMBITS(1) [],
    ],
];

register_structs! {
    Pca9685Registers {
        (0x00 => mode1: InMemoryRegister<u8, MODE_1::Register>),
        (0x01 => mode2: InMemoryRegister<u8, MODE_2::Register>),
        (0x02 => subaddress1: InMemoryRegister<u8>),
        (0x03 => subaddress2: InMemoryRegister<u8>),
        (0x04 => subaddress3: InMemoryRegister<u8>),
        (0x05 => allcalladdr: InMemoryRegister<u8>),
        (0x06 => led: [InMemoryRegister<u32, LED_CTRL::Register>; 16]),
        (0x46 => _reserved),
        (0xFA => all_led: InMemoryRegister<u32, LED_CTRL::Register>),
        (0xFE => prescale: InMemoryRegister<u8>),
        (0xFF => testmode: InMemoryRegister<u8>),
        (0x100 => @END),
    }
}

pub fn init(address: u8, i2c: &mut impl _embedded_hal_blocking_i2c_Write) {
    let mode1_reg: InMemoryRegister<u8, MODE_1::Register> = InMemoryRegister::new(0);
    let mode2_reg: InMemoryRegister<u8, MODE_2::Register> = InMemoryRegister::new(0);

    // Turn on autoincrement
    // Start disabled
    // Enable response to all call address
    mode1_reg.modify(MODE_1::AI::SET + MODE_1::SLEEP::SET + MODE_1::ALLCALL::SET);

    // Configure output for totem pole drive
    mode2_reg.modify(MODE_2::OUTDRV::SET);

    let buffer: [u8; 3] = [
        offset_of!(Pca9685Registers => mode1).get_byte_offset() as u8,
        mode1_reg.get(),
        mode2_reg.get(),
    ];

    match i2c.write(address, &buffer) {
        Ok(_) => (),
        Err(_) => panic!(),
    }

    let prescale_reg: InMemoryRegister<u8> = InMemoryRegister::new(0);

    // Set PWM frequency to 1526Hz
    prescale_reg.set(0x03);

    let buffer: [u8; 2] = [
        offset_of!(Pca9685Registers => prescale).get_byte_offset() as u8,
        prescale_reg.get(),
    ];

    match i2c.write(address, &buffer) {
        Ok(_) => (),
        Err(_) => panic!(),
    }

    // Re-enable outputs
    mode1_reg.modify(MODE_1::SLEEP::CLEAR);

    let buffer: [u8; 2] = [
        offset_of!(Pca9685Registers => mode1).get_byte_offset() as u8,
        mode1_reg.get(),
    ];

    match i2c.write(address, &buffer) {
        Ok(_) => (),
        Err(_) => panic!(),
    }
}

pub fn set_digit(
    address: u8,
    i2c: &mut impl _embedded_hal_blocking_i2c_Write,
    digit: usize,
    pwm_start: u32,
    pwm_end: u32,
) {
    let led_reg: InMemoryRegister<u32, LED_CTRL::Register> = InMemoryRegister::new(0);

    led_reg.modify(LED_CTRL::On.val(pwm_start));
    led_reg.modify(LED_CTRL::On.val(pwm_end));

    if pwm_end == 0 {
        led_reg.modify(LED_CTRL::Off_Full::SET)
    } else if pwm_end >= MAX_BRIGHTNESS {
        led_reg.modify(LED_CTRL::On_Full::SET)
    }

    let buffer: [u8; 5] = [
        (offset_of!(Pca9685Registers => led).get_byte_offset() + size_of::<u32>() * digit) as u8,
        led_reg.get().to_le_bytes()[0],
        led_reg.get().to_le_bytes()[1],
        led_reg.get().to_le_bytes()[2],
        led_reg.get().to_le_bytes()[3],
    ];

    match i2c.write(address, &buffer) {
        Ok(_) => (),
        Err(_) => panic!(),
    }
}