/*
 * 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 <iostream>
#include <stdexcept>

#include "ForceSeatMI_GoToAPI.h"

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

namespace
{
	inline constexpr double Deg2Rad(double deg)
	{
		return deg * (M_PI / 180.0);
	}

	inline constexpr double Rad2Deg(double deg)
	{
		return deg * (180.0 / M_PI);
	}
}

int main(int argc, wchar_t* argv[])
{
	// This is needed to increase accuracy of Sleep. You can find more about it in MSDN.
	timeBeginPeriod(1);
	
	std::cout << "Initializing..." << std::endl;

	ForceSeatMI_GoToAPI api("SDK - Positioning");
	try
	{
		std::cout << "Parking..." << std::endl;
		api.ParkSync();

		// Make sure that the motion platform is fully waken-up
		std::cout << "Going to center..." << std::endl;
		api.GoToSync(0, Deg2Rad(100), Deg2Rad(100),
					 0, Deg2Rad(100), Deg2Rad(100),
					 0, Deg2Rad(100), Deg2Rad(100),
					 0, 150,          100,
					 0, 150,          100,
					 0, 150,          100);
		api.KeepAliveFor(5000);

		std::cout << "Going to 5 deg..." << std::endl;
		api.GoToSync(Deg2Rad(5), Deg2Rad(100), Deg2Rad(100),
					 0,          Deg2Rad(100), Deg2Rad(100),
					 0,          Deg2Rad(100), Deg2Rad(100),
					 0,          150,          100,
					 0,          150,          100,
					 0,          150,          100);
		
		api.KeepAliveFor(1000);
		std::cout << "Current roll is: " << Rad2Deg(api.GetCurrentPosition().roll) << std::endl;
		api.KeepAliveFor(2000);

		std::cout << "Going to 7 deg..." << std::endl;
		api.GoToSync(Deg2Rad(7), Deg2Rad(30), Deg2Rad(10),
					 0,          Deg2Rad(100), Deg2Rad(200),
					 0,          Deg2Rad(100), Deg2Rad(200),
					 0,          150,          300,
					 0,          150,          300,
					 0,          150,          300);

		api.KeepAliveFor(1000);
		std::cout << "Current roll is: " << Rad2Deg(api.GetCurrentPosition().roll) << std::endl;
		api.KeepAliveFor(2000);

		std::cout << "Going to -5 deg..." << std::endl;
		api.GoToSync(Deg2Rad(-5), Deg2Rad(30), Deg2Rad(10),
					 0,           Deg2Rad(100), Deg2Rad(200),
					 0,           Deg2Rad(100), Deg2Rad(200),
					 0,           150,          300,
					 0,           150,          300,
					 0,           150,          300);

		api.KeepAliveFor(1000);
		std::cout << "Current roll is: " << Rad2Deg(api.GetCurrentPosition().roll) << std::endl;
		api.KeepAliveFor(2000);

		std::cout << "Going to 0 deg..." << std::endl;
		api.GoToSync(Deg2Rad(0),  Deg2Rad(30), Deg2Rad(10),
					 0,           Deg2Rad(100), Deg2Rad(200),
					 0,           Deg2Rad(100), Deg2Rad(200),
					 0,           150,          300,
					 0,           150,          300,
					 0,           150,          300);

		api.KeepAliveFor(1000);
		std::cout << "Current roll is: " << Rad2Deg(api.GetCurrentPosition().roll) << std::endl;
		api.KeepAliveFor(2000);

		std::cout << "Parking..." << std::endl;
		api.ParkSync();

		std::cout << "FINISHED" << std::endl;
	}
	catch (const std::runtime_error& e)
	{
		printf("%s\n", e.what());
	}

	timeEndPeriod(1);
	return 0;
}
