#include "ForceSeatMI_Simulink.h"
#include "ForceSeatMI_Functions.h"

#include <stdio.h>
#include <stdlib.h>
#include <time.h>

void logToFile(const char *message)
{
	FILE *file = fopen("fsmi_simulink_log.txt", "a");
	if (NULL == file)
	{
		return;
	}

	time_t now = time(NULL);
	struct tm* t = localtime(&now);

	fprintf(file, "%04d-%02d-%02d_%02d:%02d:%02d : %s\n",
			t->tm_year + 1900, t->tm_mon + 1, t->tm_mday,
			t->tm_hour, t->tm_min, t->tm_sec, message);

	fclose(file);
}

FSMI_Handle                   g_handle = 0;
FSMI_TopTablePositionPhysical g_positionPhy;
FSMI_TopTablePositionLogical  g_positionLog;
FSMI_PlatformInfo             g_platformInfo;
FSMI_TelemetryACE             g_telemetryACE;
const char*                   g_profileName = 0;

const char* const POSITIONING_PROFILE_NAME = "SDK - Positioning";
const char* const ACE_PROFILE_NAME         = "SDK - Vehicle Telemetry ACE";

void activatePositioningProfile(FSMI_Handle api)
{
	if (g_profileName != POSITIONING_PROFILE_NAME)
	{
		ForceSeatMI_ActivateProfile(api, POSITIONING_PROFILE_NAME);
		g_profileName = POSITIONING_PROFILE_NAME;
	}
}

void activateACEProfile(FSMI_Handle api)
{
	if (g_profileName != ACE_PROFILE_NAME)
	{
		ForceSeatMI_ActivateProfile(api, ACE_PROFILE_NAME);
		g_profileName = ACE_PROFILE_NAME;
	}
}

void initialize()
{
	logToFile("Initialize has been called");

	g_handle = ForceSeatMI_Create();

	if (g_handle)
	{
		logToFile("Api has been created");

		memset(&g_positionPhy, 0, sizeof(g_positionPhy));
		g_positionPhy.structSize = sizeof(g_positionPhy);
		g_positionPhy.mask = FSMI_POS_BIT_STATE | FSMI_POS_BIT_POSITION | FSMI_POS_BIT_MAX_SPEED;

		memset(&g_positionLog, 0, sizeof(g_positionLog));
		g_positionLog.structSize = sizeof(g_positionLog);
		g_positionLog.mask = FSMI_POS_BIT_STATE | FSMI_POS_BIT_POSITION | FSMI_POS_BIT_MAX_SPEED;

		memset(&g_telemetryACE, 0, sizeof(g_telemetryACE));
		g_telemetryACE.structSize = sizeof(g_telemetryACE);
		g_telemetryACE.state = FSMI_STATE_PAUSE;

		memset(&g_platformInfo, 0, sizeof(g_platformInfo));

		ForceSeatMI_BeginMotionControl(g_handle);
	}
}

void terminate()
{
	logToFile("Terminate has been called");

	if(0 != g_handle)
	{
		ForceSeatMI_EndMotionControl(g_handle);
		ForceSeatMI_Delete(g_handle);
		g_handle = 0;
	}
}

void Refresh(int enable)
{
	if (0 < enable)
	{
		ForceSeatMI_GetPlatformInfoEx(g_handle, &g_platformInfo, sizeof(g_platformInfo), 100);
	}
	else
	{
		logToFile("Refresh cannot be called because enabled is 0!");
	}
}

void SetPosPhy(double yaw_rad, double pitch_rad, double roll_rad, double heave_mm, double sway_mm, double surge_mm, int enable)
{
	activatePositioningProfile(g_handle);

	if (0 >= enable)
	{
		g_positionPhy.state = FSMI_STATE_PAUSE;
	}
	else
	{
		g_positionPhy.state    = FSMI_STATE_NO_PAUSE;
		g_positionPhy.maxSpeed = 65535;
		g_positionPhy.yaw      = yaw_rad;
		g_positionPhy.pitch    = pitch_rad;
		g_positionPhy.roll     = roll_rad;
		g_positionPhy.heave    = heave_mm;
		g_positionPhy.sway     = sway_mm;
		g_positionPhy.surge    = surge_mm;
	}

	ForceSeatMI_SendTopTablePosPhy(g_handle, &g_positionPhy);
	ForceSeatMI_GetPlatformInfoEx(g_handle, &g_platformInfo, sizeof(g_platformInfo), 100);
}

void SetPosLog(double yaw, double pitch, double roll, double heave, double sway, double surge, int enable)
{
	activatePositioningProfile(g_handle);

	if (0 >= enable)
	{
		g_positionLog.state = FSMI_STATE_PAUSE;
	}
	else
	{
		g_positionLog.state    = FSMI_STATE_NO_PAUSE;
		g_positionLog.maxSpeed = 65535;
		g_positionLog.yaw      = yaw;
		g_positionLog.pitch    = pitch;
		g_positionLog.roll     = roll;
		g_positionLog.heave    = heave;
		g_positionLog.sway     = sway;
		g_positionLog.surge    = surge;
	}

	ForceSeatMI_SendTopTablePosLog(g_handle, &g_positionLog);
	ForceSeatMI_GetPlatformInfoEx(g_handle, &g_platformInfo, sizeof(g_platformInfo), 100);
}

void SendTelemetryACE(double yawVelocity, double pitchVelocity, double rollVelocity, double heaveAcc, double swayAcc, double surgeAcc,
					  double extraYaw_rad, double extraPitch_rad, double extraRoll_rad, double extraHeave_m, double extraSway_m, double extraSurge_m,
					  int rpm, int maxRpm, int enable)
{
	activateACEProfile(g_handle);

	if (0 >= enable)
	{
		g_telemetryACE.state = FSMI_STATE_PAUSE;
	}
	else
	{
		g_telemetryACE.state = FSMI_STATE_NO_PAUSE;

		g_telemetryACE.bodyAngularVelocity.yaw        = yawVelocity;
		g_telemetryACE.bodyAngularVelocity.pitch      = pitchVelocity;
		g_telemetryACE.bodyAngularVelocity.roll       = rollVelocity;
		g_telemetryACE.bodyLinearAcceleration.forward = surgeAcc;
		g_telemetryACE.bodyLinearAcceleration.upward  = heaveAcc;
		g_telemetryACE.bodyLinearAcceleration.right   = swayAcc;

		g_telemetryACE.extraRotation.yaw        = extraYaw_rad;
		g_telemetryACE.extraRotation.pitch      = extraPitch_rad;
		g_telemetryACE.extraRotation.roll       = extraRoll_rad;
		g_telemetryACE.extraTranslation.forward = extraSurge_m;
		g_telemetryACE.extraTranslation.upward  = extraHeave_m;
		g_telemetryACE.extraTranslation.right   = extraSway_m;

		g_telemetryACE.rpm    = rpm;
		g_telemetryACE.maxRpm = maxRpm;
	}

	ForceSeatMI_SendTelemetryACE(g_handle, &g_telemetryACE);
	ForceSeatMI_GetPlatformInfoEx(g_handle, &g_platformInfo, sizeof(g_platformInfo), 100);
}

double GetYaw_rad()
{
	return g_platformInfo.fkYaw;
}

double GetPitch_rad()
{
	return g_platformInfo.fkPitch;
}

double GetRoll_rad()
{
	return g_platformInfo.fkRoll;
}

double GetHeave_mm()
{
	return g_platformInfo.fkHeave;
}

double GetSway_mm()
{
	return g_platformInfo.fkSway;
}

double GetSurge_mm()
{
	return g_platformInfo.fkSurge;
}
