/*
 * BasicSetup.c
 * 
 * Example code showing the basic workflow with multiple sensors in the Gocator API.
 * This code shows how to manually configure exposure multiplexing and how to receive 
 * data when it's not required to match data between the sensors (i.e. when each sensor scans an independent surface)
 *
 * 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>

#define SENSOR_COUNT 2 // Number of sensors

// Represents a context object for data callbacks. 
typedef struct
{
	GoSystem system;
}DataContext;

typedef struct SensorSetupElement
{
	k64f exposureDelay;
	k64f timePeriod;
	unsigned char ipAddressString[16];
}SensorSetupElement;


//Sensor setup definition. The number of entries MUST match with SENSOR_COUNT. 
SensorSetupElement sensorSetup[SENSOR_COUNT] = { {0, 6000, "192.168.1.10"}, 
                                                 {350, 6000, "192.168.1.11"} };

// data callback function
kStatus kCall onData(void* ctx, void* sys, void* dataset)
{
	unsigned int i,j;
	k64u timeStamp;
	k64u frameIndex;
	k64s encoderStamp;
	k32u sensorId;
	DataContext *context = ctx;
	GoSensor sensor;
	kStatus status;

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

	if ((status = GoSystem_FindSensorById(context->system, sensorId, &sensor)) != kOK)
	{
		// This should not happen!
		printf ("Receive data from invalid sensor, Id %d\n", sensorId);
	}
	else
	{
		kChar ipAddressString[16];
		GoAddressInfo addrInfo;
		
		if (( status = GoSensor_Address(sensor, &addrInfo)) != kOK)
		{
			printf ("Unable to retrieve IP address, Id %d\n", sensorId);
		}
		else
		{
			kIpAddress_Format(addrInfo.address, ipAddressString, 16);
			printf ("Received data from sensor Id %d, IP address %s\n", sensorId, ipAddressString);
		}
	}
			
	for (i = 0; i < GoDataSet_Count(dataset); ++i)
	{
		GoDataMsg dataObj = GoDataSet_At(dataset, i);
		// retrieve GoStamp message
		switch(GoDataMsg_Type(dataObj))
		{
		case GO_DATA_MESSAGE_TYPE_STAMP:	
			{
				GoStampMsg stampMsg = dataObj;
				for (j = 0; j < GoStampMsg_Count(stampMsg); ++j)
				{
					// Gocator data messages are always tagged with time and encoder stamps, plus frame index
					// The stamps can be used to match data between sensors, used in resampling strategies etc
					GoStamp *stamp = GoStampMsg_At(stampMsg, j);
					timeStamp = stamp->timestamp;
					encoderStamp = stamp->encoder; 
					frameIndex = stamp->frameIndex;	
				}
			}
			break;
		case GO_DATA_MESSAGE_TYPE_RESAMPLED_PROFILE:			
			// Refer to Gocator SDK example ReceiveProfile on how to receive data				
			
			break;
		}				
	}
	GoDestroy(dataset);
	return kOK;
}

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

	// 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;

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

	// 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;
	}	

	// loop over all sensors
	for (i=0; i < SENSOR_COUNT; i++)
	{		
		kIpAddress ipAddress;
		GoSetup setup;
		GoLayout layout;

		// 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;
		}

		// retrieve setup handle from sensor object
		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;
		}	

		// enable multiplexing - NOTE - when enabling multiplexing the sensor runs at the speed dictated by
		// the multiplex time period setting below, rather than the sensor frame rate normally used (GoSetup_FrameRate)
		if ((status = GoLayout_EnableMultiplexSingle(layout, kTRUE)) != kOK)
		{
			printf("Error: GoLayout_EnableMultiplexSingle:%d\n", status);
			return;
		}

		// manually set the exposure delay for this sensor
		if ((status = GoLayout_SetMultiplexSingleDelay(layout, sensorSetup[i].exposureDelay)) != kOK)
		{
			printf("Error: GoLayout_SetMultiplexSingleDelay:%d\n", status);
			return;
		}

		// manually set the time period for this sensor - this setting now controls the speed of the sensor
		if ((status = GoLayout_SetMultiplexSinglePeriod(layout, sensorSetup[i].timePeriod)) != kOK)
		{
			printf("Error: GoLayout_SetMultiplexSinglePeriod:%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("Scanning 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();

	free(callbackContext);
	GoDestroy(system);
	GoDestroy(api);
    return;    
}
