using RAYLASE.SPICE3;
using RAYLASE.SPICE3.ClientLib;
using System;
using System.Diagnostics;

namespace EvenlySpaced
{
	class Program
	{
		// insert the IP of your SP-ICE-3
		readonly string _CardIp = "192.168.2.138";
		ClientAPI _Client = new ClientAPI();

		static void Main( string[] args )
		{
			Program p = new Program();

			p._Client.Connect( p._CardIp );
			// 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.)
			p._Client.System.ResetToDefaults();
			p._Client.Laser.ArmLaser( true );
			LaserConfig lc = p._Client.Laser.GetConfig();
			lc.PowerScale = 1.0;
			p._Client.Laser.SetConfig( lc );

			p.markEvenlySpaced();
		}

		/// <summary>
		/// mark multiple rectangles with a specific belt distance inbetween
		/// </summary>
		private void markEvenlySpaced()
		{
			double jumpSpeed = 4;
			double markSpeed = 1;
			double beltSpeed = 0.27; // 1 km/h

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

			ScannerConfig scanner = _Client.Scanner.GetConfig();

			double fieldSize = scanner.FieldSize.X;
			double sizeX = 0.1 * fieldSize;
			double sizeY = 0.1 * fieldSize;
			double beltDistance = 0.25 * fieldSize;

			// remove comment for encoder simulation
			//_Client.Motf.SetSimulationSpeed( encoderID, 0.005, 0 );
			_Client.Motf.QuadratureDecoder.ResetEncoderPosition( encoderID );
			double beltTimeMs = 1e-3 * beltDistance / beltSpeed;

			bool beltMovesPositive = true;

			QuadratureDecoderConfig qd = new QuadratureDecoderConfig();
			qd.Modules[encoderID].InvertDirection = true;
			qd.Modules[encoderID].DistancePerCount = 36;
			_Client.Motf.QuadratureDecoder.SetConfig( qd );

			MotfConfig config = new MotfConfig();
			config.MainSource.SourceType = MotfSourceType.QuadratureDecoder;
			config.MainSource.ModuleID = encoderID;
			//config.MainEncoder.SimulateEncoder = true;
			// in this exaple, the belt is moving in Y direction
			config.SetDirectionFromAngle( Math.PI / 2 + ( beltMovesPositive ? 0 : Math.PI ) );
			config.CompensationSource.SourceType = MotfSourceType.QuadratureDecoder;
			config.CompensationSource.ModuleID = ( encoderID + 1 ) % 2;
			_Client.Motf.SetConfig( config );

			int beltSign = beltMovesPositive ? 1 : -1;
			CommandList list = new CommandList();
			list.AppendJumpSpeed( jumpSpeed );
			list.AppendJumpDelay( 0 );
			list.AppendMarkSpeed( markSpeed );
			list.AppendPolyDelay( 0 );
			list.AppendMarkDelay( 0 );
			list.AppendLmFrequency( 1.0 / 200 );
			list.AppendLmWidth( 200 );
			list.AppendPower( 10000 );
			list.AppendJumpAbs( -beltSign * 0.45 * fieldSize, 0 );
			// wait a specific distance, before the next object is marked
			list.AppendMotfWaitDistance( beltDistance, MotfDistanceMode.RelativeToCurrent );
			list.AppendProgress( progressID );
			list.AppendMarkRel( beltSign * sizeX, 0 );
			list.AppendMarkRel( 0, sizeY );
			list.AppendMarkRel( -beltSign * sizeX, 0 );
			list.AppendMarkRel( 0, -sizeY );
			list.AppendLaserOff();
			list.AppendStopMotf( 0, 0 );
			double markTimeMs = 2.0e-3 * ( sizeX + sizeY ) / markSpeed;

			_Client.List.Set( listID, list );

			const int iterations = 50;
			double[] startTimes = new double[iterations];
			double[] endTimes = new double[iterations];
			int[] encoderPositions = new int[iterations];
			Stopwatch sw = new Stopwatch();
			for ( int iteration = 0; iteration < iterations; iteration++ )
			{
				_Client.List.Execute( listID );
				if ( iteration == 0 )
				{
					qd.Modules[encoderID].SimulationSpeed = beltSpeed / qd.Modules[encoderID].DistancePerCount;
					_Client.Motf.QuadratureDecoder.SetConfig( qd );
					sw.Restart();
				}

				int? pid;
				_Client.List.WaitForProgress( out pid, (int)beltTimeMs + 4000 );
				startTimes[iteration] = sw.ElapsedMilliseconds;

				encoderPositions[iteration] = _Client.Motf.QuadratureDecoder.ReadEncoderPosition( encoderID );

				int? doneID;
				_Client.List.WaitForListDone( out doneID, (int)markTimeMs + 4000 );
				endTimes[iteration] = sw.ElapsedMilliseconds;
			}
			sw.Stop();

			int failures = 0;
			Console.Out.WriteLine( "Iteration\tExp. Start @\tAct. Start @\tExp. End @\tAct. End @\tEncoder\tStatus" );
			for ( int iteration = 0; iteration < iterations; iteration++ )
			{
				const int maxDeviationMs = 10;

				string statusText = "";
				double expectedStart = ( iteration + 1 ) * beltTimeMs;
				if ( Math.Abs( startTimes[iteration] - expectedStart ) > maxDeviationMs )
				{
					statusText += string.Format( "Start Time deviates by more than {0}ms; ", maxDeviationMs );
					failures++;
				}

				double expectedEnd = expectedStart + markTimeMs;
				if ( Math.Abs( endTimes[iteration] - expectedEnd ) > maxDeviationMs )
				{
					statusText += string.Format( "End Time deviates by more than {0}ms; ", maxDeviationMs );
					failures++;
				}

				Console.Out.WriteLine( "{0}\t{1:F2}\t{2:F2}\t{3:F2}\t{4:F2}\t{5}\t{6}",
					iteration + 1,
					expectedStart, startTimes[iteration],
					expectedEnd, endTimes[iteration],
					encoderPositions[iteration],
					statusText );
			}
		}
	}
}
