/*---------------------------------------------------------------------------*
  Project:    WPAD demo program
  File:       distance.c
  Programmer: HIRATSU Daisuke

  Copyright (C) 2005-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.
 *---------------------------------------------------------------------------*/

#include <stdlib.h>
#include <string.h>
#include <stddef.h>
#include <stdarg.h>
#include <stdio.h>
#include <ctype.h>
#include <math.h>

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

#define DEMO_USE_MEMLIB=1 // This turns on the DEMO library's MEM heaps.
#include <demo.h>

/*---------------------------------------------------------------------------*
 * Local definitions
 *---------------------------------------------------------------------------*/

#define SCREEN_WIDTH    320
#define SCREEN_HEIGHT   240
#define SEPARATION     0.20f   // Meter unit.  You must modify this value according to your environment.

/*---------------------------------------------------------------------------*
 * Function prototypes
 *---------------------------------------------------------------------------*/

// MEM2 memory allocation routines. The application must provide these to 
// WPAD, so it can setup the data transfer buffer. This buffer must reside
// in MEM2. 
static void *myAlloc        (u32 size);
static u8    myFree         (void *ptr);


static void initialize      (void);
static void renderStatus    (const WPADStatus *stat);
static void renderDistance  (const WPADStatus *stat);
static  f32 getDistance     (const WPADStatus *stat);

/*===========================================================================*
 *                   F U N C T I O N    D E F I N I T I O N S
 *===========================================================================*/

/*---------------------------------------------------------------------------*
 * Name        : main()
 * Description : 
 * Arguments   : None.
 * Returns     : None.
 *---------------------------------------------------------------------------*/
int main(void)
{

    WPADStatus wpad;

    s32 wpad_state;
    s32 status; 
    u32 type;

        initialize();

        // we should clear the WPADStatus block returned by 
        // WPADRead(), because if no channel is connected, nothing
        // is copied. So we would be staring at garbage data.
        memset( (void *)(&wpad), 0, sizeof(WPADStatus));

        // must register MEM2 allocation functions before invoking WPADInit()
        WPADRegisterAllocator(myAlloc, myFree);
        WPADInit();


        // The WPAD initialization process is asynchronous. 
        // So we should wait until it's completed.
        do 
        {

            wpad_state = WPADGetStatus();

        } while (WPAD_STATE_SETUP != wpad_state);


        // Main loop
        while(1)
        {

            status = WPADProbe(WPAD_CHAN0, &type);
            WPADRead(WPAD_CHAN0, &wpad);

            if (WPAD_ERR_NONE == status)
            {

                // Note: If the controller has disconnected and re-connected, 
                // we need to reset and re-enable the DPD and accelerometers.
                // But we have to ensure that the device type is correct, because
                // it's possible a different type of device has connected on channel 0.
                if ((WPAD_DEV_CORE == type) || (WPAD_DEV_FREESTYLE == type))
                {

                    // Are the DPD and Accelerometers enabled? 
                    if (FALSE == WPADIsDpdEnabled(WPAD_CHAN0))
                    {
                        // if not, then set the format and turn on DPD and accelerometer
                        WPADSetDataFormat(WPAD_CHAN0, WPAD_FMT_CORE_ACC_DPD); // should be called before WPADControlDPD()
                        WPADControlDpd(WPAD_CHAN0, WPAD_DPD_EXP, NULL);        
                    }
                    
                } // if device type is correct

            } // if no error...


            DEMOBeforeRender();
            renderStatus( &wpad );
            renderDistance( &wpad );
            DEMODoneRender();

        } // while 

        return 0;

} // end

/*---------------------------------------------------------------------------*
 * Name        : initialize
 * Description : 
 * Arguments   : None.
 * Returns     : None.
 *---------------------------------------------------------------------------*/
static void initialize( void )
{

    const GXColor DARKBLUE = { 0, 0, 40, 255 };

        OSInit();

        DEMOInit( &GXNtsc480IntDf );
        GXSetCopyClear( DARKBLUE, GX_MAX_Z24 );
        GXCopyDisp( DEMOGetCurrentBuffer(), GX_TRUE );
        DEMOInitCaption( DM_FT_XLU, SCREEN_WIDTH, SCREEN_HEIGHT );
        GXSetZMode( GX_ENABLE, GX_ALWAYS, GX_ENABLE );                       // Set pixel processing mode
        GXSetBlendMode( GX_BM_BLEND, GX_BL_ONE, GX_BL_ONE, GX_LO_CLEAR );    // Translucent mode

} // end


/*---------------------------------------------------------------------------*
 * Name        : renderStatus
 * Description : 
 * Arguments   : None.
 * Returns     : None.
 *---------------------------------------------------------------------------*/
static void renderStatus(const WPADStatus *stat)
{

    const int FONT_HEIGHT = 8;
    s16 x = FONT_HEIGHT;
    s16 y = FONT_HEIGHT;
    int i = 0;

        y+=FONT_HEIGHT;
        DEMOPrintf( x, y+=FONT_HEIGHT, 0, "DPD-ID     X    Y SIZE");

        for( i=0; i<WPAD_DPD_MAX_OBJECTS; ++i )
        {
            const DPDObject *p = &( stat->obj[i] );
            DEMOPrintf( x, y+=FONT_HEIGHT, 0, "%1d       %4d %4d  %3d", p->traceId, p->x, p->y, p->size );

        }

        y+=FONT_HEIGHT;
        y+=FONT_HEIGHT;
        DEMOPrintf( x, y+=FONT_HEIGHT, 0, "Controller Type: %d", stat->dev );
        DEMOPrintf( x, y+=FONT_HEIGHT, 0, "ERROR Info     : %d", stat->err );

        if (WPAD_ERR_NO_CONTROLLER == stat->err)
        {
            y+=FONT_HEIGHT;
            y+=FONT_HEIGHT;
            y+=FONT_HEIGHT;
            y+=FONT_HEIGHT;
            y+=FONT_HEIGHT;
            y+=FONT_HEIGHT;
            DEMOPrintf(x, y+=FONT_HEIGHT, 0, ">> No controller is connected!");
            DEMOPrintf(x, y+=FONT_HEIGHT, 0, ">> Please press a button on a paired");
            DEMOPrintf(x, y+=FONT_HEIGHT, 0, ">> controller to connect.");
        }

} // end

/*---------------------------------------------------------------------------*
 * Name        : renderDistance
 * Description : 
 * Arguments   : None.
 * Returns     : None.
 *---------------------------------------------------------------------------*/
static void renderDistance(const WPADStatus *stat)
{
    char buf[256];
    f32  dist;
    
        dist = getDistance(stat);

        if(dist > 0.0f)
        {
            sprintf(buf, "Distance: %.02f m", dist);
        }
        else
        {
            sprintf(buf, "Distance: Unknown");
        }
        DEMOPrintf(100, 120, 0, buf);

} // end

/*---------------------------------------------------------------------------*
 * Name        : getDistance
 * Description : 
 * Arguments   : None.
 * Returns     : None.
 *---------------------------------------------------------------------------*/
static f32 getDistance(const WPADStatus *stat)
{

    const f32 PI=3.141592f;
    const f32 TH=WPAD_DPD_ANGLE/2*PI/180;
    f32 xDiff; 
    f32 yDiff; 
    f32 dpdDist;

        xDiff   = stat->obj[0].x - stat->obj[1].x;
        yDiff   = stat->obj[0].y - stat->obj[1].y;
        dpdDist = (f32)sqrt(xDiff*xDiff+yDiff*yDiff);

        if ((stat->obj[0].size > 0) && (stat->obj[1].size > 0))
        {
            return (f32)(SEPARATION/2/tan(TH)*WPAD_DPD_IMG_RESO_WX/dpdDist);
        }
        else
        {
            return -1.0f;
        }

} // end


/*---------------------------------------------------------------------------*
 * Name        : myAlloc()
 * Description : Callback needed by WPAD to allocate mem from MEM2 heap
 * Arguments   : size of block, in bytes.
 * Returns     : pointer to allocated block.
 *---------------------------------------------------------------------------*/
static void *myAlloc(u32 size)
{
    void *ptr;
    
    ptr = MEMAllocFromAllocator(&DemoAllocator2, size);
    ASSERTMSG(ptr, "Memory allocation failed\n");

    return(ptr);

} // myAlloc()

/*---------------------------------------------------------------------------*
 * Name        : myFree()
 * Description : Callback needed by WPAD to free mem from MEM2 heap
 * Arguments   : None.
 * Returns     : Always 1.
 *---------------------------------------------------------------------------*/
static u8 myFree(void *ptr)
{
    
    MEMFreeToAllocator(&DemoAllocator2, ptr);

    // we should ensure that memory is free'd properly, but oh well
    return(1);

} // myFree()