/*
 * Copyright (C) 2012-2026 MotionSystems
 * 
 * This file is part of ForceSeatMI SDK.
 *
 * www.motionsystems.eu
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT
 * LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
 * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
 * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */
using MotionSystems;
using System;
using System.Diagnostics;
using System.Runtime.InteropServices;
using System.Threading;
using System.Windows.Input;

namespace TablePhyPos_CS
{
	class Program
	{
		const int INTERVAL = 4;

		[STAThread]
		static void Main(string[] args)
		{
			using (var mi = new ForceSeatMI_NET8())
			{
				if (!mi.IsLoaded())
				{
					Console.WriteLine("ForceSeatMI library has not been found! Please install ForceSeatPM.");
					return;
				}

				// Activate appropriate ForceSeatPM profile
				mi.SetAppID(""); // If you have dedicated app id, remove ActivateProfile calls from your code
				mi.ActivateProfile("SDK - Positioning");

				Work(mi);
			}
		}

		static double CalcSin(double t_ms, double amplitude, double hz)
		{
			return amplitude * Math.Sin((2.0 / 1000.0 * Math.PI) * hz * t_ms);
		}

		static double Deg2Rad(double deg)
		{
			return deg * Math.PI / 180;
		}

		static void Work(ForceSeatMI_NET8 mi)
		{
			// This is needed to increase accuracy of Thread.Sleep. You can find more
			// about it in MSDN.
			TimeBeginPeriod(1);

			var pos          = new FSMI_TopTablePositionPhysical();
			var platformInfo = new FSMI_PlatformInfo();

			Console.WriteLine("Giving time for the client to connect...");
			Thread.Sleep(3000);

			// Prepare structure by clearing it and setting correct size
			pos.structSize = (byte)Marshal.SizeOf(pos);
			pos.mask       = FSMI_POS_BIT.STATE | FSMI_POS_BIT.POSITION | FSMI_POS_BIT.MAX_SPEED;
			pos.maxSpeed   = 65535;

			// Let's start the motion
			mi.BeginMotionControl();

			Console.WriteLine("SIM started...");
			Console.WriteLine("Press 'q' to exit");

			var stopWatch = new Stopwatch();
			stopWatch.Start();

			ulong recentMark = 0;

			while (true)
			{
				if (Console.KeyAvailable && Console.ReadKey().KeyChar == 'q')
				{
					break;
				}

				pos.state = FSMI_State.NO_PAUSE;

				pos.pitch = 0;
				pos.roll  = (float)CalcSin(stopWatch.ElapsedMilliseconds, Deg2Rad(5), 0.5/*hz*/);
				pos.yaw   = 0;
				pos.sway =  0;
				pos.surge = 0;
				pos.heave = 0;

				mi.SendTopTablePosPhy(ref pos);

				var statusRoutineMark = stopWatch.ElapsedMilliseconds;

				// Get current status
				if (mi.GetPlatformInfoEx(ref platformInfo, (uint)Marshal.SizeOf(platformInfo), 100))
				{
					if (platformInfo.structSize != Marshal.SizeOf(platformInfo))
					{
						Console.WriteLine("Incorrect structure size: {0} vs {1}", platformInfo.structSize, Marshal.SizeOf(platformInfo));
						break;
					}
					else if (platformInfo.timemark == recentMark)
					{
						Console.WriteLine("No new platform info");
					}
					else
					{
						Console.WriteLine("Connected: {0}, Paused: {1}, Pos: {2}, {3}, {4}, {5}, {6}, {7}, Time: {8}, Module {9} status: {10}",
							platformInfo.isConnected != 0 ? "yes" : "no",
							(platformInfo.state & FSMI_PlatformCurrentState.AnyPaused) != 0 ? "yes" : "no",
							platformInfo.actualMotor1_Position,
							platformInfo.actualMotor2_Position,
							platformInfo.actualMotor3_Position,
							platformInfo.actualMotor4_Position,
							platformInfo.actualMotor5_Position,
							platformInfo.actualMotor6_Position,
							platformInfo.timemark,
							platformInfo.worstModuleStatusIndex,
							platformInfo.worstModuleStatusCode);

						Console.WriteLine("    Actual Top Frame: {0} roll {1}, pitch {2}, yaw {3}, heave {4}, surge {5}, sway {6}",
							platformInfo.fkRecentStatus != 0 ? "ok" : "failed",
							platformInfo.fkRoll,
							platformInfo.fkPitch,
							platformInfo.fkYaw,
							platformInfo.fkHeave,
							platformInfo.fkSurge,
							platformInfo.fkSway);

						recentMark = platformInfo.timemark;
					}
				}
				else
				{
					Console.WriteLine("Failed to get platform info");
				}

				// Calculate time wasted on status reading and printing
				var wastedTime = Math.Min(INTERVAL - 1, stopWatch.ElapsedMilliseconds - statusRoutineMark);

				// Without TimeBeginPeriod(1), this sleep is not accurate (due to Windows scheduler policy).
				// For more advanced application (e.g. with UI), this code should be moved to background 
				// task/thread and calling intervals should be controlled more precisely.
				Thread.Sleep(INTERVAL - (int)wastedTime);
			}
			stopWatch.Stop();
			mi.EndMotionControl();
			Console.WriteLine("SIM ended...");

			TimeEndPeriod(1);
		}

		[DllImport("winmm.dll", EntryPoint = "timeBeginPeriod")]
		private static extern uint TimeBeginPeriod(uint uMilliseconds);

		[DllImport("winmm.dll", EntryPoint = "timeEndPeriod")]
		private static extern uint TimeEndPeriod(uint uMilliseconds);
	}
}
