/*
 * Multi-sensorthread.c
 * 
 * Example code showing the basic workflow with multiple sensors in the Gocator API version 4.1.
 * Each sensor is assigned different data callback functions.
 *
 * 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_1_IP	"192.168.1.10"
#define SENSOR_2_IP	"192.168.1.11"

// Represents a context object for data callbacks. 
typedef struct
{
	GoSensor sensor;
}DataContext_Sensor1;

typedef struct
{
	GoSensor sensor;
}DataContext_Sensor2;

// data callback functions
kStatus kCall onData1(void* ctx, void* sys, void* dataset)
{
	unsigned int j;
	k64u timeStamp;
	k32u sensorId;
	DataContext_Sensor1 *context = ctx;

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

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

	printf ("Thread1 - Received data from sensor Id %d, Timestamp %llu\n", sensorId, timeStamp);

	GoDestroy(dataset);

	return kOK;
}

kStatus kCall onData2(void* ctx, void* sys, void* dataset)
{
	unsigned int j;
	k64u timeStamp;
	k32u sensorId;
	DataContext_Sensor2 *context = ctx;

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

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

	printf ("Thread2 - Received data from sensor Id %d, Timestamp %llu\n", sensorId, timeStamp);

	GoDestroy(dataset);

	return kOK;
}

void main(int argc, char **argv)
{
	kStatus status;
	kAssembly api;
	kIpAddress ipAddress1;
	kIpAddress ipAddress2;
	DataContext_Sensor1 *context1;
	DataContext_Sensor2 *context2;
	GoSystem system;
	GoSensor sensor1;
	GoSensor sensor2;

	// alocate memory for callback contexts
	context1 = calloc(sizeof(DataContext_Sensor1), 1);
	if (!context1)
	{
		printf ("Error: Cannot allocate memory for sensor 1 context\n");
		return;
	}
	context2 = calloc(sizeof(DataContext_Sensor2), 1);
	if (!context2)
	{
		printf ("Error: Cannot allocate memory for sensor 2 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(sensor1):%d\n", status);
		return;
	}

	// obtain GoSensor objects by sensor serial number
	kIpAddress_Parse(&ipAddress1, SENSOR_1_IP);
	if ((status = GoSystem_FindSensorByIpAddress(system, &ipAddress1, &sensor1)) != kOK)
	{
		printf("Error: GoSystem_FindSensorByIpAddress:%d, IP Address %s\n", status, SENSOR_1_IP);
		return;
	}
	kIpAddress_Parse(&ipAddress2, SENSOR_2_IP);
	if ((status = GoSystem_FindSensorByIpAddress(system, &ipAddress2, &sensor2)) != kOK)
	{
		printf("Error: GoSystem_FindSensorByIpAddress:%d, IP Address %s\n", status, SENSOR_2_IP);
		return;
	}

	// set callback contexts to hold GoSensor object handle
	context1->sensor = sensor1;
	context2->sensor = sensor2;

	// set data handlers to receive data asynchronously with GoSensor_SetDataHandler()
	if ((status = GoSensor_SetDataHandler(sensor1, onData1, context1)) != kOK)
	{
		printf("Error: GoSystem_SetDataHandler(sensor1):%d\n", status);
		return;
	}	
	if ((status = GoSensor_SetDataHandler(sensor2, onData2, context2)) != kOK)
	{
		printf("Error: GoSystem_SetDataHandler(sensor2):%d\n", status);
		return;
	}	

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

	// enable data channels for the sensors
	if ((status = GoSystem_EnableData(system, kTRUE)) != kOK)
	{
		printf("Error: GoSystem_EnableData(system):%d\n", status);
		return;
	}

	// send start command to the sensors
	if ((status = GoSystem_Start(system)) != kOK) 
	{
		printf("Error: GoSystem_Start(system):%d\n", status);
		return;
	}

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

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

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

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

	free(context1);
	free(context2);
	GoDestroy(system);
	GoDestroy(api);
    return;    
}
