/*
 * Automatic.c
 * 
 * Example code showing how to work with multiple sensors in the Gocator API using automatic method.
 * Copyright (C) 2014 by LMI Technologies Inc.
 * 
 * Licensed under The MIT License.
 * Redistributions of files must retain the above copyright notice.
 *
 * This sample code assumes that all scan settings and Ethernet output are pre-configured for scanning operations
 */

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

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

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

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

#define BANK_COUNT 2 // Number of banks
#define SENSOR_COUNT 2 // Number of sensors

//Sensor setup definition. The number of entries MUST match with SENSOR_COUNT. Also, the bank must be between 0 and BANK_COUNT-1
                                            // Bank, IP Address
SensorSetupElement sensorSetup[SENSOR_COUNT] = { {0, "192.168.1.10"}, 
                                                 {0, "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 ("Receive 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 *appContext;
	kAssembly api;
	unsigned int i;
	GoMultiplexBank bank[BANK_COUNT];
	GoSensor sensor[SENSOR_COUNT];
	GoSystem system;

	appContext = calloc(sizeof(DataContext), 1);
	if (!appContext)
	{
		printf ("Error: Cannot allocate memroy for app context\n");
		return;
	}

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

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

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

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

		// create connection to sensor
		if ((status = GoSensor_Connect(sensor[i])) != kOK)
		{
			printf("Error: GoSensor_Connect:%d, IP Address %s\n", status, sensorSetup[i].ipAddressString);
			return;
		}

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

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

	// enable data channels for all sensors in system
	if ((status = GoSystem_EnableData(system, kTRUE)) != kOK)
	{
		printf("Error: GoSystem_EnableData:%d\n", status);
		return;
	}
	
	appContext->system = system;
	appContext->scanCount = SENSOR_COUNT;
	// set data handler to receive data asynchronously
	if ((status = GoSystem_SetDataHandler(system, onData, appContext)) != kOK)
	{
		printf("Error: GoSystem_SetDataHandler:%d\n", status);
		return;
	}	
    			
	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();

	if ((status = GoSystem_Stop(system)) != kOK)
	{
		printf("Error: GoSystem_Stop:%d\n", status);
		return;
	}

	for (i = 0; i < SENSOR_COUNT; i++)
    {
	    if ((status = GoSensor_Disconnect(sensor[i])) != kOK)
		{
			printf("Error: GoSensor_Disconnect:%d\n", status);
			return;
		}
	}
	printf("\nSystem disconnected.\n\n"); 
	printf("[Press 'enter' to exit]");
    getchar();

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