#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <sys/time.h>
#include <sys/socket.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <iostream>
#include "CAN_send.h"
#include "g_variable.h"
#include "zxwl_log.hpp"

using namespace std;

/* ECU 报文的本地心跳 */
unsigned char ecu_hb_eps = 0, ecu_hb_speed = 0, ecu_hb_control = 0;

/* 设置CAN报文结构体中的初始值 */
void ecu_struct_init()
{
	// ZXWL_DEBUG << "ecu_struct_init" << ZXWL_REND;

	g_ecu_speed_cmd.gear_state = GEAR_N;	// 空挡
	g_ecu_speed_cmd.acceleration = 0x80;	// 加速度置零
	g_ecu_speed_cmd.epb_state = 2;			// 释放手刹
	g_ecu_speed_cmd.emergency_brake = 0b00; // 紧急制动无状态

	g_ecu_eps_cmd.steering_wheel_angle = 10800; // 方向置零
	g_ecu_eps_cmd.eps_state = 0x20;				// 自动控制方式（角度控制方式)
	g_ecu_eps_cmd.ang_velocity_max = 0x01;

	g_ecu_control_cmd.autopilot = 0b01;			   // 请求无人驾驶模式请求
	g_ecu_control_cmd.system_status = 0b01;		   // 启动完成
	g_ecu_control_cmd.system_reliability = 0b10;   // 最佳状态，完全无人控制
	g_ecu_control_cmd.system_failure_level = 0b00; // 无故障
}

/* 若超时，则向VCU发送EPS, SPEED和CONTROL报文 */
void ecu_send_packet(int fd)
{
	// ZXWL_DEBUG << "ecu_send_packet" << ZXWL_REND;

	/* 1.1 定时发送EPS控制报文 */
	if (Timer_GetReached(&t1))
	{
		send_eps_cmd(fd);
		Timer_Set(&t1, EPS_TIMER);
	}

	/* 1.2 定时发送车速(SPEED)控制报文 */
	if (Timer_GetReached(&t2))
	{
		send_speed_cmd(fd);
		Timer_Set(&t2, SPEED_TIMER);
	}

	/* 1.3 定时发送车身状态(CONTROL)控制报文 */
	if (Timer_GetReached(&t3))
	{
		send_control_cmd(fd);
		Timer_Set(&t3, CONTROL_TIMER);
	}

	/*
	Output++;
	if (Output % OutputSpeed == 0)
	{
		printf("gear_state--------------------------%d\n", g_ecu_speed_cmd.gear_state);
		printf("acceleration  %d\n", g_ecu_speed_cmd.acceleration - 128);
		printf("SPEED1        %f\n", (g_vcu_vstate1.speed - 50) / 3.6);
		printf("SPEED2        %d\n", g_vcu_vstate1.speed - 50);
		printf("ANGLE         %d\n", g_ecu_eps_cmd.steering_wheel_angle - 10800);
	}
	*/
}

/* 发送ecu_eps结构体变量至CAN网关 */
void send_eps_cmd(int s)
{
	g_ecu_eps_cmd.heartbeat = ecu_hb_eps++;

	/* 异或校验 */
	unsigned char xor_rslt = 0;
	unsigned char *ptr = (unsigned char *)(&g_ecu_eps_cmd);

	for (int i = 0; i < 7; ++i)
	{
		xor_rslt ^= ptr[i];
	}
	g_ecu_eps_cmd.xor_verify = xor_rslt;

	/* 发送CAN报文 */
	struct can_frame frame;
	frame.can_dlc = sizeof(g_ecu_eps_cmd);
	// ZXWL_DEBUG << "TO CAN STEERING : " << g_ecu_eps_cmd.steering_wheel_angle << ZXWL_REND;
	pthread_mutex_lock(&Mutex_EPS_DATA);
	memcpy(frame.data, &g_ecu_eps_cmd, frame.can_dlc);
	pthread_mutex_unlock(&Mutex_EPS_DATA);
	frame.can_id = ECU_EPS_CMD | CAN_EFF_FLAG;

	ecu_send(s, &frame);
	// printf("ecu_eps_send: ");
	// for (int j = 0; j < frame.can_dlc; j++)
	// {
	// 	printf("%02X ", frame.data[j]);
	// }
	// printf("\n");
	return;
}

/* 发送SPEED_CMD结构体内的CAN报文 */
void send_speed_cmd(int s)
{
	g_ecu_speed_cmd.heartbeat = ecu_hb_speed++;

	/* 异或校验 */
	unsigned char xor_rslt = 0;
	unsigned char *ptr = (unsigned char *)(&g_ecu_speed_cmd);

	for (int i = 0; i < 7; ++i)
	{
		xor_rslt ^= ptr[i];
	}
	g_ecu_speed_cmd.xor_verify = xor_rslt;

	struct can_frame frame;
	frame.can_dlc = sizeof(g_ecu_speed_cmd);
	// ZXWL_DEBUG << "TO CAN acceleration : " << static_cast<int>(g_ecu_speed_cmd.acceleration) << ZXWL_REND;
	pthread_mutex_lock(&Mutex_SPEED_DATA);
	memcpy(frame.data, &g_ecu_speed_cmd, frame.can_dlc);
	pthread_mutex_unlock(&Mutex_SPEED_DATA);
	frame.can_id = ECU_SPEED_CMD | CAN_EFF_FLAG;

	ecu_send(s, &frame);
	// printf("ecu_speed_send: ");
	// for (int j = 0; j < frame.can_dlc; j++)
	// {
	// 	printf("%02X ", frame.data[j]);
	// }
	// printf("\n");
	return;
}

void send_control_cmd(int s)
{
	/* 心跳 */
	g_ecu_control_cmd.heartbeat = ecu_hb_control++;

	struct can_frame frame;
	frame.can_dlc = sizeof(g_ecu_control_cmd);
	pthread_mutex_lock(&Mutex_CONTROL_DATA);
	memcpy(frame.data, &g_ecu_control_cmd, frame.can_dlc);
	pthread_mutex_unlock(&Mutex_CONTROL_DATA);
	frame.can_id = ECU_CONTROL_CMD | CAN_EFF_FLAG;
	ecu_send(s, &frame);
	/*CAN消息打印*/

	// printf("ecu_control_send: ");
	// for (int j = 0; j < frame.can_dlc; j++)
	// {
	// 	printf("%02X ", frame.data[j]);
	// }
	// printf("\n");

	return;
}

void ecu_send(int s, struct can_frame *frame)
{
	// if(ctrl_mode==CTRL_MODE_AD)
	//{
	can_send(s, frame);
	//}
	return;
}

int can_send(int s, struct can_frame *frame)
{
	int nbytes;
	nbytes = write(s, frame, sizeof(struct can_frame));
	// printf("发送了----------------%d\n", nbytes);
	// ZXWL_DEBUG << "发送了---------------- " << nbytes << ZXWL_REND;
	return nbytes;
}
