/*---------------------------------------------------------------------------*
  Project:     KPAD sample program
  File:        sample.c
  Programmer:  Keizo Ohta
               HIRATSU Daisuke
               Tojo Haruki

  Copyright 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 <revolution.h>
#include <math.h>
#include <stdio.h>

#include <revolution/kpad.h>
#include "main.h"
#include "kfont.h"
#include "graphic.h"
#include "sample.h"


/***************************************************************
    Definitions
***************************************************************/
#define CMD_LEFT        ( (kpads[0][0].trig & KPAD_BUTTON_LEFT ) != 0 ) // Cursor movement
#define CMD_RIGHT       ( (kpads[0][0].trig & KPAD_BUTTON_RIGHT) != 0 )
#define CMD_UP          ( (kpads[0][0].trig & KPAD_BUTTON_UP   ) != 0 )
#define CMD_DOWN        ( (kpads[0][0].trig & KPAD_BUTTON_DOWN ) != 0 )

#define CMD_INC         ( (kpads[0][0].trig & KPAD_BUTTON_A    ) != 0 ) // Increase/decrease value
#define CMD_DEC         ( (kpads[0][0].trig & KPAD_BUTTON_B    ) != 0 )

#define CMD_ENABLE_DPD  ( (kpads[0][0].trig & KPAD_BUTTON_1    ) != 0 ) // DPD ON/OFF
#define CMD_DISABLE_DPD ( (kpads[0][0].trig & KPAD_BUTTON_2    ) != 0 )

#define BTN_CALIBRATE   ( KPAD_BUTTON_PLUS | KPAD_BUTTON_MINUS )        // Calibration operation
#define CMD_CALIBRATE   ( (kpads[0][0].hold & BTN_CALIBRATE) == BTN_CALIBRATE && kpads[0][0].trig )

#define CMD_AIMMODE     ( (kpads[0][0].trig & KPAD_BUTTON_HOME ) != 0 )

#define CMD_STICK_CLAMP_HOLD1 ( kpads[0][0].dev_type == WPAD_DEV_FREESTYLE && (kpads[0][0].hold & KPAD_BUTTON_Z) != 0 )
#define CMD_STICK_CLAMP_HOLD2 ( ( ( kpads[0][0].dev_type == WPAD_DEV_CLASSIC ) \
                               || ( kpads[0][0].dev_type == WPAD_DEV_GUITAR  ) ) \
                             && ( kpads[0][0].ex_status.cl.hold & KPAD_CL_BUTTON_PLUS ) != 0 )

#define CMD_STICK_CLAMP_TRIG1 ( kpads[0][0].dev_type == WPAD_DEV_FREESTYLE && (kpads[0][0].trig & KPAD_BUTTON_C) != 0 )
#define CMD_STICK_CLAMP_TRIG2 ( ( ( kpads[0][0].dev_type == WPAD_DEV_CLASSIC ) \
                               || ( kpads[0][0].dev_type == WPAD_DEV_GUITAR  ) ) \
                             && ( kpads[0][0].ex_status.cl.trig & KPAD_CL_BUTTON_MINUS ) != 0 )


/***************************************************************
 Adjustment item
***************************************************************/
static f32 pointing_scale = 200.0f ; // Screen pointing scale

static f32 obj_interval     = 0.20f ; // TV side light emitting point placement interval (in meters)

static f32 pos_play_radius  = 0.00f ; // 'pos' sensitivity setting
static f32 pos_sensitivity  = 1.00f ;
static f32 pos_play_mode    = (f32)KPAD_PLAY_MODE_LOOSE ;
static f32 hori_play_radius = 0.00f ; // 'horizon' sensitivity setting
static f32 hori_sensitivity = 1.00f ;
static f32 hori_play_mode   = (f32)KPAD_PLAY_MODE_LOOSE ;
static f32 dist_play_radius = 0.00f ; // 'dist' sensitivity setting
static f32 dist_sensitivity = 1.00f ;
static f32 dist_play_mode   = KPAD_PLAY_MODE_LOOSE ;
static f32 acc_play_radius  = 0.00f ; // 'acc' sensitivity setting
static f32 acc_sensitivity  = 1.00f ;
static f32 acc_play_mode    = KPAD_PLAY_MODE_LOOSE ;

static f32 repeat_delay_sec = 0.75f ; // Key repeat settings
static f32 repeat_pulse_sec = 0.25f ;

#define EDIT_DATA_MAX 16 // Edit data count

typedef struct {
    f32 *vp ;   // Pointer to the variable
    f32 min ;   // Variable minimum value
    f32 max ;   // Variable maximum value
    u8  y ;     // y-cursor position
    u8  x[3] ;  // x-cursor position for each digit
    f32 v[3] ;  // Value change for each digit
} EditData ;

static EditData  edit_data[ EDIT_DATA_MAX ] = {
    { &  pointing_scale, 0.0f,900.0f,  3, {10,11,12}, {100.0f, 10.0f, 1.0f} },
    { &    obj_interval, 0.00f,9.00f,  5, { 9,11,12}, {1.00f, 0.10f, 0.01f} },
    { & pos_play_radius, 0.00f,9.00f,  8, { 4, 6, 7}, {1.00f, 0.10f, 0.01f} },
    { & pos_sensitivity, 0.00f,1.00f,  8, { 9,11,12}, {1.00f, 0.10f, 0.01f} },
    { & pos_play_mode,   0.00f,1.00f,  8, {14,14,14}, {1.00f, 1.00f, 1.00f} },
    { &hori_play_radius, 0.00f,9.00f,  9, { 4, 6, 7}, {1.00f, 0.10f, 0.01f} },
    { &hori_sensitivity, 0.00f,1.00f,  9, { 9,11,12}, {1.00f, 0.10f, 0.01f} },
    { &hori_play_mode,   0.00f,1.00f,  9, {14,14,14}, {1.00f, 1.00f, 1.00f} },
    { &dist_play_radius, 0.00f,9.00f, 10, { 4, 6, 7}, {1.00f, 0.10f, 0.01f} },
    { &dist_sensitivity, 0.00f,1.00f, 10, { 9,11,12}, {1.00f, 0.10f, 0.01f} },
    { &dist_play_mode,   0.00f,1.00f, 10, {14,14,14}, {1.00f, 1.00f, 1.00f} },
    { & acc_play_radius, 0.00f,9.00f, 11, { 4, 6, 7}, {1.00f, 0.10f, 0.01f} },
    { & acc_sensitivity, 0.00f,1.00f, 11, { 9,11,12}, {1.00f, 0.10f, 0.01f} },
    { & acc_play_mode,   0.00f,1.00f, 11, {14,14,14}, {1.00f, 1.00f, 1.00f} },
    { &repeat_delay_sec, 0.00f,9.00f, 14, { 4, 6, 7}, {1.00f, 0.10f, 0.01f} },
    { &repeat_pulse_sec, 0.00f,9.00f, 14, { 9,11,12}, {1.00f, 0.10f, 0.01f} },
} ;


/***************************************************************
 Variables
***************************************************************/
static const f32 O1Y = -1.5f;
static const f32 O2Y = -0.5f;
static const f32 O3Y = -1.0f;
static const f32 O4Y = -20.8f;
static const f32 O5Y = -20.8f;
static const f32 O6Y = -1.0f;


static s32 csr_x, csr_y ;   // Value adjustment cursor position
static s32 csr_i, csr_k ;   // Position of edited data index and digit
static s32 calib_disp_ct ;  // Calibration message display count

static f32 dpd_disp_scale = 140.0f ; // DPD object display scale
static f32 dpd_disp_ox    = 205.0f ; // DPD object display position
static f32 dpd_disp_oy    =  65.0f+O5Y ;

static BOOL aim_mode ;
static BOOL stick_clamp_mode ;
static BOOL stick_clamp_mode_work ;
static BOOL reset_request ;

static WPADInfo              wpad_info ;
static KPADUnifiedWpadStatus unified_status[ KPAD_BUF_SIZE * 2 ] ;

s32                 MyDpdCallbackLatestIdx = -1 ;
MyDpdCallbackStatus MyDpdCallbackStatusBuf[ MY_DPD_START_COUNTS_MAX ] ;
vu32                MySamplingCount ;

/*******************************************************************************
 Initialization Functions
*******************************************************************************/
void init_sample( void )
{
    //----- Initial controller settings
    KPADReset() ;
    KPADSetPosParam ( 0,  pos_play_radius,  pos_sensitivity ) ;
    KPADSetHoriParam( 0, hori_play_radius, hori_sensitivity ) ;
    KPADSetDistParam( 0, dist_play_radius, dist_sensitivity ) ;
    KPADSetAccParam ( 0,  acc_play_radius,  acc_sensitivity ) ;
    KPADSetBtnRepeat( 0, repeat_delay_sec, repeat_pulse_sec ) ;

    //----- Cursor position initialization
    csr_i = 0 ;  // Data index being edited
    csr_k = 2 ;  // Digit being edited
    csr_x = edit_data[ csr_i ].x[ csr_k ] ;
    csr_y = edit_data[ csr_i ].y ;

    //----- Calibration message count initialization
    calib_disp_ct = 0 ; // Negative: display failed, 0: not displayed, positive: successful display

    aim_mode = TRUE;
    stick_clamp_mode = stick_clamp_mode_work = FALSE;
}


/*******************************************************************************
 Cursor movement process
*******************************************************************************/
static void move_cursor( void )
{
    EditData *ep ;
    s32  vx,vy, wx,wy, fy ;
    s32  i,w,k, im,wm,km ;

    /***********************************************************************
    Cursor movement process
    ***********************************************************************/
    //------ Temporary movement vector
    if ( CMD_LEFT ) vx = -1 ;
    else if ( CMD_RIGHT ) vx = 1 ;
    else vx = 0 ;

    if ( CMD_UP ) vy = -1 ;
    else if ( CMD_DOWN ) vy = 1 ;
    else vy = 0 ;

    //------ Move to the nearest digit location using edit data
    wm = 1 << 30 ;  // Maximum distance
    i = EDIT_DATA_MAX - 1 ;
    do {
        ep = &edit_data[i] ;

        fy = ( wy = ep->y - csr_y ) * vy ; // Preparation for inner product calculation
        wy *= wy ;    // Preparation for distance calculation
        k = 0 ;
        do {
            //----- Inner product must be positive
            wx = ep->x[k] - csr_x ;
            if ( wx * vx + fy <= 0 ) continue ;

            //----- Distance must be shortest
            w = wx * wx + wy ;
            if ( w >= wm ) continue ;

            //----- Candidate recording
            wm = w ;
            im = i ;
            km = k ;
        } while ( ++k < 3 ) ;
    } while ( --i >= 0 ) ;

    //----- Move cursor if candidate is recorded
    if ( wm != 1 << 30 ) {
        csr_i = im ;
        csr_k = km ;
        csr_x = edit_data[ csr_i ].x[ csr_k ] ;
        csr_y = edit_data[ csr_i ].y ;
    }
}


/*******************************************************************************
 Process for increasing/decreasing numerical values
*******************************************************************************/
static void push_cursor( void )
{
    EditData *ep = &edit_data[ csr_i ] ;

    //----- Was a button pressed?
    if ( CMD_INC ) {
        *(ep->vp) += ep->v[ csr_k ] ;
        if ( *(ep->vp) > ep->max ) *(ep->vp) = ep->max ;
    } else if ( CMD_DEC ) {
        *(ep->vp) -= ep->v[ csr_k ] ;
        if ( *(ep->vp) < ep->min ) *(ep->vp) = ep->min ;
    } else {
        return ; // No value change
    }

    //----- Reset because value has changed
    if ( csr_i >= 14 ) {
        KPADSetBtnRepeat( 0, repeat_delay_sec, repeat_pulse_sec ) ;
    } else if ( csr_i >=11 ) {
        KPADSetAccParam    ( 0, acc_play_radius,  acc_sensitivity ) ;
        KPADSetAccPlayMode ( 0, (KPADPlayMode)acc_play_mode ) ;
    } else if ( csr_i >= 8 ) {
        KPADSetDistParam   ( 0, dist_play_radius, dist_sensitivity ) ;
        KPADSetDistPlayMode( 0, (KPADPlayMode)dist_play_mode ) ;
    } else if ( csr_i >= 5 ) {
        KPADSetHoriParam   ( 0, hori_play_radius, hori_sensitivity ) ;
        KPADSetHoriPlayMode( 0, (KPADPlayMode)hori_play_mode ) ;
    } else if ( csr_i >= 2 ) {
        KPADSetPosParam    ( 0, pos_play_radius,  pos_sensitivity ) ;
        KPADSetPosPlayMode ( 0, (KPADPlayMode)pos_play_mode ) ;
    } else if ( csr_i == 1 ) {
        KPADSetObjInterval( obj_interval ) ;
        reset_request = TRUE;
    }
}


/*******************************************************************************
 CPU process function
*******************************************************************************/
void work_sample( void )
{
    if (kpad_reads <= 0) {
        return;
    }

    if (kpads[0][0].hold == 0 && reset_request) {
        reset_request = FALSE;
        OSReport("KPADReset() from work_sample()\n");
        KPADReset();
        return;
    }
    
    /***********************************************************************
    Calibration process
    ***********************************************************************/
    if ( calib_disp_ct < 0 ) ++ calib_disp_ct ;
    else if ( calib_disp_ct ) -- calib_disp_ct ;

    if ( CMD_CALIBRATE ) {
        if ( KPADCalibrateDPD( 0 ) == 2 ) {
            calib_disp_ct = 180 ; // Successful
        } else {
            calib_disp_ct = -180 ; // Failed
        }
    }

    /***********************************************************************
    DPD ON/OFF
    ***********************************************************************/
    if ( CMD_ENABLE_DPD ) {
        KPADEnableDPD( 0 );
    }
    if ( CMD_DISABLE_DPD ) {
        KPADDisableDPD( 0 );
    }

    /***********************************************************************
    AIMMODE on/off
    ***********************************************************************/
    if ( CMD_AIMMODE ) {
        aim_mode = (aim_mode) ? FALSE : TRUE;
        if( aim_mode ) {
            KPADEnableAimingMode( 0 );
        }
        else {
            KPADDisableAimingMode( 0 );
        }
    }

    /***********************************************************************
    Stick clamp mode ON/OFF
    ***********************************************************************/
    if ( CMD_STICK_CLAMP_TRIG1 || CMD_STICK_CLAMP_TRIG2 ) {
        stick_clamp_mode_work = (stick_clamp_mode_work) ? FALSE : TRUE;
    }
    if ( CMD_STICK_CLAMP_HOLD1 || CMD_STICK_CLAMP_HOLD2 ) {
        stick_clamp_mode = (stick_clamp_mode_work) ? FALSE : TRUE;
    } else {
        stick_clamp_mode = stick_clamp_mode_work;
    }
    if ( stick_clamp_mode ) {
        KPADEnableStickCrossClamp();
    } else {
        KPADDisableStickCrossClamp();
    }

    /***********************************************************************
    Value edit process
    ***********************************************************************/
    move_cursor() ;  // Cursor movement process
    push_cursor() ;  // Process for increasing/decreasing numerical values
}


/*******************************************************************************
 Rendering pointing scale frame
*******************************************************************************/
static void draw_pointing_frame( void )
{
    draw_line( -pointing_scale,-pointing_scale, pointing_scale,-pointing_scale, blue_clr, 2.5f ) ;
    draw_line(  pointing_scale,-pointing_scale, pointing_scale, pointing_scale, blue_clr, 2.5f ) ;
    draw_line(  pointing_scale, pointing_scale,-pointing_scale, pointing_scale, blue_clr, 2.5f ) ;
    draw_line( -pointing_scale, pointing_scale,-pointing_scale,-pointing_scale, blue_clr, 2.5f ) ;
}


/*******************************************************************************
 Rendering pointing coordinate
*******************************************************************************/
static void draw_position( KPADStatus *sp )
{
    Vec2  pos, vec ;
    f32  scale ;

    if (sp->dpd_valid_fg <= 0) {
        return;
    }
    switch (sp->data_format) {
      case WPAD_FMT_CORE_ACC_DPD :
      case WPAD_FMT_FREESTYLE_ACC_DPD :
      case WPAD_FMT_CLASSIC_ACC_DPD :
      case WPAD_FMT_GUITAR :
        break;

      default:
        return;
    }

    //------ Crosshair
    pos.x = pointing_scale * sp->pos.x ; // Center coordinate
    pos.y = pointing_scale * sp->pos.y ;

    scale = 72.0f ;
    vec.x = scale * sp->horizon.x ;  // Right direction vector
    vec.y = scale * sp->horizon.y ;
    draw_line( pos.x-vec.x,pos.y-vec.y, pos.x+vec.x,pos.y+vec.y, yellow_clr, 3 ) ;

    scale = 28.0f ;
    vec.x = scale * -sp->horizon.y ; // Lower direction vector
    vec.y = scale *  sp->horizon.x ;
    draw_arrow( pos.x+vec.x,pos.y+vec.y, pos.x-vec.x,pos.y-vec.y, silver_clr, 3 ) ;

    //------ Center point
    draw_point( pos.x,pos.y, black_clr, 11 ) ;
    draw_point( pos.x,pos.y,  cyan_clr,  9 ) ;
}


/*******************************************************************************
 Rendering grids
*******************************************************************************/
static void draw_grid( f32 ox, f32 oy, f32 radius, GXColor clr )
{
    f32  r = radius * 1.4f ; // Crosshair length

    draw_arrow( ox-r,oy, ox+r,oy, clr, 3 ) ; // Horizontal line
    draw_arrow( ox,oy+r, ox,oy-r, clr, 3 ) ; // Vertical line
    draw_circle( ox,oy, radius, clr, 3 ) ;  // Circle
}


/*******************************************************************************
 Rendering acceleration
*******************************************************************************/
static void draw_acc( f32 ox, f32 oy, f32 radius, f32 ax, f32 ay )
{
    f32  px = ax * radius + ox ;
    f32  py = ay * radius + oy ;

    draw_arrow( ox,oy, px,py, red_clr, 4 ) ;
    draw_point( px,py, white_clr, 4 ) ;
}


/*******************************************************************************
 Rendering Nunchuk unit acceleration
*******************************************************************************/
static void draw_acc2( f32 ox, f32 oy, f32 radius, f32 ax, f32 ay )
{
    f32  px = ax * radius + ox ;
    f32  py = ay * radius + oy ;

    draw_dashed_arrow( ox,oy, px,py, red_clr, 4 ) ;
    draw_point( px,py, white_clr, 4 ) ;
}

/*******************************************************************************
 Analog Stick Rendering
*******************************************************************************/
static void draw_stick( f32 ox, f32 oy, f32 radius, f32 sx, f32 sy, GXColor clr, BOOL dashed )
{
    f32 px = sx * radius + ox;
    f32 py = sy * radius + oy;

    if ( dashed ) {
        draw_dashed_arrow( ox,oy, px,py, clr, 4 ) ;
    } else {
        draw_arrow( ox,oy, px,py, clr, 4 ) ;
    }
    draw_point( px,py, white_clr, 4 ) ;
}

/*******************************************************************************
 Analog Trigger Rendering
*******************************************************************************/
static void draw_box( f32 ox, f32 oy, f32 width, f32 height, GXColor clr )
{
    f32  x1,y1, x2,y2 ;

    x1 = ox ;
    x2 = ox - width ;
    y1 = oy ;
    y2 = oy - height ;
    
    GXSetTevColor( GX_TEVREG0, clr ) ;

    GXBegin( GX_QUADS, GX_VTXFMT0, 4 ) ;
      GXPosition2f32( x1, y1 ) ;
      GXPosition2f32( x2, y1 ) ;
      GXPosition2f32( x2, y2 ) ;
      GXPosition2f32( x1, y2 ) ;
    GXEnd() ;
}

/*******************************************************************************
 Rendering value adjustment cursor
*******************************************************************************/
static void draw_cursor( void )
{
    f32  x1,y1, x2,y2 ;

    x1 = calc_kfont_x1( csr_x ) ;
    y1 = calc_kfont_y1( csr_y+O1Y ) ;
    y2 = calc_kfont_y2( csr_y+O1Y ) - 1.0f ;
    if ( csr_x > 12 ) {
        x2 = calc_kfont_x2( csr_x + 4.0f ) - 1.0f ;
    } else {
        x2 = calc_kfont_x2( csr_x ) - 1.0f ;
    }
    draw_line( x1,y1, x2,y1, white_clr, 2 ) ;
    draw_line( x2,y1, x2,y2, white_clr, 2 ) ;
    draw_line( x2,y2, x1,y2, white_clr, 2 ) ;
    draw_line( x1,y2, x1,y1, white_clr, 2 ) ;
}


/*******************************************************************************
 Rendering DPD sensor frame
*******************************************************************************/
static void draw_dpd_frame( void )
{
    f32  rx,ry, x1,y1, x2,y2 ;

    rx = dpd_disp_scale * 0.5f ;
    ry = rx * (f32)WPAD_DPD_IMG_RESO_WY / (f32)WPAD_DPD_IMG_RESO_WX ;
    x1 = dpd_disp_ox - rx ;
    y1 = dpd_disp_oy - ry ;
    x2 = dpd_disp_ox + rx ;
    y2 = dpd_disp_oy + ry ;

    GXSetTevColor( GX_TEVREG0, smoke_clr ) ;

    GXBegin( GX_QUADS, GX_VTXFMT0, 4 ) ;
      GXPosition2f32( x1, y1 ) ;
      GXPosition2f32( x2, y1 ) ;
      GXPosition2f32( x2, y2 ) ;
      GXPosition2f32( x1, y2 ) ;
    GXEnd() ;
}


/*******************************************************************************
 Rendering DPD object
*******************************************************************************/
static void draw_object( void )
{
    DPDObject              *op, *op1 ;
    f32                     px,py, scale, ofx,ofy ;
    s32                     n;
    KPADUnifiedWpadStatus  *up;

    // Caution: The return value of KPADRead() (number of samples) is not the same as the number of valid samples in unified_status[].
    //        
    //
    // Also, KPADGetUnifiedWpadStatus() gets the latest data at this point.
    // Accordingly, buffer contents are not the same as data retrieved inside KPADRead().
    //  
    //
    // In order to get exactly the same data as is obtained inside KPADRead(), call KPADRead() and KPADGetUnifiedWpadStatus() in succession while interrupts are disabled.
    //  
    //  However, the period during which interrupts are disabled will become longer.

    // kpad_reads is used as the number of samples without strictly synchronizing data because DPD objects are rendered simply in this sample program.
    //  
    n = kpad_reads ;   // Current sampling count
    if (n <= 0) {
        return;
    }

    KPADGetUnifiedWpadStatus( 0, unified_status, (u32)n );

    //----- Display position calculation
    scale = dpd_disp_scale / (f32)WPAD_DPD_IMG_RESO_WX ;
    ofx = dpd_disp_scale * -0.5f ;
    ofy = ofx * (f32)WPAD_DPD_IMG_RESO_WY / (f32)WPAD_DPD_IMG_RESO_WX ;
    ofx += dpd_disp_ox ;
    ofy += dpd_disp_oy ;

    //----- Display all values sampled at 200Hz
        // Render starting from older items

    while ( --n >= 0 ) {
        up = &unified_status[n];
        if (up->u.core.err == WPAD_ERR_NONE) {
            switch (up->fmt) {
              case WPAD_FMT_CORE_ACC :
              case WPAD_FMT_CORE_ACC_DPD :
              case WPAD_FMT_FREESTYLE_ACC :
              case WPAD_FMT_FREESTYLE_ACC_DPD :
              case WPAD_FMT_CLASSIC_ACC :
              case WPAD_FMT_CLASSIC_ACC_DPD :
              case WPAD_FMT_GUITAR :
                op1 = up->u.core.obj ;
                break;

              default:
                op1 = NULL;
                break;
            }
            if (op1) {
                op = op1 + (WPAD_DPD_MAX_OBJECTS - 1);
                do {
                    if ( op->size == 0 ) continue ;

                    //----- Change to display coordinates
                    px = (f32)op->x * scale + ofx ;
                    py = (f32)op->y * scale + ofy ;

                    //----- Render object
                    draw_point( px,py, white_clr, 5 ) ;
                    draw_circle( px,py, 5, black_clr, 2 ) ;
                    draw_circle( px,py, 4, smoke_clr, 2 ) ;
                    draw_circle( px,py, 3,  gray_clr, 2 ) ;
                } while ( --op >= op1 ) ;
            }
        }
    }
}
/*
static void draw_object_by_kpad1_style( void )
{
    WPADStatus   *wp ;
    WPADFSStatus *fp ;
    WPADCLStatus *cp ;
    DPDObject    *op, *op1 ;
    f32          px,py, scale, ofx,ofy ;
    s32          n, i ;


    if ( kpads[0][0].dev_type == WPAD_DEV_NOT_FOUND ) {
        return;
    }

    //----- Calculate the display position
    scale = dpd_disp_scale / (f32)WPAD_DPD_IMG_RESO_WX ;
    ofx = dpd_disp_scale * -0.5f ;
    ofy = ofx * (f32)WPAD_DPD_IMG_RESO_WY / (f32)WPAD_DPD_IMG_RESO_WX ;
    ofx += dpd_disp_ox ;
    ofy += dpd_disp_oy ;

    //----- Display all values sampled at 200Hz
    switch ( kpads[0][0].dev_type ) {
      case WPAD_DEV_CORE:
      case WPAD_DEV_FUTURE:
      case WPAD_DEV_NOT_SUPPORTED:
      case WPAD_DEV_UNKNOWN:
        wp = KPADGetWPADRingBuffer( 0 ) ;   // WPAD ring buffer
        break;
      case WPAD_DEV_FREESTYLE:
        fp = KPADGetWPADFSRingBuffer( 0 ) ; // WPAD ring buffer
        break;
      case WPAD_DEV_CLASSIC:
        cp = KPADGetWPADCLRingBuffer( 0 ) ; // WPAD ring buffer
        break;
    }
    i = (s32)WPADGetLatestIndexInBuf( 0 ) ; // Latest index
    n = kpad_reads ;                        // Current sampling count

    i -= n - 1 ;    // Render the oldest items first
    if ( i < 0 ) i += KPAD_BUF_SIZE ;

    while ( --n >= 0 ) {
        switch ( kpads[0][0].dev_type ) {
          case WPAD_DEV_CORE:
          case WPAD_DEV_FUTURE:
          case WPAD_DEV_NOT_SUPPORTED:
          case WPAD_DEV_UNKNOWN:
            op = &wp[i].obj[ WPAD_DPD_MAX_OBJECTS - 1 ] ;
            op1 = wp[i].obj ;
            break;
          case WPAD_DEV_FREESTYLE:
            op = &fp[i].obj[ WPAD_DPD_MAX_OBJECTS - 1 ] ;
            op1 = fp[i].obj ;
            break;
          case WPAD_DEV_CLASSIC:
            op = &cp[i].obj[ WPAD_DPD_MAX_OBJECTS - 1 ] ;
            op1 = cp[i].obj ;
        }
        do {
            if ( op->size == 0 ) continue ;

            //----- Convert to screen coordinates
            px = (f32)op->x * scale + ofx ;
            py = (f32)op->y * scale + ofy ;

            //----- Render objects
            draw_point( px,py, white_clr, 5 ) ;
            draw_circle( px,py, 5, black_clr, 2 ) ;
            draw_circle( px,py, 4, smoke_clr, 2 ) ;
            draw_circle( px,py, 3,  gray_clr, 2 ) ;
        } while ( --op >= op1 ) ;

        if ( ++i == KPAD_BUF_SIZE ) i = 0 ;
    }
}
*/
static void draw_sampling_rate( void )
{
    static OSTime time ;
    static u32 countDisp ;
    BOOL enabled ;
    OSTime t ;

    t = OSGetTime() ;
    if ( OSTicksToSeconds((OSTime)(t - time)) > 0 ) {
        time = t ;
        enabled = OSDisableInterrupts() ;
        countDisp = MySamplingCount ;
        MySamplingCount = 0 ;
        (void)OSRestoreInterrupts( enabled ) ;
    }

    draw_kfont_letter( 0,17+O6Y, gray_clr, "SMP.RATE" ) ;
    (void)draw_kfont_s32( 14,17+O6Y, white_clr, (s32)countDisp ) ;
}

static void draw_battery_level( void )
{
    static OSTime time ;
    static u8 level ;
    OSTime t ;
    int i ;
    f32 x ;
    f32 y ;
    
    t = OSGetTime() ;
    if ( OSTicksToSeconds((OSTime)(t - time)) > 0 ) {
        time = t ;
        WPADGetInfoAsync( 0, &wpad_info, NULL ) ;
    }

    draw_kfont_letter( 20.0f,17+O6Y, gray_clr, "BAT" ) ;
    
    x = 40.0f ;
    y = 96.0f ;
    init_draw_graphic( rmode_p->fbWidth, rmode_p->efbHeight ) ;
    for ( i = 0 ; i < 4 ; i++ ) {
        if ( i < wpad_info.battery ) {
            draw_box( x + i * 20.0f, y, 15.0f, 15.0f, cyan_clr ) ;
        } else {
            draw_box( x + i * 20.0f, y, 15.0f, 15.0f, gray_clr ) ;
        }
    }
}

static void draw_dpd_callback_status( void )
{
    u32 i;
    MyDpdCallbackStatus buf[MY_DPD_START_COUNTS_MAX];
    BOOL enabled;
    char str[32];
    s32  latestIdx;

    enabled = OSDisableInterrupts();
    for (i = 0; i < MY_DPD_START_COUNTS_MAX; i++) {
        if (MyDpdCallbackStatusBuf[i].count) {
            MyDpdCallbackStatusBuf[i].count--;
        }
        buf[i] = MyDpdCallbackStatusBuf[i];
    }
    latestIdx = MyDpdCallbackLatestIdx;
    (void)OSRestoreInterrupts( enabled );

    for (i = 0; i < MY_DPD_START_COUNTS_MAX; i++) {
        if ((s32)i == latestIdx ||
            buf[i].count) {
            sprintf(str, "DPDCB %d", buf[i].reason);
            draw_kfont_letter( (f32)(buf[i].reason == KPAD_STATE_CTRL_DPD_START
                                     ? 0.0f
                                     : 8.0f),
                               buf[i].count * 0.05f + 0.5f, white_clr, str );
        }
    }
}


/*******************************************************************************
 Drawing function
*******************************************************************************/
void draw_sample( void )
{
    s32     cx,cy ;
    f32     x1,x2, y, r ;
    u32     hold, trig, release ;
    u32     flag ;
    GXColor clr ;

    /***********************************************************************
        Pointing scale frame
    ***********************************************************************/
    init_draw_graphic( rmode_p->fbWidth, rmode_p->efbHeight ) ;
    draw_pointing_frame() ;

    /***********************************************************************
        Adjustment item
    ***********************************************************************/
    init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 15.6f, 20.8f ) ;
    cx = 0 ;
    cy = 3 ;
    draw_kfont_letter( cx,cy+O1Y, gray_clr, "POINTING") ;
    (void)draw_kfont_s32( cx+12,cy+O1Y, blue_clr, (s32)pointing_scale ) ;
    cy += 2 ;
    draw_kfont_letter( cx,cy+O1Y, gray_clr, "INTERVAL") ;
    (void)draw_kfont_f32( cx+9,cy+O1Y, peagreen_clr, obj_interval, 2 ) ;
    cy += 2 ;
    draw_kfont_letter( cx,cy+O1Y, gray_clr, "    PLAY SENS MODE") ;
    ++ cy ;
    draw_kfont_letter( cx,cy+O1Y, gray_clr, "POS" ) ;
    (void)draw_kfont_f32   ( cx+ 4,cy+O1Y,   cyan_clr,  pos_play_radius, 2 ) ;
    (void)draw_kfont_f32   ( cx+ 9,cy+O1Y,   cyan_clr,  pos_sensitivity, 2 ) ;
    (void)draw_kfont_letter( cx+14,cy+O1Y,   cyan_clr, (pos_play_mode) ? "TIGHT" : "LOOSE" ) ;
    ++ cy ;
    draw_kfont_letter( cx,cy+O1Y, gray_clr, "HOR" ) ;
    (void)draw_kfont_f32   ( cx+ 4,cy+O1Y, yellow_clr, hori_play_radius, 2 ) ;
    (void)draw_kfont_f32   ( cx+ 9,cy+O1Y, yellow_clr, hori_sensitivity, 2 ) ;
    (void)draw_kfont_letter( cx+14,cy+O1Y, yellow_clr,(hori_play_mode) ? "TIGHT" : "LOOSE" ) ;
    ++ cy ;
    draw_kfont_letter( cx,cy+O1Y, gray_clr, "DIS" ) ;
    (void)draw_kfont_f32   ( cx+ 4,cy+O1Y,  green_clr, dist_play_radius, 2 ) ;
    (void)draw_kfont_f32   ( cx+ 9,cy+O1Y,  green_clr, dist_sensitivity, 2 ) ;
    (void)draw_kfont_letter( cx+14,cy+O1Y,  green_clr,(dist_play_mode) ? "TIGHT" : "LOOSE" ) ;
    ++ cy ;
    draw_kfont_letter( cx,cy+O1Y, gray_clr, "ACC" ) ;
    (void)draw_kfont_f32   ( cx+ 4,cy+O1Y,    red_clr,  acc_play_radius, 2 ) ;
    (void)draw_kfont_f32   ( cx+ 9,cy+O1Y,    red_clr,  acc_sensitivity, 2 ) ;
    (void)draw_kfont_letter( cx+14,cy+O1Y,    red_clr, (acc_play_mode) ? "TIGHT" : "LOOSE" ) ;
    cy += 2 ;
    draw_kfont_letter( cx,cy+O1Y, gray_clr, "    DLAY PULS") ;
    ++ cy ;
    draw_kfont_letter( cx,cy+O1Y, gray_clr, "RPT") ;
    (void)draw_kfont_f32( cx+4,cy+O1Y, orange_clr, repeat_delay_sec, 2 ) ;
    (void)draw_kfont_f32( cx+9,cy+O1Y, orange_clr, repeat_pulse_sec, 2 ) ;

    //----- Various data title
    draw_kfont_letter( 27.0f, 2.5f+O6Y, gray_clr, "ACC" ) ; // Acceleration information
    draw_kfont_letter( 27.0f, 3.5f+O6Y, gray_clr, "STICK") ; // Control Stick information
    draw_kfont_letter( 15.2f,12.5f+O6Y, gray_clr, "VALID") ; // DPD enable flag
    draw_kfont_letter( 23.0f,12.5f+O6Y, gray_clr, "DIST" ) ; // Distance information
    draw_kfont_letter( 33.8f,10.1f+O6Y, gray_clr, "OBJ"  ) ; // Object

    /***********************************************************************
        Value adjustment cursor
    ***********************************************************************/
    init_draw_graphic( rmode_p->fbWidth, rmode_p->efbHeight ) ;
    draw_cursor() ;

    /***********************************************************************
        DPD enable flag & distance information
    ***********************************************************************/
    init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 28.5f, 38.0f ) ;
    (void)draw_kfont_s32(  9.0f,7.5f+O2Y, violet_clr, kpads[0][0].dpd_valid_fg ) ;
    (void)draw_kfont_f32( 11.5f,7.5f+O2Y,  green_clr, kpads[0][0].dist, 2 ) ;

    /***********************************************************************
        Battery level
    ***********************************************************************/
    init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 14.0f, 18.8f ) ;
    draw_battery_level() ;

    /***********************************************************************
        Sampling rate
    ***********************************************************************/
    init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 14.0f, 18.8f ) ;
    draw_sampling_rate() ;

    /***********************************************************************
        Button bit information
    ***********************************************************************/
    init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 14.0f, 18.8f ) ;
    cx =  0 ;
    cy =  18 ;

    hold    = kpads[0][0].hold;
    trig    = kpads[0][0].trig;
    release = kpads[0][0].release;

    draw_kfont_letter( cx,cy+O6Y, gray_clr, "HLD") ;
    if (kpad_reads > 0) {
        draw_kfont_bit( cx+39,cy+O6Y, orange_clr, hold   , 32 ) ;
    }
    ++ cy ;
    draw_kfont_letter( cx,cy+O6Y, gray_clr, "TRG") ;
    if (kpad_reads > 0) {
        draw_kfont_bit( cx+39,cy+O6Y, orange_clr, trig   , 32 ) ;
    }
    ++ cy ;
    draw_kfont_letter( cx,cy+O6Y, gray_clr, "REL") ;
    if (kpad_reads > 0) {
        draw_kfont_bit( cx+39,cy+O6Y, orange_clr, release, 32 ) ;
    }

    if (kpad_reads > 0) {
        if ( kpads[0][0].wpad_err == WPAD_ERR_NONE 
        &&   ( kpads[0][0].dev_type == WPAD_DEV_CLASSIC 
            || kpads[0][0].dev_type == WPAD_DEV_GUITAR 
            || kpads[0][0].dev_type == WPAD_DEV_TRAIN ) ) {
            hold    = kpads[0][0].ex_status.cl.hold;
            trig    = kpads[0][0].ex_status.cl.trig;
            release = kpads[0][0].ex_status.cl.release;

            switch( kpads[0][0].dev_type ) {
              case WPAD_DEV_CLASSIC : clr = peagreen_clr ;  break ;
              case WPAD_DEV_GUITAR :  clr =     cyan_clr ;  break ;
              case WPAD_DEV_TRAIN :   clr =   yellow_clr ;  break ;
            }
            
            ++ cy ;
            draw_kfont_letter( cx,cy+O6Y, gray_clr, "HLD") ;
            draw_kfont_bit( cx+39,cy+O6Y, clr, hold   , 32 ) ;
            ++ cy ;
            draw_kfont_letter( cx,cy+O6Y, gray_clr, "TRG") ;
            draw_kfont_bit( cx+39,cy+O6Y, clr, trig   , 32 ) ;
            ++ cy ;
            draw_kfont_letter( cx,cy+O6Y, gray_clr, "REL") ;
            draw_kfont_bit( cx+39,cy+O6Y, clr, release, 32 ) ;
        }
    } else {
        switch ( kpad_err ) {
          case KPAD_READ_ERR_INIT :
          case KPAD_READ_ERR_SETUP :
            cy -= 2 ;
            draw_kfont_letter( cx+8,cy+O6Y, white_clr, "WPAD OR KPAD IS NOT INITIALIZED.") ;
            cy += 2 ;
            // break through
          case KPAD_READ_ERR_NO_CONTROLLER :
            cy -= 1;
            draw_kfont_letter( cx+8,cy+O6Y, white_clr, "  OTHER TESTS BY GC CONT. 1P") ;
            ++ cy ;
            draw_kfont_letter( cx+8,cy+O6Y, white_clr, "   A BUTTON - KPADRESET") ;
            ++ cy ;
            draw_kfont_letter( cx+8,cy+O6Y, white_clr, "   B BUTTON - SKIP KPADREAD") ;
            ++ cy ;
            draw_kfont_letter( cx+8,cy+O6Y, white_clr, "   X BUTTON - CHANGE REVISE MODE") ;
            ++ cy ;
            draw_kfont_letter( cx+8,cy+O6Y, white_clr, "   Y BUTTON - KPADINIT/KPADSHUTDOWN") ;
            ++ cy ;
            draw_kfont_letter( cx+8,cy+O6Y, white_clr, "   Z BUTTON - CHANGE BUTTON PROC MODE") ;
            break ;
        }
    }

    /***********************************************************************
        Acceleration information
    ***********************************************************************/
    init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 15.0f, 20.0f ) ;
    draw_kfont_letter( 25.0f,2.5f+O3Y, smoke_clr, "Y            Y") ;
    draw_kfont_letter( 29.0f,6.5f+O3Y, smoke_clr, "X            Z") ;

    x1 =   35.0f ;      // x-coordinate of left grid
    x2 =  210.0f ;      // x-coordinate of right grid
    y  = -100.0f+O4Y ;  // Common y-coordinate
    r  =   60.0f ;      // Common radius

    init_draw_graphic( rmode_p->fbWidth, rmode_p->efbHeight ) ;
    draw_grid( x1, y, r, smoke_clr ) ;
    draw_grid( x2, y, r, smoke_clr ) ;

    cx = kpad_reads ;
    while ( --cx >= 0 ) {
        if (kpads[0][cx].wpad_err == WPAD_ERR_NONE) {
            switch (kpads[0][cx].data_format) {
              case WPAD_FMT_CORE_ACC :
              case WPAD_FMT_CORE_ACC_DPD :
              case WPAD_FMT_CLASSIC_ACC :
              case WPAD_FMT_CLASSIC_ACC_DPD :
              case WPAD_FMT_GUITAR :
                flag = 0x01 ;
                break ;

              case WPAD_FMT_FREESTYLE_ACC :
              case WPAD_FMT_FREESTYLE_ACC_DPD :
                flag = 0x03 ;
                break ;

              default :
                flag = 0x00 ;
                break ;
            }

            if (flag & 0x01) {
                draw_acc( x1, y, r, kpads[0][cx].acc.x, -kpads[0][cx].acc.y ) ;
                draw_acc( x2, y, r, kpads[0][cx].acc.z, -kpads[0][cx].acc.y ) ;
            }
            if (flag & 0x02) {
                draw_acc2( x1, y, r, kpads[0][cx].ex_status.fs.acc.x, -kpads[0][cx].ex_status.fs.acc.y ) ;
                draw_acc2( x2, y, r, kpads[0][cx].ex_status.fs.acc.z, -kpads[0][cx].ex_status.fs.acc.y ) ;
            }
        }
    }

    /***********************************************************************
        Control Stick information
    ***********************************************************************/
    init_draw_graphic( rmode_p->fbWidth, rmode_p->efbHeight ) ;
    cx = kpad_reads ;
    while ( --cx >= 0 ) {
        if ( kpads[0][cx].wpad_err == WPAD_ERR_NONE ) {
            if( kpads[0][cx].dev_type == WPAD_DEV_FREESTYLE ) {
                draw_stick( x1, y, r, kpads[0][cx].ex_status.fs.stick.x, -kpads[0][cx].ex_status.fs.stick.y, yellow_clr, TRUE ) ;
            } else if ( kpads[0][cx].dev_type == WPAD_DEV_CLASSIC 
                     || kpads[0][cx].dev_type == WPAD_DEV_GUITAR 
                     || kpads[0][cx].dev_type == WPAD_DEV_TRAIN ) {
                     
                switch ( kpads[0][cx].dev_type ) {
                  case WPAD_DEV_CLASSIC :  clr =  green_clr ;  break ;
                  case WPAD_DEV_GUITAR :   clr =   blue_clr ;  break ;
                  case WPAD_DEV_TRAIN :    clr =   cyan_clr ;  break ;
                }
                draw_stick( x1, y, r, kpads[0][cx].ex_status.cl.lstick.x, -kpads[0][cx].ex_status.cl.lstick.y, clr, FALSE ) ;
                draw_stick( x2, y, r, kpads[0][cx].ex_status.cl.rstick.x, -kpads[0][cx].ex_status.cl.rstick.y, clr, FALSE ) ;
            }
        }
    }

    /***********************************************************************
        Trigger information
    ***********************************************************************/
    x1 = 245.0f ;
    x2 = 275.0f ;
    y  = 215.0f ;

    cx = kpad_reads ;
    while ( --cx >= 0 ) {
        if ( kpads[0][cx].dev_type == WPAD_DEV_CLASSIC 
            || kpads[0][cx].dev_type == WPAD_DEV_GUITAR 
            || kpads[0][cx].dev_type == WPAD_DEV_TRAIN ) {

            init_draw_graphic( rmode_p->fbWidth, rmode_p->efbHeight ) ;
            
            draw_box( x1, y, 20.0f, 55.0f, smoke_clr ) ;
            draw_box( x2, y, 20.0f, 55.0f, smoke_clr ) ;

            if ( kpads[0][cx].wpad_err == WPAD_ERR_NONE ) {
                draw_box( x1, y, 20.0f, 55.0f * kpads[0][cx].ex_status.cl.ltrigger, yellow_clr ) ;
                draw_box( x2, y, 20.0f, 55.0f * kpads[0][cx].ex_status.cl.rtrigger, yellow_clr ) ;
            }

            init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 15.6f, 20.8f ) ;
            draw_kfont_letter( 36.2f,17.8f+O6Y, gray_clr, "TRIG"  ) ; // Trigger
        }
    }

    /***********************************************************************
        DPD object display
    ***********************************************************************/
    init_draw_graphic( rmode_p->fbWidth, rmode_p->efbHeight ) ;
    draw_dpd_frame() ;
    draw_object() ;
//  draw_object_by_kpad1_style() ;

    /***********************************************************************
        Pointing cursor
    ***********************************************************************/
    init_draw_graphic( rmode_p->fbWidth, rmode_p->efbHeight ) ;
    cx = kpad_reads ;
    while ( --cx >= 0 ) {
        draw_position( &kpads[0][cx] ) ;
    }

    /***********************************************************************
        Calibration message
    ***********************************************************************/
    init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 60.0f, 80.0f ) ;
    if ( calib_disp_ct < 0 ) {
        draw_kfont_letter( 0.5f,1.4f, magenta_clr, "  ERROR  ") ; // Fail message
    } else if ( calib_disp_ct ) {
        draw_kfont_letter( 0.5f,1.4f, magenta_clr, "CALIBRATE") ; // Success message
    }

    /***********************************************************************
        KPAD DPD callback
    ***********************************************************************/
    init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 15.0f, 20.0f ) ;
    draw_dpd_callback_status() ;

    /***********************************************************************
        Display for clamp processing and sensor bar position correction
    ***********************************************************************/
    init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 15.0f, 20.0f ) ;
    draw_kfont_letter( 20.0f,0.5f, white_clr, aim_mode ? "AIM ON" : "AIM OFF");
    draw_kfont_letter( 30.0f,0.5f, white_clr, stick_clamp_mode ? "CROSS CLAMP" : "CIRCLE CLAMP");
}