#include "app_config.h"
#include "interface.h"
#include "app_request.h"
#include "app_param_manage.h"

uint16_t request_id = 0;


static void processRequestframe(uint16_t id)
{    
    uint32_t TempAcc = 0;
    uint8_t i = 0;  
    static uint8_t VehicleStaACC0 = 0;
    static uint8_t VehicleStaACC1 = 0;
    static uint8_t VehicleStaACC2 = 0;
    static uint8_t VehicleStaACC3 = 0;
    
    uint16_t RgExchangeTemp = 0;
    uint32_t Rg32ExchangeTemp = 0;    
//----------------------------------------------------------- 
//     printf("request_read_id:%d\n",id);   
     switch (id)//注意是高位在前，低位在后
     {
         case 0x2000://状态帧                 
            un_vehicle_Info_output.bit_data.frame_header = 0xCCAA;//帧头
            un_vehicle_Info_output.bit_data.frame_type = 0x2000;//帧类型
            un_vehicle_Info_output.bit_data.frame_length = 0x2900;//帧长
            un_vehicle_Info_output.bit_data.accumulated = VehicleStaACC0++;//累加值

            TempAcc = 0;
            for (i = 0; i < 40; i++)//累加前40个字节
            {
                TempAcc = TempAcc + (uint32_t)(un_vehicle_Info_output.arr[i]);
            }
            un_vehicle_Info_output.bit_data.crc  = (uint8_t)TempAcc;//累加值
            
            publishMessage(&un_vehicle_Info_output, 1);

         break;

         case 0x2100://电机帧
             un_motor_status_output.bit_data.frame_header = 0xCCAA;//帧头
             un_motor_status_output.bit_data.frame_type = 0x2100;//帧类型
             un_motor_status_output.bit_data.frame_length = 0x1E00;//帧长
             un_motor_status_output.bit_data.accumulated = VehicleStaACC1++;//累加值
             RgExchangeTemp = ( (uint16_t)getParam("maxTorq") + 300 ) *100 ;
             un_motor_status_output.bit_data.left_torque_limit =  ((RgExchangeTemp << 8) | (RgExchangeTemp >> 8));//左侧扭矩限制
             un_motor_status_output.bit_data.right_torque_limit = ((RgExchangeTemp << 8) | (RgExchangeTemp >> 8));//右侧扭矩限制
             un_motor_status_output.bit_data.left_power_in =   (((uint16_t)getParam("feedPwr") << 8) | ((uint16_t)getParam("feedPwr") >> 8));//左侧馈电功率
             un_motor_status_output.bit_data.right_power_in =  (((uint16_t)getParam("feedPwr") << 8) | ((uint16_t)getParam("feedPwr") >> 8));//右侧馈电功率
             un_motor_status_output.bit_data.left_power_out =  (((uint16_t)getParam("dispPwr") << 8) | ((uint16_t)getParam("dispPwr") >> 8));//左侧放电功率
             un_motor_status_output.bit_data.right_power_out = (((uint16_t)getParam("dispPwr") << 8) | ((uint16_t)getParam("dispPwr") >> 8));//右侧放电功率
             
             TempAcc = 0;
             for (i = 0; i < 31; i++)//累加前31个字节
             {
                 TempAcc = TempAcc + (uint32_t)(un_motor_status_output.arr[i]);
                 un_motor_status_output.bit_data.checksum = (uint8_t)TempAcc;//累加值
             }
             
            publishMessage(&un_motor_status_output, 1);                      
          break;

         case 0x2200:
             un_pid_output.bit_data.frame_header = 0xCCAA;//帧头
             un_pid_output.bit_data.frame_type = 0x2200;//帧类型
             un_pid_output.bit_data.frame_length = 0x3800;//帧长
             un_pid_output.bit_data.accumulated = VehicleStaACC2++;//累加值                 
             

             Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_kp")*1000);
             Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
             un_pid_output.bit_data.rc_straight_p = Rg32ExchangeTemp;//遥控直行P参数

             Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_ki")*1000);
             Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
             un_pid_output.bit_data.rc_straight_i = Rg32ExchangeTemp;//遥控直行I参数

             Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_kd")*1000);
             Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
             un_pid_output.bit_data.rc_straight_d = Rg32ExchangeTemp;//遥控直行D参数

             Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_il")*1000);
             Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
             un_pid_output.bit_data.auto_straight_p = Rg32ExchangeTemp;//自主直行P参数

             Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_ol")*1000);
             Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
             un_pid_output.bit_data.auto_straight_i = Rg32ExchangeTemp;//自主直行I参数

             Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_kd")*1000);
             Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
             un_pid_output.bit_data.auto_straight_d = Rg32ExchangeTemp;//自主直行D参数

             Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("crv_kp")*1000);
             Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
             un_pid_output.bit_data.rc_turn_p = Rg32ExchangeTemp;//遥控转弯P参数

             Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("crv_ki")*1000);
             Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
             un_pid_output.bit_data.rc_turn_i = Rg32ExchangeTemp;//遥控转弯I参数

             Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("crv_kd")*1000);
             Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
             un_pid_output.bit_data.rc_turn_d = Rg32ExchangeTemp;//遥控转弯D参数

             Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("crv_il")*1000);
             Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
             un_pid_output.bit_data.auto_turn_p = Rg32ExchangeTemp;//自主转弯P参数

             Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("crv_ol")*1000);
             Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
             un_pid_output.bit_data.auto_turn_i = Rg32ExchangeTemp;//自主转弯I参数

             Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_kd")*1000);
             Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
             un_pid_output.bit_data.auto_turn_d = Rg32ExchangeTemp;//自主转弯I参数

            TempAcc = 0;
            for (i = 0; i < 55; i++)//累加前40个字节
            {
                TempAcc = TempAcc + (uint32_t)(un_pid_output.arr[i]);
            }
            un_pid_output.bit_data.checksum  = (uint8_t)TempAcc;//累加值
            
            publishMessage(&un_pid_output, 1);
                      
         break;

         case 0x2300:
//            ETHTxtempArr[0] = 0xAA;
//            ETHTxtempArr[1] = 0xCC;
//            ETHTxtempArr[2] = 0x00;
//            ETHTxtempArr[3] = 0x23;
//            ETHTxtempArr[4] = 0;
//            ETHTxtempArr[5] = 27;
//            ETHTxtempArr[6] = VehicleStaACC3++;
//            //AIAO
//            memcpy(&ETHTxtempArr[7],(uint8_t *)&(UnAIAOSignal_1.ArrData.ArrRx[0]),17);//电池数据拷贝
//            ETHTxtempArr[24] = (u8)(RgCanToAiAOCnt>>8);
//            ETHTxtempArr[25] = (u8)RgCanToAiAOCnt;//累加值
//
//            TempAcc = 0;
//            for (i = 0; i < 26; i++)//累加前40个字节
//            {
//               TempAcc = TempAcc + (u32)(ETHTxtempArr[i]);
//            }
//            ETHTxtempArr[26]  = (uint8_t)TempAcc;//累加值
//
//            TxLen = 27;
//            UdpSendToData(UDPCB_2,(uint8_t *)(&un_analog_signal_output), 27, (uint8_t *)&ethernet_parameter.upper_ip[0], ethernet_parameter.target_upper_port);                 
         break;

         case 0x2400:           
             un_remote_control_output.bit_data.frame_header = 0xCCAA;//帧头
             un_remote_control_output.bit_data.frame_type = 0x2400;//帧类型
             un_remote_control_output.bit_data.frame_length = 0x1400;//帧长
             un_remote_control_output.bit_data.accumulated = VehicleStaACC3++;//累加值           
             //RCH_3
  
             TempAcc = 0;
             for (i = 0; i < 17; i++)//累加前40个字节
             {
                 TempAcc = TempAcc + (uint32_t)(un_remote_control_output.arr[i]);
             }
             un_remote_control_output.bit_data.crc  = (uint8_t)TempAcc;//累加值
             
             publishMessage(&un_remote_control_output, 1);                           
         break;

         case 0x2500:
              un_manual_control_output.bit_data.frame_type = 0x2500;

              TempAcc = 0;
              for (i = 0; i < 14; i++)//累加前40个字节
              {
                 TempAcc = TempAcc + (uint32_t)(un_manual_control_output.arr[i]);
              }
              un_manual_control_output.bit_data.crc_2  = (uint8_t)TempAcc;//累加值
              
              publishMessage(&un_manual_control_output, 1);
         break;

         case 0x2600:
              un_auto_control_output.bit_data.frame_type = 0x2600;    
            
              TempAcc = 0;
              for (i = 0; i < 27; i++)//累加前40个字节
              {
                 TempAcc = TempAcc + (uint32_t)(un_auto_control_output.arr[i]);
              }
              un_auto_control_output.bit_data.crc  = (uint8_t)TempAcc;//累加值

              publishMessage(&un_auto_control_output, 1);        
         break;

//            case 0x2700:
//                memcpy(&ETHTxtempArr[0],(uint8_t *)&RemoteControlRoomToVcuBuf,11);//手柄拷贝
//                ETHTxtempArr[2] = 0;//修改帧类型为0x0025
//                ETHTxtempArr[3] = 0x27;
//                ETHTxtempArr[11] = (u8)(RgETHToRemoteControlCnt>>8);
//                ETHTxtempArr[12] = (u8)RgETHToRemoteControlCnt;//累加值
//                TempAcc = 0;
//                for (i = 0; i < 13; i++)//累加前40个字节
//                {
//                    TempAcc = TempAcc + (u32)(ETHTxtempArr[i]);
//                }
//                ETHTxtempArr[13]  = (uint8_t)TempAcc;//累加值
//
//                TxLen = 14;
//                UdpSendToData(SocketId2,ETHTxtempArr,&TxLen,ip_addr,port);//测试
//            break;

//         case 0xFFFF:
//                memcpy(&ETHTxtempArr[0],(uint8_t *)&RemoteControlRoomToVcuBuf,11);//手柄拷贝
//                ETHTxtempArr[2] = 0x27;//修改帧类型为0x0025
//                ETHTxtempArr[3] = 0;
//                ETHTxtempArr[11] = (u8)RgETHToRemoteControlCnt;
//                ETHTxtempArr[12] = (u8)(RgETHToRemoteControlCnt>>8);//累加值
//                TempAcc = 0;
//                for (i = 0; i < 13; i++)//累加前40个字节
//                {
//                    TempAcc = TempAcc + (u32)(ETHTxtempArr[i]);
//                }
//                ETHTxtempArr[13]  = (uint8_t)TempAcc;//累加值
//
//                TxLen = 14;
//                UdpSendToData(SocketId2,ETHTxtempArr,&TxLen,ip_addr,port);//测试
//         break;
         default: break;
     }    
}



// 处理所有输入信号的函数
static void requestInput(void *signal_id)
{  
    uint16_t request16_temp = 0;
    
//-------------------------------------------------------- 
    if (signal_id == &diff_data)
    {
        request16_temp = (uint16_t)(int16_t)(diff_data.desired_speed * 100.0);
        un_vehicle_Info_output.bit_data.desired_speed = ((request16_temp << 8) | (request16_temp >> 8));//当前速度
        
        request16_temp = (uint16_t)(int16_t)(diff_data.desired_curvature * 10000.0);
        un_vehicle_Info_output.bit_data.desired_curvature = ((request16_temp << 8) | (request16_temp >> 8));//当前曲率                          

        request16_temp = (uint16_t)(int16_t)(diff_data.speed * 100.0);
        un_vehicle_Info_output.bit_data.speed = ((request16_temp << 8) | (request16_temp >> 8));//当前速度

        request16_temp = (uint16_t)(int16_t)(diff_data.curvature * 10000.0);
        un_vehicle_Info_output.bit_data.curvature = ((request16_temp << 8) | (request16_temp >> 8));//当前曲率 
        
        request16_temp = (uint16_t)(int16_t)(diff_data.left_motor_speed *6);
        un_vehicle_Info_output.bit_data.set_left_speed = ((request16_temp << 8) | (request16_temp >> 8));//当前速度

        request16_temp = (uint16_t)(int16_t)(diff_data.right_motor_speed *6);
        un_vehicle_Info_output.bit_data.set_right_speed = ((request16_temp << 8) | (request16_temp >> 8));//当前曲率         

    }
    else if(signal_id == &un_auto_computer_input)
    {
        un_vehicle_Info_output.bit_data.longitude = un_auto_computer_input.bit_data.longitude;//经度

        un_vehicle_Info_output.bit_data.latitude = un_auto_computer_input.bit_data.latitude;

        un_vehicle_Info_output.bit_data.altitude = un_auto_computer_input.bit_data.altitude;

        un_vehicle_Info_output.bit_data.heading_angle = un_auto_computer_input.bit_data.heading;//航向 
        
        memcpy(&un_auto_control_output.arr[0],&(un_auto_computer_input.arr[0]),25);//手柄拷贝            
    }
    else if(signal_id == &un_bms_input)
    {
        un_vehicle_Info_output.bit_data.battery_voltage = un_bms_input.bit_data.bus_voltage/10;//电池电压读取得的10mV，所以输出的是100mV单位，所以缩小10倍       
        un_vehicle_Info_output.bit_data.battery_soc = un_bms_input.bit_data.soc;//SOC
        un_vehicle_Info_output.bit_data.battery_current = un_bms_input.bit_data.bus_current;//电池电流      
    }    
    else if(signal_id == &un_motor_input1)
    {
//        un_motor_status_output.bit_data.left_wheel_speed = SWAP_ENDIAN_16( (uint16_t)((int16_t)(un_motor_input1.bit_data.MotCon_1Signal4) + 30000) );
              
//        un_motor_status_output.bit_data.left_torque = ((un_motor_input1.bit_data.torque << 8) | (un_motor_input1.bit_data.torque >> 8));//左侧扭矩 
//        un_motor_status_output.bit_data.left_voltage = ((un_motor_input1.bit_data.bus_voltage << 8) | (un_motor_input1.bit_data.bus_voltage >> 8));//左侧电压
//        un_motor_status_output.bit_data.left_fault_code = un_motor_input1.bit_data.fault_code;//左侧故障码        
      
    }  
    else if(signal_id == &un_motor_input2)
    {
//        un_motor_status_output.bit_data.right_wheel_speed = SWAP_ENDIAN_16 ( (uint16_t)((int16_t)(un_motor_input1.bit_data.MotCon_1Signal3) + 30000) );//左侧轮速 
//        un_motor_status_output.bit_data.right_torque = ((un_motor_input2.bit_data.torque << 8) | (un_motor_input2.bit_data.torque >> 8));//右侧扭矩 
//        un_motor_status_output.bit_data.right_fault_code = un_motor_input2.bit_data.fault_code;//右侧故障码             
//        un_motor_status_output.bit_data.right_voltage = ((un_motor_input2.bit_data.bus_voltage << 8) | (un_motor_input2.bit_data.bus_voltage >> 8));//右侧电压          
        
    }  
    else if(signal_id == &un_remote_control_input)
    {
        //RCH_3
        un_remote_control_output.bit_data.speed = ((un_remote_control_input.bit_data.speed << 8) | (un_remote_control_input.bit_data.speed >> 8));//遥控器期望速度
        un_remote_control_output.bit_data.curvature = ((un_remote_control_input.bit_data.curvature << 8) | (un_remote_control_input.bit_data.curvature >> 8));//遥控器期望曲率         
        memcpy(&un_remote_control_output.arr[11],&un_remote_control_input.arr[4],4);//遥控数据拷贝
    }  
    else if(signal_id == &un_manual_computer_input)
    {
        memcpy(&un_manual_control_output.arr[0],&(un_manual_computer_input.arr[0]),12);//手柄拷贝
    }
    else if(signal_id == &ethernet_fault_Info)
    {
        un_auto_control_output.arr[25] = (uint8_t)(ethernet_fault_Info.bit_data.auto_count>>8);
        un_auto_control_output.arr[26] = (uint8_t)(ethernet_fault_Info.bit_data.auto_count);//累加值  
        un_manual_control_output.arr[12] = (uint8_t)(ethernet_fault_Info.bit_data.manual_count>>8);
        un_manual_control_output.arr[13] = (uint8_t)(ethernet_fault_Info.bit_data.manual_count);//累加值                 
    }
      
    else if(signal_id == &can_fault_info)
    {
        un_remote_control_output.arr[15] = (uint8_t)(can_fault_info.bit_data.remote_count>>8);
        un_remote_control_output.arr[16] = (uint8_t)(can_fault_info.bit_data.remote_count);//累加值      
    }
    else if(signal_id == &un_request_frame)
    {
        request_id = REQUEST_READ_ID;
        processRequestframe(request_id);
    }
    else{}
}

// 修改APP模块的初始化函数
void requestAppInit(void)
{
    // 订阅消息，使用静态成员函数作为回调 参数回馈
    subscribe(&diff_data, requestInput);
    subscribe(&un_auto_computer_input, requestInput);    
    subscribe(&un_bms_input, requestInput);    
    subscribe(&un_motor_input1, requestInput);    
    subscribe(&un_motor_input2, requestInput);
    subscribe(&un_remote_control_input, requestInput);
    subscribe(&can_fault_info, requestInput);
    subscribe(&ethernet_fault_Info, requestInput);    
    
//     // 订阅消息，使用静态成员函数作为回调 参数回馈
    subscribe(&un_request_frame, requestInput);

    printf("app_request: initial OK \n");
}