#ifdef _WIN32
#define WIN32_LEAN_AND_MEAN             // 从 Windows 头文件中排除极少使用的信息

#include <windows.h>
#include <Winsock2.h>
#pragma comment(lib, "ws2_32.lib")

#else
#include <unistd.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <pthread.h>
#include <string.h>
#endif

#include <stdio.h>
#include <stdlib.h>
#include "include/motionGloveSDK.h"
#include "inc/motionGloveSdkHelper.h"
#include "inc/decodeAsGloveCSV.h"
#include "inc/readWriteLock.h"

#define SUIT_MGR_MAXLEN (32)

ReadWriteLock gloveSuitManagerLock;

GloveSuitManager_TypeDef gloveSuitManager[SUIT_MGR_MAXLEN];

SDK_Status_TypeDef sdk_status = SDK_Status_None;

//socket ( udp )
#ifdef _WIN32
SOCKET socketObj = INVALID_SOCKET; //socket list
#else
int socketObj = -1; // Linux下socket是int类型
#endif

//error msg
char  errorMsg[100] = { ' ' };

#ifdef _WIN32
//socket init - Windows only
WSADATA wsaData;
#endif

bool isNewPosePending = false;
int newPoseCount = 0;

//lock
#ifdef _WIN32
CRITICAL_SECTION m_cs;
#else
pthread_mutex_t m_cs = PTHREAD_MUTEX_INITIALIZER;
#endif

KHHS32PosAttitude_TypeDef KHHS32PosAttitude;

void resetGloveSuitManager()
{
	int arrLen = sizeof(gloveSuitManager) / sizeof(gloveSuitManager[0]);
	for (int i = 0; i < arrLen; i++)
	{
		gloveSuitManagerLock.writeLock();
		memset((void*)(&(gloveSuitManager[i])), 0x00, sizeof(GloveSuitManager_TypeDef));
		gloveSuitManagerLock.writeUnLock();
	}
}

/*
清空容器
*/
void resetKHHS32PosAttitude(KHHS32PosAttitude_TypeDef* input)
{
	memset((void*)input, 0x00, sizeof(KHHS32PosAttitude_TypeDef));
}

//thread function
#ifdef _WIN32
static DWORD WINAPI ThreadProc(LPVOID lpParam)
#else
static void* ThreadProc(void* lpParam)
#endif
{
	resetGloveSuitManager();

	//socket
	struct sockaddr_in from;
#ifdef _WIN32
	int fromlen = sizeof(from);
#else
	socklen_t fromlen = sizeof(from);
#endif

	const static int maxFrameLen = 32 * 1024;
	//recv buffer 如果开启巨型帧 最大可达到9KB

	char* rxBuffer = (char*)calloc(1, maxFrameLen);
	if (rxBuffer == 0)
	{
#ifdef _WIN32
		return -1;
#else
		return NULL;
#endif
	}

	while (1)
	{
		if (SDK_Status_ThreadPause == sdk_status)
		{
#ifdef _WIN32
			Sleep(100);
#else
			usleep(100 * 1000); // 100ms
#endif
			continue;
		}
#ifdef _WIN32
		if (socketObj == INVALID_SOCKET)
#else
		if (socketObj == -1)
#endif
		{
			break;
		}
		int nRet = recvfrom(socketObj, rxBuffer, maxFrameLen, 0, (struct sockaddr*)&from, &fromlen);
#ifdef _WIN32
		if (nRet != SOCKET_ERROR)
#else
		if (nRet >= 0)
#endif
		{
			rxBuffer[nRet] = 0;
#ifdef _WIN32
			KHHS32PosAttitude.remoteIP[0] = from.sin_addr.S_un.S_un_b.s_b1;
			KHHS32PosAttitude.remoteIP[1] = from.sin_addr.S_un.S_un_b.s_b2;
			KHHS32PosAttitude.remoteIP[2] = from.sin_addr.S_un.S_un_b.s_b3;
			KHHS32PosAttitude.remoteIP[3] = from.sin_addr.S_un.S_un_b.s_b4;
#else
			// Linux下IP地址处理
			uint32_t ip = ntohl(from.sin_addr.s_addr);
			KHHS32PosAttitude.remoteIP[0] = (ip >> 24) & 0xFF;
			KHHS32PosAttitude.remoteIP[1] = (ip >> 16) & 0xFF;
			KHHS32PosAttitude.remoteIP[2] = (ip >> 8) & 0xFF;
			KHHS32PosAttitude.remoteIP[3] = ip & 0xFF;
#endif

			KHHS32PosAttitude.remotePort = from.sin_port;
			KHHS32PosAttitude.sdkVersion = MotionGloveSDK_getVersion();

			//printf("FrameLength:%d Bytes\r\n", nRet);
			if (0 == isFrameCSVFormat(rxBuffer))
			{
				/*
				不是json字符串 有可能是csv字符串
				*/
				if (0 == isFrameCSV_Glove(rxBuffer))
				{
					KHHS32PosAttitude_TypeDef kpa;
					resetKHHS32PosAttitude(&kpa);

#ifdef _WIN32
					kpa.remoteIP[0] = from.sin_addr.S_un.S_un_b.s_b1;
					kpa.remoteIP[1] = from.sin_addr.S_un.S_un_b.s_b2;
					kpa.remoteIP[2] = from.sin_addr.S_un.S_un_b.s_b3;
					kpa.remoteIP[3] = from.sin_addr.S_un.S_un_b.s_b4;
#else
					kpa.remoteIP[0] = (ip >> 24) & 0xFF;
					kpa.remoteIP[1] = (ip >> 16) & 0xFF;
					kpa.remoteIP[2] = (ip >> 8) & 0xFF;
					kpa.remoteIP[3] = ip & 0xFF;
#endif

					kpa.remotePort = from.sin_port;
					kpa.sdkVersion = MotionGloveSDK_getVersion();

					int decodeRet = decodeAsGloveCSV_Func(rxBuffer,
						&kpa.hdr, 
						kpa.skeletons,
						KHHS32_SkeletonCount);
					if (decodeRet == -1)
						continue;
					if (kpa.hdr.leftFigureSkeletonCount == 0)
					{
						//printf("");
					}
					int index = GloveSuitManager_SearchByName(gloveSuitManager, SUIT_MGR_MAXLEN, kpa.hdr.AvatarName, kpa.hdr.AvatarNameLength);
					if (index == -1)
					{
						/*没有找到，添加*/
						index = GloveSuitManager_getFirstUnOccupy(gloveSuitManager, SUIT_MGR_MAXLEN);
						index = GloveSuitManager_AddName(gloveSuitManager, SUIT_MGR_MAXLEN, kpa.hdr.AvatarName, kpa.hdr.AvatarNameLength);
					}
					if (index != -1)
					{
						gloveSuitManagerLock.writeLock();
						/*找到了名称相同的容器*/
						resetKHHS32PosAttitude(&gloveSuitManager[index].KHHS32PosAttitude);
						
						/*拷贝一次*/
						memcpy(&gloveSuitManager[index].KHHS32PosAttitude, &kpa, sizeof(KHHS32PosAttitude_TypeDef));
						//printf("fn:%d\r\n", kpa.hdr.frameNumber);
						gloveSuitManager[index].isNewFramePending = 1;
						gloveSuitManagerLock.writeUnLock();
					}

					//printf("");
				}
			}
			else
			{
				/*
				不是json字符串 有可能是二进制字符
				*/
				uint16_t protocolVersion = 0;
				memcpy(&protocolVersion, rxBuffer, sizeof(protocolVersion));

				switch (protocolVersion)
				{
				// 协议版本处理
				}

				isNewPosePending = true;
				newPoseCount++;
			}
		}
		else
		{
#ifdef _WIN32
			printf_s(errorMsg, "recvfrom error no: %d.", nRet);
#else
			printf("recvfrom error no: %d.\n", nRet);
#endif
		}
#ifdef _WIN32
		Sleep(1);
#else
		usleep(1000); // 1ms
#endif
	}

	if (rxBuffer)
	{
		free(rxBuffer);
		rxBuffer = 0;
	}
#ifdef _WIN32
	if (socketObj != INVALID_SOCKET)
	{
		closesocket((SOCKET)socketObj);
		socketObj = INVALID_SOCKET;
	}
	return 0;
#else
	if (socketObj != -1)
	{
		close(socketObj);
		socketObj = -1;
	}
	return NULL;
#endif
}

#ifdef _WIN32
HANDLE hThreadProc = nullptr;
#else
pthread_t hThreadProc = 0;
#endif

int MotionGloveSDK_ListenUDPPort(int nPort)
{
	int nRet = 0;
	struct sockaddr_in local;
	local.sin_family = AF_INET;
	local.sin_port = htons(nPort); ///监听端口
	local.sin_addr.s_addr = INADDR_ANY; ///本机

#ifdef _WIN32
	{
		if (WSAStartup(MAKEWORD(2, 1), &wsaData)) //调用Windows Sockets DLL
		{
			memcpy(errorMsg, "socket init failed.\0", sizeof(errorMsg));
			WSACleanup();
			return FALSE;
		}
	}
#endif

	if (SDK_Status_None == sdk_status)
	{
		/*如果socket打开了 就先关闭它*/
#ifdef _WIN32
		if (socketObj != INVALID_SOCKET)
		{
			closesocket((SOCKET)socketObj);
			socketObj = INVALID_SOCKET;
		}
#else
		if (socketObj != -1)
		{
			close(socketObj);
			socketObj = -1;
		}
#endif

		socketObj = socket(AF_INET, SOCK_DGRAM, 0);
#ifdef _WIN32
		if (socketObj == INVALID_SOCKET)
#else
		if (socketObj == -1)
#endif
		{
			return -1;
		}
		
		int nRecvBuf = 32 * 1024;//接收缓冲区为32K
		int retSetSockOpt = setsockopt(socketObj, SOL_SOCKET, SO_RCVBUF, (const char*)&nRecvBuf, sizeof(int));

		int bindRet = bind(socketObj, (struct sockaddr*)&local, sizeof(local));
		nRet += bindRet;

		if (0 == nRet)
		{
			sdk_status = SDK_Status_OpenUDP;
		}
		else
		{
			/*udp打开失败 一般是由于端口已经被占用*/
		}
	}

	if (SDK_Status_OpenUDP == sdk_status)
	{
		/*socket打开了 那么就尝试创建线程*/
#ifdef _WIN32
		hThreadProc = CreateThread(
			NULL,              // default security attributes
			0,                 // use default stack size  
			ThreadProc,        // thread function 
			(void*)0,             // argument to thread function 
			CREATE_SUSPENDED,                 // use default creation flags 
			NULL);           // returns the thread identifier 
		if (0 != hThreadProc)
		{
			ResumeThread(hThreadProc);
			sdk_status = SDK_Status_ThreadRunning;
		}
		else
		{
			nRet += -1;
		}
#else
		int threadRet = pthread_create(&hThreadProc, NULL, ThreadProc, NULL);
		if (threadRet == 0)
		{
			sdk_status = SDK_Status_ThreadRunning;
		}
		else
		{
			nRet += -1;
		}
#endif
	}

	if (SDK_Status_ThreadRunning == sdk_status)
	{
		// 线程运行中
	}

	if (SDK_Status_ThreadPause == sdk_status)
	{
		/*如果socket打开了 就先关闭它*/
#ifdef _WIN32
		if (socketObj != INVALID_SOCKET)
		{
			closesocket((SOCKET)socketObj);
			socketObj = INVALID_SOCKET;
		}
#else
		if (socketObj != -1)
		{
			close(socketObj);
			socketObj = -1;
		}
#endif

		socketObj = socket(AF_INET, SOCK_DGRAM, 0);
#ifdef _WIN32
		if (socketObj == INVALID_SOCKET)
#else
		if (socketObj == -1)
#endif
		{
			return -1;
		}
		
		int nRecvBuf = 32 * 1024;//接收缓冲区为32K
		int retSetSockOpt = setsockopt(socketObj, SOL_SOCKET, SO_RCVBUF, (const char*)&nRecvBuf, sizeof(int));

		int bindRet = bind(socketObj, (struct sockaddr*)&local, sizeof(local));
		nRet += bindRet;

		if (0 == nRet)
		{
			sdk_status = SDK_Status_ThreadRunning;
		}
		else
		{
			/*udp打开失败 一般是由于端口已经被占用*/
			printf("Bind local port %d failed!\r\n", nPort);
			printf("Port %d is occupied!\r\n", nPort);
			nRet += -1;
		}
	}

	return nRet;
}

int MotionGloveSDK_CloseUDPPort(void)
{
	int ret = -1;

	if (SDK_Status_ThreadRunning == sdk_status)
	{
#ifdef _WIN32
		if (nullptr != hThreadProc)
#else
		if (hThreadProc != 0)
#endif
		{
#ifdef _WIN32
			if (socketObj != INVALID_SOCKET)
#else
			if (socketObj != -1)
#endif
			{
				/*关闭socket */
#ifdef _WIN32
				closesocket((SOCKET)socketObj);
				socketObj = INVALID_SOCKET;
#else
				close(socketObj);
				socketObj = -1;
#endif
				sdk_status = SDK_Status_ThreadPause;
				ret = 0;
			}
		}
	}

	if (SDK_Status_ThreadPause == sdk_status)
	{
		/*do nothing*/
	}

	return ret;
}

bool MotionGloveSDK_isGloveNewFramePending(const char* actorName)
{
	bool  ret = false;
	int actualNameLen = 0;
	for (int i = 0; i < ACTOR_NAME_LEN_MAX; i++)
	{
		if (actorName[i] != 0)
		{
			actualNameLen++;
		}
		else
		{
			break;
		}
	}
	if (actualNameLen == 0)
	{
		return ret;
	}

	int index = GloveSuitManager_SearchByName(gloveSuitManager, SUIT_MGR_MAXLEN, (uint8_t*)actorName, actualNameLen);
	if (index != -1)
	{
		if (gloveSuitManager[index].isNewFramePending !=0)
		{
			ret = true;
		}
		else
		{
			ret = false;
		}
	}

	return ret;
}

bool MotionGloveSDK_resetGloveNewFramePending(const char* actorName)
{
	bool ret = false;

	int actualNameLen = 0;
	for (int i = 0; i < ACTOR_NAME_LEN_MAX; i++)
	{
		if (actorName[i] != 0)
		{
			actualNameLen++;
		}
		else
		{
			break;
		}
	}
	if (actualNameLen == 0)
		return ret;

	int index = GloveSuitManager_SearchByName(gloveSuitManager, SUIT_MGR_MAXLEN, (uint8_t*)actorName, actualNameLen);
	if (index != -1)
	{
		gloveSuitManagerLock.writeLock();
		gloveSuitManager[index].isNewFramePending = 0;
		gloveSuitManagerLock.writeUnLock();
		ret = true;
	}

	return ret;
}

int  MotionGloveSDK_GetGloveSkeletonsFrame(const char* actorName, KHHS32PosAttitude_TypeDef* pKHHS32PosAttitude)
{
	int ret = -1;
	int actualNameLen = 0;
	for (int i = 0; i < ACTOR_NAME_LEN_MAX; i++)
	{
		if (actorName[i] != 0)
		{
			actualNameLen++;
		}
		else
		{
			break;
		}
	}
	if (actualNameLen == 0)
		return ret;
	
	if (0 == pKHHS32PosAttitude)
	{
		return -1;
	}

	int index = GloveSuitManager_SearchByName(gloveSuitManager, SUIT_MGR_MAXLEN, (uint8_t*)actorName, actualNameLen);
	if (index != -1)
	{
		gloveSuitManagerLock.readLock();
		memcpy((uint8_t*)pKHHS32PosAttitude, (uint8_t*)&gloveSuitManager[index].KHHS32PosAttitude, sizeof(KHHS32PosAttitude_TypeDef));
		gloveSuitManagerLock.readUnLock();
	}

	return 0;
}