/*
 * 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.
 */

#define _USE_MATH_DEFINES

#include <windows.h>
#include <stdio.h>
#include <math.h>
#include <string>
#include <chrono>
#include <thread>

#pragma comment(lib, "Winmm.lib")

#include "ForceSeatMI_Functions.h"

namespace
{
	static const int64_t INTERVAL = 10;

	static double CalcSin(double t_s, double amplitude, double hz)
	{
		return amplitude * sin((2.0 * M_PI) * hz * t_s);
	}

	static double Deg2Rad(double deg)
	{
		return deg * (M_PI / 180.0);
	}

	class Timer
	{
	public:
		typedef std::chrono::high_resolution_clock Clock;

		Timer()
		{
			m_start = Clock::now();
		}

		int64_t Elapsed_ms() const
		{
			auto now = Clock::now();
			std::chrono::duration<double, std::milli> elapsed(now - m_start);
			return static_cast<int64_t>(elapsed.count());
		}

		double Elapsed_s() const
		{
			auto now = Clock::now();
			std::chrono::duration<double> elapsed(now - m_start);
			return elapsed.count();
		}

		void Wait(int64_t duration)
		{
			if (duration > 0)
			{
				std::this_thread::sleep_for(std::chrono::milliseconds(duration));
			}
		}

	private:
		Clock::time_point m_start;
	};
}

static void append(std::wstring& str, const wchar_t* text)
{
	if (! str.empty())
	{
		str += L" ";
	}
	str += text;
}

static std::wstring stateToString(FSMI_UINT8 state)
{
	std::wstring result;
	if ((state & FSMI_PlatformCurrentState_AnyPaused) != 0)
	{
		append(result, L"paused");
	}
	if ((state & FSMI_PlatformCurrentState_Offline) != 0)
	{
		append(result, L"offline");
	}
	auto parkMode = state & FSMI_PlatformCurrentState_ParkModeMask;
	switch (parkMode)
	{
		case FSMI_PlatformCurrentState_SoftParkToCenter:
			append(result, L"soft center");
			break;
		case FSMI_PlatformCurrentState_SoftParkNormal:
			append(result, L"soft normal");
			break;
		case FSMI_PlatformCurrentState_SoftParkForTransport:
			append(result, L"soft transport");
			break;
		case FSMI_PlatformCurrentState_OperatorParkToCenter:
			append(result, L"operator center");
			break;
		case FSMI_PlatformCurrentState_OperatorParkNormal:
			append(result, L"operator normal");
			break;
		case FSMI_PlatformCurrentState_OperatorParkForTransport:
			append(result, L"operator transport");
			break;
		default:
			break;
	}

	if (parkMode != FSMI_PlatformCurrentState_NoParking && (state & FSMI_PlatformCurrentState_ParkingCompleted) != 0)
	{
		append(result, L"(completed)");
	}

	return result.empty() ? L"running" : result;
}

static void work(FSMI_Handle api)
{
	// This is needed to increase accuracy of Thread.Sleep. You can find more
	// about it in MSDN.
	timeBeginPeriod(1);

	printf("Giving time for the client to connect...\n");
	Sleep(3000);

	// Communication structures
	FSMI_TelemetryACE telemetry;
	FSMI_SFX sfx;
	FSMI_TactileAudioBasedFeedbackEffects audioEffects;
	FSMI_PlatformInfo platformInfo;

	// Prepare structures
	memset(&telemetry, 0, sizeof(telemetry));
	telemetry.structSize = sizeof(telemetry);
	telemetry.state = FSMI_STATE_PAUSE;

	memset(&sfx, 0, sizeof(sfx));
	sfx.structSize = sizeof(sfx);

	memset(&audioEffects, 0, sizeof(audioEffects));
	audioEffects.structSize = sizeof(audioEffects);

	// Demo data
	unsigned int sfxIterator = 0;
	
	// Configure SFX
	// Level 2 is supported by "PS" and "QS" motion platforms, Level 3 and 4 are supported by "QS" motion platforms.
	sfx.effects[0].type      = FSMI_SFX_EffectType_SinusLevel2;
	sfx.effects[0].area      = FSMI_SFX_AreaFlags_FrontLeft;
	sfx.effects[0].amplitude = 0.05f;
	sfx.effects[0].frequency = 0;
	sfx.effectsCount         = 1;	

	// This demo application does not use any TactileAudioBaseFeedbackEffects

	// Let's start the motion
	ForceSeatMI_BeginMotionControl(api);

	printf("SIM started...\n");
	printf("Press 'q' to exit\n");

	Timer stopWatch;
	FSMI_UINT64 recentMark = 0;

	while (GetKeyState('Q') == 0)
	{
		telemetry.state = FSMI_STATE_NO_PAUSE;

		telemetry.bodyAngularVelocity.yaw   = 0;
		telemetry.bodyAngularVelocity.pitch = 0;
		telemetry.bodyAngularVelocity.roll  = (float)CalcSin(stopWatch.Elapsed_s(), 20000, 0.5/*hz*/);
		telemetry.bodyLinearAcceleration.forward = 0;
		telemetry.bodyLinearAcceleration.upward  = 0;
		telemetry.bodyLinearAcceleration.right   = (float)CalcSin(stopWatch.Elapsed_s(), 10, 0.7/*hz*/);

		// Use extra parameters to generate custom effects, for exmp. vibrations. They will NOT be
		// filtered, smoothed or processed in any way.
		telemetry.extraRotation.yaw   = 0;
		telemetry.extraRotation.pitch = 0;
		telemetry.extraRotation.roll  = (float)CalcSin(stopWatch.Elapsed_s(), Deg2Rad(5), 0.5/*hz*/);
		telemetry.extraTranslation.right   = 0;
		telemetry.extraTranslation.upward  = 0;
		telemetry.extraTranslation.forward = 0;

		sfx.effects[0].frequency = (byte)(sfxIterator);
		ForceSeatMI_SendTelemetryACE3(api, &telemetry, &sfx, &audioEffects, nullptr);

		// Change values somehow
		++sfxIterator;
		if (sfxIterator > 50)
		{
			sfxIterator = 0;
		}

		auto statusRoutineMark = stopWatch.Elapsed_ms();

		// Get current status
		if (ForceSeatMI_GetPlatformInfoEx(api, &platformInfo, sizeof(platformInfo), 100))
		{
			if (platformInfo.structSize != sizeof(platformInfo))
			{
				wprintf(L"Incorrect structure size: %d vs %d\n", (int)platformInfo.structSize, (int)sizeof(platformInfo));
				break;
			}
			else if (platformInfo.timemark == recentMark)
			{
				wprintf(L"No new platform info\n");
			}
			else
			{
				wprintf(L"Connected: %ls, State: %ls, Pos: %u, %u, %u, %u, %u, %u, Time: %llu, Module %u status: %u\n",
					platformInfo.isConnected != FSMI_False ? L"yes" : L"no",
					stateToString(platformInfo.state).c_str(),
					platformInfo.actualMotorPosition[0],
					platformInfo.actualMotorPosition[1],
					platformInfo.actualMotorPosition[2],
					platformInfo.actualMotorPosition[3],
					platformInfo.actualMotorPosition[4],
					platformInfo.actualMotorPosition[5],
					platformInfo.timemark,
					platformInfo.worstModuleStatusIndex,
					platformInfo.worstModuleStatusCode);

				wprintf(L"    Actual Top Frame: %ls roll %f, pitch %f, yaw %f, heave %f, surge %f, sway %f\n",
					platformInfo.fkRecentStatus ? L"ok" : L"failed",
					platformInfo.fkRoll,
					platformInfo.fkPitch,
					platformInfo.fkYaw,
					platformInfo.fkHeave,
					platformInfo.fkSurge,
					platformInfo.fkSway);

				recentMark = platformInfo.timemark;
			}
		}
		else
		{
			wprintf(L"Failed to get platform info\n");
		}

		// Calculate time wasted on status reading and printing
		auto wastedTime = std::min<int64_t>(INTERVAL - 1, stopWatch.Elapsed_ms() - 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.
		stopWatch.Wait(INTERVAL - wastedTime);
	}
	ForceSeatMI_EndMotionControl(api);
	printf("SIM ended...\n");

	timeEndPeriod(1);
}

int main(int argc, wchar_t* argv[])
{
	FSMI_Handle api = ForceSeatMI_Create();
	if (! api)
	{
		wprintf(L"ForceSeatMI DLL was not loaded, so the game will not send anything to the ForceSeatPM\n");
		return -1;
	}

	// Activate appropriate ForceSeatPM profile
	ForceSeatMI_SetAppID(api, ""); // If you have dedicated app id, remove ActivateProfile calls from your code
	ForceSeatMI_ActivateProfile(api, "SDK - Vehicle Telemetry ACE");

	work(api);
	ForceSeatMI_Delete(api);
	return 0;
}
