/***************************************************************************\
 JoyInput.cpp
 Scott Randolph
 February 18, 1998

 Provide interaction through a joystick.
\***************************************************************************/
#include <stdio.h>
#include <math.h>
#include "JoyInput.h"


#define SIM_SPEED 1000.0f // Feet per second
#define SIM_ANG_RATE 2.0f // Radians per second
#define HEAD_ANG_RATE 1.0f // Radians per second
#define IIR_RATE 0.5f // Percent new value added in (1.0 = no filter)
#define PI 3.14159265359f


JoyInputClass TheJoystick;



/***************************************************************************\
 Detect the joystick and initialize our internal structures.
\***************************************************************************/
void JoyInputClass::Setup(void)
{
    ShiAssert(!IsReady());

    // Initialize our outputs to default values
    throttleIIR = 1.0f;
    throttle = 0.003f;
    pitchRate = 0.0f;
    yawRate = 0.0f;
    rollRate = 0.0f;
    deltaMatrix = IMatrix;
    headPitchRate = 0.0f;
    headYawRate = 0.0f;
    headDeltaMatrix = IMatrix;
    buttons = 0;

    // Ask for the joystick properties
    DWORD ret = joyGetDevCaps(JOYSTICKID1, &joyCaps, sizeof(joyCaps));

    if (ret != JOYERR_NOERROR)
    {
        haveJoystick = FALSE;
    }
    else
    {
        haveJoystick = TRUE;
    }

    // Initialize the joystick information structure for use in polling
    memset(&joyInfoEx, 0, sizeof(joyInfoEx));
    joyInfoEx.dwSize = sizeof(joyInfoEx);
    joyInfoEx.dwFlags = JOY_RETURNALL;

    lastTime = 0;

    ready = TRUE;
}



/***************************************************************************\
 Clean up when the simulation loop is no longer needed.
\***************************************************************************/
void JoyInputClass::Cleanup(void)
{
    ShiAssert(IsReady());

    ready = FALSE;
    return;
}



/***************************************************************************\
 Do the computations for one time step in the simulation.
\***************************************************************************/
void JoyInputClass::Update(DWORD time)
{
    float deltaTime = 0.0f;


    ShiAssert(IsReady());

    // Determine how long its been since the last sim update
    if (time > lastTime)
    {
        deltaTime = (float)(time - lastTime) / 1000.0f;
    }

    lastTime = time;


    // Quit now if we don't have a joystick
    if (!haveJoystick)
    {
        return;
    }


    // Update our joystick information
    joyGetPosEx(JOYSTICKID1, &joyInfoEx);
    buttons = joyInfoEx.dwButtons;


    // Based on the joystick position, set the roll and pitch angles
    // Positive pitch is upward
    pitchRate = ((int)joyInfoEx.dwYpos - 32768) / 32768.0f * SIM_ANG_RATE * deltaTime;

    // Positive roll is "right" roll (ie: clockwise)
    rollRate  = ((int)joyInfoEx.dwXpos - 32768) / 32768.0f * SIM_ANG_RATE * deltaTime;

    // Positive yaw is "right"
    if (joyCaps.wCaps & JOYCAPS_HASR)
    {
        yawRate  = ((int)joyInfoEx.dwRpos - 32768) / 32768.0f * SIM_ANG_RATE * deltaTime;
    }

    // Positive Z is decrease in throttle
    if (joyCaps.wCaps & JOYCAPS_HASZ)
    {
#if 0
        throttle  = (65535 - (int)joyInfoEx.dwZpos) / 65535.0f;
#else
        throttleIIR *= 1.0f - IIR_RATE;
        throttleIIR += IIR_RATE * (65535 - (int)joyInfoEx.dwZpos) / 65535.0f;
        throttle = throttleIIR;
#endif
        throttle  = throttle * throttle * throttle; // Use a polynomial curve to get better low end response
    }

    // Update the viewing rotation
    ConstructDeltaMatrix(pitchRate, rollRate, yawRate, &deltaMatrix);


    // Update the view direction if the joystick provides POV information
    if (joyCaps.wCaps & JOYCAPS_HASPOV)
    {
        if (joyInfoEx.dwPOV == JOY_POVCENTERED)
        {
            headPitchRate = 0.0f;
            headYawRate = 0.0f;
        }
        else
        {
            // Extract the head motion from the POV data
            float controlAngle = joyInfoEx.dwPOV * 0.01f * PI / 180.0f;
            headPitchRate = HEAD_ANG_RATE * cos(controlAngle) * deltaTime;
            headYawRate = HEAD_ANG_RATE * sin(controlAngle) * deltaTime;
        }

        ConstructDeltaMatrix(headPitchRate, 0.0f, headYawRate, &headDeltaMatrix);
    }
}



/***************************************************************************\
 Update the rotation matrix to account for the user's control inputs.
 NOTE:  This is an approximation and will gimbal lock as well...
\***************************************************************************/
void JoyInputClass::ConstructDeltaMatrix(float p, float r, float y, Trotation *T)
{
    Tpoint at, up, rt;
    float mag;

    // TODO:  Add in roll component.

    // at is a point on the unit sphere
    at.x = cos(y) * cos(p);
    at.y = sin(y) * cos(p);
    at.z = -sin(p);

    // (0, 0, 1) rolled and crossed with at [normalized]
    up.x = 0.0f;
    up.y = -sin(r);
    up.z = cos(r);
    rt.x = up.y * at.z - up.z * at.y;
    rt.y = up.z * at.x - up.x * at.z;
    rt.z = up.x * at.y - up.y * at.x;
    mag = sqrt(rt.x * rt.x + rt.y * rt.y + rt.z * rt.z);
    rt.x /= mag;
    rt.y /= mag;
    rt.z /= mag;

    // at cross rt [normalized]
    up.x = at.y * rt.z - at.z * rt.y;
    up.y = at.z * rt.x - at.x * rt.z;
    up.z = at.x * rt.y - at.y * rt.x;
    mag = sqrt(up.x * up.x + up.y * up.y + up.z * up.z);
    up.x /= mag;
    up.y /= mag;
    up.z /= mag;

    // Load the matrix
    T->M11 = at.x;
    T->M12 = rt.x;
    T->M13 = up.x;
    T->M21 = at.y;
    T->M22 = rt.y;
    T->M23 = up.y;
    T->M31 = at.z;
    T->M32 = rt.z;
    T->M33 = up.z;
}
