/*
Plugin to show how to use the "sim/flightmodel/position/q" dataref
It also has functions to convert from Quats to HPR and from HPR to Quats
  
Version 1.0.0.1			Intitial Sandy Barbour - 14/11/2005
*/

#include <stdio.h>
#include <math.h>

#include "XPLMPlugin.h"
#include "XPLMDisplay.h"
#include "XPLMGraphics.h"
#include "XPLMProcessing.h"
#include "XPLMUtilities.h"
#include "XPLMDataAccess.h"

const float Pi = 3.14159265358979;
const float RadToDeg = 180.0 / Pi;
const float DegToRad = 1.0 / RadToDeg;

typedef struct _QUATERNION
{
	float w;
	float x;
	float y;
	float z;
} QUATERNION;

typedef struct _HPR
{
	float Heading;
	float Pitch;
	float Roll;
} HPR;

char Buffer[14][80];

XPLMWindowID gWindow = NULL;
XPLMDataRef gqDataRef = NULL;
XPLMDataRef gHeadingDataRef = NULL;
XPLMDataRef gRollDataRef = NULL;
XPLMDataRef gPitchDataRef = NULL;

float TestQuaternionsLoopCB(float elapsedMe, float elapsedSim, int counter, void * refcon);

void TestQuaternionsDrawWindowCallback(
                                   XPLMWindowID         inWindowID,    
                                   void *               inRefcon);    

void TestQuaternionsHandleKeyCallback(
                                   XPLMWindowID         inWindowID,    
                                   char                 inKey,    
                                   XPLMKeyFlags         inFlags,    
                                   char                 inVirtualKey,    
                                   void *               inRefcon,    
                                   int                  losingFocus);    

int TestQuaternionsHandleMouseClickCallback(
                                   XPLMWindowID         inWindowID,    
                                   int                  x,    
                                   int                  y,    
                                   XPLMMouseStatus      inMouse,    
                                   void *               inRefcon);    

void QuaternionToHPR(QUATERNION quaternion, HPR *phpr);
void HPRToQuaternion(HPR hpr, QUATERNION *pquaternion);

//---------------------------------------------------------------------------

PLUGIN_API int XPluginStart(
						char *		outName,
						char *		outSig,
						char *		outDesc)
{
	strcpy(outName, "TestQuaternions");
	strcpy(outSig, "SandyBarbour.Example.TestQuaternions");
	strcpy(outDesc, "A plug-in that tests Quaternions.");

	gWindow = XPLMCreateWindow(
		                      50, 600, 200, 430,                  /* Area of the window. */
                              1,                                          /* Start visible. */
                              TestQuaternionsDrawWindowCallback,            /* Callbacks */
                              TestQuaternionsHandleKeyCallback,
                              TestQuaternionsHandleMouseClickCallback,
                              NULL);                                    /* Refcon - not used. */

	XPLMRegisterFlightLoopCallback(TestQuaternionsLoopCB, 1.0, NULL);
	
	gqDataRef = XPLMFindDataRef("sim/flightmodel/position/q");
	gHeadingDataRef = XPLMFindDataRef("sim/flightmodel/position/psi");
	gPitchDataRef = XPLMFindDataRef("sim/flightmodel/position/theta");
	gRollDataRef = XPLMFindDataRef("sim/flightmodel/position/phi");

	memset(Buffer, 0, sizeof(Buffer));

	return 1;
}

//---------------------------------------------------------------------------

PLUGIN_API void	XPluginStop(void)
{
    XPLMDestroyWindow(gWindow);
	XPLMUnregisterFlightLoopCallback(TestQuaternionsLoopCB, NULL);
}

//---------------------------------------------------------------------------

PLUGIN_API int XPluginEnable(void)
{
	return 1;
}

//---------------------------------------------------------------------------

PLUGIN_API void XPluginDisable(void)
{
}

//---------------------------------------------------------------------------

PLUGIN_API void XPluginReceiveMessage(XPLMPluginID inFrom, long inMsg, void * inParam)
{
}

//---------------------------------------------------------------------------

void TestQuaternionsDrawWindowCallback(
                                   XPLMWindowID         inWindowID,    
                                   void *               inRefcon)
{

	float		rgb [] = { 1.0, 1.0, 1.0 };
	int			l, t, r, b;
	char 		buf[512];

	XPLMGetWindowGeometry(inWindowID, &l, &t, &r, &b);
	XPLMDrawTranslucentDarkBox(l, t, r, b);

	for (int i=0; i<14; i++)
		XPLMDrawString(rgb, l+10, (t-20) - (10*i), Buffer[i], NULL, xplmFont_Basic);
}                                   

//---------------------------------------------------------------------------

void TestQuaternionsHandleKeyCallback(
                                   XPLMWindowID         inWindowID,    
                                   char                 inKey,    
                                   XPLMKeyFlags         inFlags,    
                                   char                 inVirtualKey,    
                                   void *               inRefcon,    
                                   int                  losingFocus)
{
}                                   

//---------------------------------------------------------------------------

int TestQuaternionsHandleMouseClickCallback(
                                   XPLMWindowID         inWindowID,    
                                   int                  x,    
                                   int                  y,    
                                   XPLMMouseStatus      inMouse,    
                                   void *               inRefcon)
{
	return 1;
}                                      

//---------------------------------------------------------------------------

float TestQuaternionsLoopCB(float elapsedMe, float elapsedSim, int counter, void * refcon)
{
	QUATERNION q1, q2;
	HPR hpr1, hpr2;
	float FloatVals[4];

	int count = XPLMGetDatavf(gqDataRef, FloatVals, 0, 4);
	q1.w = FloatVals[0];
	q1.x = FloatVals[1];
	q1.y = FloatVals[2];
	q1.z = FloatVals[3];
	QuaternionToHPR(q1, &hpr1);

	int Hdg = (int) hpr1.Heading;
	Hdg %= 360;
	hpr1.Heading = (float) Hdg;
	if (hpr1.Heading < 0)
		hpr1.Heading += 360;

	hpr2.Heading = XPLMGetDataf(gHeadingDataRef);
	if (hpr2.Heading > 180)
		hpr2.Heading = hpr2.Heading - 360;

	hpr2.Roll = XPLMGetDataf(gRollDataRef);
	hpr2.Pitch = XPLMGetDataf(gPitchDataRef);
	HPRToQuaternion(hpr2, &q2);

	FloatVals[0] = q2.w;
	FloatVals[1] = q2.x;
	FloatVals[2] = q2.y;
	FloatVals[3] = q2.z;
	XPLMSetDatavf(gqDataRef, FloatVals, 0, 4);

	sprintf(Buffer[0], "w1 = %f", q1.w);
	sprintf(Buffer[1], "x1 = %f", q1.x);
	sprintf(Buffer[2], "y1 = %f", q1.y);
	sprintf(Buffer[3], "z1 = %f", q1.z);
	sprintf(Buffer[4], "Heading1 = %f", hpr1.Heading);
	sprintf(Buffer[5], "Roll1 = %f", hpr1.Roll);
	sprintf(Buffer[6], "Pitch1 = %f", hpr1.Pitch);
	sprintf(Buffer[7], "w2 = %f", q2.w);
	sprintf(Buffer[8], "x2 = %f", q2.x);
	sprintf(Buffer[9], "y2 = %f", q2.y);
	sprintf(Buffer[10], "z2 = %f", q2.z);
	sprintf(Buffer[11], "Heading2 = %f", hpr2.Heading);
	sprintf(Buffer[12], "Roll2 = %f", hpr2.Roll);
	sprintf(Buffer[13], "Pitch2 = %f", hpr2.Pitch);

	return 0.1;
}

//---------------------------------------------------------------------------

void QuaternionToHPR(QUATERNION quaternion, HPR *phpr)
{
	double local_w = quaternion.w;
	double local_x = quaternion.x;
	double local_y = quaternion.y;
	double local_z = quaternion.z;
    	
	double sq_w = local_w * local_w;
    double sq_x = local_x * local_x;
    double sq_y = local_y * local_y;
    double sq_z = local_z * local_z;

    phpr->Heading = atan2(2.0 * (local_x * local_y + local_z * local_w),(sq_x - sq_y - sq_z + sq_w)) * RadToDeg;
    phpr->Pitch = asin(-2.0 * (local_x * local_z - local_y * local_w)) * RadToDeg;
    phpr->Roll = atan2(2.0 * (local_y * local_z + local_x * local_w),(-sq_x - sq_y + sq_z + sq_w)) * RadToDeg;
}

//---------------------------------------------------------------------------

void HPRToQuaternion(HPR hpr, QUATERNION *pquaternion)
{
	double local_Heading = hpr.Heading * DegToRad;
	double local_Pitch = hpr.Pitch * DegToRad;
	double local_Roll = hpr.Roll * DegToRad;

	double Cosine1 = cos(local_Roll / 2);
	double Cosine2 = cos(local_Pitch / 2);
	double Cosine3 = cos(local_Heading / 2); 
	double Sine1 = sin(local_Roll / 2);
	double Sine2 = sin(local_Pitch / 2);
	double Sine3 = sin(local_Heading / 2);

	pquaternion->w = Cosine1 * Cosine2 * Cosine3 + Sine1 * Sine2 * Sine3;
	pquaternion->x = Sine1 * Cosine2 * Cosine3 - Cosine1 * Sine2 * Sine3;
	pquaternion->y = Cosine1 * Sine2 * Cosine3 + Sine1 * Cosine2 * Sine3;
	pquaternion->z = Cosine1 * Cosine2 * Sine3 - Sine1 * Sine2 * Cosine3;
}

//---------------------------------------------------------------------------