#include "board_stm32.h"
#include "bsplib.h"
#include "print.h"
#include <stdio.h>
Board_STM32 Board_STM32::board;

#define CONFIG_EEPROM_BASE ((uint32_t)0x800F400)

#ifndef MOTOR_DRIVER
#define MOTOR_DRIVER MOTOR_DRIVER_TB6612
#endif

Board* Board::get()
{
    return &Board_STM32::board;
}

Board_STM32::Board_STM32()
{
}

Board_STM32::~Board_STM32()
{
}

void Board_STM32::init()
{
    PB_System_Timer_Init();
    DOInit();
    DIInit();
}

void Board_STM32::enable_irq()
{
}

void Board_STM32::disable_irq()
{
}

void Board_STM32::usart_debug_init()
{
    PB_USART_Init(2, 115200, 0);
}

#ifdef __cplusplus
extern "C" {
#endif
int fputc(int ch, FILE *f)
{
    PB_USART_Put_Char(2,  ch);
    return (ch);
}
#ifdef __cplusplus
}
#endif

void Board_STM32::usart_init(unsigned char num, unsigned long buad)
{
    if (num == (unsigned char)USART_1) {
        PB_USART_Init(1, 115200, 0);
    } else if (num == (unsigned char)USART_2) {
        PB_USART_Init(2, 115200, 0);
    } else if (num == (unsigned char)USART_3) {
        PB_USART_Init(3, 115200, 0);
    }
}

Queue* Board_STM32::usart_getDataQueue(unsigned char num)
{
    if (num == (unsigned char)USART_1) {
    } else if (num == (unsigned char)USART_2) {
        return &usart3_queue;
    }else if (num == (unsigned char)USART_3) {
        return &usart3_queue;
    }
    return 0;
}

void Board_STM32::usart_write(unsigned char num, unsigned char ch)
{
    if (num == (unsigned char)USART_1) {
        PB_USART_Put_Char(1,  ch);
    } else if (num == (unsigned char)USART_2) {
        PB_USART_Put_Char(3,  ch);
    }else if (num == (unsigned char)USART_3) {
        PB_USART_Put_Char(3,  ch);
    }
}

void Board_STM32::usart_write(unsigned char num, unsigned char* data, unsigned char len)
{
    if (num == (unsigned char)USART_1) {
        while(len--)
            PB_USART_Put_Char(1,  *data++);
    } else if (num == (unsigned char)USART_2) {
        while(len--)
            PB_USART_Put_Char(2,  *data++);
    }else if (num == (unsigned char)USART_3) {
        while(len--)
            PB_USART_Put_Char(3,  *data++);
    }
}

void Board_STM32::set_config(unsigned char* data, unsigned short len)
{
    if (len%2==0) {
        PB_Flash_EnableWrite();
        for(int i=0;i<len/2;i++) {
            uint16_t a = 0;
            memcpy(&a,data+i*2,2);
            PB_Flash_WriteHalfWord(i*2, a);
            delay_us(10);
        }
        PB_Flash_DisableWrite();
    }
}

void Board_STM32::get_config(unsigned char* data, unsigned short len)
{
    if (len%2==0) {
        for(int i=0;i<len/2;i++) {
            uint16_t a = PB_Flash_ReadHalfWord(i*2);
            memcpy(data+i*2,&a,2);
        }
    }
}
    
void Board_STM32::DOInit(void)
{
    GPIO_InitTypeDef GPIO_InitStructure;
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO, ENABLE);
    GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);

    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_5;

    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;	
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;  
    GPIO_Init(GPIOA, &GPIO_InitStructure);

    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;	
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;  
    GPIO_Init(GPIOC, &GPIO_InitStructure);		
    GPIO_SetBits(GPIOC, GPIO_Pin_13);	  


    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;	
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;  
    GPIO_Init(GPIOC, &GPIO_InitStructure);		
    GPIO_SetBits(GPIOC, GPIO_Pin_9);	
}

void Board_STM32::DIInit(void)
{
    GPIO_InitTypeDef GPIO_InitStructure;
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);

    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;

    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;	
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;  
    GPIO_Init(GPIOA, &GPIO_InitStructure);
}

void Board_STM32::setDOState(unsigned char _id, unsigned char operation)
{
    if ( _id == _RUN_LED) {
        if (operation == 0) { 
            GPIO_SetBits(GPIOC, GPIO_Pin_9);
        } else if (operation == 1) {
            GPIO_ResetBits(GPIOC, GPIO_Pin_9);
        } else if (operation == 2) {
            GPIO_ToggleBits(GPIOC, GPIO_Pin_9);
        }
    } else if ( _id == _JS_CMD) {
        if (operation == 0) {
            GPIO_SetBits(GPIOA, GPIO_Pin_5);
        } else if (operation == 1) {
            GPIO_ResetBits(GPIOA, GPIO_Pin_5);
        } else if (operation == 2) {
            GPIO_ToggleBits(GPIOA, GPIO_Pin_5);
        }
    } else if ( _id == _JS_CS) {
        if (operation == 0) {
            GPIO_SetBits(GPIOA, GPIO_Pin_0);
        } else if (operation == 1) {
            GPIO_ResetBits(GPIOA, GPIO_Pin_0);
        } else if (operation == 2) {
            GPIO_ToggleBits(GPIOA, GPIO_Pin_0);
        }
    } else if ( _id == _JS_CLK) {
        if (operation == 0) {
            GPIO_SetBits(GPIOA, GPIO_Pin_1);
        } else if (operation == 1) {
            GPIO_ResetBits(GPIOA, GPIO_Pin_1);
        } else if (operation == 2) {
            GPIO_ToggleBits(GPIOA, GPIO_Pin_1);
        }
    }
}

bool Board_STM32::getDIState(unsigned char _id)
{
    if ( _id == _JS_DAT) {
        return GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_4);
    }
		
    return false;
}

void Board_STM32::motor_init(unsigned char num, unsigned long period)
{
    
    GPIO_InitTypeDef GPIO_InitStructure;
    GPIO_StructInit(&GPIO_InitStructure);
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;

    // STBY = PA12
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
    GPIO_Init(GPIOA, &GPIO_InitStructure);
    GPIO_ResetBits(GPIOA, GPIO_Pin_12);

    if (num == MOTOR_1) {
    #if MOTOR_DRIVER == MOTOR_DRIVER_TB6612
        // PB12和PB13 控制电机1, PB0 控制电机1 PWM
        RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12 | GPIO_Pin_13;
        GPIO_Init(GPIOB, &GPIO_InitStructure);
        GPIO_ResetBits(GPIOB, GPIO_Pin_12);
        GPIO_ResetBits(GPIOB, GPIO_Pin_13);
        // TIM3 Ch3 = PB0
        PB_PWMChannel_Init(TIM3, 3, 0, period, 0);

    #else
        RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
        GPIO_Init(GPIOB, &GPIO_InitStructure);
        GPIO_ResetBits(GPIOB, GPIO_Pin_12);
        PB_PWMChannel_Init(TIM1, 1, 0, period, 0);
        PB_PWMChannel_Init(TIM2, 4, 0, period, 0);
    #endif
    } else if (num == MOTOR_2) {
    #if MOTOR_DRIVER == MOTOR_DRIVER_TB6612

        // PB14和PB15 控制电机2, PB1 控制电机2
        RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_14 | GPIO_Pin_15;
        GPIO_Init(GPIOB, &GPIO_InitStructure);
        GPIO_ResetBits(GPIOB, GPIO_Pin_14);
        GPIO_ResetBits(GPIOB, GPIO_Pin_15);
        // TIM3 Ch4 = PB1
        PB_PWMChannel_Init(TIM3, 4, 0, period, 0);
    #else
        RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_14;
        GPIO_Init(GPIOB, &GPIO_InitStructure);
        GPIO_ResetBits(GPIOB, GPIO_Pin_14);

        PB_PWMChannel_Init(TIM1, 4, 0, period, 0);
        PB_PWMChannel_Init(TIM2, 3, 0, period, 0);
    #endif
    } else if (num == MOTOR_3) {
      
    } else if (num == MOTOR_4) {
       
    }
}
/**----------------------------------------------------------------------
* Function    : motor_pwm
* Description : 对于2轮差分模型: Motor1为左轮, Motor2是右轮
                对于4轮运行模型: Motor1为右后轮, Motor2为右前轮, Motor3为
                左前轮, Motor4左后轮
                左电机硬件连接: 
                    左电机控制: IN1、IN2 -> PB14、PB15
                    左电机PWM: PB1 -> TIM3 ch4
                    左电机编码器: PA0、PA1 ->  TIM2, AF0
                右电机硬件连接: 
                    右电机控制: IN1、IN2 -> PB12、PB13
                    右电机PWM: PB1 -> TIM3 ch3
                    右电机编码器: PB6、PB7 ->  TIM4, AF0
* Author      : zhanli&719901725@qq.com
* Date        : 2024/01/17 zhanli 更新部分定义说明
*---------------------------------------------------------------------**/ 
void Board_STM32::motor_pwm(unsigned char num, long pwmVal)
{
    pb_printf("motor_pwm: num = %d, pwm = %d\n", num, pwmVal);
    // pwmVal = 2000;
    if (num == MOTOR_1) {
        if (pwmVal > 5) {
        #if MOTOR_DRIVER == MOTOR_DRIVER_TB6612
            GPIO_ResetBits(GPIOB, GPIO_Pin_15);
            GPIO_SetBits(GPIOB, GPIO_Pin_14);
            PB_Set_PWM(TIM3, 4, (uint16_t)pwmVal);
        #else
            PB_Set_PWM(TIM1, 4, (uint16_t)pwm_value);
            PB_Set_PWM(TIM2, 3, 0);
        #endif
        } else if (pwmVal < -5) {
        #if MOTOR_DRIVER == MOTOR_DRIVER_TB6612
            GPIO_SetBits(GPIOB, GPIO_Pin_15);
            GPIO_ResetBits(GPIOB, GPIO_Pin_14);
            PB_Set_PWM(TIM3, 4, (uint16_t)-pwmVal);
        #else
            PB_Set_PWM(TIM1, 4, 0);
            PB_Set_PWM(TIM2, 3, (uint16_t)-pwm_value);
        #endif
        } else {
        #if MOTOR_DRIVER == MOTOR_DRIVER_TB6612
            GPIO_ResetBits(GPIOB, GPIO_Pin_15);
            GPIO_ResetBits(GPIOB, GPIO_Pin_14);
            PB_Set_PWM(TIM3, 4, 0);
        #else
            PB_Set_PWM(TIM1, 4, 0);
            PB_Set_PWM(TIM2, 3, 0);
        #endif
        }
    } else if (num == MOTOR_2) {
        if (pwmVal > 5) {
        #if MOTOR_DRIVER == MOTOR_DRIVER_TB6612
            GPIO_SetBits(GPIOB, GPIO_Pin_12);
            GPIO_ResetBits(GPIOB, GPIO_Pin_13);
            PB_Set_PWM(TIM3, 3, (uint16_t)pwmVal);
        #else
            PB_Set_PWM(TIM2, 4, (uint16_t)pwm_value);
            PB_Set_PWM(TIM1, 1, 0);
        #endif
        } else if (pwmVal < -5) {
        #if MOTOR_DRIVER == MOTOR_DRIVER_TB6612
            GPIO_ResetBits(GPIOB, GPIO_Pin_12);
            GPIO_SetBits(GPIOB, GPIO_Pin_13);
            PB_Set_PWM(TIM3, 3, (uint16_t)-pwmVal);
        #else
            PB_Set_PWM(TIM1, 1, (uint16_t)-pwm_value);
            PB_Set_PWM(TIM2, 4, 0);
        #endif
        } else {
        #if MOTOR_DRIVER == MOTOR_DRIVER_TB6612
            GPIO_ResetBits(GPIOB, GPIO_Pin_12);
            GPIO_ResetBits(GPIOB, GPIO_Pin_13);
            PB_Set_PWM(TIM3, 3, 0);
        #else
            PB_Set_PWM(TIM1, 1, 0);
            PB_Set_PWM(TIM2, 4, 0);
        #endif
        }
    } else if (num == MOTOR_3) {
        
    } else if (num == MOTOR_4) {
       
    }
}

unsigned long Board_STM32::get_tick_count()
{
	return PB_Get_System_Time() / 1000;;
}

void Board_STM32::encoder_init(unsigned char motor_id)
{
    // MOTOR 1是左电机
    if (motor_id == MOTOR_1) {
        PB_Encoder_Init(TIM2, 0);
    } else if (motor_id == MOTOR_2) {
        PB_Encoder_Init(TIM4, 0);
    }
}

long Board_STM32::get_encoder_count(unsigned char motor_id)
{
    if (motor_id == MOTOR_1) {
        return -PB_Get_Encode_TIM2();
    } else if (motor_id == MOTOR_2) {
        return PB_Get_Encode_TIM4();
    }
    return 0;
}

void Board_STM32::i2c_init()
{
    PB_I2C_Init();
}

unsigned char Board_STM32::i2c_write_byte(unsigned char equipment_address, unsigned char reg_address, unsigned char pt_char)
{
    return PB_I2C_Write_Byte(equipment_address, reg_address, pt_char);
}

unsigned char Board_STM32::i2c_write_buf(unsigned char equipment_address, unsigned char reg_address, unsigned char* pt_char, unsigned char size)
{
    return PB_I2C_Write_Buf(equipment_address, reg_address, pt_char, size);
}

unsigned char Board_STM32::i2c_read_byte(unsigned char equipment_address, unsigned char reg_address, unsigned char* pt_char)
{
    return PB_I2C_Read_Byte(equipment_address, reg_address, pt_char);
}

unsigned char Board_STM32::i2c_read_buf(unsigned char equipment_address, unsigned char reg_address, unsigned char* pt_char, unsigned char size)
{
    return PB_I2C_Read_Buf(equipment_address, reg_address, pt_char, size);
}
