#include "bsp_TCPClient.h"
#include "lwip/netif.h"
#include "lwip/ip.h"
#include "lwip/tcp.h"
#include "lwip/init.h"
#include "netif/etharp.h"
#include "lwip/udp.h"
#include "lwip/pbuf.h"
#include <stdio.h>
#include <string.h>
#include "BHBF_ROBOT.h"

#pragma pack (1) /*指定按1字节对齐*/

//typedef struct _LazorData
//{
//	float Feature_X; //特征点X
//	float Feature_Y; //特征点Y
//	float Feature_Z; //特征点Z
//	float Gap; //间隙
//	float WrongEdgeQuantity; //错边量
//	float Area; //面积
//} LazorData;

#pragma pack () /*取消指定对齐，恢复缺省对齐*/
LazorData *ReadLazorData;

uint8_t ReceivedData[3000];
static uint16_t receivedLength = 0;

void IntiazlieLazor();

void IntiazlieLazor1();

static err_t client_connected(void *arg, struct tcp_pcb *pcb, err_t err);

ip4_addr_t server_ip;
static struct tcp_pcb *client_pcb = NULL;

static void client_err(void *arg, err_t err)
{
	//printf("connect error! closed by core!!\n");
	//printf("try to connect to server again!!\n");

	//连接失败的时候释放TCP控制块的内存
	tcp_close(client_pcb);

	//重新连接
	TCP_Client_Init();
}



int index_0 = 0;

static err_t client_send(void *arg, struct tcp_pcb *tpcb)
{

	switch (index_0)
	{

		case 0:
		{
			uint8_t send_buf[4] =
			{ 0x00, 0x00, 0x00, 0x00 };
			tcp_write(tpcb, send_buf, sizeof(send_buf), 1);
			tcp_output(client_pcb);
			index_0++;
			break;
		}

		case 1:
		{

			uint8_t send_buf[4] =
			{ 0x09, 0x00, 0x00, 0x00 };
			//发送数据到服务器
			tcp_write(client_pcb, send_buf, sizeof(send_buf), 1);
			tcp_output(client_pcb);
			index_0++;
			break;
		}
		case 2:
		{
			uint8_t send_buf[4] =
			{ 0x02, 0x00, 0x00, 0x00 };
			tcp_write(tpcb, send_buf, sizeof(send_buf), 1);
			tcp_output(client_pcb);
			index_0++;
			break;
		}

		case 3:
		{

			uint8_t send_buf[4] =
			{ 0x08, 0x00, 0x04, 0x00 };
			//发送数据到服务器
			tcp_write(client_pcb, send_buf, sizeof(send_buf), 1);
			tcp_output(client_pcb);
			index_0++;
			break;
		}
	}

	return ERR_OK;
}

static err_t client_recv(void *arg, struct tcp_pcb *tpcb, struct pbuf *p,
		err_t err)
{
	if (p != NULL)
	{
		/* 接收数据*/


		HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"Weld_seam_Tracking",1);
		/* 返回接收到的数据*/
		//tcp_write(tpcb, p->payload, p->tot_len, 1);
		receivedLength = p->tot_len;
//		if (p->tot_len == 36)
//		{
		 	memcpy(ReceivedData, p->payload, p->tot_len);
//		}

		//memset(p->payload, 0, p->tot_len);
		tcp_recved(tpcb, p->tot_len);
		pbuf_free(p);
	} else if (err == ERR_OK)
	{
		//服务器断开连接
		//printf("server has been disconnected!\n");
		tcp_close(tpcb);

		//重新连接
		TCP_Client_Init();
		// tcp_connect(client_pcb, &server_ip, TCP_CLIENT_PORT, client_connected);
	}
	return ERR_OK;
}

static err_t client_connected(void *arg, struct tcp_pcb *pcb, err_t err)
{
	// printf("connected ok!\n");

	//注册一个周期性回调函数
	tcp_poll(pcb, client_send, 1);

	//注册一个接收函数
	tcp_recv(pcb, client_recv);

	return ERR_OK;
}

void TCP_Client_Init(void)
{
	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"Weld_seam_Tracking",0,Weld_seam_Tracking);
	ReadLazorData = &ReceivedData[12];

	/* 创建一个TCP控制块  */
	client_pcb = tcp_new();
	client_pcb->flags |= TF_NODELAY;
	IP4_ADDR(&server_ip, DEST_IP_ADDR0, DEST_IP_ADDR1, DEST_IP_ADDR2,
			DEST_IP_ADDR3);

	//printf("client start connect!\n");

	//开始连接
	tcp_connect(client_pcb, &server_ip, DEST_PORT, client_connected);

	//注册异常处理
	tcp_err(client_pcb, client_err);
}