/*---------------------------------------------------------------------------*
  Project:    WPAD Health Demo Program
  File:       handling_weight.c

  Copyright (C) 2007 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: $
 *---------------------------------------------------------------------------*/
#include <stdlib.h>
#include <string.h>
#include <stddef.h>
#include <stdarg.h>
#include <stdio.h>
#include <ctype.h>

#include <revolution.h>
#include <revolution/wpad.h>
#include <revolution/wpadBalance.h>
#include <revolution/wbc.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 FONT_HEIGHT     8

GXColor smoke_clr    = {  61, 61, 61, 255 } ;
GXColor red_clr      = { 237, 28, 36, 255 } ;
GXColor blue_clr     = {   0, 84,166, 255 } ;
GXColor yellow_clr   = { 255,242,  0, 255 } ;
GXColor peagreen_clr = { 141,198, 63, 255 } ;

WPADBLStatus status;


u8  rxBuf[128] ATTRIBUTE_ALIGN(32);
u8  txBuf[128] ATTRIBUTE_ALIGN(32);


/*---------------------------------------------------------------------------*
 * 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 );


// Callbacks for CONNECT and EXTENSION events.
void         connectCallback         ( s32 chan, s32 reason );
void         extensionCallback       ( s32 chan, s32 result );

// Internal functions
static void  initialize              ( void );
static void  renderStatus            ( void );
static void  init_draw_graphic       ( u16 fb_width, u16 fb_height );
static void  draw_line               ( f32 x1, f32 y1, f32 x2, f32 y2, GXColor clr, f32 width );
static void  draw_box                ( f32 x1, f32 y1, f32 x2, f32 y2, GXColor clr, f32 width );

static double zero_point[WPAD_PRESS_UNITS];
static double sum;
static double ave;
static u32    counter;
static BOOL   get_weight_flag = FALSE;
static BOOL   set_zero_flag   = FALSE;

#define NSAMPLES_2SEC 120

/*===========================================================================*
 *                   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.
 *---------------------------------------------------------------------------*/
static void ctrlWbc( s32 chan, s32 result )
{
    OSReport("chan%d [%d]\n", chan, result);
}

static void WaitMilliTime(s32 msec)
{
    OSTime t=OSGetTime();
    while(OSTicksToMilliseconds(OSGetTime()-t) < msec)
        ;
}


static void ZeroSetStart2( s32 chan, s32 result )
{
    #pragma unused(chan, result)
    
    // Very rarely, the AD value fluctuates wildly after the temperature value is updated, so we wait a little here (200ms).

    WaitMilliTime(200);
    
    
    // Start zero-point compensation.
    set_zero_flag = TRUE;
    counter = 0;
    zero_point[0] = 0;
    zero_point[1] = 0;
    zero_point[2] = 0;
    zero_point[3] = 0;
}


static void ZeroSetStart( s32 chan, s32 result )
{
    #pragma unused(chan, result)
    
    // Check temperature.
    WPADRead(WPAD_CHAN3, &status);
    
    
    if((status.temp == 127) || (status.temp == -128))
    {
        // If it couldn't be obtained completely, make another attempt to update temperature.
        WPADControlBLC(WPAD_CHAN3, WPAD_BLCMD_UPDATE_TEMP, ZeroSetStart2);
    }
    else
    {
        // Very rarely, the AD value fluctuates wildly after the temperature value is updated, so we wait a little here (200ms).

        WaitMilliTime(200);
    
        // Start zero-point compensation.
        set_zero_flag = TRUE;
        counter = 0;
        zero_point[0] = 0;
        zero_point[1] = 0;
        zero_point[2] = 0;
        zero_point[3] = 0;
    }
}

int main( void )
{

    initialize();

    WPADRegisterAllocator(myAlloc, myFree);

    WPADInit();

    WPADSetConnectCallback(WPAD_CHAN3, connectCallback);

    while (WPAD_STATE_SETUP != WPADGetStatus())
    {
        ;
    }

    while(1)
    {
        DEMOPadRead();

        if (DEMOPadGetButtonDown(0) & PAD_BUTTON_A)
        {
            // Turns on power to the Wii Balance Board.
            WPADControlBLC(WPAD_CHAN3, WPAD_BLCMD_ON, ctrlWbc);
        }
        if (DEMOPadGetButtonDown(0) & PAD_BUTTON_B)
        {
            // Turns off power to the Wii Balance Board.
            WPADControlBLC(WPAD_CHAN3, WPAD_BLCMD_OFF, ctrlWbc);
        }
        if (DEMOPadGetButtonDown(0) & PAD_TRIGGER_Z)
        {
            // Updates the Wii Balance Board temperature value.
            WPADControlBLC(WPAD_CHAN3, WPAD_BLCMD_UPDATE_TEMP, ctrlWbc);
        }
        if (DEMOPadGetButtonDown(0) & PAD_BUTTON_MENU)
        {
            if((set_zero_flag == FALSE) && (get_weight_flag == FALSE))
            {
                // Updates the Wii Balance Board temperature value immediately before producing a zero-point.
                WPADControlBLC(WPAD_CHAN3, WPAD_BLCMD_UPDATE_TEMP, ZeroSetStart);
            }
        }
        if (DEMOPadGetButtonDown(0) & PAD_BUTTON_LEFT)
        {
            if((set_zero_flag == FALSE) && (get_weight_flag == FALSE))
            {
                // Gets the 2-second average of the weight conversion value.
                get_weight_flag = TRUE;
                counter = 0;
                sum     = 0;
            }
        }
        
        
        // Caption
        DEMOInitCaption( DM_FT_XLU, SCREEN_WIDTH, SCREEN_HEIGHT );
        GXSetZMode( GX_ENABLE, GX_ALWAYS, GX_ENABLE );
        GXSetBlendMode( GX_BM_BLEND, GX_BL_ONE, GX_BL_ONE, GX_LO_CLEAR );

        DEMOBeforeRender();
        renderStatus();
        DEMODoneRender();
    }

} // End main()

/*---------------------------------------------------------------------------*
 * Name        : connectCallback()
 *
 * Description : This callback is invoked when a controller is connected or
 *               disconnected.
 *
 * Arguments   : The channel (chan) for which the event has occurred.
 *               The channel status (reason): 
 *                 WPAD_ERR_NONE means a controller has been connected.
 *                 WPAD_ERR_NO_CONTROLLER means a controller disconnected.
 *
 * Returns     : None.
 *---------------------------------------------------------------------------*/
void connectCallback(s32 chan, s32 reason)
{
    u32 type;

    if (reason == WPAD_ERR_NONE)
    {
        // 4P is reserved for the Wii Balance Board, so anything else is disconnected.

        WPADProbe(chan, &type);
        if (chan == WPAD_CHAN3 && type != WPAD_DEV_BALANCE_CHECKER)
        {
            OSReport("Channel%d is reserved for the balance checker.\n", chan);
            WPADDisconnect(chan);
        }
        else
        {
            // Reads the calibration value for calculating a weight.
            if(WPAD_ERR_NONE != WBCSetupCalibration())
            {
                OSHalt("WBC FATAL ERROR!!\n");
            }
            
            OSReport("Channel%d is connected.\n", chan);
            WPADSetExtensionCallback(chan, extensionCallback);
        }
    }
    else
    {
        OSReport("Channel%d is disconnected.\n", chan);
    }
} // End connectCallback()

/*---------------------------------------------------------------------------*
 * Name        : extensionCallback()
 *
 * Description : This callback is invoked when an Extension has been attached.
 *
 * Arguments   : The channel (chan) for which the extension event occurred.
 *               The device type (result): 
 *
 *                 WPAD_DEV_UNKNOWN means that something has been attached, but
 *                 it's being initialized, and we won't know what it is until
 *                 initialization is complete.
 *
 *                 WPAD_DEV_CORE means that an extension has been removed and
 *                 we're back to just the core device.
 *
 *                 WPAD_DEV_FREESTYLE means that the "NUNCHAK" extension has
 *                 been attached and initialized.
 *
 *                 WPAD_DEV_CLASSIC means that the "CLASSIC" extension has been
 *                 attached and initialized.
 *
 * Returns     : None.
 *---------------------------------------------------------------------------*/
void extensionCallback(s32 chan, s32 result)
{
    switch(result)
    {
        case WPAD_DEV_UNKNOWN:  
            OSReport("Initializing extension on channel%d...\n", chan);
            break;

        case WPAD_DEV_CORE:
            WPADControlDpd(chan, WPAD_DPD_EXP, NULL);        
            WPADSetDataFormat(chan, WPAD_FMT_CORE_ACC_DPD);

            OSReport("Extension removed on channel%d.\n", chan);
            break;
            
        case WPAD_DEV_NOT_SUPPORTED:
        case WPAD_DEV_FUTURE:
            WPADControlDpd(chan, WPAD_DPD_EXP, NULL);        
            WPADSetDataFormat(chan, WPAD_FMT_CORE_ACC_DPD);

            OSReport("Extension is not useful on channel%d.\n", chan);
            break;

        case WPAD_DEV_FREESTYLE:
            WPADControlDpd(chan, WPAD_DPD_STD, NULL);        
            WPADSetDataFormat(chan, WPAD_FMT_FREESTYLE_ACC_DPD);

            OSReport("Freestyle initialized on channel%d.\n", chan);
            break;

        case WPAD_DEV_CLASSIC:
            WPADControlDpd(chan, WPAD_DPD_STD, NULL);        
            WPADSetDataFormat(chan, WPAD_FMT_CLASSIC_ACC_DPD);

            OSReport("Classicstyle initialized on channel%d.\n", chan);
            break;

        case WPAD_DEV_BALANCE_CHECKER:
            WPADControlDpd(chan, WPAD_DPD_OFF, NULL);
            WPADSetDataFormat(chan, WPAD_FMT_BALANCE_CHECKER);
            WPADControlBLC(chan, WPAD_BLCMD_ON, NULL);
            
            OSReport("Balance checker initialized on channel%d.\n", chan);
            break;

        default:
            // Here is WPAD_DEV_NOT_FOUND.
            // If the controller is disconnected while the extension is initializing
            // it reaches here. There is nothing to do.
            break;

    } // End

} // End extensionCallback()

/*---------------------------------------------------------------------------*
 * 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()


static void renderStatus(void)
{
    u32 type;
    s32 isExist = 0;

    if (WPADProbe(WPAD_CHAN3, &type) == WPAD_ERR_NO_CONTROLLER)
    {
        if (WPADIsRegisteredBLC())
        {
            DEMOPrintf( 50, 100, 0, "WBC is not connected.");
        }
        else
        {
            DEMOPrintf( 20, 100, 0, "WBC is not registered.");
            DEMOPrintf( 20, 110, 0, "Please regist WBC with SYNC button.");
        }
    }
    else
    {
        if (type == WPAD_DEV_BALANCE_CHECKER)
        {
            double weight[WPAD_PRESS_UNITS];
            double all;
            
            
            WPADRead(WPAD_CHAN3, &status);
            
            DEMOPrintf( 95,  40, 0, "%5d", status.press[0]);
            DEMOPrintf( 95, 110, 0, "%5d", status.press[1]);
            DEMOPrintf( 10,  40, 0, "%5d", status.press[2]);
            DEMOPrintf( 10, 110, 0, "%5d", status.press[3]);
            
            isExist = WBCRead(&status, weight, (u32)(sizeof(weight) / sizeof(weight[0])));
            all = (double)(weight[0] + weight[1] + weight[2] + weight[3]);
            
            
            
            if((get_weight_flag || set_zero_flag) && counter < NSAMPLES_2SEC)
            {
                counter++;
                sum += all;
                zero_point[0] += status.press[0];
                zero_point[1] += status.press[1];
                zero_point[2] += status.press[2];
                zero_point[3] += status.press[3];
            }
            
            
            if(counter == NSAMPLES_2SEC)
            {
                ave  = sum / NSAMPLES_2SEC;
                zero_point[0] /= NSAMPLES_2SEC;
                zero_point[1] /= NSAMPLES_2SEC;
                zero_point[2] /= NSAMPLES_2SEC;
                zero_point[3] /= NSAMPLES_2SEC;
                get_weight_flag = FALSE;
                
                if(set_zero_flag)
                {
                    //Sets the zero point internally in the Library.
                    if(0 == WBCSetZEROPoint( zero_point,(u32)(sizeof(zero_point) / sizeof(zero_point[0])) ))
                    {
                        //Succeeded
                        set_zero_flag = FALSE;
                    }
                    else
                    {
                        //Retries if it failed.
                        counter = 0;
                        zero_point[0] = 0;
                        zero_point[1] = 0;
                        zero_point[2] = 0;
                        zero_point[3] = 0;
                    }
                }
            }
            
            
            if(get_weight_flag || set_zero_flag)
            {
                DEMOPrintf( 95,  55, 0, "wait...");
                DEMOPrintf( 95, 125, 0, "wait...");
                DEMOPrintf( 10,  55, 0, "wait...");
                DEMOPrintf( 10, 125, 0, "wait...");
                
                if(get_weight_flag)
                {
                    DEMOPrintf(175,  60, 0, "Wt     :WAIT");
                    DEMOPrintf(175,  70, 0, "Wt(TGC):WAIT");
                }
                else
                {
                    DEMOPrintf(175,  60, 0, "Wt     :%3.1f[kg]", ave);
                    DEMOPrintf(175,  70, 0, "Wt(TGC):%3.1f[kg]", WBCGetTGCWeight(ave, &status));
                }
            }
            else
            {
                DEMOPrintf( 95,  55, 0, "%3.1f", weight[0]);
                DEMOPrintf( 95, 125, 0, "%3.1f", weight[1]);
                DEMOPrintf( 10,  55, 0, "%3.1f", weight[2]);
                DEMOPrintf( 10, 125, 0, "%3.1f", weight[3]);
                DEMOPrintf(175,  60, 0, "Wt     :%3.1f[kg]", ave);
                DEMOPrintf(175,  70, 0, "Wt(TGC):%3.1f[kg]", WBCGetTGCWeight(ave, &status));
                DEMOPrintf(175,  80, 0, "Setup  :%s", WBCGetCalibrationStatus()==TRUE ? "OK":"NG");
            }
            
            DEMOPrintf(175,  20, 0, "Temp %d", status.temp);
            DEMOPrintf(175,  30, 0, "Batt %d/Level %d", status.battery, WBCGetBatteryLevel(status.battery));
            DEMOPrintf(175,  40, 0, "IsEXIST = %s", isExist == 1 ? "EXIST" : "NONE");
            
            DEMOPrintf( 10, 160, 0, "A: Turn On  SENSOR");
            DEMOPrintf( 10, 170, 0, "B: Turn Off SENSOR");
            DEMOPrintf( 10, 180, 0, "MENU: Set ZERO point");
            DEMOPrintf( 10, 190, 0, "LEFT: Get AVE Weight");
            DEMOPrintf( 10, 220, 0, "Z: Update the current temperature");

            init_draw_graphic(SCREEN_WIDTH, SCREEN_HEIGHT);
            draw_box(-143, -100, -68, -50, peagreen_clr, 2);
            init_draw_graphic(SCREEN_WIDTH, SCREEN_HEIGHT);
            draw_box(-143,  -30, -68,  20, yellow_clr, 2);
            init_draw_graphic(SCREEN_WIDTH, SCREEN_HEIGHT);
            draw_box(-63,  -100, 12, -50, blue_clr, 2);
            init_draw_graphic(SCREEN_WIDTH, SCREEN_HEIGHT);
            draw_box(-63,   -30, 12,  20, red_clr, 2);
        }
        else
        {
            DEMOPrintf(50, 100, 0, "No balance checker is attached.");
        }
    }

} // End renderStatus()


static void initialize( void )
{
    OSInit();

    DEMOInit( &GXNtsc480IntDf );

    GXSetCopyClear( smoke_clr, GX_MAX_Z24 );
    GXCopyDisp( DEMOGetCurrentBuffer(), GX_TRUE );

    DEMOPadInit();
} // End


static void init_draw_graphic( u16 fb_width, u16 fb_height )
{
    Mtx44   proj_mtx ;
    Mtx     view_mtx ;
    f32     canvas_wd, canvas_ht ;

    // CANVAS
    canvas_wd = fb_width * 0.91346f ;
    canvas_ht = fb_height ;

    // MTX
    MTXOrtho( proj_mtx, canvas_ht * -0.5f,canvas_ht * 0.5f, canvas_wd * -0.5f,canvas_wd * 0.5f, -10.0f,10.0f ) ;
    GXSetProjection( proj_mtx, GX_ORTHOGRAPHIC ) ;

    MTXIdentity( view_mtx ) ;
    GXLoadPosMtxImm( view_mtx, GX_PNMTX0 ) ;
    GXSetCurrentMtx( GX_PNMTX0 ) ;

    // VERTEX
    GXClearVtxDesc() ;
    GXSetVtxDesc( GX_VA_POS, GX_DIRECT ) ;
    GXSetVtxAttrFmt( GX_VTXFMT0, GX_VA_POS, GX_POS_XY, GX_F32, 0 ) ;

    // CHANNEL
    GXSetNumChans( 1 ) ;
    GXSetChanCtrl( GX_COLOR0A0, GX_DISABLE, GX_SRC_REG, GX_SRC_REG, GX_LIGHT_NULL, GX_DF_NONE, GX_AF_NONE ) ;
    GXSetChanCtrl( GX_COLOR1A1, GX_DISABLE, GX_SRC_REG, GX_SRC_REG, GX_LIGHT_NULL, GX_DF_NONE, GX_AF_NONE ) ;

    // TEXTURE
    GXSetNumTexGens( 0 ) ;

    // TEV
    GXSetNumTevStages( 1 ) ;
    GXSetTevOrder( GX_TEVSTAGE0, GX_TEXCOORD_NULL, GX_TEXMAP_NULL, GX_COLOR_NULL ) ;
    GXSetTevColorIn( GX_TEVSTAGE0, GX_CC_ZERO, GX_CC_ZERO, GX_CC_ZERO, GX_CC_C0 ) ;
    GXSetTevColorOp( GX_TEVSTAGE0, GX_TEV_ADD, GX_TB_ZERO, GX_CS_SCALE_1, GX_ENABLE, GX_TEVPREV ) ;
    GXSetTevAlphaIn( GX_TEVSTAGE0, GX_CA_ZERO, GX_CA_ZERO, GX_CA_ZERO, GX_CA_A0 ) ;
    GXSetTevAlphaOp( GX_TEVSTAGE0, GX_TEV_ADD, GX_TB_ZERO, GX_CS_SCALE_1, GX_ENABLE, GX_TEVPREV ) ;
    GXSetAlphaCompare( GX_ALWAYS,0, GX_AOP_OR, GX_ALWAYS,0 ) ;

    // SCREEN
    GXSetBlendMode( GX_BM_BLEND, GX_BL_SRCALPHA, GX_BL_INVSRCALPHA, GX_LO_NOOP ) ;
    GXSetAlphaUpdate( GX_DISABLE ) ;

    GXSetZMode( GX_DISABLE, GX_ALWAYS, GX_DISABLE ) ;
    GXSetCullMode( GX_CULL_BACK ) ;
}

/*******************************************************************************
    Draw object
*******************************************************************************/
static void draw_line( f32 x1, f32 y1, f32 x2, f32 y2, GXColor clr, f32 width )
{
    GXSetTevColor( GX_TEVREG0, clr ) ;
    GXSetLineWidth( (u8)(s32)(width * 6.0f + 0.5f), GX_TO_ZERO ) ;

    GXBegin( GX_LINES, GX_VTXFMT0, 2 ) ;
      GXPosition2f32( x1, y1 ) ;
      GXPosition2f32( x2, y2 ) ;
    GXEnd() ;
}

static void draw_box( f32 x1, f32 y1, f32 x2, f32 y2, GXColor clr, f32 width )
{
    draw_line(x1, y1, x2, y1, clr, width);
    draw_line(x2, y1, x2, y2, clr, width);
    draw_line(x2, y2, x1, y2, clr, width);
    draw_line(x1, y2, x1, y1, clr, width);
}