/*
 * 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 "ForceSeatMI_GoToAPI.h"

#include <thread>
#include <chrono>
#include <stdexcept>
#include <cmath>

namespace
{
	constexpr const double  TIMEOUT_S             = 15;
	constexpr const int64_t TIMEOUT_MS            = static_cast<int64_t>(TIMEOUT_S) * 1000;
	constexpr const int64_t SLOW_RATE_INTERVAL_MS = 100;
	constexpr const int64_t FAST_RATE_INTERVAL_MS = 4;
	constexpr const int64_t STARTUP_DELAY         = 3000;
	constexpr const int64_t SHUTDOWN_DELAY        = 500;

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

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

		inline 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());
		}

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

		inline void Wait(int64_t duration_ms)
		{
			if (duration_ms > 0)
			{
				// Without timeBeginPeriod(1), this sleep is not accurate (due to Windows scheduler policy).
				std::this_thread::sleep_for(std::chrono::milliseconds(duration_ms));
			}
		}

	private:
		Clock::time_point m_start;
	};

	struct Trajectory
	{
		struct
		{
			double pos;
			double vel;
			double acc;
		} params;
		
		double posStart;
		double sTotal;

		double tAccFinish;
		double tConstVelFinish;
		double tAllFinish;

		double tmp;
	};

	template <class T>
	T Sign(T a)
	{
		return a < 0 ? -1 : 1;
	}
}

ForceSeatMI_GoToAPI::ForceSeatMI_GoToAPI(const char* profileName)
{
	m_api = ForceSeatMI_Create();
	if (! m_api)
	{
		throw std::runtime_error("ForceSeatMI DLL was not loaded");
	}

	ForceSeatMI_ActivateProfile(m_api, profileName);
	ForceSeatMI_BeginMotionControl(m_api);

	// Wait for ForceSeatPM to connect
	FSMI_TopTablePositionPhysical req {};
	req.structSize = sizeof(req);
	req.mask       = 0;

	Timer timer;
	do 
	{
		ForceSeatMI_SendTopTablePosPhy(m_api, &req);
		timer.Wait(SLOW_RATE_INTERVAL_MS);
	} while (timer.Elapsed_ms() < STARTUP_DELAY);
}

ForceSeatMI_GoToAPI::~ForceSeatMI_GoToAPI()
{
	if (m_api)
	{
		ForceSeatMI_EndMotionControl(m_api);
		Timer().Wait(SHUTDOWN_DELAY);
		ForceSeatMI_Delete(m_api);
		m_api = nullptr;
	}
}

void ForceSeatMI_GoToAPI::ParkSync()
{
	Timer timer;
	FSMI_PlatformInfo info {};

	do
	{
		Read(info);

		if (info.state && FSMI_PlatformCurrentState_ParkingCompleted)
		{ 
			auto parkMode = info.state & FSMI_PlatformCurrentState_ParkModeMask;

			if (parkMode == FSMI_PlatformCurrentState_SoftParkNormal ||
				parkMode == FSMI_PlatformCurrentState_OperatorParkNormal)
			{
				return;
			}
		}

		ForceSeatMI_Park(m_api, FSMI_ParkMode_Normal);

		timer.Wait(SLOW_RATE_INTERVAL_MS);
	} while (timer.Elapsed_ms() < TIMEOUT_MS);
}

void ForceSeatMI_GoToAPI::CenterSync()
{
	Timer timer;
	FSMI_PlatformInfo info {};

	do
	{
		Read(info);

		if (info.state && FSMI_PlatformCurrentState_ParkingCompleted)
		{ 
			auto parkMode = info.state & FSMI_PlatformCurrentState_ParkModeMask;

			if (parkMode == FSMI_PlatformCurrentState_SoftParkToCenter ||
				parkMode == FSMI_PlatformCurrentState_OperatorParkToCenter)
			{
				return;
			}
		}

		ForceSeatMI_Park(m_api, FSMI_ParkMode_ToCenter);

		timer.Wait(SLOW_RATE_INTERVAL_MS);
	} while (timer.Elapsed_ms() < TIMEOUT_MS);
}

void ForceSeatMI_GoToAPI::GoToSync(double roll,  double rollVel,  double rollAcc,
								   double pitch, double pitchVel, double pitchAcc,
								   double yaw,   double yawVel,   double yawAcc,
								   double heave, double heaveVel, double heaveAcc,
								   double sway,  double swayVel,  double swayAcc,
								   double surge, double surgeVel, double surgeAcc)
{
	if (! m_api)
	{
		throw std::runtime_error("API not initialized");
	}

	std::array<Trajectory, MotionId::__Count> data {};

	data[MotionId::Roll] .params = { roll,  std::max(0.01, std::abs(rollVel)),  std::max(0.01, std::abs(rollAcc))  };
	data[MotionId::Pitch].params = { pitch, std::max(0.01, std::abs(pitchVel)), std::max(0.01, std::abs(pitchAcc)) };
	data[MotionId::Yaw]  .params = { yaw,   std::max(0.01, std::abs(yawVel)),   std::max(0.01, std::abs(yawAcc))   };
	data[MotionId::Heave].params = { heave, std::max(0.01, std::abs(heaveVel)), std::max(0.01, std::abs(heaveAcc)) };
	data[MotionId::Sway] .params = { sway,  std::max(0.01, std::abs(swayVel)),  std::max(0.01, std::abs(swayAcc))  };
	data[MotionId::Surge].params = { surge, std::max(0.01, std::abs(surgeVel)), std::max(0.01, std::abs(surgeAcc)) };

	for (int i = MotionId::__First; i < MotionId::__Count; ++i)
	{
		auto& traj = data[i];
		traj.posStart = m_recentReqPos[i];
		m_recentReqPos[i] = traj.params.pos;
	}

	/*
	 * DISCLAIMER: The following implementation is not optimized for performance. Use at your own discretion.
	 */
	double tMax = 0;
	for (auto& traj : data)
	{
		traj.sTotal = std::abs(traj.params.pos - traj.posStart);

		// Acceleration limit:
		// sTotal / 2 = acc*t*t/2 => t = sqrt(sTotal / acc)
		auto t1 = std::sqrt(traj.sTotal / traj.params.acc);

		// Velocity limit:
		// Vel = acc * t => t = Vel / acc;
		auto t2 = traj.params.vel / traj.params.acc;

		// Timeout limit:
		auto tAcc   = std::min({ TIMEOUT_S / 2, t1, t2 });
		auto tBrake = tAcc;

		auto sAcc   = traj.params.acc * tAcc * tAcc / 2;
		auto sBrake = sAcc;

		// V = s / t => t = s / V;
		auto sConstVel = std::max(0.0, traj.sTotal - sBrake - sAcc);
		auto tConstVel = sConstVel / traj.params.vel;

		tConstVel = std::min(tConstVel, TIMEOUT_S - tAcc - tBrake);

		traj.tAccFinish      = tAcc;
		traj.tConstVelFinish = tAcc + tConstVel;
		traj.tAllFinish      = tAcc + tConstVel + tBrake;

		tMax = std::max(tMax, traj.tAllFinish);
	}

	FSMI_TopTablePositionPhysical req {};
	req.structSize = sizeof(req);
	req.mask       = FSMI_POS_BIT_STATE | FSMI_POS_BIT_POSITION | FSMI_POS_BIT_MAX_SPEED;
	req.maxSpeed   = 65535;
	req.state      = FSMI_STATE_NO_PAUSE;

	Timer timer;
	while (timer.Elapsed_s() < tMax)
	{
		auto mark_s = timer.Elapsed_s();

		for (auto& traj : data)
		{
			if (mark_s < traj.tAccFinish)
			{
				traj.tmp = traj.params.acc * mark_s * mark_s / 2;
			}
			else if (mark_s < traj.tConstVelFinish)
			{
				traj.tmp = traj.params.acc * traj.tAccFinish * traj.tAccFinish / 2;
				traj.tmp += traj.params.vel * (mark_s - traj.tAccFinish);
			}
			else if (mark_s < traj.tAllFinish)
			{
				traj.tmp = traj.sTotal - traj.params.acc * (traj.tAllFinish - mark_s) * (traj.tAllFinish - mark_s) / 2;
			}

			traj.tmp = traj.posStart + traj.tmp * Sign(traj.params.pos - traj.posStart);
		}

		req.roll  = static_cast<float>(data[MotionId::Roll].tmp);
		req.pitch = static_cast<float>(data[MotionId::Pitch].tmp);
		req.yaw   = static_cast<float>(data[MotionId::Yaw].tmp);
		req.heave = static_cast<float>(data[MotionId::Heave].tmp);
		req.sway  = static_cast<float>(data[MotionId::Sway].tmp);
		req.surge = static_cast<float>(data[MotionId::Surge].tmp);

		if (! ForceSeatMI_SendTopTablePosPhy(m_api, &req))
		{
			throw std::runtime_error("Failed to call ForceSeatMI_SendTopTablePosPhy");
		}

		timer.Wait(FAST_RATE_INTERVAL_MS);
	}

	req.roll  = static_cast<float>(m_recentReqPos[MotionId::Roll]);
	req.pitch = static_cast<float>(m_recentReqPos[MotionId::Pitch]);
	req.yaw   = static_cast<float>(m_recentReqPos[MotionId::Yaw]);
	req.heave = static_cast<float>(m_recentReqPos[MotionId::Heave]);
	req.sway  = static_cast<float>(m_recentReqPos[MotionId::Sway]);
	req.surge = static_cast<float>(m_recentReqPos[MotionId::Surge]);

	if (! ForceSeatMI_SendTopTablePosPhy(m_api, &req))
	{
		throw std::runtime_error("Failed to call ForceSeatMI_SendTopTablePosPhy");
	}
}

void ForceSeatMI_GoToAPI::KeepAliveFor(unsigned int duration_ms)
{
	if (! m_api)
	{
		throw std::runtime_error("API not initialized");
	}

	FSMI_TopTablePositionPhysical req {};
	req.structSize = sizeof(req);
	req.mask       = FSMI_POS_BIT_STATE | FSMI_POS_BIT_POSITION | FSMI_POS_BIT_MAX_SPEED;
	req.maxSpeed   = 65535;
	req.state      = FSMI_STATE_NO_PAUSE;

	req.roll  = static_cast<float>(m_recentReqPos[MotionId::Roll]);
	req.pitch = static_cast<float>(m_recentReqPos[MotionId::Pitch]);
	req.yaw   = static_cast<float>(m_recentReqPos[MotionId::Yaw]);
	req.heave = static_cast<float>(m_recentReqPos[MotionId::Heave]);
	req.sway  = static_cast<float>(m_recentReqPos[MotionId::Sway]);
	req.surge = static_cast<float>(m_recentReqPos[MotionId::Surge]);

	Timer timer;
	do 
	{
		if (! ForceSeatMI_SendTopTablePosPhy(m_api, &req))
		{
			throw std::runtime_error("Failed to call ForceSeatMI_SendTopTablePosPhy");
		}
		timer.Wait(SLOW_RATE_INTERVAL_MS);
	} while (timer.Elapsed_ms() < duration_ms);
}

ForceSeatMI_GoToAPI::Position ForceSeatMI_GoToAPI::GetCurrentPosition() const
{
	Position pos {};
	pos.valid = false;

	FSMI_PlatformInfo info {};

	Read(info);

	pos.roll  = info.fkRoll;
	pos.pitch = info.fkPitch;
	pos.yaw   = info.fkYaw;
	pos.heave = info.fkHeave;
	pos.sway  = info.fkSway;
	pos.surge = info.fkSurge;
	pos.valid = info.fkRecentStatus == FSMI_True;

	return pos;
}

void ForceSeatMI_GoToAPI::Read(FSMI_PlatformInfo& info) const
{
	if (! m_api)
	{
		throw std::runtime_error("API not initialized");
	}

	if (! ForceSeatMI_GetPlatformInfoEx(m_api, &info, sizeof(info), 100))
	{
		throw std::runtime_error("Failed to call ForceSeatMI_GetPlatformInfoEx");
	}

	if (info.structSize != sizeof(info))
	{
		throw std::runtime_error("FSMI_PlatformInfo size is incorrect");
	}
}
