#include "Bsp/Drivers/GpioDriver.h"
#include "Bsp/macros.h"

namespace BSP {

    /*********************************************
     *
     * GpioPin
     *
     *********************************************/

    GpioDriver::GpioDriver(GPIO_TypeDef *gpio)
        : m_gpio(gpio)
    {}

    void GpioDriver::init() {
        return;
    }

    void GpioDriver::enable() {
#if defined(STM32L0XX)
        if (m_gpio == GPIOA) {
            SET(RCC->IOPENR, RCC_IOPENR_IOPAEN);
        } else if (m_gpio == GPIOB) {
            SET(RCC->IOPENR, RCC_IOPENR_IOPBEN);
        } else if (m_gpio == GPIOC) {
            SET(RCC->IOPENR, RCC_IOPENR_IOPCEN);
        } else if (m_gpio == GPIOH) {
            SET(RCC->IOPENR, RCC_IOPENR_IOPHEN);
        }
#elif defined(STM32L4XX)
        if (m_gpio == GPIOA) {
            SET(RCC->AHB2ENR, RCC_AHB2ENR_GPIOAEN);
        } else if (m_gpio == GPIOB) {
            SET(RCC->AHB2ENR, RCC_AHB2ENR_GPIOBEN);
        } else if (m_gpio == GPIOC) {
            SET(RCC->AHB2ENR, RCC_AHB2ENR_GPIOCEN);
        } else if (m_gpio == GPIOH) {
            SET(RCC->AHB2ENR, RCC_AHB2ENR_GPIOHEN);
        }
#else
#error "Unknown device family"
#endif
    }

    void GpioDriver::disable() {
#if defined(STM32L0XX)
        if (m_gpio == GPIOA) {
            CLR(RCC->IOPENR, RCC_IOPENR_IOPAEN);
        } else if (m_gpio == GPIOB) {
            CLR(RCC->IOPENR, RCC_IOPENR_IOPBEN);
        } else if (m_gpio == GPIOC) {
            CLR(RCC->IOPENR, RCC_IOPENR_IOPCEN);
        } else if (m_gpio == GPIOH) {
            CLR(RCC->IOPENR, RCC_IOPENR_IOPHEN);
        }
#elif defined(STM32L4XX)
        if (m_gpio == GPIOA) {
            CLR(RCC->AHB2ENR, RCC_AHB2ENR_GPIOAEN);
        } else if (m_gpio == GPIOB) {
            CLR(RCC->AHB2ENR, RCC_AHB2ENR_GPIOBEN);
        } else if (m_gpio == GPIOC) {
            CLR(RCC->AHB2ENR, RCC_AHB2ENR_GPIOCEN);
        } else if (m_gpio == GPIOH) {
            CLR(RCC->AHB2ENR, RCC_AHB2ENR_GPIOHEN);
        }
#else
#error "Unknown device family"
#endif
    }

    void GpioDriver::set_pin_moder(uint32_t index, moder_t mode) {
        uint32_t moder_value = 0;
        switch (mode) {
        case moder_t::INPUT:
            moder_value = 0;
            break;
        case moder_t::OUTPUT:
            moder_value = 1;
            break;
        case moder_t::ALTERNATE_FUNCTION:
            moder_value = 2;
            break;
        }
        SET_STRIDE_TO(m_gpio->MODER, 2, index,  moder_value);
    }
    
    void GpioDriver::set_pin_pupdr(uint32_t index, input_pull_t pull_mode) {
        uint32_t pupdr_value = 0;
        switch (pull_mode) {
        case input_pull_t::FLOATING:
            pupdr_value = 0;
            break;
        case input_pull_t::PULL_UP:
            pupdr_value = 1;
            break;
        case input_pull_t::PULL_DOWN:
            pupdr_value = 2;
            break;
        }
        SET_STRIDE_TO(m_gpio->PUPDR, 2, index, pupdr_value);
    }

    void GpioDriver::set_pin_otyper(uint32_t index, output_mode_t mode) {
        uint32_t mask = 1 << index;
        if (mode == output_mode_t::OPEN_DRAIN) {
            SET(m_gpio->OTYPER, mask);
        } else {
            CLR(m_gpio->OTYPER, mask);
        }
    }

    void GpioDriver::set_pin_ospeedr(uint32_t index, output_speed_t speed) {
        uint32_t speed_val = 0;

        if (speed == output_speed_t::LOW) {
            speed_val = 0;
        } else if (speed == output_speed_t::MEDIUM) {
            speed_val = 1;
        } else if (speed == output_speed_t::HIGH) {
            speed_val = 2;
        } else if (speed == output_speed_t::VERY_HIGH) {
            speed_val = 3;
        }

        SET_STRIDE_TO(m_gpio->OSPEEDR, 2, index, speed_val);
    }

    void GpioDriver::configure_pin_input(uint32_t index, input_pull_t pull_mode) {
        if (index > 15) {
            return;
        }

        set_pin_pupdr(index, pull_mode);
        set_pin_moder(index, moder_t::INPUT);
    }

    void GpioDriver::configure_pin_output(uint32_t index, output_mode_t mode, output_speed_t speed) {
        if (index > 15) {
            return;
        }

        set_pin_moder(index, moder_t::INPUT); // Avoid glitches during configuration
        set_pin_otyper(index, mode);
        set_pin_ospeedr(index, speed);
        set_pin_moder(index, moder_t::OUTPUT);
    }

    void GpioDriver::configure_pin_alternate_function(uint32_t index, uint32_t function) {
        if (index > 15) {
            return;
        }
        if (function > 7) {
            return;
        }

        set_pin_moder(index, moder_t::INPUT); // Avoid glitches during configuration
        if (index < 8) {
            SET_STRIDE_TO(m_gpio->AFR[0], 4, index, function);
        } else {
            SET_STRIDE_TO(m_gpio->AFR[1], 4, index - 8, function);
        }
        set_pin_moder(index, moder_t::ALTERNATE_FUNCTION);
    }

    bool GpioDriver::read_pin(uint32_t index) {
        return (m_gpio->IDR >> index) & 1;
    }

    void GpioDriver::write_pin(uint32_t index, bool value) {
        SET_STRIDE_TO(m_gpio->ODR, 1, index, value);
    }

    /*********************************************
     *
     * GpioPin
     *
     *********************************************/

    GpioPin::GpioPin(GpioDriver &driver, uint32_t index)
        : m_gpio(driver)
        , m_index(index)
    {}

    void GpioPin::configure_input(GpioDriver::input_pull_t pull_mode) {
        m_gpio.configure_pin_input(m_index, pull_mode);
    }

    void GpioPin::configure_output(GpioDriver::output_mode_t mode, GpioDriver::output_speed_t speed) {
        m_gpio.configure_pin_output(m_index, mode, speed);
    }

    void GpioPin::configure_alternate_function(uint32_t function) {
        m_gpio.configure_pin_alternate_function(m_index, function);
    }

    bool GpioPin::read() {
        return m_gpio.read_pin(m_index);
    }

    void GpioPin::write(bool val) {
        return m_gpio.write_pin(m_index, val);
    }

    uint32_t GpioPin::get_index() {
        return m_index;
    }

    GpioDriver& GpioPin::get_driver() {
        return m_gpio;
    }


}