using RAYLASE.SPICE3;
using RAYLASE.SPICE3.ClientLib;
using RAYLASE.SPICE3.Utils;
using System;
using System.Collections.Generic;
using System.Diagnostics;

namespace ObjectGtField
{
	internal class Program
	{
		// insert the IP of your SP-ICE-3
		private readonly string _CardIp = "192.168.2.138";

		private ClientAPI _Client = new ClientAPI();
		private Dictionary<char, double> _CharWidth = new Dictionary<char, double>();

		private 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.configureFont();
			p.buildCharWidthDict();

			p.markText( "1234567890l1234567890l1234567890l" );
			p.markText( "0123456789012345678901234567890" );
			p.markText( "ABC-DEF:ABC-DEF " );
		}

		/// <summary>
		/// mark a text greater than ne field, using motf
		/// </summary>
		private void markText( string text )
		{
			bool beltMovesPositive = true;

			double jumpSpeed = 10;
			double markSpeed = 3.4;
			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.05 * fieldSize;
			double traveledDistance = 0;

			// simulatate encoder
			_Client.Motf.QuadratureDecoder.ResetEncoderPosition( encoderID );
			double beltTimeMs = 1e-3 * beltDistance / beltSpeed;

			QuadratureDecoderConfig qd = new QuadratureDecoderConfig();
			qd.Modules[encoderID].InvertDirection = false;
			qd.Modules[encoderID].SimulateEncoder = true;
			qd.Modules[encoderID].DistancePerCount = 36;
			qd.Modules[encoderID].SimulationSpeed = 0.01;
			qd.Modules[encoderID].SimulationAcceleration = 0;
			_Client.Motf.QuadratureDecoder.SetConfig( qd );

			MotfConfig motfConfig = new MotfConfig();
			motfConfig.MainSource.SourceType = MotfSourceType.QuadratureDecoder;
			motfConfig.MainSource.ModuleID = encoderID;
			motfConfig.SetDirectionFromAngle( Math.PI / 2 + ( beltMovesPositive ? 0 : Math.PI ) );
			motfConfig.CompensationSource.SourceType = MotfSourceType.QuadratureDecoder;
			motfConfig.CompensationSource.ModuleID = ( encoderID + 1 ) % 2;
			_Client.Motf.SetConfig( motfConfig );

			int beltSign = beltMovesPositive ? 1 : -1;
			CommandList list = new CommandList();
			list.AppendJumpSpeed( jumpSpeed );
			list.AppendJumpDelay( 300 );
			list.AppendMarkSpeed( markSpeed );
			list.AppendPolyDelay( 0 );
			list.AppendMarkDelay( 0 );
			list.AppendLmFrequency( 1.0 / 200 );
			list.AppendLmWidth( 200 );
			list.AppendPower( 35000 );

			// rotation of the object to mark, value could be delivered by camera etc
			double rotation = 5 * Math.PI / 180;
			int scalingFactor = 20;

			// rotate and scale, as the size of the font is only 1000
			Transform3D transformationScaled = new Transform3D();
			transformationScaled.RotateZ( rotation - Math.PI / 2 );
			transformationScaled.Scale( scalingFactor, scalingFactor );

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

			// reverse text
			// in this exapmple, the text is marked form the right to the left side
			char[] textReversed = ( " " + text ).ToCharArray();
			Array.Reverse( textReversed );

			// 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
			char? lastChar = null;
			foreach ( char c in textReversed )
			{
				if ( lastChar == null )
				{
					lastChar = c;
					continue;
				}

				list.AppendTransformation( transformationDefault );

				list.AppendJumpAbs( 0.3 * fieldSize - Math.Sin( rotation ) * traveledDistance, 0.49 * fieldSize );
				list.AppendProgress( progressID );
				list.AppendTransformation( transformationScaled );
				list.AppendMarkText( lastChar.ToString() );
				// 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
				list.AppendMotfWaitDistance( ( _CharWidth[c] + 550 ) / 1.85 * scalingFactor * Math.Cos( rotation ), MotfDistanceMode.RelativeToPrevious );
				// sum the distance for the Vertical offset
				traveledDistance += ( _CharWidth[c] + 550 ) / 1.85 * scalingFactor;
				lastChar = c;
			}

			list.AppendLaserOff();
			list.AppendStopMotf( 0, 0 );

			// estimate marking time
			double markTimeMs = text.Length * scalingFactor * 4000 / markSpeed;

			_Client.List.Set( listID, list );

			const int iterations = 2;
			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 );

				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 + 40000 ) * text.Length );
				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 );
			}
		}

		/// <summary>
		/// create Font
		/// only digits space dash colon and A-F 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
		/// </summary>
		private void configureFont()
		{
			CommandList c0 = new CommandList();
			c0.AppendJumpRel( 500, 1000 );
			c0.AppendMarkRel( 0, -500 );
			c0.AppendMarkRel( 0, -500 );
			c0.AppendMarkRel( -500, 0 );
			c0.AppendMarkRel( 0, 500 );
			c0.AppendMarkRel( 0, 500 );
			c0.AppendMarkRel( 500, 0 );
			c0.AppendJumpRel( 100, -1000 );

			CommandList c1 = new CommandList();
			c1.AppendMarkRel( 0, 1000 );
			c1.AppendJumpRel( 100, -1000 );

			CommandList c2 = new CommandList();
			c2.AppendJumpRel( 0, 1000 );
			c2.AppendMarkRel( 500, 0 );
			c2.AppendMarkRel( 0, -500 );
			c2.AppendMarkRel( -500, 0 );
			c2.AppendMarkRel( 0, -500 );
			c2.AppendMarkRel( 500, 0 );
			c2.AppendJumpRel( 100, 0 );

			CommandList c3 = new CommandList();
			c3.AppendJumpRel( 0, 1000 );
			c3.AppendMarkRel( 500, 0 );
			c3.AppendMarkRel( 0, -500 );
			c3.AppendMarkRel( -500, 0 );
			c3.AppendJumpRel( 500, 0 );
			c3.AppendMarkRel( 0, -500 );
			c3.AppendMarkRel( -500, 0 );
			c3.AppendJumpRel( 600, 0 );

			CommandList c4 = new CommandList();
			c4.AppendJumpRel( 0, 1000 );
			c4.AppendMarkRel( 0, -500 );
			c4.AppendMarkRel( 500, 0 );
			c4.AppendJumpRel( 0, 500 );
			c4.AppendMarkRel( 0, -1000 );
			c4.AppendJumpRel( 100, 0 );

			CommandList c5 = new CommandList();
			c5.AppendJumpRel( 500, 1000 );
			c5.AppendMarkRel( -500, 0 );
			c5.AppendMarkRel( 0, -500 );
			c5.AppendMarkRel( 500, 0 );
			c5.AppendMarkRel( 0, -500 );
			c5.AppendMarkRel( -500, 0 );
			c5.AppendJumpRel( 600, 0 );

			CommandList c6 = new CommandList();
			c6.AppendJumpRel( 500, 1000 );
			c6.AppendMarkRel( -500, 0 );
			c6.AppendMarkRel( 0, -500 );
			c6.AppendMarkRel( 0, -500 );
			c6.AppendMarkRel( 500, 0 );
			c6.AppendMarkRel( 0, 500 );
			c6.AppendMarkRel( -500, 0 );
			c6.AppendJumpRel( 600, -500 );

			CommandList c7 = new CommandList();
			c7.AppendJumpRel( 0, 1000 );
			c7.AppendMarkRel( 500, 0 );
			c7.AppendMarkRel( 0, -500 );
			c7.AppendMarkRel( 0, -500 );
			c7.AppendJumpRel( 100, 0 );

			CommandList c8 = new CommandList();
			c8.AppendJumpRel( 500, 1000 );
			c8.AppendMarkRel( -500, 0 );
			c8.AppendMarkRel( 0, -500 );
			c8.AppendMarkRel( 500, 0 );
			c8.AppendMarkRel( 0, 500 );
			c8.AppendJumpRel( 0, -500 );
			c8.AppendMarkRel( 0, -500 );
			c8.AppendMarkRel( -500, 0 );
			c8.AppendMarkRel( 0, 500 );
			c8.AppendJumpRel( 600, -500 );

			CommandList c9 = new CommandList();
			c9.AppendJumpRel( 500, 1000 );
			c9.AppendMarkRel( -500, 0 );
			c9.AppendMarkRel( 0, -500 );
			c9.AppendMarkRel( 500, 0 );
			c9.AppendMarkRel( 0, 500 );
			c9.AppendJumpRel( 0, -500 );
			c9.AppendMarkRel( 0, -500 );
			c9.AppendMarkRel( -500, 0 );
			c9.AppendJumpRel( 600, 0 );

			CommandList A = new CommandList();
			A.AppendMarkRel( 250, 1000 );
			A.AppendMarkRel( 250, -1000 );
			A.AppendJumpRel( -125, 500 );
			A.AppendMarkRel( -250, 0 );
			A.AppendJumpRel( 475, -500 );

			CommandList B = new CommandList();
			B.AppendMarkRel( 0, 1000 );
			B.AppendMarkRel( 400, 0 );
			B.AppendMarkRel( 100, -100 );
			B.AppendMarkRel( 0, -400 );
			B.AppendMarkRel( -500, 0 );
			B.AppendJumpRel( 500, 0 );
			B.AppendMarkRel( 0, -400 );
			B.AppendMarkRel( -100, -100 );
			B.AppendMarkRel( -400, -0 );
			B.AppendJumpRel( 600, 0 );

			CommandList C = new CommandList();
			C.AppendJumpRel( 500, 0 );
			C.AppendMarkRel( -500, 0 );
			C.AppendMarkRel( 0, 1000 );
			C.AppendMarkRel( 500, 0 );
			C.AppendJumpRel( 100, -1000 );

			CommandList D = new CommandList();
			D.AppendMarkRel( 0, 1000 );
			D.AppendMarkRel( 400, 0 );
			D.AppendMarkRel( 100, -100 );
			D.AppendMarkRel( 0, -800 );
			D.AppendMarkRel( -100, -100 );
			D.AppendMarkRel( -400, -0 );
			D.AppendJumpRel( 600, 0 );

			CommandList E = new CommandList();
			E.AppendJumpRel( 500, 1000 );
			E.AppendMarkRel( -500, 0 );
			E.AppendMarkRel( 0, -500 );
			E.AppendMarkRel( 500, 0 );
			E.AppendJumpRel( -500, 0 );
			E.AppendMarkRel( 0, -500 );
			E.AppendMarkRel( 500, 0 );
			E.AppendJumpRel( 100, 0 );

			CommandList F = new CommandList();
			F.AppendJumpRel( 500, 1000 );
			F.AppendMarkRel( -500, 0 );
			F.AppendMarkRel( 0, -500 );
			F.AppendMarkRel( 500, 0 );
			F.AppendJumpRel( -500, 0 );
			F.AppendMarkRel( 0, -500 );
			F.AppendJumpRel( 600, 0 );

			CommandList colon = new CommandList();
			colon.AppendJumpRel( 0, 200 );
			colon.AppendMarkRel( 200, 0 );
			colon.AppendMarkRel( 0, 200 );
			colon.AppendMarkRel( -200, 0 );
			colon.AppendMarkRel( 0, -200 );
			colon.AppendJumpRel( 0, 400 );
			colon.AppendMarkRel( 200, 0 );
			colon.AppendMarkRel( 0, 200 );
			colon.AppendMarkRel( -200, 0 );
			colon.AppendMarkRel( 0, -200 );
			colon.AppendJumpRel( 300, -600 );

			CommandList space = new CommandList();
			space.AppendJumpRel( 500, 0 );

			CommandList dash = new CommandList();
			dash.AppendJumpRel( 0, 500 );
			dash.AppendMarkRel( 500, 0 );
			dash.AppendJumpRel( 100, -500 );

			CommandList longSlash = new CommandList();
			longSlash.AppendMarkRel( 4900, 1000 );
			longSlash.AppendJumpRel( 100, -1000 );

			Font font = new Font( 0 );
			font.AddChar( ' ', space );
			font.AddChar( '0', c0 );
			font.AddChar( '1', c1 );
			font.AddChar( '2', c2 );
			font.AddChar( '3', c3 );
			font.AddChar( '4', c4 );
			font.AddChar( '5', c5 );
			font.AddChar( '6', c6 );
			font.AddChar( '7', c7 );
			font.AddChar( '8', c8 );
			font.AddChar( '9', c9 );
			font.AddChar( ':', colon );
			font.AddChar( '-', dash );
			font.AddChar( 'l', longSlash );
			font.AddChar( 'A', A );
			font.AddChar( 'B', B );
			font.AddChar( 'C', C );
			font.AddChar( 'D', D );
			font.AddChar( 'E', E );
			font.AddChar( 'F', F );

			_Client.Font.Set( font );
			_Client.Font.SetActive( 0 );
		}

		/// <summary>
		/// extract the length of the chars defenied in configureFont()
		/// </summary>
		private void buildCharWidthDict()
		{
			CommandValueReader reader = new CommandValueReader();
			_CharWidth.Clear();
			Font font = _Client.Font.Get( 0 );
			var charlist = font.GetCharList();
			foreach ( var c in charlist )
			{
				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 )
					{
						reader.SetCommand( command );
						width += reader.AsPoint2D.X;
					}
				}
				_CharWidth.Add( c, width );
			}
		}
	}
}
