/*
 * AutomaticSetup.c
 * 
 * Example code showing how to work with multiple sensors in the Gocator API using an automatic method.
 * This code also shows how to match data from multiple sensors and how to handle sensors dropping data  
 *
 * NOTE! This example does not support Gocator Displacement Single-Point sensors where data output is batched
 *
 * This sample code assumes that all scan settings and Ethernet output are pre-configured for scanning operations
 *
 * Copyright (C) 2014 by LMI Technologies Inc.
 * 
 * Licensed under The MIT License.
 * Redistributions of files must retain the above copyright notice.
 *
 */

#include <GoSdk/GoSdk.h>
#include <kApi/kApi.h>

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <windows.h>

#include "CircularQueue.h"

#define BANK_COUNT 3			// Number of banks
#define SENSOR_COUNT 3			// Number of sensors
#define QUEUE_CAPACITY 5000		// Capacity of queues to hold incoming messages
#define STAMP_JITTER_MARGIN 3	// Stamp jitter margin for matching frames 
#define BACKWARDS_ENCODER 0		// Set to 0 if encoder moving forward, 1 if encoder moving backwards 

#define MATH_ABS(x) ((x)<0 ? -(x) : (x))

// Represents a context object for data callbacks. 
typedef struct DataContext
{
	GoSystem system;
	GoTrigger triggerSource;
	struct CircularQueue* queue[SENSOR_COUNT];
	unsigned int matchedFrameCount;
}DataContext;

typedef struct SensorSetupElement
{
	unsigned int bank; // Number is between 0 to BANK_COUNT
	unsigned char ipAddressString[16];
	unsigned int sensorId;
}SensorSetupElement;

//Sensor setup definition. The number of entries MUST match with SENSOR_COUNT. Also, the bank must be between 0 and BANK_COUNT-1
SensorSetupElement sensorSetup[SENSOR_COUNT] = { {0, "192.168.1.10", 9829}, 
                                                 {1, "192.168.1.11", 9811},
                                                 {2, "192.168.1.12", 9987} };

// forward declaration of functions used for frame matching - implementation at bottom of this source file
k64u DataSet_Timestamp(void* dataset);
k64s DataSet_Encoder(void* dataset);
kBool MatchFrames(void* ctx, unsigned int sensorIndex, void* dataset, void** matched_set);

// data callback function
kStatus kCall onData(void* ctx, void* sys, void* dataset)
{
	unsigned int i;
	unsigned int sensorIndex;
	k32u sensorId;
	DataContext *context = ctx;
	GoDataSet matched_set[SENSOR_COUNT];

	//which sensor is the data coming from?
	sensorId = GoDataSet_SenderId(dataset);

	// look-up sensor index in hardcoded SensorSetupElement - (in a real system the system information would be stored in a settings file)
	sensorIndex = 0; while ((sensorIndex < SENSOR_COUNT) && (sensorSetup[sensorIndex].sensorId != sensorId)) sensorIndex++;
	
	if (sensorIndex == SENSOR_COUNT)
	{
		// sensor id not found in hardcoded SensorSetupElement - not able to process data
		printf ("Sensor %d not defined in system!\n", sensorId);

		GoDestroy(dataset);
	}
	else
	{
		// data received from sensor defined in system
		printf ("Received data from sensor Id %d, Index %d\n", sensorSetup[sensorIndex].sensorId, sensorIndex);

		if (MatchFrames(ctx, sensorIndex, dataset, &matched_set[0]))
		{
			// a matched set of frames available
			printf ("\nMatch %d - ", context->matchedFrameCount);
			for (i=0; i < SENSOR_COUNT; i++)
			{
				// the array matched_set[SENSOR_COUNT] now holds a frame-matched set of dataset handles
				// note that in case the sensor dropped a certain frame that dataset handle will be NULL
				// refer to Gocator SDK example ReceiveProfile on how to receive data from a dataset

				// print stamps of matched set to console
				if (context->triggerSource == GO_TRIGGER_ENCODER)
				{
					if (matched_set[i] != NULL) printf("[E: %lld]", DataSet_Encoder(matched_set[i]));
					else printf("[E: NULL]");
				}
				else
				{
					if (matched_set[i] != NULL) printf("[T: %llu]", DataSet_Timestamp(matched_set[i]));
					else printf("[T: NULL]");
				}

				GoDestroy(matched_set[i]);
			}
			printf ("\n\n", context->matchedFrameCount);
			
			context->matchedFrameCount++;
		}
	}

	return kOK;
}

void main(int argc, char **argv)
{
	kStatus status;
	DataContext *callbackContext;
	kAssembly api;
	unsigned int i;
	int j;
	GoMultiplexBank bank[BANK_COUNT];
	GoSensor sensor[SENSOR_COUNT];
	GoSystem system;

	printf ("-------------------------------------------------------\n");
	printf ("- Gocator 4.x multisensor example with frame matching -\n");
	printf ("- Not supporting Gocator Displacement sensors         -\n");
	printf ("-------------------------------------------------------\n\n");

	// alocate memory for callback context
	callbackContext = calloc(sizeof(DataContext), 1);
	if (!callbackContext)
	{
		printf ("Error: Cannot allocate memory for app context\n");
		return;
	}

	// construct Gocator API Library
	if ((status = GoSdk_Construct(&api)) != kOK)
	{
		printf("Error: GoSdk_Construct:%d\n", status);
		return;
	}

	// construct GoSystem object
	if ((status = GoSystem_Construct(&system, kNULL)) != kOK)
	{
		printf("Error: GoSystem_Construct:%d\n", status);
		return;
	}

	// set callback context to hold GoSystem object handle
	callbackContext->system = system;
	callbackContext->matchedFrameCount = 0;

	// open command channels to all sensors in system 
	if ((status = GoSystem_Connect(system)) != kOK)
	{
		printf("Error: GoSystem_Connect:%d\n", status);
		return;
	}		

	// loop over banks
	for (i = 0; i < BANK_COUNT; i++)
	{		
		//create banks
		if ((status = GoSystem_AddMultiplexBank(system, &bank[i])) != kOK)
		{
			printf("Error: GoSystem_AddMultiplexBank:%d\n", status);
			return;
		}
	}

	// loop over sensors
	for (i=0; i < SENSOR_COUNT; i++)
	{		
		kIpAddress ipAddress;

		// parse IP address into address data structure
		kIpAddress_Parse(&ipAddress, sensorSetup[i].ipAddressString);

		// obtain GoSensor object by sensor serial number
		if ((status = GoSystem_FindSensorByIpAddress(system, &ipAddress, &sensor[i])) != kOK)
		{
			printf("Error: GoSystem_FindSensorByIpAddress:%d, IP Address %s\n", status, sensorSetup[i].ipAddressString);
			return;
		}

		// assign sensors to banks
		if ((status = GoMultiplexBank_AddSensor(bank[sensorSetup[i].bank], sensor[i])) != kOK) 
		{
			printf("Error: GoMultiplexBank_AddSensor:%d\n", status);
			return;
		}	

		// construct queue to hold incoming data messages
		callbackContext->queue[i] = CircularQueue_Construct(QUEUE_CAPACITY);

		// get id of sensor
		sensorSetup[i].sensorId = GoSensor_Id(sensor[i]);
	}

	// Update all multiplexing parameters. Note that this will set the sensor to run at the fastest possible rate, regardless of the 
	// trigger panel frame rate setting. 
	if ((status = GoSystem_UpdateAllMultiplexParameters(system, 0.0)) != kOK)
	{
		printf("Error: GoSystem_UpdateMultiplexPeriod:%d\n", status);
		return;
	}		
	
	printf("Automatic multiplex configuration:\n");
	// query sensors for multiplex timing to confirm automatic configuration
	for (i = 0; i < SENSOR_COUNT; i++)
	{
		GoSetup setup;
		GoLayout layout;
		k64f exposure, delay, period;
		
		// retrieve setup handle
		if ((setup = GoSensor_Setup(sensor[i])) == kNULL)
		{
			printf("Error: GoSensor_Setup: Invalid Handle\n");
			return;
		}	
		
		// retrieve layout handle from setup object
		if ((layout = GoSetup_Layout(setup)) == kNULL)
		{
			printf("Error: GoSetup_Layout: Invalid Handle\n");
			return;
		}	

		// get exposure time
		exposure = GoSetup_Exposure(setup, GO_ROLE_MAIN); 

		// get exposure delay
		delay = GoLayout_MultiplexSingleDelay(layout);

		// get time period (sensor speed)
		period = GoLayout_MultiplexSinglePeriod(layout); 

		printf("Sensor %d: Exposure = %4.1f, Delay = %4.1f, Time Period = %4.0f (%4.1f Hz)\n", i, exposure, delay, period, (k64f)(1000000/period));		

		// get trigger mode - NOTE! all sensors in the system must be configured to run in the same trigger mode
		callbackContext->triggerSource = GoSetup_TriggerSource(setup);
	}
	if (callbackContext->triggerSource == GO_TRIGGER_ENCODER) printf("\nSystem is encoder triggered\n");
	else printf("\nSystem is time triggered\n");
	printf("\n[Press 'enter' to continue]");
	getchar();

	// enable data channels for all sensors in system
	if ((status = GoSystem_EnableData(system, kTRUE)) != kOK)
	{
		printf("Error: GoSystem_EnableData:%d\n", status);
		return;
	}
	
	// set data handler to receive data asynchronously
	if ((status = GoSystem_SetDataHandler(system, onData, callbackContext)) != kOK)
	{
		printf("Error: GoSystem_SetDataHandler:%d\n", status);
		return;
	}	
    			
	// send start command to all sensors in system
	if ((status = GoSystem_Start(system)) != kOK) 
	{
		printf("Error: GoSystem_Start:%d\n", status);
		return;
	}

	printf("\nScanning in progress... press [Enter] to stop sensor...\n\n");
	getchar();

	// send stop command to all sensors in system
	if ((status = GoSystem_Stop(system)) != kOK)
	{
		printf("Error: GoSystem_Stop:%d\n", status);
		return;
	}

	// disconnect command channel from all sensors in system
    if ((status = GoSystem_Disconnect(system)) != kOK)
	{
		printf("Error: GoSystem_Disconnect:%d\n", status);
		return;
	}

	printf("\nSystem disconnected.\n\n"); 
	printf("[Press 'enter' to exit]");
    getchar();

	for (i=0; i < SENSOR_COUNT; i++)
	{		
		if (!CircularQueue_IsEmpty(callbackContext->queue[i]))
		{
			// empty queue from any remaining datasets and destroy them
			for (j=0; j<CircularQueue_Size(callbackContext->queue[i]); j++)
			{
				GoDestroy(CircularQueue_Remove(callbackContext->queue[i]));
			}
		}
		
		// destroy queues
		CircularQueue_Destroy(callbackContext->queue[i]);
	}
	free(callbackContext);
	GoDestroy(system);
	GoDestroy(api);
    return;    
}

// helper function to retrieve timestamp of first stamp message in batch
k64u DataSet_Timestamp(void* dataset)
{
	unsigned int j;
	k64u timeStamp;

	for (j = 0; j < GoDataSet_Count(dataset); ++j)
	{
		GoDataMsg dataObj = GoDataSet_At(dataset, j);
		if (GoDataMsg_Type(dataObj) == GO_DATA_MESSAGE_TYPE_STAMP)
		{
			GoStampMsg stampMsg = dataObj;
			GoStamp *stamp = GoStampMsg_At(stampMsg, 0);
			timeStamp = stamp->timestamp;
		}
	}

	return timeStamp;
}

// helper function to retrieve encoder stamp of first stamp message in batch
k64s DataSet_Encoder(void* dataset)
{
	unsigned int j;
	k64s encoder;

	for (j = 0; j < GoDataSet_Count(dataset); ++j)
	{
		GoDataMsg dataObj = GoDataSet_At(dataset, j);
		if (GoDataMsg_Type(dataObj) == GO_DATA_MESSAGE_TYPE_STAMP)
		{
			GoStampMsg stampMsg = dataObj;
			GoStamp *stamp = GoStampMsg_At(stampMsg, 0);
			encoder = stamp->encoder;
		}
	}

	return encoder;
}

// frame-matching logic
kBool MatchFrames(void* ctx, unsigned int sensorIndex, void* dataset, void** matched_set)
{
	unsigned int i;
	DataContext *context = ctx;
	k64u timeStamp[SENSOR_COUNT];
	k64u oldestTimeStamp;
	k64s encoderStamp[SENSOR_COUNT];
	k64s oldestEncoderStamp;
	kBool matchedSetAvailable;

	// check that queue for this sensor is not full
	if (CircularQueue_IsFull(context->queue[sensorIndex]))
	{
		// if a sensor queue has exhausted the QUEUE_CAPACITY it's a sign the system is misconfigured 
		printf ("Receiving queue exhausted for sensor %d!\n", GoDataSet_SenderId(dataset));

		matchedSetAvailable = kFALSE;
		GoDestroy(dataset);
	}
	else
	{
		// add dataset to queue
		CircularQueue_Add(context->queue[sensorIndex], dataset);

		// check how many queues are not empty
		i = 0; while ((i < SENSOR_COUNT) && !CircularQueue_IsEmpty(context->queue[i])) i++;

		if (i < SENSOR_COUNT)
		{
			// data not available from all sensor - do nothing, wait for more data to arrive
			matchedSetAvailable = kFALSE;
		}
		else
		{
			// data available from all sensors - match frames!

			if (context->triggerSource == GO_TRIGGER_ENCODER)
			{
				// match by encoder

				// peek into front of queues to retrieve encoder stamps from all sensors and to determine the oldest dataset
				encoderStamp[0] = DataSet_Encoder(CircularQueue_Peek(context->queue[0]));
				oldestEncoderStamp = encoderStamp[0];
				for (i=1; i < SENSOR_COUNT; i++)
				{
					encoderStamp[i] = DataSet_Encoder(CircularQueue_Peek(context->queue[i]));
					if (BACKWARDS_ENCODER) 
					{
						if (encoderStamp[i] > oldestEncoderStamp) oldestEncoderStamp = encoderStamp[i];
					}
					else 
					{
						if (encoderStamp[i] < oldestEncoderStamp) oldestEncoderStamp = encoderStamp[i];
					}
				}

				// loop over sensors to determine matched set
				// datasets at the front of the queue that match the oldest stamp belong to the same matched set
				for (i=0; i < SENSOR_COUNT; i++)
				{
					if (MATH_ABS(oldestEncoderStamp - encoderStamp[i]) < STAMP_JITTER_MARGIN)
					{
						// frames match - remove from queue and add to matched set
						matched_set[i] = CircularQueue_Remove(context->queue[i]);
					}
					else
					{
						// no stamp match - frame with this stamp missing/dropped from this sensor
						matched_set[i] = NULL;
					}
				}
			}
			else
			{
				// match by time 

				// peek into front of queues to retrieve time stamps from all sensors and to determine the oldest dataset
				timeStamp[0] = DataSet_Timestamp(CircularQueue_Peek(context->queue[0]));
				oldestTimeStamp = timeStamp[0];
				for (i=1; i < SENSOR_COUNT; i++)
				{
					timeStamp[i] = DataSet_Timestamp(CircularQueue_Peek(context->queue[i]));
					if (timeStamp[i] < oldestTimeStamp) oldestTimeStamp = timeStamp[i];
				}

				// loop over sensors to determine matched set
				// datasets at the front of the queue that match the oldest stamp belong to the same matched set
				for (i=0; i < SENSOR_COUNT; i++)
				{
					if (oldestTimeStamp - timeStamp[i] < STAMP_JITTER_MARGIN)
					{
						// frames match - remove from queue and add to matched set
						matched_set[i] = CircularQueue_Remove(context->queue[i]);
					}
					else
					{
						// no stamp match - frame with this stamp missing/dropped from this sensor
						matched_set[i] = NULL;
					}
				}
			}

			matchedSetAvailable = kTRUE;
		}
	}

	return matchedSetAvailable;
}


