#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_Scheduler/AP_Scheduler.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_HAL/utility/sparse-endian.h>
#include "Sub.h"

extern const AP_HAL::HAL& hal;

#if HAL_WITH_UAVCAN
#include "UserCAN.h"


#define CAN_PACKET_SET_DUTY				                 0

#define CAN_PACKET_SET_RPM				                 3
#define CAN_PACKET_CONF_STORE_CURRENT_LIMITS			22
#define CAN_PACKET_CONF_STORE_CURRENT_LIMITS_IN 		24
#define CAN_PACKET_STATUS 					             9
#define CAN_PACKET_STATUS_3 					        15
#define CAN_PACKET_STATUS_4 					        16
#define CAN_PACKET_STATUS_5					            27

extern mavlink_motor_speed_t mav_motor_speed;

static const uint16_t USERCAN_SEND_TIMEOUT_US = 500;
//static const uint16_t COMMAND_MOTOR1 = 0x0301;
//static const uint16_t COMMAND_MOTOR1_RETURN = 0x0281;

static const uint16_t COMMAND_MOTOR1_ID = 0x01;
static const uint16_t COMMAND_MOTOR2_ID = 0x02;


static const uint8_t CAN_IFACE_INDEX = 0;

UserCAN::UserCAN()
{
    update_count_sent = 1;
	update_count_buffered = 1;
	send_ID = 0;
}
void UserCAN::init(uint8_t driver_index, bool enable_filters)
{
	

   _driver_index = driver_index;

   

    if (_initialized) {
       
        return;
    }

    AP_HAL::CANManager* can_mgr = hal.can_mgr[driver_index];

    if (can_mgr == nullptr) {
        
        return;
    }

    if (!can_mgr->is_initialized()) {
       
        return;
    }

    _can_driver = can_mgr->get_driver();

    if (_can_driver == nullptr) {
        
        return;
    }

    // start calls to loop in separate thread
    if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&UserCAN::loop, void), _thread_name, 4096, AP_HAL::Scheduler::PRIORITY_MAIN, 1)) {
        
        return;
    }

    _initialized = true;

    
    return;
}

void UserCAN::loop()
{
    uavcan::MonotonicTime timeout;
    const uint32_t timeout_us = MIN(AP::scheduler().get_loop_period_us(), USERCAN_SEND_TIMEOUT_US);

    while (true) {
		
        if (!_initialized) {
            // if not initialised wait 5ms
           
            hal.scheduler->delay_microseconds(5000);
            continue;
        }
		if (1) {
				  hal.scheduler->delay_microseconds(50);
				  
				 // continue;
			   }
		uavcan::CanFrame recv_frame;
		 if(read_frame(recv_frame, timeout)) {
				 motor_reply_data1_t reply_data;
                 memcpy(reply_data.data, recv_frame.data, sizeof(reply_data.data));
		 		if((recv_frame.id & 0x000000FF)>=1 && (recv_frame.id & 0x000000FF)<=6){//6个电调的ID分别为1，2，3，4，5,6
					 for (uint8_t i = 1; i < 7; i++)
					 {
				 			uint8_t COMMAND_MOTOR_ID=i;
				 			
				 			if((recv_frame.id&0x1FFFFFFFU) == (CAN_PACKET_STATUS<<8|COMMAND_MOTOR_ID)){
							
							int32_t temp = (int32_t)(reply_data.data[0]<<24)+(int32_t)(reply_data.data[1]<<16)+(int32_t)(reply_data.data[2]<<8)+(int32_t)reply_data.data[3];

							sub._telemetry[i-1].rpm =temp;
							sub._telemetry[i-1].totalcurrent = (int16_t)(reply_data.data[4]<<8)+(int16_t)reply_data.data[5];//总电流*10
							sub._telemetry[i-1].dutycycle = (int16_t)(reply_data.data[6]<<8)+(int16_t)reply_data.data[7];//占空比*1000
							//break;
							   
							}
							if((recv_frame.id&0x1FFFFFFFU) == ((CAN_PACKET_STATUS_4<<8|COMMAND_MOTOR_ID)&0x1FFFFFFFU)){
								sub._telemetry[i-1].toalcurrentIn =(int16_t)(reply_data.data[4]<<8)+(int16_t)reply_data.data[5];	//总输入电流
								sub._telemetry[i-1].mostemperature =(int16_t)(reply_data.data[0]<<8)+(int16_t)reply_data.data[1];//mos温度
								sub._telemetry[i-1].mottremperature = (int16_t)(reply_data.data[2]<<8)+(int16_t)reply_data.data[3];//电机温度
								//break;
							}
							if((recv_frame.id&0x1FFFFFFFU) == ((CAN_PACKET_STATUS_5<<8|COMMAND_MOTOR_ID)&0x1FFFFFFFU)){
									
								sub._telemetry[i-1].voltage = (int16_t)(reply_data.data[4]<<8)+(int16_t)reply_data.data[5];//总电压
								//break;
								
							}


					 }


		 		}

			

		}
		
			
			uint32_t nowtime = AP_HAL::micros();
			static uint32_t last_send_time = nowtime;
			if(nowtime -  last_send_time >1000000UL/400 )//发送周期  
			{   last_send_time = nowtime;
			    
			  	 send_ID++;
				const AP_Motors6DOF &motors6dof = AP::motors6dof();//6自由度电机计算出来的PWM
				 //motor_rotation_cmd_t mot_rot_cmd1={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
				 sent_command mot_rot_cmd1={0x00,0x00,0x00,0x00};

			   if (send_ID<7)
				{


				static int k = 0;
				k++;
				if(k>400)
				{
					gcs().send_text(MAV_SEVERITY_INFO, "motor_out1 %d %d %d \n", (int)motors6dof.motor_to_can[0],(int)motors6dof.motor_to_can[1],(int)motors6dof.motor_to_can[2]);
					gcs().send_text(MAV_SEVERITY_INFO, "motor_out2 %d %d %d \n", (int)motors6dof.motor_to_can[3],(int)motors6dof.motor_to_can[4],(int)motors6dof.motor_to_can[5]);
					//gcs().send_text(MAV_SEVERITY_INFO, "motor_out %f %f %d \f", (float)motor_to_can[3],(float)motor_to_can[4], (float)motor_to_can[5]);
					k=0;
				}

					mot_rot_cmd1.data[0] = (uint8_t)(motors6dof.motor_to_can[send_ID-1]>>24);
					mot_rot_cmd1.data[1] = (uint8_t)(motors6dof.motor_to_can[send_ID-1]>>16);
					mot_rot_cmd1.data[2] = (uint8_t)(motors6dof.motor_to_can[send_ID-1]>>8);
					mot_rot_cmd1.data[3] = (uint8_t)motors6dof.motor_to_can[send_ID-1];
					
			
			    	//mot_rot_frame1 = {((CAN_PACKET_SET_DUTY<<8 |send_ID) | 0x80000000U), mot_rot_cmd1.data, sizeof(mot_rot_cmd1.data)};

					mot_rot_frame1 = {((CAN_PACKET_SET_RPM<<8 |send_ID) | 0x80000000U), mot_rot_cmd1.data, sizeof(mot_rot_cmd1.data)};
					timeout = uavcan::MonotonicTime::fromUSec(AP_HAL::micros64() + timeout_us);
					if (!write_frame(mot_rot_frame1, timeout)) {
	                	continue;
	            	}
				   
				 }
			   else{
			   	send_ID = 0;
			   	}
			  
				
			}
			
  		

  		
						/*if ((be16toh)(reply_data.packed_data.command_code) == 0x5156)//be16toh改变字节顺序
						{
						    sub._telemetry[0].rpm =((be32toh)(reply_data.packed_data.data_reply));//速度        //(int32_t)(reply_data.data[4]<<24)+(int32_t)(reply_data.data[5]<<16)+(int32_t)(reply_data.data[6]<<8)+(int32_t)reply_data.data[7];//
						    
						}else if((be16toh)(reply_data.packed_data.command_code) == 0x5143){
							sub._telemetry[0].current =(float)((be32toh)(reply_data.packed_data.data_reply))/10;//电流
							
						}else if((be16toh)(reply_data.packed_data.command_code) == 0x5150){
							sub._telemetry[0].voltage =(float)((be32toh)(reply_data.packed_data.data_reply))/10;//电压
							
						}else if((be16toh)(reply_data.packed_data.command_code) == 0x5154){
							sub._telemetry[0].temperature =((be32toh)(reply_data.packed_data.data_reply));//温度
							
						}else if((be16toh)(reply_data.packed_data.command_code) == 0x4546){
							sub._telemetry[0].EF =((be16toh)(reply_data.packed_data.data_reply));//故障
							
						}*/
						/*static int f = 0;
						f++;
						if(f>200)
						{
						
						gcs().send_text(MAV_SEVERITY_INFO, " oshen_motor_show %d %f,%f ",(int)sub._telemetry[0].rpm,(float)sub._telemetry[0].current,(float)sub._telemetry[0].voltage);
						gcs().send_text(MAV_SEVERITY_INFO, " oshen_motor_show %d %x ",(int)sub._telemetry[0].temperature,(int)sub._telemetry[0].EF);
						
						//gcs().send_text(MAV_SEVERITY_INFO, " oshen_motor_show %x %x ",(int)((be16toh)(reply_data.command_code)), (int)((be32toh)(reply_data.data_reply)));

						f=0;
						}
					   
				 	}
					else{
						update_count2++;//没有收到指令ID
						}
						
				}
				 else{
				 	update_count2++;//没有收到任何ID
				 	}
				 if(update_count2==0 ||update_count2 >10){
				 	update_count2 = 0;
					send_stage=0;
					
				 	
				 	}
			

			//}
           

		}*/
		
		

				
    }
}
bool UserCAN::write_frame(uavcan::CanFrame &out_frame, uavcan::MonotonicTime timeout)
{
    // wait for space in buffer to send command
    uavcan::CanSelectMasks inout_mask;
    do {
        inout_mask.read = 0;
        inout_mask.write = 1 << CAN_IFACE_INDEX;
        _select_frames[CAN_IFACE_INDEX] = &out_frame;
        _can_driver->select(inout_mask, _select_frames, timeout);

        // delay if no space is available to send
        if (!inout_mask.write) {
            hal.scheduler->delay_microseconds(50);
        }
    } while (!inout_mask.write);

    // send frame and return success
    return (_can_driver->getIface(CAN_IFACE_INDEX)->send(out_frame, timeout, uavcan::CanIOFlagAbortOnError) == 1);
}

bool UserCAN::read_frame(uavcan::CanFrame &recv_frame, uavcan::MonotonicTime timeout)
{
    // wait for space in buffer to read
    uavcan::CanSelectMasks inout_mask;
    inout_mask.read = 1 << CAN_IFACE_INDEX;
    inout_mask.write = 0;
    _select_frames[CAN_IFACE_INDEX] = &recv_frame;
    _can_driver->select(inout_mask, _select_frames, timeout);

    // return false if no data is available to read
    if (!inout_mask.read) {
        return false;
    }
    uavcan::MonotonicTime time;
    uavcan::UtcTime utc_time;
    uavcan::CanIOFlags flags {};

    // read frame and return success
    return (_can_driver->getIface(CAN_IFACE_INDEX)->receive(recv_frame, time, utc_time, flags) == 1);
}

#endif // HAL_WITH_UAVCAN