/*---------------------------------------------------------------------------*
  Project:     WPAD Library
  File:        WPADClamp.c

  Copyright (C) 2006 Nintendo.  All rights reserved.

  These coded instructions, statements, and computer programs contain
  proprietary information of Nintendo of America Inc. and/or Nintendo
  Company Ltd., and are protected by Federal copyright law.  They may
  not be disclosed to third parties or copied or duplicated in any form,
  in whole or in part, without the prior written consent of Nintendo.

  $Log: WPADClamp.c,v $
  Revision 1.4  2008/03/25 02:31:07  tojo
  Fixed the clamp parameter.

  Revision 1.3  2006/08/11 09:12:35  tojo
  (none)

  Revision 1.2  2006/08/03 11:58:40  tojo
  Changed clamp apis.
  Fixed acc clamp bugs.

  Revision 1.1  2006/08/01 05:59:41  tojo
  (none)


 *---------------------------------------------------------------------------*/

#include <revolution/os.h>
#include <revolution/wpad.h>

#include <math.h>
#include <string.h>

/*---------------------------------------------------------------------------*
   Static function definitions
 *---------------------------------------------------------------------------*/

// clamp
static void __ClampStickOctagon   ( void *px, void *py, s16 max, s16 xy, s16 min, u8 type );
static void __ClampStickCircle    ( void *px, void *py, s16 radius, s16 min, u8 type );
static void __ClampTrigger        ( u8 *trigger, u8 min, u8 max );
static void __ClampCube           ( s16 *px, s16 *py, s16 *pz, WPADAcc unit, f32 max );
static void __ClampSphere         ( s16 *px, s16 *py, s16 *pz, WPADAcc unit, f32 max );


// Parameters for controller
typedef struct WPADClampStickRegion
{
    s16  minStick;       // the play of the control stick
    s16  maxStick;
    s16  xyStick;        // max on x = y
    s16  radStick;       // max radius of the control stick
} WPADClampStickRegion;

// Freestyle 
static const WPADClampStickRegion FSRegion =
{
    15,     // minStick
    67,     // maxStick
    43,     // xyStick
    56,     // radius
};
// Freestyle Ex
static const WPADClampStickRegion FSRegion2 = 
{
     0,
    82,
    58,
    75,
};

// Classic 
static const WPADClampStickRegion CLRegion =
{
    60,
   288,
   188,
   248,
};
// Classic Ex
static const WPADClampStickRegion CLRegion2 = 
{
     0,
   348,
   248,
   322,
};


/*---------------------------------------------------------------------------*
    Clamp
 *---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*
  Name:         WPADClampStick

  Description:  Clamp the analog inputs according to device type.

  Arguments:    fmt             data format.
                status          point to the data to be clamped. 
                type            clamp type.
    
  Returns:      None.
 *---------------------------------------------------------------------------*/
void WPADClampStick( s32 chan, void *status, u32 type )
{
    WPADFSStatus *fp;
    WPADCLStatus *cp;
    const WPADClampStickRegion *stkreg;
    u32 fmt;
    
    ASSERT( status != NULL );

    if (((WPADStatus*)status)->err != WPAD_ERR_NONE)
    {
        return;
    }
    
    fmt = WPADGetDataFormat(chan);
    switch(fmt)
    {
        case WPAD_FMT_FREESTYLE:
        case WPAD_FMT_FREESTYLE_ACC:
        case WPAD_FMT_FREESTYLE_ACC_DPD:
            fp = (WPADFSStatus*)status;
            if (type == WPAD_STICK_CLAMP_OCTA_WITH_PLAY
            ||  type == WPAD_STICK_CLAMP_CIRCLE_WITH_PLAY)
            {
                stkreg = &FSRegion;
            }
            else
            {
                stkreg = &FSRegion2;
            }

            if (type == WPAD_STICK_CLAMP_OCTA_WITH_PLAY
            ||  type == WPAD_STICK_CLAMP_OCTA_WITHOUT_PLAY)
            {
                __ClampStickOctagon(&fp->fsStickX,
                                    &fp->fsStickY,
                                    stkreg->maxStick,
                                    stkreg->xyStick,
                                    stkreg->minStick,
                                    0);
            }
            else
            {
                __ClampStickCircle(&fp->fsStickX,
                                   &fp->fsStickY,
                                   stkreg->radStick,
                                   stkreg->minStick,
                                   0);
            }
            break;
            
        case WPAD_FMT_CLASSIC:
        case WPAD_FMT_CLASSIC_ACC:
        case WPAD_FMT_CLASSIC_ACC_DPD:
            cp = (WPADCLStatus*)status;
            if (type == WPAD_STICK_CLAMP_OCTA_WITH_PLAY
            ||  type == WPAD_STICK_CLAMP_CIRCLE_WITH_PLAY)
            {
                stkreg = &CLRegion;
            }
            else
            {
                stkreg = &CLRegion2;
            }

            if (type == WPAD_STICK_CLAMP_OCTA_WITH_PLAY
            ||  type == WPAD_STICK_CLAMP_OCTA_WITHOUT_PLAY)
            {
                __ClampStickOctagon(&cp->clLStickX,
                                    &cp->clLStickY,
                                    stkreg->maxStick,
                                    stkreg->xyStick,
                                    stkreg->minStick,
                                    1);
                __ClampStickOctagon(&cp->clRStickX,
                                    &cp->clRStickY,
                                    stkreg->maxStick,
                                    stkreg->xyStick,
                                    stkreg->minStick,
                                    1);
            }
            else
            {
                __ClampStickCircle(&cp->clLStickX,
                                   &cp->clLStickY,
                                   stkreg->radStick,
                                   stkreg->minStick,
                                   1);
                __ClampStickCircle(&cp->clRStickX,
                                   &cp->clRStickY,
                                   stkreg->radStick,
                                   stkreg->minStick,
                                   1);
                
            }
            break;
    
        default:
            break;
    }
}

/*---------------------------------------------------------------------------*
  Name:         WPADClampTrigger

  Description:  Clamp the trigger inputs according to device type.

  Arguments:    chan            controller port
                fmt             data format.
                status          point to the data to be clamped. 
                type            clamp type.
    
  Returns:      None.
 *---------------------------------------------------------------------------*/
void WPADClampTrigger( s32 chan, void *status, u32 type )
{
    WPADCLStatus *cp;
    u8 min_l, min_r;
    u8 max_l, max_r;
    u32 fmt;

    ASSERT(status != NULL);
    
    if (((WPADStatus*)status)->err != WPAD_ERR_NONE)
    {
        return;
    }
    
    fmt = WPADGetDataFormat(chan);
    switch(fmt)
    {
        case WPAD_FMT_CLASSIC:
        case WPAD_FMT_CLASSIC_ACC:
        case WPAD_FMT_CLASSIC_ACC_DPD:
            cp = (WPADCLStatus*)status;
            if (type == WPAD_TRIGGER_FIXED_BASE)
            {
                min_l = min_r = 30;
                max_l = max_r = 180;
            }
            else
            {
                WPADGetCLTriggerThreshold(chan, &min_l, &min_r);
                max_l = (u8)((min_l < 76) ? min_l+180 : 255);
                max_r = (u8)((min_r < 76) ? min_r+180 : 255);
            }
            __ClampTrigger(&cp->clTriggerL,min_l,max_l);
            __ClampTrigger(&cp->clTriggerR,min_r,max_r);
            break;
            
        default:
            break;
    }
}
 

/*---------------------------------------------------------------------------*
  Name:         WPADClampAcc

  Description:  Clamp the accelerometer inputs according to device type

  Arguments:    fmt             data format.
                status          point to the data to be clamped. 
                unit            structure of gravity unit for each axes.
                type            clamp type.
                
  Returns:      None.
 *---------------------------------------------------------------------------*/
void WPADClampAcc( s32 chan, void *status, u32 type )
{
    WPADStatus   *wp;
    WPADFSStatus *fp;
    WPADAcc      wunit;
    WPADAcc      funit;
    u32          fmt;

    ASSERT(status != NULL);
    
    if (((WPADStatus*)status)->err != WPAD_ERR_NONE)
    {
        return;
    }
    
    fmt = WPADGetDataFormat(chan);
    switch(fmt)
    {
        case WPAD_FMT_CORE_ACC:
        case WPAD_FMT_CORE_ACC_DPD:
            wp = (WPADStatus*)status;
            WPADGetAccGravityUnit(chan, WPAD_DEV_CORE, &wunit);
            if (type == WPAD_ACC_CLAMP_CUBE)
            {
                __ClampCube(&wp->accX, &wp->accY, &wp->accZ, wunit, WPAD_ACC_MAX);
            }
            else
            {
                __ClampSphere(&wp->accX, &wp->accY, &wp->accZ, wunit, WPAD_ACC_MAX);
            }
            break;
            
        case WPAD_FMT_FREESTYLE_ACC:
        case WPAD_FMT_FREESTYLE_ACC_DPD:
            fp = (WPADFSStatus*)status;
            WPADGetAccGravityUnit(chan, WPAD_DEV_CORE,      &wunit);
            WPADGetAccGravityUnit(chan, WPAD_DEV_FREESTYLE, &funit);
            if (type == WPAD_ACC_CLAMP_CUBE)
            {
                __ClampCube(&fp->accX,   &fp->accY,   &fp->accZ,   wunit, WPAD_ACC_MAX);
                __ClampCube(&fp->fsAccX, &fp->fsAccY, &fp->fsAccZ, funit, WPAD_FS_ACC_MAX);
            }
            else
            {
                __ClampSphere(&fp->accX,   &fp->accY,   &fp->accZ,   wunit, WPAD_ACC_MAX);
                __ClampSphere(&fp->fsAccX, &fp->fsAccY, &fp->fsAccZ, funit, WPAD_FS_ACC_MAX);
            }
            break;
            
        default:
            break;
    }
}


/*---------------------------------------------------------------------------*
  Name:         __ClampStickOctagon

  Description:  Adjusts stick movement data within the octagon, or clamps
                the data to the origin if stick is close to the origin as
                the play.

  Arguments:    px          pointer to movement data in terms of x-axis
                py          pointer to movement data in terms of y-axis
                max         max on y = 0
                xy          max on x = y
                min         deadzone

  Returns:      None.
 *---------------------------------------------------------------------------*/
static void __ClampStickOctagon(void* px, void* py, s16 max, s16 xy, s16 min, u8 type)
{
    int x;
    int y;
    int signX;
    int signY;
    int d;

    if (type == 0)
    {
        x = *((s8*)px);
        y = *((s8*)py);
    }
    else
    {
        x = *((s16*)px);
        y = *((s16*)py);
    }

    if (0 <= x)
    {
        signX = 1;
    }
    else
    {
        signX = -1;
        x = -x;
    }

    if (0 <= y)
    {
        signY = 1;
    }
    else
    {
        signY = -1;
        y = -y;
    }

    // Clamp dead zone
    if (x <= min)
    {
        x = 0;
    }
    else
    {
        x -= min;
    }
    if (y <= min)
    {
        y = 0;
    }
    else
    {
        y -= min;
    }

    if (x == 0 && y == 0)
    {
        if (type == 0)
        {
            *((s8*)px) = 0;
            *((s8*)py) = 0;
        }
        else
        {
            *((s16*)px) = 0;
            *((s16*)py) = 0;
        }
        return;
        // NOT REACHED HERE
    }

    // Clamp outer octagon
    if (xy * y <= xy * x)
    {
        d = xy * x + (max - xy) * y;
        if (xy * max < d)
        {
            if (type == 0)
            {
                x = (s8) (xy * max * x / d);
                y = (s8) (xy * max * y / d);
            }
            else
            {
                x = (s16) (xy * max * x / d);
                y = (s16) (xy * max * y / d);
            }
        }
    }
    else
    {
        d = xy * y + (max - xy) * x;
        if (xy * max < d)
        {
            if (type == 0)
            {
                x = (s8) (xy * max * x / d);
                y = (s8) (xy * max * y / d);
            }
            else
            {
                x = (s16) (xy * max * x / d);
                y = (s16) (xy * max * y / d);
            }
        }
    }

    if (type == 0)
    {
        *(s8*)px = (s8) (signX * x);
        *(s8*)py = (s8) (signY * y);
    }
    else
    {
        *(s16*)px = (s16) (signX * x);
        *(s16*)py = (s16) (signY * y);
    }
}

/*---------------------------------------------------------------------------*
  Name:         __ClampStickCircle

  Description:  Adjusts stick movement data.

  Arguments:    px          pointer to movement data in terms of x-axis
                py          pointer to movement data in terms of y-axis
                radius      max valid radius
                min         deadzone

  Returns:      None.
 *---------------------------------------------------------------------------*/
static void __ClampStickCircle(void *px, void *py, s16 radius, s16 min, u8 type)
{
    int x;
    int y;
    int squared;
    int length;

    if (type == 0)
    {
        x = *((s8*)px);
        y = *((s8*)py);
    }
    else
    {
        x = *((s16*)px);
        y = *((s16*)py);
    }

    // Remove vertical zone
    if (-min < x && x < min)
    {
        x = 0;
    }
    else if (0 < x)
    {
        x -= min;
    }
    else
    {
        x += min;
    }

    // Remove horizontal zone
    if (-min < y && y < min)
    {
        y = 0;
    }
    else if (0 < y)
    {
        y -= min;
    }
    else
    {
        y += min;
    }

    // Clamp input to unit circle of radius
    squared = x*x + y*y;
    if (radius*radius < squared)
    {
        // Vector too long - clamp
        length = (int) sqrtf(squared);
        x = (x * radius) / length;
        y = (y * radius) / length;
    }

    if (type == 0)
    {
        *(s8*)px = (s8)x;
        *(s8*)py = (s8)y;
    }
    else
    {
        *(s16*)px = (s16)x;
        *(s16*)py = (s16)y;
    }
}

/*---------------------------------------------------------------------------*
  Name:         __ClampTrigger

  Description:  Adjusts trigger movement data

  Arguments:    trigger     trigger magnitude
                min         minimum valid value for trigger
                max         maximum valid value for trigger

  Returns:      None.
 *---------------------------------------------------------------------------*/
static void __ClampTrigger(u8* trigger, u8 min, u8 max)
{
    if (*trigger <= min)
    {
        *trigger = 0;
    }
    else
    {
        if (max < *trigger)
        {
            *trigger = max;
        }
        *trigger -= min;
    }
}


/*---------------------------------------------------------------------------*
  Name:         __ClampCube

  Description:  Adjusts accelerometer movement data

  Arguments:    px          pointer to movement data in terms of x-axis
                py          pointer to movement data in terms of y-axis
                py          pointer to movement data in terms of z-axis
                unit        gravity unit for each axes
                max         max valid gravity

  Returns:      None.
 *---------------------------------------------------------------------------*/
static void __ClampCube(s16 *px, s16 *py, s16 *pz, WPADAcc unit, f32 max)
{
    f32 x   = (f32)((f32)*px / (f32)unit.x);
    f32 y   = (f32)((f32)*py / (f32)unit.y);
    f32 z   = (f32)((f32)*pz / (f32)unit.z);
    f32 signX = 1.0f;
    f32 signY = 1.0f;
    f32 signZ = 1.0f;
   
    if (x < 0.0f) 
    {
        signX = -1.0f;
        x = -x;
    }
    if (y < 0.0f) 
    {
        signY = -1.0f;
        y = -y;
    }
    if (z < 0.0f) 
    {
        signZ = -1.0f;
        z = -z;
    }
    
    if (x > max)
    {
        x = max;
    }
    if (y > max)
    {
        y = max;
    }
    if (z > max)
    {
        z = max;
    }

    x *= signX;
    y *= signY;
    z *= signZ;

    *px = (s16)(x * (f32)unit.x);
    *py = (s16)(y * (f32)unit.y);
    *pz = (s16)(z * (f32)unit.z);
}

/*---------------------------------------------------------------------------*
  Name:         __ClampSphere

  Description:  Adjusts accelerometer movement data

  Arguments:    px          pointer to movement data in terms of x-axis
                py          pointer to movement data in terms of y-axis
                py          pointer to movement data in terms of z-axis
                unit        gravity unit for each axes
                max         max valid gravity

  Returns:      None.
 *---------------------------------------------------------------------------*/
static void __ClampSphere(s16 *px, s16 *py, s16 *pz, WPADAcc unit, f32 max)
{
    f32 x   = (f32)((f32)*px / (f32)unit.x);
    f32 y   = (f32)((f32)*py / (f32)unit.y);
    f32 z   = (f32)((f32)*pz / (f32)unit.z);
    f32 length;
    f32 squared;

    squared = x*x + y*y + z*z;
    if (max * max < squared)
    {
        length = sqrtf(squared);
        x = x * max / length;
        y = y * max / length;
        z = z * max / length;
    }

    *px = (s16)(x * (f32)unit.x);
    *py = (s16)(y * (f32)unit.y);
    *pz = (s16)(z * (f32)unit.z);
}