#include <Arduino.h>
#include "stm32f1xx_ll_dma.h"
#include "stm32f1xx_hal_dma.h"
#include "math.h"

#define OUTPIN PA8
#define OUTPIN_N PB13
#define HVGENPIN PB9

#define M_PI 3.141526535

HardwareTimer *shockTimer;
uint32_t shockPwmChannel;
uint32_t shockPwmChannel_N;

HardwareTimer *hvTimer;
uint32_t hvPwmChannel;

uint32_t frequency = 50;
uint8_t dutyCycle = 50;
uint8_t deadTime = 50;
bool readyToSend = false;


uint32_t hvFrequency = 250000;
uint8_t hvDutyCycle = 61;
/**
 * output = red, green jumper wire
 * input = yellow
 *
 *
 *
 */


__attribute__((aligned(4)))  volatile uint16_t dmaBuf[] = { /*100, 150, 127, 100*/ 1, 1, 200, 200, 1, 1, 100, 100};
size_t dmaBuf_len = sizeof(dmaBuf) / sizeof(dmaBuf[0]);

void setup(void) {
    pinMode(PA0, INPUT_ANALOG);
    pinMode(LED_BUILTIN, OUTPUT);

    TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0};

    // https://github.com/ahmetonat/STM32F103-ADC-DMA-example/blob/master/Src/stm32f1xx_hal_msp.c
    // https://www.stm32duino.com/viewtopic.php?f=41&t=110
    // https://controllerstech.com/dma-with-adc-using-registers-in-stm32/
    // https://stm32duinoforum.com/forum/viewtopic_f_48_t_4399.html

    // DMA:
    // https://controllerstech.com/pwm-with-dma-in-stm32/ -- good
    // https://deepbluembedded.com/stm32-change-pwm-duty-cycle-with-dma-for-sine-wave-generation/
    // https://electronics.stackexchange.com/questions/652738/stm32-pwm-and-output-compare
    // https://community.st.com/s/question/0D53W000007zeyoSAA/pwm-output-synchronization-problem

    // LL_TIM_OC_SetDeadTime


    TIM_TypeDef *shockInstance = (TIM_TypeDef *)pinmap_peripheral(digitalPinToPinName(OUTPIN), PinMap_PWM);
    shockPwmChannel = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(OUTPIN), PinMap_PWM));
    shockPwmChannel_N = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(OUTPIN_N), PinMap_PWM));
    shockTimer = new HardwareTimer(shockInstance);

    /* TIM_TypeDef *hvInstance = (TIM_TypeDef *)pinmap_peripheral(digitalPinToPinName(HVGENPIN), PinMap_PWM);
    hvPwmChannel = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(HVGENPIN), PinMap_PWM));
    hvTimer = new HardwareTimer(hvInstance); */

    // hvTimer->handle
    // _timerObj.handle.Init
    // hvTimer->getHandle()->Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3;

    Serial.begin(0);

    for (size_t i = 0; i < dmaBuf_len; i++) {
        // dmaBuf[i] = sin(2 * M_PI * i / dmaBuf_len);
    }

    /*shockTimer->pause();
    shockTimer->setMode(shockPwmChannel, TIMER_OUTPUT_COMPARE_PWM1, OUTPIN);
    shockTimer->setOverflow(5, HERTZ_FORMAT); // Hertz
    shockTimer->setCaptureCompare(shockPwmChannel, 50,
                                PERCENT_COMPARE_FORMAT); // 50% Duty Cycle
    shockTimer->resume(); */

    shockTimer->setMode(shockPwmChannel_N, TIMER_OUTPUT_COMPARE_PWM1, OUTPIN_N);
    shockTimer->setPWM(shockPwmChannel, OUTPIN, frequency, dutyCycle);
    TIM1->CCER |= TIM_CCER_CC1NE;


    sBreakDeadTimeConfig.BreakState = TIM_BREAK_ENABLE;
    sBreakDeadTimeConfig.DeadTime = deadTime;
    sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_LOW;
    sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_1;
    sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_ENABLE;
    sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_ENABLE;
    sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_ENABLE;
    HAL_TIMEx_ConfigBreakDeadTime(shockTimer->getHandle(), &sBreakDeadTimeConfig);
    // LL_TIM_OC_SetDeadTime(TIM1, 50);
    // shockInstance->BDTR |= 0xFF;


    //Inserts Dead Time approx 14ns per increment 0 to 128
    //(DTG Bits 7:0)
    //DGT Bit 2&5 = 00100100 = 560ns
    // bitSet(TIMER1_BASE->BDTR,2);
    // bitSet(TIMER1_BASE->BDTR,5);

    //shockTimer->setPWM(shockPwmChannel, OUTPIN_N, frequency, dutyCycle);
    // shockTimer->setPWM(shockPwmChannel, OUTPIN_N, frequency, dutyCycle);
    // setMode(shockPwmChannel, TIMER_, pin);

    // hvTimer->setPWM(hvPwmChannel, HVGENPIN, hvFrequency, hvDutyCycle);


    // delay(3000);

    /* shockTimer->pause();
    // TIM1->CCER |= 0x555; //3ch compl enable. 0x55 = 2ch compl enable. 0x5 = 1ch compl enable. As seen on RM0008 pages 353 & 354, CCxNE and CCxE bits
    TIM1->CCER &= ~TIM_CCER_CC1NP; -- polarity low (or |= to high)
    TIM1->CCER |= TIM_OCNPOLARITY_HIGH;
    TIM1->CCER |= 0;
    TIM1->CCER |= 2;
    TIM1->CCER |= TIM_CCER_CC1NE;
    TIM1->CR1 |= TIM_CR1_CMS_1; // center waveform
    // shockTimer->setPWM(shockPwmChannel, OUTPIN, frequency, dutyCycle);
    shockTimer->resume(); */

    // auto timerMax = (__HAL_TIM_GET_AUTORELOAD(&(shockTimer->getHandle())) + 1);

    /* for (uint8_t i = 0; i < 200; i++) {
        angle = ASR*(float)i;
        IV[i] = (uint16_t) rint(100 + 99*sinf(angle*(PI/180)));
    } */

    DMA_HandleTypeDef hdma_tim2_ch1;
    hdma_tim2_ch1.Instance = DMA1_Channel5; // Table 78
    hdma_tim2_ch1.Init.Direction = DMA_MEMORY_TO_PERIPH;
    hdma_tim2_ch1.Init.PeriphInc = DMA_PINC_DISABLE;
    hdma_tim2_ch1.Init.MemInc = DMA_MINC_ENABLE;
    // on an STM32F407 Tim1 is 16bit so DMA transfer needs to be half word, eg Tim2 is 32bit so use word
    hdma_tim2_ch1.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
    hdma_tim2_ch1.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD;
    hdma_tim2_ch1.Init.Mode = DMA_CIRCULAR;
    hdma_tim2_ch1.Init.Priority = DMA_PRIORITY_HIGH;

    // HAL_TIM_PWM_DeInit(shockTimer->getHandle());

    if (HAL_DMA_Init(&hdma_tim2_ch1) != HAL_OK) {
        // Error_Handler();
        for (int i = 0; i < 5; i++) {
            digitalWrite(LED_BUILTIN, HIGH);
            delay(100);
            digitalWrite(LED_BUILTIN, LOW);
            delay(100);
        }
    }

    // __HAL_RCC_DMA1_CLK_ENABLE();

    // __HAL_LINKDMA(shockTimer->getHandle(), hdma[TIM_DMA_ID_CC1], hdma_tim2_ch1); // TIM_DMA_ID_CC1 -> shockPwmChannel ???

    __HAL_LINKDMA(shockTimer->getHandle(), hdma[TIM_DMA_ID_CC4], hdma_tim2_ch1); // TIM_DMA_ID_CC1 -> shockPwmChannel ???
    __HAL_LINKDMA(shockTimer->getHandle(), hdma[TIM_DMA_ID_TRIGGER], hdma_tim2_ch1);

    // page 358 Mastering STM32

    // If anyone else is forced by their pinout to use one of the complementary PWM outputs (e.g. CH3N)
    // then be sure to change the call to HAL_TIM_PWM_Start_DMA() for HAL_TIMEx_PWMN_Start_DMA()
    // HAL_TIM_PWM_Start_DMA(shockTimer->getHandle(), TIM_CHANNEL_1, (uint32_t*) &dmaBuf, dmaBuf_len);
    HAL_TIM_PWM_Start_DMA(shockTimer->getHandle(), TIM_CHANNEL_1, (uint32_t*) &dmaBuf[0], dmaBuf_len);


    /* LL_DMA_InitTypeDef DMA_InitStruct;
    DMA_InitStruct.Channel = LL_DMA_CHANNEL_4;
    DMA_InitStruct.Direction = LL_DMA_DIRECTION_PERIPH_TO_MEMORY;
    DMA_InitStruct.MemoryOrM2MDstAddress = (uint32_t)DMA_RX_Buffer;
    DMA_InitStruct.NbData = DMA_RX_BUFFER_SIZE;
    DMA_InitStruct.MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT;
    DMA_InitStruct.PeriphOrM2MSrcAddress = (uint32_t)&TIM1->DR;
    LL_DMA_Init(DMA1, LL_DMA_STREAM_5, &DMA_InitStruct); */


    analogReadResolution(12);


}

void setShockFrequency(uint32_t freq) {

}

void setRegister(uint8_t reg, uint32_t val) {
    if (reg == 1) {
        frequency = val;
    } else if (reg == 2) {
        dutyCycle = val;
    } else if (reg == 3) {
        hvFrequency = val;
    } else if (reg == 4) {
        hvDutyCycle = val;
    } else if (reg == 5) {
        deadTime = val;
    }

    if (reg == 1 || reg == 2) {
        shockTimer->setOverflow(frequency, HERTZ_FORMAT);
        shockTimer->setCaptureCompare(shockPwmChannel, dutyCycle, PERCENT_COMPARE_FORMAT);
    } else if (reg == 3 || reg == 4) {
        hvTimer->setOverflow(hvFrequency, HERTZ_FORMAT);
        hvTimer->setCaptureCompare(hvPwmChannel, hvDutyCycle, PERCENT_COMPARE_FORMAT);
    } else if (reg == 5) {
        LL_TIM_OC_SetDeadTime(TIM1, deadTime);
    }

    /* char buffer[256];
    sprintf(buffer, "Setting register: %d to %d\n", reg, val);
    Serial.print(buffer); */
}

uint32_t getRegister(uint8_t reg) {
    if (reg == 1) {
        return frequency;
    } else if (reg == 2) {
        return dutyCycle;
    } else if (reg == 3) {
        return hvFrequency;
    } else if (reg == 4) {
        return hvDutyCycle;
    } else if (reg == 5) {
        return deadTime;
    }
}

void processSerial() {
    uint8_t msgType = Serial.read();
    if (msgType == 1) {
        // ping
        uint8_t ack = 0x69;
        Serial.write((uint8_t*) &ack, sizeof(ack));
    } else if (msgType == 2) {
        // set register
        uint8_t reg = Serial.read();
        uint32_t buf;
        Serial.readBytes((char*) &buf, sizeof(buf));
        setRegister(reg, buf);
        uint8_t ack = 0x69;
        Serial.write((uint8_t*) &ack, sizeof(ack));
    } else if (msgType == 3) {
        // get register
        uint8_t reg = Serial.read();
        uint32_t val = getRegister(reg);
        // Serial.write((uint8_t*) &val, sizeof(val));
        Serial.write((uint8_t*) &val, sizeof(val));
        // Serial.write((uint8_t*) &val, sizeof(val));
        // uint32_t val = getRegister(reg);
    } else {
        for (int i = 0; i < 5; i++) {
            digitalWrite(LED_BUILTIN, HIGH);
            delay(100);
            digitalWrite(LED_BUILTIN, LOW);
            delay(100);
        }
    }
}

void sendMsg(uint8_t type) {

}

void loop(void) {
    if (Serial.available()) {
        processSerial();
    }

    uint16_t v = (uint16_t) analogRead(PA0);
    // Serial.write((uint8_t*) &v, sizeof(v));
    /* Serial.print(v);
    Serial.println(); */
    delay(1);
}