#include "stdafx.h"
#include "ClientAPI.h"
#include <string>
#include <iostream>
#include <map>
#define _USE_MATH_DEFINES
#include <math.h>

// insert the IP of your SP-ICE-3
#define CARD_IP "192.168.2.138"

static rlResult configureFont( rlHandle handle );
static rlResult markText( rlHandle handle, char* text, int charLen );
static rlResult buildCharWidthDict( rlHandle handle );
static int printLastError( void );

static std::map<char, double> _CharWidth;

int main()
{
	// establish a connection with the card
	rlHandle handle = rlConnect( CARD_IP, 49374 );
	if ( handle < 0 )
		return printLastError();

	rlResult status;

	// Reset the card to its default settings:
	// This restores the following settings to the values that were last saved on the card:
	// - scanner configuration (such as field size, field correction, etc.)
	// - laser configuration (such as gate/LM timing parameters, power target, etc.)
	// - process variables (such as jump speed, settling times, mark speed, etc.)
	if ( ( status = rlSystemResetToDefaults( handle ) ) != rlSUCCESS )
		goto Cleanup;

	// Set the laser power to 100%
	rlLaserConfig lc;
	if ( ( status = rlLaserGetConfig( handle, &lc ) ) != rlSUCCESS ) goto Cleanup;
	lc.PowerScale = 1;
	if ( ( status = rlLaserSetConfig( handle, &lc ) ) != rlSUCCESS ) goto Cleanup;

	// Enable the laser so it responds to commands that turn on the laser.
	// Note that this does not fire the laser until e.g. LaserOn or Mark command is executed.
	if ( ( status = rlLaserArmLaser( handle, true ) ) != rlSUCCESS ) goto Cleanup;

	// define the needed characters
	if ( ( status = configureFont( handle ) ) != rlSUCCESS ) goto Cleanup;

	if ( ( status = markText( handle, " 1234567890", 11 ) ) != rlSUCCESS ) goto Cleanup;

Cleanup:
	// print any errors
	if ( status != rlSUCCESS )
		printLastError();

	// disconnect from the card
	printf( "Disconnecting from card...\n" );
	if ( rlDisconnect( handle ) != rlSUCCESS )
		return printLastError();

	printf( "Exiting with status = %d\n", status );
	return status;
}

/// <summary>
/// Mark the specified text
/// Every used character has to be defined in the font
/// </summary>
/// <param name="text">text to mark</param>
static rlResult markText( rlHandle handle, char* text, int charLen )
{
	rlResult result;

	bool beltMovesPositive = true;

	double jumpSpeed = 10;
	double markSpeed = 3.4;
	double beltSpeed = 0.27; // 1 km/h

	int encoder = 0;
	int listID = 13;
	int progressID = 40;

	// get the scanner's current field size
	rlScannerConfig sc;
	if ( ( result = rlScannerGetConfig( handle, &sc ) ) != rlSUCCESS ) return result;

	double fieldSize = sc.FieldSize.X;
	double sizeX = 0.1 * fieldSize;
	double sizeY = 0.1 * fieldSize;
	double beltDistance = 0.05 * fieldSize;
	int scalingFactor = 20;
	double traveledDistance = 0;

	double beltTimeMs = 1e-3 * beltDistance / beltSpeed;

	rlQuadratureDecoderConfig qc;
	if ( ( result = rlMotfQuadratureDecoderInitConfig( &qc ) ) != rlSUCCESS ) return result;
	qc.Modules[encoder].InvertDirection = false;
	qc.Modules[encoder].SimulateEncoder = true;
	qc.Modules[encoder].SimulationSpeed = 0.01;
	qc.Modules[encoder].SimulationAcceleration = 0;
	qc.Modules[encoder].DistancePerCount = 36;
	if ( ( result = rlMotfQuadratureDecoderSetConfig( handle, &qc ) ) != rlSUCCESS ) return result;

	if ( ( result = rlMotfQuadratureDecoderResetEncoderPosition( handle, encoder ) ) != rlSUCCESS ) return result;

	rlMotfConfig config;
	if ( ( result = rlMotfGetConfig( handle, &config ) ) != rlSUCCESS ) return result;
	config.MainSource.SourceType = stQuadratureDecoder;
	config.MainSource.ModuleID = encoder;
	config.CompensationSource.SourceType = stQuadratureDecoder;
	config.CompensationSource.ModuleID = ( encoder + 1 ) % 2;
	rlMotfConfigSetDirectionFromAngle( &config, M_PI / 2 + ( beltMovesPositive ? 0 : M_PI ) );
	if ( ( result = rlMotfSetConfig( handle, &config ) ) != rlSUCCESS ) return result;

	int beltSign = beltMovesPositive ? 1 : -1;

	// rotation of marking object, value could be delivered by camera etc
	double rotation = 5 * M_PI / 180;
	// rotate and scale, as the size of the font is only 1000
	rlTransform3D* transformationScaled = new rlTransform3D();
	transformationScaled->A[0][0] = cos( rotation + M_PI / 2 ) * scalingFactor;
	transformationScaled->A[0][1] = sin( rotation + M_PI / 2 )* scalingFactor;
	transformationScaled->A[0][2] = 0;
	transformationScaled->A[1][0] = -sin( rotation + M_PI / 2 )* scalingFactor;
	transformationScaled->A[1][1] = cos( rotation + M_PI / 2 )* scalingFactor;
	transformationScaled->A[1][2] = 0;
	transformationScaled->A[2][0] = 0;
	transformationScaled->A[2][1] = 0;
	transformationScaled->A[2][2] = scalingFactor;

	// unscaled transformation for the jumps
	rlTransform3D* transformationDefault = new rlTransform3D();

	rlCommandListHandle list = rlListAllocate( handle );

	if ( ( result = rlListAppendPower( list, 35000 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendJumpSpeed( list, jumpSpeed ) ) != rlSUCCESS )	return result;
	if ( ( result = rlListAppendJumpDelay( list, 0 ) ) != rlSUCCESS )	return result;
	if ( ( result = rlListAppendMarkSpeed( list, markSpeed ) ) != rlSUCCESS )	return result;
	if ( ( result = rlListAppendPolyDelay( list, 0 ) ) != rlSUCCESS )	return result;
	if ( ( result = rlListAppendMarkDelay( list, 0 ) ) != rlSUCCESS )	return result;

	char lastChar;
	bool firstRun = true;

	// jump to the edge of the field, mark character and wait for the marked distance
	// if the object is rotated, the vertical offset will increase on every iteration
	//reverse order (marking from right to left)
	for ( int i = charLen - 1; i >= 0; i-- )
	{
		if ( firstRun )
		{
			lastChar = text[i];
			firstRun = false;
			continue;
		}

		if ( ( result = rlListAppendTransformation( list, transformationDefault ) ) != rlSUCCESS )	return result;
		if ( ( result = rlListAppendJumpAbs2D( list, 0.3 * fieldSize - sin( rotation ) * traveledDistance, 0.49 * fieldSize ) ) != rlSUCCESS )	return result;
		if ( ( result = rlListAppendProgress( list, progressID ) ) != rlSUCCESS )	return result;
		if ( ( result = rlListAppendTransformation( list, transformationScaled ) ) != rlSUCCESS )	return result;
		if ( ( result = rlListAppendMarkChar( list, lastChar ) ) != rlSUCCESS )	return result;
		// the waiting distance is calculated by the width of the next char
		// the offset and dividing factor can be adjusted for the desired distance between the chars
		// when rotated, the distance will be shortend by cosinus
		if ( ( result = rlListAppendMotfWaitDistance( list, ( _CharWidth[text[i]] + 1000 ) / 1.85 * scalingFactor * cos( rotation ), dmRelativeToPrevious ) ) != rlSUCCESS )	return result;
		// sum the distance for the Vertical offset
		traveledDistance += ( _CharWidth[text[i]] + 1000 ) / 1.85 * scalingFactor;
		lastChar = text[i];
	}

	if ( ( result = rlListAppendLaserOff( list ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendStopMotf2D( list, 0, 0 ) ) != rlSUCCESS ) return result;

	double markTimeMs = 2.0e-3 * ( sizeX + sizeY ) / markSpeed;

	// close the list causing its contents to be transferred to the card
	printf( "Closing list: index=%d...\n", listID );
	if ( ( result = rlListSet( handle, listID, list, false, -1 ) ) != rlSUCCESS ) return result;

	for ( int iteration = 0; iteration < 1; iteration++ )
	{
		printf( "Iteration %d: Executing...\n", iteration + 1 );
		if ( ( result = rlListExecute( handle, listID ) ) != rlSUCCESS ) return result;
		if ( iteration == 0 )
		{
			qc.Modules[encoder].SimulationSpeed = beltSpeed / qc.Modules[encoder].DistancePerCount;
			qc.Modules[encoder].SimulationAcceleration = 0;
			if ( ( result = rlMotfQuadratureDecoderSetConfig( handle, &qc ) ) != rlSUCCESS ) return result;
		}
		bool success;
		int pid;
		if ( ( result = rlListWaitForProgress( handle, (int)beltTimeMs + 4000, &success, &pid ) ) != rlSUCCESS ) return result;

		int doneID;
		bool done = false;
		if ( ( result = rlListWaitForListDone( handle, (int)markTimeMs + 4000, &done, &doneID ) ) != rlSUCCESS ) return result;

		if ( !done )
			printf( "List did not finish within the timeout period\n" );
	}

	// delete list on the card: this frees the card's memory occupied by the list
	printf( "Deleting list: index=%d...\n", listID );
	if ( ( result = rlListDelete( handle, listID, true ) ) != rlSUCCESS ) return result;

	// release the local list: this frees the PC's memory occupied by the card
	if ( ( result = rlListReleaseHandle( list ) ) != rlSUCCESS ) return result;

	return result;
}

/// create Font
/// only digits space dash and colon are defined in this example
/// Every character is defined with relative commands from the anchor point in the lower left corner
/// and finishes at the anchor point of the next char
static rlResult configureFont( rlHandle handle )
{
	rlResult result;

	rlCommandListHandle c0 = rlListAllocate( handle );
	if ( ( result = rlListAppendJumpRel2D( c0, 500, 1000 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c0, 0, -500 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c0, 0, -500 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c0, -500, 0 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c0, 0, 500 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c0, 0, 500 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c0, 500, 0 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendJumpRel2D( c0, 100, -1000 ) ) != rlSUCCESS ) return result;

	rlCommandListHandle c1 = rlListAllocate( handle );
	if ( ( result = rlListAppendJumpRel2D( c1, 500, 0 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c1, 0, 1000 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendJumpRel2D( c1, 100, -1000 ) ) != rlSUCCESS ) return result;

	rlCommandListHandle c2 = rlListAllocate( handle );
	if ( ( result = rlListAppendJumpRel2D( c2, 0, 1000 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c2, 500, 0 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c2, 0, -500 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c2, -500, 0 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c2, 0, -500 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c2, 500, 0 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendJumpRel2D( c2, 100, 0 ) ) != rlSUCCESS ) return result;

	rlCommandListHandle c3 = rlListAllocate( handle );
	if ( ( result = rlListAppendJumpRel2D( c3, 0, 1000 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c3, 500, 0 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c3, 0, -500 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c3, -500, 0 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendJumpRel2D( c3, 500, 0 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c3, 0, -500 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c3, -500, 0 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendJumpRel2D( c3, 600, 0 ) ) != rlSUCCESS ) return result;

	rlCommandListHandle c4 = rlListAllocate( handle );
	if ( ( result = rlListAppendJumpRel2D( c4, 0, 1000 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c4, 0, -500 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c4, 500, 0 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendJumpRel2D( c4, 0, 500 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c4, 0, -1000 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendJumpRel2D( c4, 100, 0 ) ) != rlSUCCESS ) return result;

	rlCommandListHandle c5 = rlListAllocate( handle );
	if ( ( result = rlListAppendJumpRel2D( c5, 500, 1000 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c5, -500, 0 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c5, 0, -500 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c5, 500, 0 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c5, 0, -500 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c5, -500, 0 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendJumpRel2D( c5, 600, 0 ) ) != rlSUCCESS ) return result;

	rlCommandListHandle c6 = rlListAllocate( handle );
	if ( ( result = rlListAppendJumpRel2D( c6, 500, 1000 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c6, -500, 0 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c6, 0, -500 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c6, 0, -500 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c6, 500, 0 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c6, 0, 500 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c6, -500, 0 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendJumpRel2D( c6, 600, -500 ) ) != rlSUCCESS ) return result;

	rlCommandListHandle c7 = rlListAllocate( handle );
	if ( ( result = rlListAppendJumpRel2D( c7, 0, 1000 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c7, 500, 0 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c7, 0, -500 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c7, 0, -500 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendJumpRel2D( c7, 100, 0 ) ) != rlSUCCESS ) return result;

	rlCommandListHandle c8 = rlListAllocate( handle );
	if ( ( result = rlListAppendJumpRel2D( c8, 500, 1000 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c8, -500, 0 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c8, 0, -500 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c8, 500, 0 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c8, 0, 500 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendJumpRel2D( c8, 0, -500 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c8, 0, -500 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c8, -500, 0 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c8, 0, 500 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendJumpRel2D( c8, 600, -500 ) ) != rlSUCCESS ) return result;

	rlCommandListHandle c9 = rlListAllocate( handle );
	if ( ( result = rlListAppendJumpRel2D( c9, 500, 1000 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c9, -500, 0 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c9, 0, -500 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c9, 500, 0 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c9, 0, 500 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendJumpRel2D( c9, 0, -500 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c9, 0, -500 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( c9, -500, 0 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendJumpRel2D( c9, 600, 0 ) ) != rlSUCCESS ) return result;

	rlCommandListHandle colon = rlListAllocate( handle );
	if ( ( result = rlListAppendJumpRel2D( colon, 0, 200 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( colon, 200, 0 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( colon, 0, 200 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( colon, -200, 0 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( colon, 0, -200 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendJumpRel2D( colon, 0, 400 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( colon, 200, 0 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( colon, 0, 200 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( colon, -200, 0 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( colon, 0, -200 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendJumpRel2D( colon, 300, -600 ) ) != rlSUCCESS ) return result;

	rlCommandListHandle space = rlListAllocate( handle );
	if ( ( result = rlListAppendJumpRel2D( space, 500, 0 ) ) != rlSUCCESS ) return result;

	rlCommandListHandle dash = rlListAllocate( handle );
	if ( ( result = rlListAppendJumpRel2D( dash, 0, 500 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( dash, 500, 0 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendJumpRel2D( dash, 100, -500 ) ) != rlSUCCESS ) return result;

	rlCommandListHandle longSlash = rlListAllocate( handle );
	if ( ( result = rlListAppendMarkRel2D( longSlash, 4500, 1000 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendJumpRel2D( longSlash, 500, -500 ) ) != rlSUCCESS ) return result;

	rlFontHandle font = rlFontAllocate( handle, 0 );

	rlFontAddChar( handle, ' ', space );
	if ( ( result = rlFontAddChar( font, '0', c0 ) ) != rlSUCCESS )	return result;
	if ( ( result = rlFontAddChar( font, '1', c1 ) ) != rlSUCCESS )	return result;
	if ( ( result = rlFontAddChar( font, '2', c2 ) ) != rlSUCCESS )	return result;
	if ( ( result = rlFontAddChar( font, '3', c3 ) ) != rlSUCCESS )	return result;
	if ( ( result = rlFontAddChar( font, '4', c4 ) ) != rlSUCCESS )	return result;
	if ( ( result = rlFontAddChar( font, '5', c5 ) ) != rlSUCCESS )	return result;
	if ( ( result = rlFontAddChar( font, '6', c6 ) ) != rlSUCCESS )	return result;
	if ( ( result = rlFontAddChar( font, '7', c7 ) ) != rlSUCCESS )	return result;
	if ( ( result = rlFontAddChar( font, '8', c8 ) ) != rlSUCCESS )	return result;
	if ( ( result = rlFontAddChar( font, '9', c9 ) ) != rlSUCCESS )	return result;
	if ( ( result = rlFontAddChar( font, ':', colon ) ) != rlSUCCESS )	return result;
	if ( ( result = rlFontAddChar( font, '-', dash ) ) != rlSUCCESS )		return result;
	if ( ( result = rlFontAddChar( font, ' ', space ) ) != rlSUCCESS )	return result;
	if ( ( result = rlFontAddChar( font, 'l', longSlash ) ) != rlSUCCESS )return result;

	if ( ( result = rlFontSet( handle, font ) ) != rlSUCCESS ) return result;
	if ( ( result = rlFontSetActive( handle, 0 ) ) != rlSUCCESS ) return result;

	return rlSUCCESS;
}

/// <summary>
/// extract the length of the chars defenied in configureFont()
/// </summary>
static rlResult buildCharWidthDict( rlHandle handle )
{
	rlResult result;
	_CharWidth.empty();
	rlFontHandle font = rlFontGet( handle, 0 );

	u_int charLen = 0;
	if ( ( result = rlFontCharsLen( font, &charLen ) ) != rlSUCCESS ) return result;
	char* charlist = new char[charLen];
	if ( ( result = rlFontChars( font, charlist, charLen, &charLen ) ) != rlSUCCESS ) return result;

	u_int commandCount = 0;

	for ( int i = 0; i < (int)charLen; i++ )
	{
		rlCommandListHandle charCommands = rlFontGetChar( font, charlist[i] );
		if ( ( result = rlListCommandCount( charCommands, &commandCount ) ) != rlSUCCESS ) return result;

		double width = 0;
		/*
		List<Command> commandList = font.GetChar(c);
		foreach(Command command in commandList)
		{
			if (command.CmdCode == CommandCode.JUMP_REL || command.CmdCode == CommandCode.MARK_REL)
				width += command.D0;
		}
		_CharWidth.Add(c, width);
		*/
	}

	return result;
}

// helper function which displays the description of the last error
static int printLastError( void )
{
	char errBuffer[1024] = { 0 };
	rlGetLastError( errBuffer, sizeof( errBuffer ) - 1 );
	printf( "%s", errBuffer );
	return rlERROR;
}
