

#include "Execute3dm.h"
#include "../Process3dm/Execute3dxStruct.h"
#include <stdio.h>
#include <stdlib.h>



ORDER_PACKET SrvPktQueue[PKT_QUEUE_SIZE];  //(this exists in "fast" memory)
double SrvRef;
double SrvMaxRef;
extern double ClntRef;
 
int SrvPktFillIdx;
QUEUE_FILL_STATE SrvQueueFillState;
int SrvPktExeIdx;

bool UpdatePktHead;
bool StopSimu = FALSE;
bool ServiceQueue = FALSE;
extern unsigned int NurbCount;

ORDER_ELEMENT * pOrderElmt = NULL;
ORDER_ELEMENT * pStartingOrderElmt = NULL;

CMD_PACKET CmdPacket;
extern RSP_PACKET RspPacket;

extern FILE* file_3dx_fp; 


void DoOneLoopClntKern(void);
void IssueClientCmd(CMD_CODE);
void LoadOrderElements(void);
void FreeOrderElements(void);
void InitSrvPktQueue(void);
void UpdateSrvReference(void);
void DebugOrderElements(void);
extern void ExecuteSpline(ORDER_ELEMENT *);
extern void UpdateClntReference(void);

int ServerNode(void)
{


		//build the "pOrderElmt" list and determine
	    //max. references.
	LoadOrderElements();

#define DEBUG_ORDER_ELEMENTS

#ifdef DEBUG_ORDER_ELEMENTS

	DebugOrderElements();


#else
	DWORD WaitResult;
	BOOL EventResult;
	int i = 0;

		//Initialize queues and associated parameters for
		//for both client and server.
	InitSrvPktQueue();
		//issue "CMD_Init" to client.
	IssueClientCmd(CMD_Init);

       

	
		//allow server client threads to start.
	Sleep(1000);


		//Issue "CMD_start" to client.
	IssueClientCmd(CMD_Start);
		//set event which allows
		//"ServerKernel()" to start running.
//	EventResult = SetEvent(hStartServerEvent);
		

	while(1)
	{
//		WaitResult = WaitForSingleObject(hSrvKernCmdEvent, INFINITE);



		if(ServiceQueue)
		{
			
			//NOTE: We follow through updating the 
			//"SrvPktQueue[]". However for now, only the
			//client is generating trajectory.


		 
	 
			if(UpdatePktHead)
			{
				if(pOrderElmt->pNextOrderElmt)
				{
					if(SrvQueueFillState == QUEUE_FILL_negative)
					{	//set index back to positive bias.
						for(i = 0; i < PKT_QUEUE_SIZE; i++)
						{
							if(pOrderElmt->pNextOrderElmt)
							{
								pOrderElmt = pOrderElmt->pNextOrderElmt;
								SrvPktFillIdx++;
								if(SrvPktFillIdx == PKT_QUEUE_SIZE)
									SrvPktFillIdx = 0;
							}
							else
								break;
						}
						SrvQueueFillState = QUEUE_FILL_positive;
					}	//now, advance the index and order element
					if(pOrderElmt->pNextOrderElmt)
					{
						pOrderElmt = pOrderElmt->pNextOrderElmt;
						SrvPktFillIdx++;
						if(SrvPktFillIdx == PKT_QUEUE_SIZE)
							SrvPktFillIdx = 0;
					}
				}
			}
			else
			{
				if(pOrderElmt->pPrevOrderElmt)
				{
					if(SrvQueueFillState == QUEUE_FILL_positive)
					{	//set index back to negative bias.
						for(i = 0; i < PKT_QUEUE_SIZE; i++)
						{
							if(pOrderElmt->pPrevOrderElmt)
							{
								pOrderElmt = pOrderElmt->pPrevOrderElmt;	
								SrvPktFillIdx--;
								if(SrvPktFillIdx == -1)
									SrvPktFillIdx = PKT_QUEUE_SIZE - 1;
							}
							else
								break;
						}
						SrvQueueFillState = QUEUE_FILL_negative;
					}   //now, advance the index and order element
					if(pOrderElmt->pPrevOrderElmt)
					{
						pOrderElmt = pOrderElmt->pPrevOrderElmt;	
						SrvPktFillIdx--;
						if(SrvPktFillIdx == -1)
							SrvPktFillIdx = PKT_QUEUE_SIZE - 1;
					}
				}
			}

			if(pOrderElmt->Order == 4)
			{
				SrvPktQueue[SrvPktFillIdx].OrderObject.Order4 = 
				pOrderElmt->pOrderObj->Order4;
				SrvPktQueue[SrvPktFillIdx].Order = 4;
			}
			else if(pOrderElmt->Order == 3)
			{
				SrvPktQueue[SrvPktFillIdx].OrderObject.Order3 = 
				pOrderElmt->pOrderObj->Order3;
				SrvPktQueue[SrvPktFillIdx].Order = 3;
			}
			else if(pOrderElmt->Order == 2)
			{
				SrvPktQueue[SrvPktFillIdx].OrderObject.Order2 = 
				pOrderElmt->pOrderObj->Order2;
				SrvPktQueue[SrvPktFillIdx].Order = 2;
			}

			ServiceQueue = FALSE;
	 

			


			//fill "CMD_PACKET" with either new "ORDER_PACKET"
			//and issue either "CMD_UpdtPktHead" or 
			//"CMD_UpdtPktTail" appropriately.
			
				//(** Temporary, just signal the client **)
			IssueClientCmd(CMD_NoCmd);

//			EventResult = SetEvent(hCmdEvent);
//			WaitResult = WaitForSingleObject(hRspEvent, INFINITE);

			

//			EventResult = SetEvent(hSrvKernRspEvent);

		}
		else if(StopSimu)
		{
			//issue "CMD_PollClntEndRef" to client
			//and wait for response. Continue to
			//do this until "ClntRef" has reached
			//"0" or "ClntMaxRef"(inorder to do
			// the above, we must
			//continuely call "DoOneLoopClntKern()"
			//within the loop above 
			//to keep "ClientKernel()" running).

			break;
		}

	}

#endif

		//free all objects referenced by "pOrderElmt"
	FreeOrderElements();




	return 1;
}


void ServerKernel( void )
{

	DWORD WaitResult;
	BOOL EventResult;
	


	//"wait for "start server" event.
//	WaitResult = WaitForSingleObject(hStartServerEvent, INFINITE);

	while(1)
	{

	 
		DoOneLoopClntKern();


		ServiceQueue = FALSE;

		if(SrvPktQueue[SrvPktExeIdx].Order == 4)
		{
			if(SrvRef >= SrvPktQueue[SrvPktExeIdx].OrderObject.Order4.Tip4[3])
			{
				UpdatePktHead = TRUE;
				ServiceQueue = TRUE;
			}
			else if(SrvRef < SrvPktQueue[SrvPktExeIdx].OrderObject.Order4.Ti[0])
			{
				UpdatePktHead = FALSE;
				ServiceQueue = TRUE;
			}
		}
		else if(SrvPktQueue[SrvPktExeIdx].Order == 3)
		{
			if(SrvRef >= SrvPktQueue[SrvPktExeIdx].OrderObject.Order3.Tip3[2])
			{
				UpdatePktHead = TRUE;
				ServiceQueue = TRUE;

			}
			else if(SrvRef < SrvPktQueue[SrvPktExeIdx].OrderObject.Order3.Ti[0])
			{
				UpdatePktHead = FALSE;
				ServiceQueue = TRUE;
			}
		}
		else if(SrvPktQueue[SrvPktExeIdx].Order == 2)
		{
			if(SrvRef >= SrvPktQueue[SrvPktExeIdx].OrderObject.Order2.Tip2[1])
			{
				UpdatePktHead = TRUE;
				ServiceQueue = TRUE;

			}
			else if(SrvRef < SrvPktQueue[SrvPktExeIdx].OrderObject.Order2.Ti[0])
			{
				UpdatePktHead = FALSE;
				ServiceQueue = TRUE;
			}
		}
		if(ServiceQueue)
		{
	 
			if(UpdatePktHead == TRUE)
			{
				SrvPktExeIdx++;
				if(SrvPktExeIdx == PKT_QUEUE_SIZE)
					SrvPktExeIdx= 0;

			}
			else
			{
				SrvPktExeIdx--;
				if(SrvPktExeIdx == -1)
					SrvPktExeIdx = PKT_QUEUE_SIZE - 1;

			}

				
//			EventResult = SetEvent(hSrvKernCmdEvent);
//			WaitResult = WaitForSingleObject(hSrvKernRspEvent, INFINITE);

		}

		UpdateSrvReference();
	 
		//the "server" could itself be a trajectory generator.
		//For this simulation, only the client generates
		//trajectory, so here we are only looking for
		//the end of the reference.
		if((SrvRef < 0) || (SrvRef >= SrvMaxRef))
		{
			if(SrvRef <= 0)
				SrvRef = 0;
			else
				SrvRef = SrvMaxRef;

			//flag to stop simulation
			StopSimu = TRUE;
//			EventResult = SetEvent(hSrvKernCmdEvent);

		}
 

	}





}



void DoOneLoopClntKern(void)
{
	DWORD WaitResult;
	BOOL EventResult;

				//this event set is used only to keep
			//"ClientKernel()" running. It is not
			//part of the our simulated communications
			//between "ServerKernel()<->"ServerNode()",
			//"ClientKernel()"<->"ClientNode()", and
			//"ServerNode()"<->"ClientNode()".
//	EventResult = SetEvent(hClntKernCmdEvent);
//	WaitResult = WaitForSingleObject(hClntKernRspEvent, INFINITE);
}

void IssueClientCmd(CMD_CODE CmdCode)
{
	DWORD WaitResult;
	BOOL EventResult;
	CmdPacket.CmdCode = CmdCode;
//	EventResult = SetEvent(hCmdEvent);
//	WaitResult = WaitForSingleObject(hRspEvent, INFINITE);

}


void LoadOrderElements(void)
{
	unsigned int i,j;
	ORDER_OBJECT OrderObject;
	ORDER_ELEMENT * pPrevOrderElmt;
	ORDER_ELEMENT * pCurOrderElmt = NULL;
	unsigned int Order;
	unsigned int OrderCount;
	for(i = 0; i < NurbCount; i++)
	{
		fread(&Order, sizeof(unsigned int), 1, file_3dx_fp);
		fread(&OrderCount, sizeof(unsigned int), 1, file_3dx_fp);
		for(j = 0; j < OrderCount; j++)
		{
			pPrevOrderElmt = pCurOrderElmt;
			pCurOrderElmt = new ORDER_ELEMENT;
			if(pPrevOrderElmt)
			{
				pPrevOrderElmt->pNextOrderElmt = pCurOrderElmt;
			}
			else
			{
				pOrderElmt = pCurOrderElmt;
				pStartingOrderElmt = pCurOrderElmt;
			}
			pCurOrderElmt->pPrevOrderElmt = pPrevOrderElmt;
			pCurOrderElmt->pNextOrderElmt = NULL;
			switch(Order)
			{
			case 2:
				fread(&OrderObject, sizeof(ORDER_2), 1, file_3dx_fp);
				pCurOrderElmt->pOrderObj = (ORDER_OBJECT *) new ORDER_2;
				pCurOrderElmt->Order = 2;
				pCurOrderElmt->pOrderObj->Order2 = OrderObject.Order2;
				break;
			case 3:
				fread(&OrderObject, sizeof(ORDER_3), 1, file_3dx_fp);
				pCurOrderElmt->pOrderObj = (ORDER_OBJECT *) new ORDER_3;
				pCurOrderElmt->Order = 3;
				pCurOrderElmt->pOrderObj->Order3 = OrderObject.Order3;
				break;
			case 4:
				fread(&OrderObject, sizeof(ORDER_4), 1, file_3dx_fp);
				pCurOrderElmt->pOrderObj = (ORDER_OBJECT *) new ORDER_4;
				pCurOrderElmt->Order = 4;
				pCurOrderElmt->pOrderObj->Order4 = OrderObject.Order4;
				break;
			}
		}
	}
	switch(Order)
	{
	case 2:
			SrvMaxRef = pCurOrderElmt->pOrderObj->Order2.Tip2[1];
			break;
	case 3:
			SrvMaxRef = pCurOrderElmt->pOrderObj->Order3.Tip3[2];
			break;
	case 4:
			SrvMaxRef = pCurOrderElmt->pOrderObj->Order4.Tip4[3];
			break;
	}  //record max. reference for client.
	CmdPacket.InitQueue.MaxRef = SrvMaxRef;

}

void FreeOrderElements(void)
{
	ORDER_ELEMENT * pNextOrderElmt;
	pOrderElmt = pStartingOrderElmt;
	while(pOrderElmt)
	{
		pNextOrderElmt = pOrderElmt->pNextOrderElmt;
		delete pOrderElmt->pOrderObj;
		delete pOrderElmt;
		pOrderElmt = pNextOrderElmt;
	}
}

void InitSrvPktQueue(void)
{
	int i;
			//Order "zero" means NULL packet.
	memset(SrvPktQueue, 0, PKT_QUEUE_SIZE*sizeof(ORDER_PACKET));
	for(i = PKT_QUEUE_SIZE/2; i < PKT_QUEUE_SIZE; i++)
	{
		if(pOrderElmt)
		{
			switch(pOrderElmt->Order)
			{
			case 2:
				SrvPktQueue[i].Order = 2;
				SrvPktQueue[i].OrderObject.Order2 = pOrderElmt->pOrderObj->Order2;
				break;
			case 3:
				SrvPktQueue[i].Order = 3;
				SrvPktQueue[i].OrderObject.Order3 = pOrderElmt->pOrderObj->Order3;
				break;	
			case 4:
				SrvPktQueue[i].Order = 4;
				SrvPktQueue[i].OrderObject.Order4 = pOrderElmt->pOrderObj->Order4;
				break;	
			}
			if(i < PKT_QUEUE_SIZE - 1)
			{
				if(pOrderElmt->pNextOrderElmt) 
					pOrderElmt = pOrderElmt->pNextOrderElmt;
				else
					break;
			}
		}
		else
			break;
			
	}
			//build the initialization queue to be set to the client.
	memcpy(CmdPacket.InitQueue.OrdPktInitQueue, SrvPktQueue, PKT_QUEUE_SIZE*sizeof(ORDER_PACKET));

	SrvPktExeIdx = PKT_QUEUE_SIZE/2;
		//(NOTE: the current "pOrderElmt" is now alligned with this index)
	SrvPktFillIdx = PKT_QUEUE_SIZE - 1;

		//we start off with fill state in positive direction.
	CmdPacket.InitQueue.InitQueueFillState = QUEUE_FILL_positive;
	SrvQueueFillState = QUEUE_FILL_positive;
 
		//record initial fill index for client
	CmdPacket.InitQueue.InitPktFillIdx = SrvPktFillIdx;

	
	
}

void UpdateSrvReference(void)
{
		//for now, the reference is generated by accumulating
		//a constant.
	SrvRef = SrvRef + REF_INCREMENT;

}

#ifdef DEBUG_ORDER_ELEMENTS

void DebugOrderElements(void)
{

	// ***** Here we are evaluating the first three NURBS of "my_curve_Rhino3d.3dx",
    //		 which is generated form "my_curves_Rhino3d_AllNurbs.3dm" by project
	//       "Process3dm".
	//
	//		 File "my_curves_Rhino3d.xyz" is generated here. We can then
	//       use Rhino3d GUI to open "my_curves_Rhino3_AllNurbs.3dm and then import
	//       "my_curve_Rhino3d.xyz" to compare the accuracy of the generated points.
	//
	//		 We are doing this test here to see what needs to be fixed in
	//		 project "Process3dm" as well as what needs to be modified in 
	//		 function "ServerKernel()" which has the logic necessary to
	//       control the "SrvPktQueue[]" used in our "Client/Server" simulation code.
	//
	//		 We should go no further with the "Client/Server" simulation test
	//		 until the issues listed below are resolved.
	//


	// ***** The first ORDER_OBJECT of the Order4 NURBS is loaded here.
	ORDER_ELEMENT * pCurrentOrderElmt = pOrderElmt;

 
	int flag1 = 0;
	int flag2 = 0;
	int flag3 = 0;
	int flag4 = 0;
	int flag5 = 0;
//	int i,j;
 

	while(1)
	{

 
		if((ClntRef >= 28.457509479747323) && !flag1)
		{
			// **** Set for execution of second ORDER_OBJECT of Order4 NURB ***


		  pCurrentOrderElmt = pCurrentOrderElmt->pNextOrderElmt;
		  
		  flag1 = 1;
		}
		else if((ClntRef >= 50.643598139805313) && !flag2)
		{
			// **** Set for execution of third segment of Order4 NURB (free hand curve) ***

			pCurrentOrderElmt = pCurrentOrderElmt->pNextOrderElmt;

			// *** here is just a quick trick to offset the generated points so
			//     I can see where they are starting and stopping.
	//		for(i = 0; i < 4; i++)
	//			for(j = 0; j < 3; j++)
	//				pCurrentOrderElmt->pOrderObj->Order4.Pi[i].D[j] += 1;
			
			flag2 = 1;

			// ****** Conclusion for Order4 NURBS execution.*************
			//
			//        We must generate an ORDER_OBJECT for every knot,
			//        including the beginning and ending knots that have
			//        "mult == 4" ("mult == 3 in .dmp file). 
			//        For this particular NURBS curve, which
			//		  has three (3) ORDER_OBJECT's, I tried skipping the
			//		  second ORDER_OBJECT. 
			//
			//		  The generated points were incorrect, even though it
			//		  seems that all the information need to generate
			//		  generate the curve was contained in first and
			//		  third ORDER_OBJECT's (I should investigate what
			//		  assumptions of mine are in error here).
			//
			//        We do NOT need to modify project "Process3dm" 
			//		  relative to the generation of Order 4 NURBS.




		}
		else if((ClntRef >= 79.023752107406125) && !flag3)
		{
			// **** Set for execution of the ORDER2 NURB (straight line) ****

			pCurrentOrderElmt = pCurrentOrderElmt->pNextOrderElmt;

			flag3 = 1;
			
			// ****** Conclusion for Order2 NURBS execution.*************
			//		  
			//		  This is a "no brainer", Since straight line NURBS
			//		  generate only one ORDER_OBJECT.
			//
			//        We do NOT need to modify project "Process3dm" 
			//		  relative to the generation of Order 2 NURBS.
			//
			


		}
		else if((ClntRef >= 79.023752107406125 + 26.909056416374032) && !flag4)
		{

			// ****** Set for execution of first ORDER_OBJECT for the Order3 NURBS (circular arc) ***


			pCurrentOrderElmt = pCurrentOrderElmt->pNextOrderElmt;

			flag4 = 1;
		}
	 
		else if((ClntRef >= 79.023752107406125 + 26.909056416374032 + 7.3018213624759989)
					&& !flag5)
		{

			// ****** Set for execution of forth ORDER_OBJECT for the Order3 NURBS (circular arc)
			//		  (skip the second and the third ORDER_OBJECT's ***

			pCurrentOrderElmt = pCurrentOrderElmt->pNextOrderElmt;
			pCurrentOrderElmt = pCurrentOrderElmt->pNextOrderElmt;
			pCurrentOrderElmt = pCurrentOrderElmt->pNextOrderElmt;

			flag5 = 1;

			// ***** Conclusion for Order3 NURBS execution. ****************
			//
			//		 This was intuitive from the beginning in that it suggests
			//		 that we need only generate an ORDER_OBJECT that "join's"
			//		 success "mult == 3" knots ("mult == 2" in .dmp file).
			//
			//		 Thus for "n" "mult == 3" knots ("mult == 2" in .dmp file),
			//		 we need "n-1" ORDER_OBJECT's
			//
			//        We DO!! need to modify project "Process3dm" 
			//		  relative to the generation of Order 2 NURBS.
			//


		}
		else if(ClntRef >= 79.023752107406125 + 26.909056416374032 + 14.603642724951998)
		{
			// **** Done execution of the first three NURBS curves of "my_curves_Rhino3d.3dx" ****
			
			break;

		}
	
		
		ExecuteSpline(pCurrentOrderElmt);
 
		UpdateClntReference();

		
		// ****** Additional observation I have made while running this test ********
		//
		//		  - The NURBS objects listed in file 
		//			"my_curves_Rhino3d_AllNurbs.dmp" (and for that matter the order
		//			of the ORDER_OBJECT's listed in the object list "pOrderElmt"
		//			are in the wronge order.
		//
		//			Refering to file "my_curves_Rhino3d_AllNurbs.dmp", "Object 7"
		//			should be where "Object 0" is (etc..).
		//
		//			This probably due to the order inwhich the curves were drawn
		//			in Rhino3d.
		//
		//			We must add code to project "Process3dm" to scan the object
		//			and re-order them based on the values of positions stored in
		//			in the "CV" array.
		//
		//			There must be no assumptions made in so far as ordering of
		//			NURBS curves, even though we are stipulating that ALL
		//			curves must "connect" to each other and be "tangent" at
		//		    each connection point.
		//
		//			Further, there is the possibility that the positions in
		//			the "CV" array itself can be out of order for a given
		//			object.
		//
		//			In this case the values in the "CV" array and associated
		//			knot vector would have to be swapped.
		//
		//			To determine if this could happen, I will have to generate
		//			some more curves with "Rhino3d", drawing them out of order
		//			and "dragging" lines, arcs, and free hand curves in different
		//			directions.
		//
		
 
		
	}
}


#endif 
