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

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

static rlResult markEvenlySpaced( rlHandle handle );
static int printLastError( void );


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 60%
	rlLaserConfig lc;
	if ( ( status = rlLaserGetConfig( handle, &lc ) ) != rlSUCCESS ) goto Cleanup;
	lc.PowerScale = 0.6;
	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;

	status = markEvenlySpaced( handle );

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

/// mark multiple rectangles with a specific belt distance inbetween
static rlResult markEvenlySpaced( rlHandle handle )
{
	rlResult result;

	bool beltMovesPositive = true;

	double jumpSpeed = 4;
	double markSpeed = 1;
	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.25 * fieldSize;

	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.PartDistance = 10000;
	config.CompensationSource.SourceType = stQuadratureDecoder;
	config.CompensationSource.ModuleID = ( encoder + 1 ) % 2;
	if ( ( result = rlMotfSetConfig( handle, &config ) ) != rlSUCCESS ) return result;

	int beltSign = beltMovesPositive ? 1 : -1;

	rlCommandListHandle list = rlListAllocate( handle );

	if ( ( result = rlListAppendPower( list, 65535 ) ) != 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;
	if ( ( result = rlListAppendJumpAbs2D( list, -beltSign * 0.45 * fieldSize, 0 ) ) != rlSUCCESS )	return result;
	// wait a specific distance, before the next object is marked
	if ( ( result = rlListAppendMotfWaitDistance( list, beltDistance, dmRelativeToCurrent ) ) != rlSUCCESS )	return result;
	if ( ( result = rlListAppendProgress( list, progressID ) ) != rlSUCCESS )	return result;
	if ( ( result = rlListAppendMarkRel2D( list, beltSign * sizeX, 0 ) ) != rlSUCCESS )	return result;
	if ( ( result = rlListAppendMarkRel2D( list, 0, sizeY ) ) != rlSUCCESS )	return result;
	if ( ( result = rlListAppendMarkRel2D( list, -beltSign * sizeX, 0 ) ) != rlSUCCESS ) return result;
	if ( ( result = rlListAppendMarkRel2D( list, 0, -sizeY ) ) != rlSUCCESS ) return result;
	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 < 5; 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;
			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;
}


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