#include "control.h"
#include "stm32g4xx_hal.h"
#include "cmsis_os.h"
//#include "menu.h"
//#include "buzzer.h"
#include <stdio.h>

#define ENC_SOFT_FILTER_CNT     3 //2
#define BEEP_TIME               1000

//SemaphoreHandle_t  semphEncoder;

bool encInit = false;
bool encDir = false;

TIM_HandleTypeDef htim2;


//
void init_encoder(void)
{
    TIM_Encoder_InitTypeDef sConfig = {0};
    TIM_MasterConfigTypeDef sMasterConfig = {0};
  
    __HAL_RCC_TIM2_CLK_ENABLE();
    
    init_gpio_encoder();

    htim2.Instance = TIM2;
    htim2.Init.Prescaler = 0;
    htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
    htim2.Init.Period = 65535;
    htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
    htim2.Init.RepetitionCounter = 0;
    htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
    
    sConfig.EncoderMode = TIM_ENCODERMODE_TI12;
    
    sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
    sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
    sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
    sConfig.IC1Filter = 100;
    
    sConfig.IC2Polarity = TIM_ICPOLARITY_RISING;
    sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;
    sConfig.IC2Prescaler = TIM_ICPSC_DIV1;
    sConfig.IC2Filter = 100;
    
    HAL_TIM_Encoder_Init(&htim2, &sConfig);
    
    sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
    sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET;
    sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
    
    HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig);
    
    HAL_NVIC_SetPriority(TIM2_IRQn, 6, 0);
    HAL_NVIC_EnableIRQ(TIM2_IRQn);
    
    HAL_TIM_Encoder_Start_IT(&htim2, TIM_CHANNEL_ALL);
}


//
void init_gpio_encoder(void)
{
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    
    __HAL_RCC_GPIOA_CLK_ENABLE();
    __HAL_RCC_GPIOB_CLK_ENABLE();
    
    GPIO_InitStruct.Pin = GPIO_PIN_0;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_PULLUP;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
    GPIO_InitStruct.Alternate = GPIO_AF1_TIM2;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
    
    GPIO_InitStruct.Pin = GPIO_PIN_3;
    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
}




extern "C" {
void TIM2_IRQHandler(void)
{
    HAL_TIM_IRQHandler(&htim2);
}
}



#if 0
//
void MENU_ControlInit(void)
{
    MENU_EncoderInit();
    //buz_timer = xTimerCreate("Buzzer tim", BEEP_TIME, pdFALSE, (void *) 0, buz_timer_cb);

    semphEncoder = xSemaphoreCreateBinary();
    encInit = true;
}

//
void MENU_EncoderInit(void)
{
    GPIO_InitTypeDef        GPIO_InitStructure;
    TIM_TimeBaseInitTypeDef TIMER_InitStructure;
    NVIC_InitTypeDef        NVIC_InitStructure;
    
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
    
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_9;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
    GPIO_Init(GPIOA, &GPIO_InitStructure);
    
    NVIC_InitStructure.NVIC_IRQChannel = TIM1_UP_IRQn; 
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 5;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);
    
    TIM_TimeBaseStructInit(&TIMER_InitStructure);
    TIMER_InitStructure.TIM_Period = 1;
    TIMER_InitStructure.TIM_CounterMode = TIM_CounterMode_Up | TIM_CounterMode_Down;
    TIM_TimeBaseInit(TIM1, &TIMER_InitStructure);
 
    TIM_EncoderInterfaceConfig(TIM1, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
    TIM_ITConfig(TIM1, TIM_IT_Update, ENABLE);
    TIM_Cmd(TIM1, ENABLE);
}


extern "C" {
  
void TIM1_UP_IRQHandler(void)
{
    portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
    
    if (TIM_GetITStatus(TIM1, TIM_IT_Update) != RESET)
    {
        TIM_ClearITPendingBit(TIM1, TIM_IT_Update);
        encDir = (TIM1->CR1 & TIM_CR1_DIR ? true : false);

        if (!encInit)
            return;
        
        xSemaphoreGiveFromISR(semphEncoder, &xHigherPriorityTaskWoken);
    }
}

}


  
//
void vEncoder(void *params)
{
    static uint8_t encRightCnt = 0;
    static uint8_t encLeftCnt = 0;
    
    for (;;)
    {
        xSemaphoreTake(semphEncoder, portMAX_DELAY);
        
        if (encDir) 
        {
            encLeftCnt = 0;
            if (++encRightCnt > ENC_SOFT_FILTER_CNT) {
                encRightCnt = 0;
                button_right();
                
                //BUZZER_ON
                //xTimerStart(buz_timer, 0);
                //DBG printf("Turn right\r\n");
            }
        }
        else 
        {
            encRightCnt = 0;
            if (++encLeftCnt > ENC_SOFT_FILTER_CNT) {
                encLeftCnt = 0;
                button_left();

                //BUZZER_ON
                //xTimerStart(buz_timer, 0);
                //DBG printf("Turn left\r\n");
            }
        }
    }   // for (;;)
}  
#endif