/**
 * @file
 *   @brief The tcp interface process
 *
 *   This process connects any external MAVLink tcp device and prints data
 *
 *   @author Tsao, Chia-Cheng, <chiacheng.tsao@gmail.com>
 *
 */

#include <winsock2.h>
#include <stdio.h>   /* Standard input/output definitions */
#include <string.h>  /* String function definitions */
#include <camera4/mavlink.h>
#include <Windows.h>

#include "mavudp.h"

#pragma comment(lib,"wsock32.lib")

bool silent = true;              ///< Wether console output should be enabled
bool verbose = false;             ///< Enable verbose output
bool debug = false;               ///< Enable debug functions and output
bool isQuit = false;
/*
    Create a TCP socket
*/
DWORD WINAPI mavudp_thread (LPVOID pArg) 
{
    WSADATA wsa;
    SOCKET s;
    struct sockaddr_in server;

    printf("Initialising Winsock... \n");

    if (WSAStartup(MAKEWORD(2,2),&wsa) != 0)
    {
        printf("Failed. Error Code : %d \n",WSAGetLastError());
        return 1;
    }

    printf("Initialised.\n");

    //Create a socket
    if((s = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP)) == INVALID_SOCKET)
    {
        printf("Could not create socket : %d \n" , WSAGetLastError());
    }

    printf("Socket created.\n");


	memset(&server, 0, sizeof(server));
	server.sin_family = AF_INET;
	server.sin_addr.s_addr = INADDR_ANY;	//inet_addr("127.0.0.1");
	server.sin_port = htons(14551);
	
	bind(s,(struct sockaddr*)&server,sizeof(server));
    mavlink_status_t lastStatus;
    lastStatus.packet_rx_drop_count = 0;

    //Receive a reply from the peer
	SOCKADDR_IN   addrClient;    // 这个地址在recvfrom收到数据后，存储有发送端的地址
    int   len=sizeof(SOCKADDR); 
    int    n; 
    fd_set   readfds; 
    timeval   timeout={5,0};    // 超时时间为5秒,后面的那个数是微秒值
#define BUFLEN 1024
	char recvbuf[BUFLEN]={0};
    while(!isQuit)
    { 
        FD_ZERO(&readfds);            // 清空readfds与所有句柄的联系
        FD_SET(s,&readfds);        // 建立readfds与sock的联系
        n   =   select(0,   &readfds,   NULL,   NULL,   &timeout);//接收数据 
		//uint8_t cp;
		mavlink_message_t message;
		mavlink_status_t status;
		int msgReceived = 0;
        if(n> 0)
        { 
            msgReceived=recvfrom(s,(char *)recvbuf,BUFLEN,0,(SOCKADDR*)&addrClient,&len); 
            printf( "len=%d \n",msgReceived);
            if   (msgReceived   ==   SOCKET_ERROR) //错误
                printf("*** ERROR.  CODE =%d\n",WSAGetLastError());
			int processindex = 0;
			while(processindex < msgReceived )
			{
				// Check if a message could be decoded, return the message in case yes
				uint8_t tmpret = mavlink_parse_char(MAVLINK_COMM_1, recvbuf[processindex], &message, &status);

				if (lastStatus.packet_rx_drop_count != status.packet_rx_drop_count)
				{
					if (verbose || debug) printf("ERROR: DROPPED %d PACKETS! \n", status.packet_rx_drop_count);
					if (debug)
					{
						unsigned char v=recvbuf[processindex];
						fprintf(stderr,"%02x, ", v);
					}
				}
				lastStatus = status;
				++processindex;
			}
			

			// If a message could be decoded, handle it
			if(msgReceived)
			{
				//if (verbose || debug) std::cout << std::dec << "Received and forwarded tcp port message with id " << static_cast<unsigned int>(message.msgid) << " from system " << static_cast<int>(message.sysid) << std::endl;

				// Do not send images over serial port

				// DEBUG output
				if (debug)
				{
					fprintf(stderr,"Received tcp data: ");
					unsigned int i;
					uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
					unsigned int messageLength = mavlink_msg_to_send_buffer(buffer, &message);

					if (messageLength > MAVLINK_MAX_PACKET_LEN)
					{
						fprintf(stderr, "\nFATAL ERROR: MESSAGE LENGTH IS LARGER THAN BUFFER SIZE \n");
					}
					else
					{
						for (i=0; i<messageLength; i++)
						{
							unsigned char v=buffer[i];
							fprintf(stderr,"%02x ", v);
						}
						fprintf(stderr,"\n");
					}
				}

				if (verbose || debug)
					printf("Received message from tcp with ID #%d (sys:%d|comp:%d):\n", message.msgid, message.sysid, message.compid);

				/* decode and print */

				// For full MAVLink message documentation, look at:
				// https://pixhawk.ethz.ch/mavlink/

				// Only print every n-th message
				static unsigned int scaled_imu_receive_counter = 0;

				switch (message.msgid)
				{
					case MAVLINK_MSG_ID_FRONT_PARAMS:
					{
						mavlink_front_params_t frontparams;
						mavlink_msg_front_params_decode(&message, &frontparams);

						printf("Got message FRONT Params");
						printf("\t woffset: %f\n", frontparams.woffset);
						printf("\t hoffset: %f\n", frontparams.hoffset);
						printf("\t height: %f\n", frontparams.height);
						printf("\t width: %f\n", frontparams.width);
						printf("\n");
						calc_front_params(frontparams.height, frontparams.width,frontparams.hoffset,frontparams.woffset,goutterwidth,ginnerwidth,goutterheight,ginnerheight);
						
						postDisplay();
					}
					break;
					case MAVLINK_MSG_ID_CARRECT:
					{
						mavlink_carrect_t carrect;
						mavlink_msg_carrect_decode(&message,&carrect);
						calc_inner_outter_rect(carrect.innerwidth,carrect.innerheight,
							carrect.outterwidth,carrect.otterheight);
						//fourReshape();
						postDisplay();
					}
					break;
					case MAVLINK_MSG_ID_BACK_PARAMS:
					{
						mavlink_back_params_t backparam;
						mavlink_msg_back_params_decode(&message,&backparam);
						calc_back_params(backparam.height,backparam.width,
							backparam.hoffset,backparam.woffset,goutterwidth,ginnerwidth,goutterheight,ginnerheight);
						postDisplay();
					}
					break;
					case MAVLINK_MSG_ID_RIGHT_PARAMS:
					{
						mavlink_right_params_t rightparam;
						mavlink_msg_right_params_decode(&message,&rightparam);
						calc_right_params(rightparam.height,rightparam.width,
							rightparam.hoffset,rightparam.woffset,goutterwidth,ginnerwidth,goutterheight,ginnerheight);
						postDisplay();
					}
					break;
					case MAVLINK_MSG_ID_LEFT_PARAMS:
					{
						mavlink_left_params_t leftparam;
						mavlink_msg_left_params_decode(&message,&leftparam);
						calc_left_params(leftparam.height,leftparam.width,
							leftparam.hoffset,leftparam.woffset,goutterwidth,ginnerwidth,goutterheight,ginnerheight);
						postDisplay();
					}
					break;
					case MAVLINK_MSG_ID_CONFIG_CMD:
					{
						mavlink_config_cmd_t configcmd;
						mavlink_msg_config_cmd_decode(&message,&configcmd);
						gFrontShow = configcmd.display_flags & FRONT_COMP;
						gLeftShow = configcmd.display_flags & LEFT_COMP;
						gBackShow = configcmd.display_flags & BACK_COMP;
						gRightShow = configcmd.display_flags & RIGHT_COMP;
					}
					break;
					default:
						postDisplay();
						break;
				}
			}
        }
        else 
        { 
            printf( "\r%ds   recv   timeout",timeout.tv_sec); 
        } 
    } 

    return 0;
}
	static HANDLE hThread; 
void mavudp_init(void)
{


    hThread = CreateThread (NULL, 0, mavudp_thread, NULL, 0, NULL ); 

    return; 

}

void mavudp_exit(void)
{
	isQuit = true;
	WaitForSingleObject(hThread, INFINITE);//等待线程返回，用sleep()就太山寨了
	CloseHandle(hThread);//句柄默认值2 这里减1，线程函数执行完后释放资源。
}