/*******************************************************************************
 * File Name         : BT.c
 * Description       :
 * Created on        : 2018Äê7ÔÂ23ÈÕ
 * Author            : ¶Å¼ü
 *******************************************************************************/

/*******************************************************************************
 *                              Include Files                                  *
 *******************************************************************************/
#include "stdio.h"
#include "stdarg.h"
#include "string.h"
#include "AppConfig.h"
#include "HIDO_VLQueue.h"
#include "HIDO_Input.h"
#include "HIDO_Timer.h"
#include "HIDO_Util.h"
#include "bluetooth.h"
#include "DBG.h"
#include "mainex.h"
#include "Uart.h"

/*******************************************************************************
 *                                  Macro                                      *
 *******************************************************************************/
// È«¾Ö״̬±êÖ¾
volatile uint8_t g_bt_configured = 0;    // ÅäÖÃÍê³É±êÖ¾
volatile uint8_t g_bt_paired = 0;        // Åä¶ÔÍê³É±êÖ¾
volatile uint8_t g_bt_at_moded = 0;        // AT_MODE

static BT_RevState bt_state = BT_STATE_IDLE;
static char bt_buffer[50];      // ½ÓÊÕ»º³åÇø
static u8 bt_index = 0;       // µ±Ç°½ÓÊÕλÖÃ

static HIDO_UINT8 l_au8BTUartRxBuf[BT_UART_RX_BUF_SIZE];
static HIDO_UINT8 l_au8BTUartTxBuf[BT_UART_TX_BUF_SIZE];
HIDO_UINT8 uart6_dma_rxbuf[UART6_DMA_RX_BUF_SIZE] = {0};
HIDO_UINT8 uart6_dma_recv_end_flag = 0;
HIDO_UINT8 uart6_dma_recv_len = 0;

/*******************************************************************************
 *                             Local Variable                                  *
 *******************************************************************************/
// ÅäÖú¯ÊýÉùÃ÷
static HIDO_UINT8 ConfigBluetoothMasterSlave(void);
static void Bluetooth_Config_Slave(void);

// Ó³É亯Êý£º½« [-100,100] Ó³Éäµ½ [1000,2000]
static uint32_t Map(int16_t input, int16_t in_min, int16_t in_max, uint32_t out_min, uint32_t out_max) {
    return (input - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}	
// ÉèÖõç»ú PWM£¨Ç°½ø/ºóÍË£©
static void Set_Motor_PWM(int16_t speed) {
    uint32_t pulse = Map(speed, -100, 100, 1000, 2000);  // -100~100 ¡ú 1000~2000
    __HAL_TIM_SetCompare(&MOTOR_TIM, MOTOR_CHANNEL, pulse);
}

// ÉèÖÃתÏò PWM£¨×óת/ÓÒת£©
static void Set_Steering_PWM(int16_t steer) {
    uint32_t pulse = Map(steer, -100, 100, 1000, 2000);  // -100~100 ¡ú 1000~2000
    __HAL_TIM_SetCompare(&STEERING_TIM, STEERING_CHANNEL, pulse);
}


/*******************************************************************************
 * Function Name     : DBG_Init
 * Description       : µ÷ÊÔ´òÓ¡³õʼ»¯
 * Input             : None
 * Output            : None
 * Return            : None
 * Author            : ¶Å¼ü
 * Modified Date:    : 2018Äê7ÔÂ23ÈÕ
 *******************************************************************************/
HIDO_VOID BT_Init(void)
{
    ST_UartInit stInit;

    memset(&stInit, 0, sizeof(stInit));
    stInit.m_eRxMode = UART_RX_MODE_DMA;
    stInit.m_eTxMode = UART_TX_MODE_DMA;
    stInit.m_pu8RxBuf = l_au8BTUartRxBuf;
    stInit.m_u32RxBufSize = BT_UART_RX_BUF_SIZE;
    stInit.m_pu8TxBuf = l_au8BTUartTxBuf;
    stInit.m_u32TxBufSize = BT_UART_TX_BUF_SIZE;
    stInit.m_u32TxQueueMemberCnt = BT_UART_TX_QUEUE_MEMBER_CNT;
    Uart_Init(UART_ID_BT, &stInit);


}

static void BTUsartParseDataHandler(uint8_t data)
{
    switch (bt_state)
   {
    case BT_STATE_IDLE:
        if(data!=NULL)
        {
        	bt_state=BT_STATE_RECEIVING;
        }
        break;
    case BT_STATE_RECEIVING:
        bt_buffer[bt_index++] = data;
		if (bt_index >2 && bt_buffer[bt_index-2] == '\r' && bt_buffer[bt_index-1] == '\n') {
            // ¼ì²âµ½ÍêÕû½áÊø·û "\r\n"
            bt_buffer[bt_index] = '\0';  // Ìí¼Ó×Ö·û´®ÖÕÖ¹·û
            bt_index = 0;  // ÖØÖûº³åÇø
            bt_state = BT_STATE_COMPLETE;
        }
        break;

    case BT_STATE_COMPLETE:
        // ״̬»ú´íÎó£¬ÖØÖÃ״̬
        bt_state = BT_STATE_IDLE;
        break;
    }
}


// À¶ÑÀÅäÖú¯ÊýʵÏÖ
static HIDO_UINT8  BT_Enter_ATmode(void)

{
	HIDO_UINT8 u8RecvChar = 0;
	//DBG_SerialPutString("AT+ENAT\r\n");
	Uart_Send(UART_ID_BT, (HIDO_UINT8 *) "AT+ENAT\r\n", strlen("AT+ENAT\r\n"));
	 while (Uart_GetChar(UART_ID_BT, &u8RecvChar) == HIDO_OK)
    {
        BTUsartParseDataHandler(u8RecvChar);

        // Èç¹ûÊý¾Ý½ÓÊÕÍê³É£¬´¦ÀíÊý¾ÝÖ¡
        if (bt_state == BT_STATE_COMPLETE)
        {
           if(0==strcmp(bt_buffer,"\r\nOK\r\n"))
           	{
           		memset(bt_buffer, 0, 50);	
				bt_state = BT_STATE_IDLE;
				g_bt_at_moded=1;
				return BT_CONFIG_SUCCESS;
           	}
        }
    }
	return BT_CONFIG_FAIL;
}

// À¶ÑÀ¶Ï¿ªÁ¬½Ó
void  BT_DisConnect(void)

{
	HIDO_UINT8 u8RecvChar = 0;
	//DBG_SerialPutString("AT+ENAT\r\n");
	Uart_Send(UART_ID_BT, (HIDO_UINT8 *) "AT+ENAT\r\n", strlen("AT+ENAT\r\n"));
//	delay_ms(10);
	Uart_Send(UART_ID_BT, (HIDO_UINT8 *) "AT+LENC\r\n", strlen("AT+LENC\r\n"));
//	delay_ms(10);
	Uart_Send(UART_ID_BT, (HIDO_UINT8 *) "AT+EXAT\r\n", strlen("AT+EXAT\r\n"));
}

// À¶ÑÀÅäÖú¯ÊýʵÏÖ
static HIDO_UINT8  BT_Get_SLaveMode(void)

{
    HIDO_UINT8 u8RecvChar = 0;
	DBG_SerialPutString("AT+ROLE\r\n");
	 while (Uart_GetChar(UART_ID_BT, &u8RecvChar) == HIDO_OK)
    {
        BTUsartParseDataHandler(u8RecvChar);

        // Èç¹ûÊý¾Ý½ÓÊÕÍê³É£¬´¦ÀíÊý¾ÝÖ¡
        if (bt_state == BT_STATE_COMPLETE)
        {
           if(0==strcmp(bt_buffer,"LAVE\r\n"))
           	{
           		memset(bt_buffer, 0, 50);	
				bt_state = BT_STATE_IDLE;
				g_bt_configured=1;
				DBG_SerialPutString(" AT+REST\r\n");//Í˳öÖ¸Áî²¢½øÈë͸´«Ä£Ê½
				return BT_CONFIG_SUCCESS;
           	}
        }
    }
	return BT_CONFIG_FAIL;
}

// À¶ÑÀÅäÖú¯ÊýʵÏÖ
static HIDO_UINT8  BT_wait_PairMode(void)

{
    HIDO_UINT8 u8RecvChar = 0;
	 while (Uart_GetChar(UART_ID_BT, &u8RecvChar) == HIDO_OK)
    {
        BTUsartParseDataHandler(u8RecvChar);

        // Èç¹ûÊý¾Ý½ÓÊÕÍê³É£¬´¦ÀíÊý¾ÝÖ¡
        if (bt_state == BT_STATE_COMPLETE)
        {
           if(0==strcmp(bt_buffer,"AT+LED1\r\n"))
           	{
           		memset(bt_buffer, 0, 50);	
				bt_state = BT_STATE_IDLE;
				g_bt_paired = 1;  
				return BT_CONFIG_SUCCESS;
           	}
        }
    }
	return BT_CONFIG_FAIL;
}

// À¶ÑÀÅäÖú¯ÊýʵÏÖ
static HIDO_UINT8  BT_connect_status(void)

{
    HIDO_UINT8 u8RecvChar = 0;
	DBG_SerialPutString("AT+CONN\r\n");
	 while (Uart_GetChar(UART_ID_BT, &u8RecvChar) == HIDO_OK)
    {
        BTUsartParseDataHandler(u8RecvChar);

        // Èç¹ûÊý¾Ý½ÓÊÕÍê³É£¬´¦ÀíÊý¾ÝÖ¡
        if (bt_state == BT_STATE_COMPLETE)
        {
           if(0==strcmp(bt_buffer,"1\r\n"))
           	{
           		memset(bt_buffer, 0, 50);	
				bt_state = BT_STATE_IDLE;
				g_bt_paired = 1;  
				return BT_CONFIG_SUCCESS;
           	}
        }
    }
	return BT_CONFIG_FAIL;
}


// À¶ÑÀÈÎÎñ´¦Àíº¯Êý//DBG_SerialPutString("AT+SLAV\r\n");//ÅäÖôӻú
static void Bluetooth_Config_Slave(void)
{
   if(g_bt_configured==0)
   	{
   	    if(g_bt_at_moded==0)
   	    {
   	    	BT_Enter_ATmode();
   	    }
		else
		{
			if(BT_Get_SLaveMode())
   		    {
   		    	g_bt_configured=1;
   		    }
		}	
   	}
}

// Ö÷´¦Àíº¯Êý
void Joystick_Process(Joystick_t *joy) {
    // ÌáÈ¡ y1 ¿ØÖÆÇ°½ø/ºóÍË
    int16_t motor_val = joy->y1;  // -100 ~ +100
    Set_Motor_PWM(motor_val);

    // ÌáÈ¡ x2 ¿ØÖÆתÏò
    int16_t steering_val = joy->x2;  // -100 ~ +100
    Set_Steering_PWM(steering_val);
}

// ʾÀýÊäÈë: "[joystick,-22,-2,0,0]"
void Parse_Joystick_Data(char *data) {
    Joystick_t joy = {0};

    if (strstr(data, "joystick") == NULL) return;

    // Ìø¹ý "[joystick,"
    char *ptr = strstr(data, "joystick");
    if (!ptr) return;
    ptr += 10;  // Ìø¹ý "joystick,"

    // ½âÎöËĸöÕûÊý
    char *end;
    joy.x1 = strtol(ptr, &end, 10);
    if (end == ptr) return;
    ptr = end + 1;

    joy.y1 = strtol(ptr, &end, 10);
    if (end == ptr) return;
    ptr = end + 1;

    joy.x2 = strtol(ptr, &end, 10);
    if (end == ptr) return;
    ptr = end + 1;

    joy.y2 = strtol(ptr, &end, 10);

    // ´¦ÀíÊý¾Ý
    Joystick_Process(&joy);
}


/*******************************************************************************
 * Function Name     : BT_Init
 * Description       : µ÷ÊÔ´òÓ¡ÂÖѯ
 * Input             : None
 * Output            : None
 * Return            : None
 * Author            : ¶Å¼ü
 * Modified Date:    : 2018Äê7ÔÂ23ÈÕ
 *******************************************************************************/
HIDO_VOID BT_Poll(void)
{
  if (uart6_dma_recv_len>0)//HAL_GPIO_ReadPin(GPIOE, GPIO_PIN_13) == GPIO_PIN_RESET
  {
       if (uart6_dma_recv_end_flag == 1) //½ÓÊÕÍê³É±êÖ¾
       	{
       	     Uart_Send(UART_ID_DBG, (HIDO_UINT8 *)uart6_dma_rxbuf,uart6_dma_recv_len);
       		// ½âÎöÊý¾Ý
            Parse_Joystick_Data(uart6_dma_rxbuf);
       	}
	    uart6_dma_recv_len = 0;//Çå³ý¼ÆÊý
        uart6_dma_recv_end_flag = 0;//Çå³ý½ÓÊÕ½áÊø±ê־λ
        memset(uart6_dma_rxbuf, 0, UART6_DMA_RX_BUF_SIZE);
        HAL_UART_Receive_DMA(&huart6, uart6_dma_rxbuf, UART6_DMA_RX_BUF_SIZE); //ÖØдò¿ªDMA½ÓÊÕ
  }
  #if 0
  else if(0==g_com_map[BT_SLAVE_STATUS])
   	{
	   DBG_SerialPutString("AT+ENAT\r\n");
	   //delay_ms(10);
       DBG_SerialPutString("AT+SLAV\r\n");
	   g_com_map[BT_SLAVE_STATUS]=1;
	   save_com_map_to_flash();
   	}
  #endif
  }