/*
 * 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;
	static constexpr const int   MAX_SPEED           = 65535;

	struct CallbackContext
	{
		FSMI_Handle api;
		FSMI_TopTablePositionPhysical* position;
	};
	
	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)
	{
		auto* context = reinterpret_cast<CallbackContext*>(userData);
	
		context->position->roll  = data->roll;
		context->position->pitch = data->pitch;
		context->position->yaw   = bound(-MAX_YAW_RAD, data->yaw, MAX_YAW_RAD);
	
		ForceSeatMI_SendTopTablePosPhy(context->api, context->position);
	}
}

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_Yaw,   MSU_GPR_NotInverted/* no invertion*/, 3.1415f/180.0f/*deg to rad*/);
	MSU_GyroPlayer_SetCsvMapping(player, 2, MSU_GPR_Pitch, MSU_GPR_NotInverted/* no invertion*/, 3.1415f/180.0f/*deg to rad*/);
	MSU_GyroPlayer_SetCsvMapping(player, 3, MSU_GPR_Roll,  MSU_GPR_NotInverted/* no invertion*/, 3.1415f/180.0f/*deg to rad*/);

	/* 
	 * If absolute path is hardcoded, then remember to use \\ instead of single \, e.g:
	 *   MSU_GyroPlayer_LoadCsv(player, L"C:\\FSMI\\examples\\CSV_RollPitch_CPP\\sample_rotation.csv", ';', SAMPLES_INTERVAL_MS, MSU_GPR_Seconds);
	 */
	auto result = MSU_GyroPlayer_LoadCsv(player, L"sample_rotation.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_TopTablePositionPhysical position;
	memset(&position, 0, sizeof(position));
	position.structSize = sizeof(position);
	position.mask       = FSMI_POS_BIT_STATE | FSMI_POS_BIT_POSITION | FSMI_POS_BIT_MAX_SPEED;
	position.state      = FSMI_STATE_NO_PAUSE;
	position.maxSpeed   = MAX_SPEED;

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

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

	CallbackContext context;
	context.api      = api;
	context.position = &position;
	
	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;
	}

	// Activate appropriate ForceSeatPM profile
	printf("Remember to update profile name if non-default is used\n");
	ForceSeatMI_ActivateProfile(api, "SDK - Positioning");

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

