/*
 * Multithread.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_1_IP	"192.168.1.10"
#define SENSOR_2_IP	"192.168.1.11"

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

typedef struct
{
	GoSystem system;
}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_sensor1;
	GoSystem system_sensor2;
	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 objects - one for each sensor
	if ((status = GoSystem_Construct(&system_sensor1, kNULL)) != kOK)
	{
		printf("Error: GoSystem_Construct(sensor1):%d\n", status);
		return;
	}
	if ((status = GoSystem_Construct(&system_sensor2, kNULL)) != kOK)
	{
		printf("Error: GoSystem_Construct(sensor2):%d\n", status);
		return;
	}

	// set callback contexts to hold GoSystem object handle
	context1->system = system_sensor1;
	context2->system = system_sensor2;

	// obtain GoSensor objects by sensor serial number
	kIpAddress_Parse(&ipAddress1, SENSOR_1_IP);
	if ((status = GoSystem_FindSensorByIpAddress(system_sensor1, &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_sensor2, &ipAddress2, &sensor2)) != kOK)
	{
		printf("Error: GoSystem_FindSensorByIpAddress:%d, IP Address %s\n", status, SENSOR_2_IP);
		return;
	}

	// set data handlers to receive data asynchronously
	if ((status = GoSystem_SetDataHandler(system_sensor1, onData1, context1)) != kOK)
	{
		printf("Error: GoSystem_SetDataHandler(sensor1):%d\n", status);
		return;
	}	
	if ((status = GoSystem_SetDataHandler(system_sensor2, onData2, context2)) != kOK)
	{
		printf("Error: GoSystem_SetDataHandler(sensor2):%d\n", status);
		return;
	}		
	
	// open command channels to the individual sensors
	// must use sensor level connect here for multithreading to work
	if ((status = GoSensor_Connect(sensor1)) != kOK)
	{
		printf("Error: GoSensor_Connect(sensor1):%d\n", status);
		return;
	}		
	if ((status = GoSensor_Connect(sensor2)) != kOK)
	{
		printf("Error: GoSensor_Connect(sensor2):%d\n", status);
		return;
	}		

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

	// send start command to the individual sensors
	// must use sensor level start here for multithreading to work
	if ((status = GoSensor_Start(sensor1)) != kOK) 
	{
		printf("Error: GoSensor_Start(sensor1):%d\n", status);
		return;
	}
	if ((status = GoSensor_Start(sensor2)) != kOK) 
	{
		printf("Error: GoSensor_Start(sensor2):%d\n", status);
		return;
	}

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

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

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

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

	free(context1);
	free(context2);
	GoDestroy(system_sensor1);
	GoDestroy(system_sensor2);
	GoDestroy(api);
    return;    
}
