//
// Created by Luca on 9/21/2025.
//

#include "SHAL_ADC.h"
#include "SHAL_GPIO.h"
#include "SHAL_UART.h"
#include <cstdio>

bool SHAL_ADC::isValid() const {
    if(m_ADCKey == ADC_Key::INVALID || m_ADCKey == ADC_Key::NUM_ADC){
        return false;
    }
    return true;
}

SHAL_Result SHAL_ADC::init(const ADC_Key key, SHAL_ADC_Sample_Mode mode){

    m_ADCKey = key;

    if(!isValid()){
        return SHAL_Result::ERROR;
    }

    auto rcc = getADCRCCEnableRegister(m_ADCKey); //Clock enable
    auto ccr = getADCCommonControl(m_ADCKey);

    SHAL_apply_bitmask(rcc.reg,rcc.mask); //Enable clock

    wakeFromDeepSleep(); //Wake and enable LDO

    //Configure clock source
    SHAL_set_bits(ccr.reg,2,static_cast<uint32_t>(SHAL_ADC_Clock_Mode::SYNC_BY_2),ccr.clock_mode_position); //TODO take as param?

    configureResolution(SHAL_ADC_Resolution::B12); //Configure resolution
    SHAL_apply_bitmask(ccr.reg,ccr.VoltageRefEnable);

    if(calibrate(mode) != SHAL_Result::OKAY){ //Calibrate
        return SHAL_Result::ERROR;
    }

    if(enable() != SHAL_Result::OKAY){
        return SHAL_Result::ERROR;
    }

    return SHAL_Result::OKAY;
}

SHAL_Result SHAL_ADC::calibrate(SHAL_ADC_Sample_Mode sampleMode) const {
    const SHAL_ADC_Control_Reg control_reg = getADCControlReg(m_ADCKey);

    if(disable() != SHAL_Result::OKAY){
        return SHAL_Result::ERROR;
    }

    SHAL_set_bits(control_reg.reg, 1, static_cast<uint32_t>(sampleMode), control_reg.sample_mode_offset); //Set sample mode (differential or single ended)

    SHAL_apply_bitmask(control_reg.reg, control_reg.calibration_mask); //Start calibration

    if (!SHAL_wait_for_bit_clear_us(control_reg.reg, control_reg.calibration_mask, 100)) { //Wait for calibration
        return SHAL_Result::ERROR;
    }

    return SHAL_Result::OKAY;
}

uint32_t SHAL_ADC::singleConvertSingle(SHAL_ADC_Channel channel, SHAL_ADC_SampleTime time) const {

    auto data_reg = getADCDataReg(m_ADCKey);
    auto ISR_reg = getADCISRReg(m_ADCKey);

    auto sampleTimeReg = getADCChannelSamplingTimeRegister(m_ADCKey, channel);

    SHAL_set_bits(sampleTimeReg.reg, 3, static_cast<uint8_t>(time), sampleTimeReg.channel_offset); //Set sample time

    if(setADCSequenceAmount(1) == SHAL_Result::ERROR) { return 0; } //Set sequence amount to 1

    addADCChannelToSequence(channel, 1);

    startConversion();

    if (!SHAL_wait_for_bit_set_us(ISR_reg.reg, ISR_reg.end_of_conversion_mask, 200)) {
        return 0;
    }

    uint32_t result = *data_reg.reg;

    return result;
}

SHAL_Result SHAL_ADC::multiConvertSingle(SHAL_ADC_Channel* channels, int numChannels, uint16_t* result, SHAL_ADC_SampleTime time) {
    auto data_reg = getADCDataReg(m_ADCKey); //Where our output will be stored

    setADCSequenceAmount(numChannels); //Convert the correct amount of channels

    for(int i = 0; i < numChannels; i++){
        auto channel = channels[i];

        auto sampleTimeReg = getADCChannelSamplingTimeRegister(m_ADCKey,channel);

        SHAL_set_bits(sampleTimeReg.reg,3,static_cast<uint8_t>(time),sampleTimeReg.channel_offset); //Set sample time register TODO un-hardcode bit width?

        addADCChannelToSequence(channel,i); //Use index 0 to convert channel
    }

    startConversion(); //Start ADC conversion

    auto ISR_reg = getADCISRReg(m_ADCKey);

    for(int i = 0; i < numChannels; i++) {
        if (!SHAL_WAIT_FOR_CONDITION_US(((*ISR_reg.reg & ISR_reg.end_of_conversion_mask) != 0),500)) { //Wait for conversion
            return SHAL_Result::ERROR; //Failed conversion
        }
        result[i] = *data_reg.reg;
    }

    if (!SHAL_WAIT_FOR_CONDITION_US(((*ISR_reg.reg & ISR_reg.end_of_sequence_mask) != 0),500)) { //Wait for conversion
        return SHAL_Result::ERROR; //Failed sequence
    }

    return SHAL_Result::OKAY;
}

SHAL_Result SHAL_ADC::enable() const {

    if(!isValid()){
        return SHAL_Result::ERROR;
    }

    const SHAL_ADC_Control_Reg control_reg = getADCControlReg(m_ADCKey);
    const SHAL_ADC_ISR_Reg ISR_reg = getADCISRReg(m_ADCKey);

    SHAL_apply_bitmask(ISR_reg.reg, ISR_reg.ready_mask); //Clear ready flag (write is correct as per datasheet)

    SHAL_apply_bitmask(control_reg.reg, control_reg.enable_mask); //Enable mask

    if (!SHAL_wait_for_bit_set_us(ISR_reg.reg,ISR_reg.ready_mask,500)) { //Check ADRDY
        return SHAL_Result::ERROR;
    }

    return SHAL_Result::OKAY;
}

/// Disables the ADC
/// @return
SHAL_Result SHAL_ADC::disable() const {
    if(!isValid()){
        return SHAL_Result::ERROR;
    }

    const auto control_reg = getADCControlReg(m_ADCKey);

    //Stop any ongoing conversion
    if (SHAL_check_bit(control_reg.reg, control_reg.start_mask)) {
        SHAL_apply_bitmask(control_reg.reg, control_reg.stop_mask);
    }

    if (SHAL_check_bit(control_reg.reg,control_reg.enable_mask)) { //DO NOT disable if the ADC is already disabled
        SHAL_apply_bitmask(control_reg.reg, control_reg.disable_mask);

        if (!SHAL_wait_for_bit_clear_ms(control_reg.reg,(control_reg.enable_mask | control_reg.disable_mask),50)) {
            return SHAL_Result::ERROR;
        }
    }

    return SHAL_Result::OKAY;
}

SHAL_Result SHAL_ADC::wakeFromDeepSleep() const {

    const SHAL_ADC_Control_Reg control_reg = getADCControlReg(m_ADCKey); //ADC Control register
    const SHAL_ADC_ISR_Reg ISR_reg = getADCISRReg(m_ADCKey);

    SHAL_clear_bitmask(control_reg.reg,control_reg.deep_power_down_mask); //Wake ADC from sleep

    SHAL_apply_bitmask(control_reg.reg,control_reg.voltage_regulator_mask);

    if (!SHAL_wait_for_bit_set_us(ISR_reg.reg,ISR_reg.ldo_ready_mask,50)) { //Wait for LDO
        return SHAL_Result::ERROR;
    }

    return SHAL_Result::OKAY;
}

SHAL_Result SHAL_ADC::startConversion() const {
    auto control_reg = getADCControlReg(m_ADCKey);

    SHAL_apply_bitmask(control_reg.reg,control_reg.start_mask);

    return SHAL_Result::OKAY;
}


SHAL_Result SHAL_ADC::configureResolution(SHAL_ADC_Resolution resolution) const {
    if(!isValid()){
        return SHAL_Result::ERROR;
    }

    const SHAL_ADC_Config_Reg config_reg = getADCConfigReg(m_ADCKey);
    /*
    SHAL_set_bits(config_reg.reg,2,static_cast<uint8_t>(resolution),config_reg.resolution_offset);

    char buff[20];
    snprintf(buff,16,"CFGR: %lu\r\n", (unsigned long)ADC1->CFGR);
    SHAL_UART3.sendString(buff);
    */

    SHAL_set_register_value(config_reg.reg, 0);

    return SHAL_Result::OKAY;
}

SHAL_Result SHAL_ADC::configureAlignment(SHAL_ADC_Alignment alignment) const {
    if(!isValid()){
        return SHAL_Result::ERROR;
    }

    const SHAL_ADC_Config_Reg config_reg = getADCConfigReg(m_ADCKey);

    //TODO check if this needs to be abstracted (Do other platforms have >2 resolution possibilities?
    SHAL_set_bits(config_reg.reg,1,static_cast<uint8_t>(alignment),config_reg.alignment_offset);

    return SHAL_Result::OKAY;
}

SHAL_Result SHAL_ADC::preselectChannel(SHAL_ADC_Channel channel) const {
    auto preselect = getADCPreselectRegister(ADC_Key::S_ADC1, SHAL_ADC_Channel::CH15);
    SHAL_set_bits(preselect.reg,1,1,preselect.channel_offset);

    return SHAL_Result::OKAY;
}

SHAL_Result SHAL_ADC::setADCSequenceAmount(uint32_t amount) const {
    if(!isValid()){return SHAL_Result::ERROR;}

    if(amount == 0){
        return SHAL_Result::ERROR;
    }

    SHAL_ADC_Sequence_Amount_Reg sequence_amount_reg = getADCSequenceAmountRegister(m_ADCKey);

    SHAL_set_bits(sequence_amount_reg.reg, 4, amount - 1, 0); //Sequence amount reg is just SQR1 least significant 4 bits, this offset should always be 0

    return SHAL_Result::OKAY;
}

SHAL_Result SHAL_ADC::addADCChannelToSequence(SHAL_ADC_Channel channel, const uint32_t conversionNumber) const {
    if(!isValid()) { return SHAL_Result::ERROR; }

    auto sqr = getADCSequenceRegister(m_ADCKey, conversionNumber);
    auto channelNum = static_cast<uint8_t>(channel);


    SHAL_wait_for_bit_clear_ms(&ADC1->CR,ADC_CR_ADSTART,50);

    SHAL_set_bits(sqr.reg, 5, channelNum, sqr.sequence_offset); //Set regular sequence register

    return SHAL_Result::OKAY;
}

SHAL_ADC &ADCManager::get(ADC_Key key) {
    return m_ADCs[static_cast<uint8_t>(key)];
}

SHAL_ADC& ADCManager::getByIndex(int index) {

    if(index < static_cast<int>(ADC_Key::NUM_ADC)){
        return m_ADCs[index];
    }
    return m_ADCs[0];
}