#include "soft_can.h"
#include "system_init.h"
#include "main.h"
#include "stm32f4xx_hal_can.h"
#include "stm32f4xx_it.h"
#include "string.h"
#include "rtthread.h"
#include "can_data_handle.h"
#include "soft_bms.h"
#include "usart_data_handle.h"
#include "soft_update.h"
//#include "can_debug.h"
#include "common.h"
#include "string.h"
#include "soft_terrain.h"
#include "soft_obstacle.h"
#include "soft_version.h"
#include "soft_flow.h"
#include "soft_water.h"
#include "soft_seed_weight.h"


extern CAN_HandleTypeDef hcan1;
extern CAN_HandleTypeDef hcan2;
/**
 * @file    Can1_filter_init
 * @brief   CAN1过滤初始化
 * @param   none
 * @details
 * @author  Zhang Sir
 **/
extern CAN_HandleTypeDef hcan1;
extern CAN_HandleTypeDef hcan2;

void CAN1_filter_init()
{
    CAN_FilterTypeDef sFilterConfig;

    // 莫之比避障、仿地雷 0
    sFilterConfig.FilterBank = 0;
    sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
    sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT;
    sFilterConfig.FilterIdHigh = (CAN_MICOB_FILTER_ID >> 13) & 0xffff;
    sFilterConfig.FilterIdLow = ((CAN_MICOB_FILTER_ID << 3) & 0xffff) | CAN_ID_EXT;
    sFilterConfig.FilterMaskIdHigh = (CAN_MICOB_MASK_ID >> 13) & 0xffff;
    sFilterConfig.FilterMaskIdLow = ((CAN_MICOB_MASK_ID << 3) & 0xffff) | CAN_ID_EXT;
    sFilterConfig.FilterFIFOAssignment = CAN_FILTER_FIFO0;
    sFilterConfig.FilterActivation = ENABLE;
    sFilterConfig.SlaveStartFilterBank = 14;
    if (HAL_CAN_ConfigFilter(&hcan1, &sFilterConfig) != HAL_OK)
    {
        Error_Handler();
    }

    // 莫之比雷达升级 1
    sFilterConfig.FilterBank = 1;
    sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
    sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT;
    sFilterConfig.FilterIdHigh = (CAN_UAVRH_FILTER_ID >> 13) & 0xffff;
    sFilterConfig.FilterIdLow = ((CAN_UAVRH_FILTER_ID << 3) & 0xffff) | CAN_ID_EXT;
    sFilterConfig.FilterMaskIdHigh = (CAN_UAVRH_MASK_ID >> 13) & 0xffff;
    sFilterConfig.FilterMaskIdLow = ((CAN_UAVRH_MASK_ID << 3) & 0xffff) | CAN_ID_EXT;
    sFilterConfig.FilterFIFOAssignment = CAN_FILTER_FIFO0;
    sFilterConfig.FilterActivation = ENABLE;
    sFilterConfig.SlaveStartFilterBank = 14;
    if (HAL_CAN_ConfigFilter(&hcan1, &sFilterConfig) != HAL_OK)
    {
        Error_Handler();
    }
    // VK协议各种设备2
    sFilterConfig.FilterBank = 2;
    sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
    sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT;
    sFilterConfig.FilterIdHigh = (CAN_MC_FILTER_ID >> 13) & 0xffff;
    sFilterConfig.FilterIdLow = ((CAN_MC_FILTER_ID << 3) & 0xffff) | CAN_ID_EXT;
    sFilterConfig.FilterMaskIdHigh = (CAN_MC_MASK_ID >> 13) & 0xffff;
    sFilterConfig.FilterMaskIdLow = ((CAN_MC_MASK_ID << 3) & 0xffff) | CAN_ID_EXT;
    sFilterConfig.FilterFIFOAssignment = CAN_FILTER_FIFO0;
    sFilterConfig.FilterActivation = ENABLE;
    sFilterConfig.SlaveStartFilterBank = 14;
    if (HAL_CAN_ConfigFilter(&hcan1, &sFilterConfig) != HAL_OK)
    {
        Error_Handler();
    }
    
    //什么ID都接收
    sFilterConfig.FilterBank = 13;
    sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
    sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT;
    sFilterConfig.FilterIdHigh = 0;
    sFilterConfig.FilterIdLow = 0;
    sFilterConfig.FilterMaskIdHigh = 0;
    sFilterConfig.FilterMaskIdLow = 0;
    sFilterConfig.FilterFIFOAssignment = CAN_FILTER_FIFO0;
    sFilterConfig.FilterActivation = ENABLE;
    sFilterConfig.SlaveStartFilterBank = 14;
    if (HAL_CAN_ConfigFilter(&hcan1, &sFilterConfig) != HAL_OK)
    {
        Error_Handler();
    }
}

/**
 * @file    Can2_filter_init
 * @brief   CAN2过滤初始化
 * @param   none
 * @details
 * @author  Zhang Sir
 **/

void CAN2_filter_init()
{
    CAN_FilterTypeDef sFilterConfig;
    // //海盈智能电池 0
    sFilterConfig.FilterBank = 14;
    sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
    sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT;
    sFilterConfig.FilterIdHigh = (HEREWIN_FILTER_ID >> 13) & 0xffff;
    sFilterConfig.FilterIdLow = ((HEREWIN_FILTER_ID << 3) & 0xffff) | CAN_ID_EXT;
    sFilterConfig.FilterMaskIdHigh = (HEREWIN_MASK_ID >> 13) & 0xffff;
    sFilterConfig.FilterMaskIdLow = ((HEREWIN_MASK_ID << 3) & 0xffff) | CAN_ID_EXT;
    sFilterConfig.FilterFIFOAssignment = CAN_FILTER_FIFO1;
    sFilterConfig.FilterActivation = ENABLE;
    sFilterConfig.SlaveStartFilterBank = 14;
    if (HAL_CAN_ConfigFilter(&hcan2, &sFilterConfig) != HAL_OK)
    {
        Error_Handler();
    }

    // 正方电池1
    sFilterConfig.FilterBank = 15;
    sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
    sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT;
    sFilterConfig.FilterIdHigh = (CAN_ZHENGFANG >> 13) & 0xffff;
    sFilterConfig.FilterIdLow = ((((uint32_t)CAN_ZHENGFANG << 3) & 0xffff) | CAN_ID_EXT);
    sFilterConfig.FilterMaskIdHigh = (CAN_ZHENGFANG_MASk >> 13) & 0xffff;
    sFilterConfig.FilterMaskIdLow = ((((uint32_t)CAN_ZHENGFANG_MASk << 3) & 0xffff) | CAN_ID_EXT);
    sFilterConfig.FilterFIFOAssignment = CAN_FILTER_FIFO1;
    sFilterConfig.FilterActivation = ENABLE;
    sFilterConfig.SlaveStartFilterBank = 14;
    if (HAL_CAN_ConfigFilter(&hcan2, &sFilterConfig) != HAL_OK)
    {
        Error_Handler();
    }

    // VK协议各种设备2
    sFilterConfig.FilterBank = 16;
    sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
    sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT;
    sFilterConfig.FilterIdHigh = (CAN_MC_FILTER_ID >> 13) & 0xffff;
    sFilterConfig.FilterIdLow = ((CAN_MC_FILTER_ID << 3) & 0xffff) | CAN_ID_EXT;
    sFilterConfig.FilterMaskIdHigh = (CAN_MC_MASK_ID >> 13) & 0xffff;
    sFilterConfig.FilterMaskIdLow = ((CAN_MC_MASK_ID << 3) & 0xffff) | CAN_ID_EXT;
    sFilterConfig.FilterFIFOAssignment = CAN_FILTER_FIFO1;
    sFilterConfig.FilterActivation = ENABLE;
    sFilterConfig.SlaveStartFilterBank = 14;
    if (HAL_CAN_ConfigFilter(&hcan2, &sFilterConfig) != HAL_OK)
    {
        Error_Handler();
    }

    // VK智能电池 3
    sFilterConfig.FilterBank = 17;
    sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
    sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT;
    sFilterConfig.FilterIdHigh = (VK_BMS_FILTER_ID >> 13) & 0xffff;
    sFilterConfig.FilterIdLow = (((uint32_t)VK_BMS_FILTER_ID << 3) & 0xffff) | CAN_ID_EXT;
    sFilterConfig.FilterMaskIdHigh = (VK_BMS_MASK_ID >> 13) & 0xffff;
    sFilterConfig.FilterMaskIdLow = (((uint32_t)VK_BMS_MASK_ID << 3) & 0xffff) | CAN_ID_EXT;
    sFilterConfig.FilterFIFOAssignment = CAN_FILTER_FIFO1;
    sFilterConfig.FilterActivation = ENABLE;
    sFilterConfig.SlaveStartFilterBank = 14;
    if (HAL_CAN_ConfigFilter(&hcan2, &sFilterConfig) != HAL_OK)
    {
        Error_Handler();
    }
    // 什么ID都接收
    sFilterConfig.FilterBank = 27;
    sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
    sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT;
    sFilterConfig.FilterIdHigh = 0;
    sFilterConfig.FilterIdLow = 0;
    sFilterConfig.FilterMaskIdHigh = 0;
    sFilterConfig.FilterMaskIdLow = 0;
    sFilterConfig.FilterFIFOAssignment = CAN_FILTER_FIFO1;
    sFilterConfig.FilterActivation = ENABLE;
    sFilterConfig.SlaveStartFilterBank = 14;
    if (HAL_CAN_ConfigFilter(&hcan2, &sFilterConfig) != HAL_OK)
    {
        Error_Handler();
    }
}

/**
 * @file    HAL_CAN_RxFifo0MsgPendingCallback
 * @brief   can中断回调
 * @param
 * @details
 * @author  Zhang Sir
 **/
canpack canbuffer = {.Head[0] = 'V',
                     .Head[1] = 'K'};
CAN_RxHeaderTypeDef RxHeader;
uint8_t RxData[8];                    
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
    if(hcan == &hcan1)
    {
        if (__HAL_CAN_GET_FLAG(hcan, CAN_FLAG_FF0) != RESET)
        {
            __HAL_CAN_CLEAR_FLAG(hcan, CAN_FLAG_FF0);
        }
        else if (__HAL_CAN_GET_FLAG(hcan, CAN_FLAG_FOV0) != RESET)
        {
            __HAL_CAN_CLEAR_FLAG(hcan, CAN_FLAG_FOV0);
        }
        if (HAL_CAN_GetRxMessage(&hcan1, CAN_RX_FIFO0, &RxHeader, RxData) != HAL_OK) 
        {
            //Error_Handler();
            return;
        }
        
        if(RxHeader.FilterMatchIndex == 0)
        {
            // 莫之比前、后避、仿地
            switch (RxHeader.ExtId)
            {
            case CAN_OBSTACLE_1:
                can_recv_mocib_F_obstacle(RxData);
                break;
            case CAN_OBSTACLE_2:
                can_recv_mocib_B_obstacle(RxData);
                break;
            case CAN_UAVH30_MSG:
                can_recv_mocib_terrain(RxData);
                break;
            default:
                can_recv_mocib_version_info(RxHeader.ExtId, RxData, RxHeader.DLC);
                break;
            }
        }
        else if(RxHeader.FilterMatchIndex == 1)
        {
            can_recv_mocib_updata_read_set_hookfunction(RxHeader.ExtId, RxData);
        }
        else if(RxHeader.FilterMatchIndex == 2)
        {
            switch (((RxHeader.ExtId) & SRC_ID_MASK) >> 5)
            {
            // 液位计
            case CAN_NODEID_LIQUED:
                // liquid_recieved_hookfuction(bufhandle.Id, bufhandle.Data, bufhandle.Dlc);
                break;
            case CAN_NODEID_FLOW:
                Flow_recieved_hookfuction(RxHeader.ExtId, RxData, RxHeader.DLC);
                break;
            // 称重传感器、播撒器
            case CAN_NODEID_WEIGHT:
                // Weight_recieved_hookfuction(bufhandle.Id, bufhandle.Data, bufhandle.Dlc);
                break;
            // VK设备升级
            case CAN_NODEID_RADAR_UPDATE:
                switch (((RxHeader.ExtId) & MSG_ID_MASK) >> 19)
                {
                case CAN_MSGID_RADAR_START:
                    if ((RxHeader.ExtId & 0x3) == 1)
                    {
                        update_info.step_200_flag = true;
                    }
                    else if ((RxHeader.ExtId & 0x3) == 2)
                    {
                        Update_Dev_Bootversion_Function(RxData);
                    }
                    break;
                case CAN_MSGID_RADAR_ING:
                    if (update_info.vk_dev_pack_num == (RxData[1] + RxData[2] * 256))
                    {
                        update_info.vk_dev_pack_num++;
                        update_info.step_201_flag = true;
                    }
                    break;
                case CAN_MSGID_RADAR_END:
                    update_info.step_202_flag = true;
                    break;
                default:
                    break;
                }
                break;
            case CAN_NODEID_DISTRIBUTOR:
                // distributor_recieved_hookfuction(bufhandle.Id, bufhandle.Data, bufhandle.Dlc);
                break;
            default:
                break;
            }
        }
        else
        {
            Can1_decode_data_function(RxHeader);
        }


        // canbuffer.Head[2] = 1;
        // canbuffer.Ide = RxHeader.IDE;
        // if(canbuffer.Ide == CAN_ID_STD)
        //     canbuffer.Id = RxHeader.StdId;
        // else if(canbuffer.Ide == CAN_ID_EXT)
        //     canbuffer.Id = RxHeader.ExtId;
        // canbuffer.Dlc = RxHeader.DLC;
        // canbuffer.FilterIndex = RxHeader.FilterMatchIndex;
        // memcpy(canbuffer.Data,RxData,canbuffer.Dlc);

        // rkfifo_in(&can1_rkfifo,&canbuffer,sizeof(canpack));

        // memset(&canbuffer.Head[2],0,sizeof(canpack) - 2);

        // rt_sem_release(&can1_sem);
    }


}

void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
    if(hcan == &hcan2)
    {
        if (__HAL_CAN_GET_FLAG(hcan, CAN_FLAG_FF1) != RESET)
        {
            __HAL_CAN_CLEAR_FLAG(hcan, CAN_FLAG_FF1);
        }
        else if (__HAL_CAN_GET_FLAG(hcan, CAN_FLAG_FOV1) != RESET)
        {
            __HAL_CAN_CLEAR_FLAG(hcan, CAN_FLAG_FOV1);
        }
        if (HAL_CAN_GetRxMessage(&hcan2, CAN_RX_FIFO1, &RxHeader, RxData) != HAL_OK) 
        {
            //Error_Handler();
            return;
        }

        if(RxHeader.FilterMatchIndex == 0)
        {
            HerewinCanRecvHookFunction(RxHeader.ExtId, RxData, RxHeader.DLC);
        }
        else if(RxHeader.FilterMatchIndex == 1)
        {
            OkcellCanRecvHookFunction(RxHeader.ExtId, RxData, RxHeader.DLC);
        }
        else if(RxHeader.FilterMatchIndex== 3)
        {
            BMSCanRecvHookFunction(RxHeader.ExtId, RxData, RxHeader.DLC);
        }
        else
        {
            Can2_decode_data_function(RxHeader);
        }

    }



        // canbuffer.Head[2] = 2;
        // canbuffer.Ide = RxHeader.IDE;
        // if(canbuffer.Ide == CAN_ID_STD)
        //     canbuffer.Id = RxHeader.StdId;
        // else if(canbuffer.Ide == CAN_ID_EXT)
        //     canbuffer.Id = RxHeader.ExtId;
        // canbuffer.Dlc = RxHeader.DLC;
        // canbuffer.FilterIndex = RxHeader.FilterMatchIndex;
        // memcpy(canbuffer.Data,RxData,canbuffer.Dlc);

        // rkfifo_in(&can1_rkfifo,&canbuffer,sizeof(canpack));

        // memset(&canbuffer.Head[2],0,sizeof(canpack) - 2);

        // rt_sem_release(&can1_sem);
    
}

/**
 * @file    HAL_CAN_ErrorCallback
 * @brief   错误处理
 * @param
 * @details
 * @author  Zhang Sir
 **/
void HAL_CAN_ErrorCallback(CAN_HandleTypeDef *hcan)
{
    /*
    if (__HAL_CAN_GET_FLAG(hcan, CAN_FLAG_BOF) != RESET)
    {
        __HAL_CAN_CLEAR_FLAG(hcan, CAN_FLAG_ERRI);
    }
    if (__HAL_CAN_GET_FLAG(hcan, CAN_FLAG_EPV) != RESET)
    {
        __HAL_CAN_CLEAR_FLAG(hcan, CAN_FLAG_ERRI);
    }
    if (__HAL_CAN_GET_FLAG(hcan, CAN_FLAG_EWG) != RESET)
    {
        __HAL_CAN_CLEAR_FLAG(hcan, CAN_FLAG_ERRI);
    }
    __HAL_CAN_CLEAR_FLAG(hcan, CAN_FLAG_ERRI);
    */

    if(HAL_CAN_GetError(hcan) != HAL_CAN_ERROR_NONE)
    {
        HAL_CAN_ResetError(hcan);
    }
}

/**
 * @file    Can_Send_Msg
 * @brief   CAN口封装发送函数标准帧   //雷达/流量计/称重/前板CAN通讯 CAN1
                                    //水泵/喷头/电池/播撒器CAN通讯 CAN2
 * @param
 * @details
 * @author  Zhang Sir
 **/
CAN_TxHeaderTypeDef TxHeader;
void Can_Send_Msg_Func(uint8_t can_num, unsigned char *data, unsigned char length, unsigned int id,
                  uint32_t id_type)
{
    uint8_t Datas[8];
    unsigned char frame_num = (length - 1) / 8 + 1;
    CAN_HandleTypeDef *ptrcan = NULL;

    if(can_num == CANID1)
        ptrcan = &hcan1;
    else
        ptrcan = &hcan2;
    
    TxHeader.RTR = CAN_RTR_DATA;
    TxHeader.IDE = id_type;

    if (id_type == CAN_ID_STD)
    {
        TxHeader.StdId = id;
    }
    else if (id_type == CAN_ID_EXT)
    {
        TxHeader.ExtId = id;
    }

    for (char sequence = 0; sequence < frame_num; sequence++)
    {
        if (length - (sequence + 1) * 8 >= 0)
            TxHeader.DLC = 8;
        else
            TxHeader.DLC = length - sequence * 8;

        for (int i = 0; i < TxHeader.DLC; i++)
        {
            Datas[i] = *(data + sequence * 8 + i);
        }
/*
        if(planep.Candebug_flag == true || HAL_GetTick() < 10000)
           put_candata_to_rkfifo_tx(TxHeader, Datas);
*/
        uint32_t time2 = HAL_GetTick();
        uint32_t TxMailBox0 = 0;
        while (HAL_CAN_GetTxMailboxesFreeLevel(ptrcan) == 0 && HAL_GetTick() - time2 < 5)
            ;
        HAL_CAN_AddTxMessage(ptrcan, &TxHeader, Datas, &TxMailBox0);
    }
}

void put_date_to_can(uint8_t *buf,uint8_t par1,uint8_t par2,uint8_t par3,uint8_t par4,uint8_t par5,
                        uint8_t par6,uint8_t par7,uint8_t par8)
{
  uint8_t *ptr = buf;
  *(ptr++) = par1;
  *(ptr++) = par2;
  *(ptr++) = par3;
  *(ptr++) = par4;
  *(ptr++) = par5;
  *(ptr++) = par6;
  *(ptr++) = par7;
  *(ptr++) = par8;
}