/*
 * ReceiveProfile.c
 * 
 * Gocator 2000 Sample
 * Copyright (C) 2011 by LMI Technologies Inc.
 * 
 * Licensed under The MIT License.
 * Redistributions of files must retain the above copyright notice.
 *
 * Purpose: Connect to Gocator system and receive range data in Profile Mode and translate to engeneering units (mm)
 *
 */
#include <Go2.h>
#include <stdio.h>
#include <stdlib.h>
#include <memory.h>

#define RECEIVE_TIMEOUT			(20000) 
#define INVALID_RANGE_16BIT		(0x8000)					// gocator transmits range data as 16-bit signed integers. 0x8000 signifies invalid range data.	
#define DOUBLE_MAX				(1.7976931348623157e+308)	// 64-bit double - largest positive value.	
#define INVALID_RANGE_DOUBLE	(-DOUBLE_MAX)				// floating point value to represent invalid range data.			 

typedef struct
{
	double x;	// x-coordinate in engineering units (mm) - position along laser line
	double z;	// z-coordinate in engineering units (mm) - height (at the given x position)
}ProfilePoint;

void main(int argc, char **argv)
{
	int status;
	Go2System system = GO2_NULL;
	Go2Data data = GO2_NULL;
	Go2IPAddress address;
	ProfilePoint* memory = NULL;
	unsigned int j;
	unsigned int arrayIndex;

	// initialize Go2 API
	if ((status = Go2Api_Initialize()) != GO2_OK)
	{
		printf("Error: Go2Api_Initialize:%d\n", status);
		return;
	}

	// construct Go2 system object
	if ((status = Go2System_Construct(&system)) != GO2_OK)
	{
		printf("Error: Go2System_Construct:%d\n", status);
		return;
	}

	// convert ip address string to Go2IPAddress object
	if ((status = Go2IPAddress_Parse("192.168.1.10", &address)) != GO2_OK)
	{
		printf("Error: Go2IPAddress_Parse:%d\n", status);
		return;
	}

	// connect to Gocator system at default address
	if ((status = Go2System_Connect(system, address)) != GO2_OK)
	{
		printf("Error: Go2System_Connect:%d\n", status);
		return;
	}

	// establish a Gocator data connection	
	if ((status = Go2System_ConnectData(system, GO2_NULL, GO2_NULL)) != GO2_OK)
	{
		printf("Error: Go2System_ConnectData:%d\n", status);
		return;
	}

	// start Gocator system	
	if ((status = Go2System_Start(system)) != GO2_OK)
	{
		printf("Error: Go2System_Start:%d\n", status);
		return;
	}

	// receive one single profile of range data
	if (Go2System_ReceiveData(system, RECEIVE_TIMEOUT, &data) == GO2_OK)
	{
		printf("Data message received:\n"); 
		printf("  Timestamp: %llu\n", Go2Data_Timestamp(data)); 
		printf("  Encoder: %lld\n", Go2Data_Encoder(data));  
		printf("  Frame index: %llu\n", Go2Data_FrameIndex(data));  
		printf("  Item count: %u\n", Go2Data_ItemCount(data));  

		//each result can have multiple data items
		for (j = 0; j < Go2Data_ItemCount(data); ++j)
		{
			Go2Data dataItem = Go2Data_ItemAt(data, j); 
			if (Go2Object_Type(dataItem) == GO2_TYPE_PROFILE_DATA)
			{   
				short* profileData = Go2ProfileData_Ranges(dataItem);
				unsigned int profilePointCount = Go2ProfileData_Width(dataItem); 
				double XResolution = Go2ProfileData_XResolution(dataItem);
				double ZResolution = Go2ProfileData_ZResolution(dataItem);
				double XOffset = Go2ProfileData_XOffset(dataItem);
				double ZOffset = Go2ProfileData_ZOffset(dataItem);
				printf("    Item[%u]: Profile data (%u points)\n", j, profilePointCount);

				//allocate memory
				if (memory == NULL)
				{	
					memory = malloc(profilePointCount * sizeof(ProfilePoint));
				}

				//translate 16-bit range data to engineering units and copy profiles to memory array
				for (arrayIndex = 0; arrayIndex < profilePointCount; ++arrayIndex)
				{
					if (profileData[arrayIndex] != INVALID_RANGE_16BIT)
					{
						memory[arrayIndex].x = XOffset + XResolution * arrayIndex;
						memory[arrayIndex].z = ZOffset + ZResolution * profileData[arrayIndex];
					}
					else
					{
						memory[arrayIndex].x = INVALID_RANGE_DOUBLE;
						memory[arrayIndex].z = INVALID_RANGE_DOUBLE;
					}
				}
			}       
		}
		if ((status = Go2Data_Destroy(data)) != GO2_OK)
		{
			printf("Error: Go2Data_Destroy:%d\n", status);
			return;
		}
	}
	
	// stop Gocator system	
	if ((status = Go2System_Stop(system)) != GO2_OK)
	{
		printf ("Error: Go2System_Stop:%d\n", status);
		return;
	}

	// destroy system handle
	if ((status = Go2System_Destroy(system)) != GO2_OK)
	{
		printf ("Error: Go2System_Destroy:%d\n", status);
		return;
	}

	// terminate Go2 API
	if ((status = Go2Api_Terminate()) != GO2_OK)
	{
		printf ("Error: Go2Api_Terminate:%d\n", status);
		return;
	}

	//free memory array
	free(memory);

	printf("Press any key to continue...\n", status);
	getchar();

	return;
}
