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

#include <chrono>
#include <thread>
#include <stdio.h>

#include "ForceSeatMI_Functions.h"
#include "MSU_GyroPlayer.h"

namespace
{
	// NOTE: Adjust to your needs
	static constexpr const int   SAMPLES_INTERVAL_MS = 10;
	static constexpr const float MAX_YAW_RAD         = 10 * 3.1415f / 180;

	struct CallbackContext
	{
		FSMI_Handle        api;
		FSMI_TelemetryACE* data;
	};

	template <class T>
	static inline constexpr T bound(T min, T value, T max) noexcept
	{
		return (value < min) ? min : ((value > max) ? max : value);
	}

	void callback(const MSU_GyroPlayerData* data, void* userData)
	{
		// NOTE: Without a washout algorithm, the top table may leave the work area quite quickly 
		// if input data recording was not started when the vehicle was at rest. In other words, 
		// the vehicle's velocities and accelerations should be 0 when input recording begins, 
		// otherwise the pre-existing vehicle velocities and accelerations will be unknown, 
		// and as a result, the top table may drift away from the center.
		// 
		// Alternatively, as it happens in this example, classical washout can be used to reproduce
		// sensation of the accelerations. This will NOT reproduce input acceleration 1:1 as high-pass
		// filters are used by the washout algorithm.
		// 
		// You can learn more about ACE (classical washout algorithm implementation) and accelerations
		// reproducing from the following documents:
		// All:       https://motionsystems.eu/files/Vehicle_physics_simulation_application.pdf
		// Chapter 8: https://motionsystems.eu/files/ForceSeatMI.pdf

		auto* context = reinterpret_cast<CallbackContext*>(userData);

		context->data->bodyLinearAcceleration.forward = data->accSurge;
		context->data->bodyLinearAcceleration.right   = data->accSway;
		context->data->bodyLinearAcceleration.upward  = data->accHeave;

		// This will be processed by ACE
		context->data->bodyPitch = data->pitch;
		context->data->bodyRoll  = data->roll;

		// ACE does not use raw yaw, only yaw-rate so let's pass the yaw as extra rotation
		context->data->extraRotation.yaw = bound(-MAX_YAW_RAD, data->yaw, MAX_YAW_RAD);

		ForceSeatMI_SendTelemetryACE(context->api, context->data);
	}
}

static void work(FSMI_Handle api, MSU_GP_Handle player)
{
	printf("Giving time for the client to connect...\n");
	std::this_thread::sleep_for(std::chrono::milliseconds(3000));

	// Skip header line in CSV
	MSU_GyroPlayer_SetCsvSkipLines(player, 1);

	// Define columns meaning in CSV file
	MSU_GyroPlayer_SetCsvMapping(player, 1, MSU_GPR_AccSway,  MSU_GPR_NotInverted /* no invertion*/, 1.0f);
	MSU_GyroPlayer_SetCsvMapping(player, 2, MSU_GPR_AccSurge, MSU_GPR_NotInverted /* no invertion*/, 1.0f);
	MSU_GyroPlayer_SetCsvMapping(player, 3, MSU_GPR_AccHeave, MSU_GPR_NotInverted /* no invertion*/, 1.0f);
	MSU_GyroPlayer_SetCsvMapping(player, 4, MSU_GPR_Pitch,    MSU_GPR_NotInverted /* no invertion*/, 1.0f);
	MSU_GyroPlayer_SetCsvMapping(player, 5, MSU_GPR_Roll,     MSU_GPR_NotInverted /* no invertion*/, 1.0f);
	MSU_GyroPlayer_SetCsvMapping(player, 6, MSU_GPR_Yaw,      MSU_GPR_NotInverted /* no invertion*/, 1.0f);

	/* 
	 * If absolute path is hardcoded, then remember to use \\ instead of single \, e.g:
	 *   MSU_GyroPlayer_LoadCsv(player, L"C:\\FSMI\\examples\\CSV_Acc_ACE_CPP\\sample_1.csv", ';', SAMPLES_INTERVAL_MS, MSU_GPR_Seconds);
	 */
	auto result = MSU_GyroPlayer_LoadCsv(player, L"sample_1.csv", ';', SAMPLES_INTERVAL_MS, MSU_GPR_Seconds);

	if (result != MSU_GPEC_Ok)
	{
		printf("Failed to load CSV, error code: %d\n", result);
		return;
	}

	// Communication structure
	FSMI_TelemetryACE data;
	memset(&data, 0, sizeof(data));
	data.structSize = sizeof(data);
	data.state      = FSMI_STATE_NO_PAUSE;

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

	printf("Playing started...\n");

	CallbackContext context;
	context.api  = api;
	context.data = &data;
	
	result = MSU_GyroPlayer_PlayAsync(player, &callback, &context);

	if (result != MSU_GPEC_Ok)
	{
		printf("Failed to play CSV, error code: %d\n", result);
		return;
	}

	MSU_GyroPlayer_Wait(player);
	ForceSeatMI_EndMotionControl(api);
}

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

	auto player = MSU_GyroPlayer_Create();
	if (!player)
	{
		printf("MSU DLL was not loaded, so the game will not send anything to the ForceSeatPM\n");
		ForceSeatMI_Delete(api);
		return 0;
	}

	printf("Samples interval is %d ms, adjust if needed\n", SAMPLES_INTERVAL_MS);

	// NOTE: You will probably have to tune ACE parameters to create desired motion platform output.
	// In order to achieve it, the SDK - Vehicle Telemetry ACE will have to be cloned. When that happen,
	// make sure to remove following line
	printf("Remember to update profile name if non-default is used\n");
	ForceSeatMI_ActivateProfile(api, "SDK - Vehicle Telemetry ACE");

	work(api, player);
	MSU_GyroPlayer_Delete(player);
	ForceSeatMI_Delete(api);
	return 0;
}

