/*---------------------------------------------------------------------------*
  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	<revolution/kpadOld.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 )

/***************************************************************
	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	hori_play_radius = 0.00f ;	// 'horizon' sensitivity setting
static f32	hori_sensitivity = 1.00f ;
static f32	dist_play_radius = 0.00f ;	// 'dist' sensitivity setting
static f32	dist_sensitivity = 1.00f ;
static f32	acc_play_radius  = 0.00f ;	// 'acc' sensitivity setting
static f32	acc_sensitivity  = 1.00f ;

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

#define EDIT_DATA_MAX	12	// 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 of 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, {12,13,14}, {100.0f, 10.0f, 1.0f} },
	{ &    obj_interval, 0.00f,9.00f,  5, {11,13,14}, {1.00f, 0.10f, 0.01f} },
	{ & pos_play_radius, 0.00f,9.00f,  8, { 6, 8, 9}, {1.00f, 0.10f, 0.01f} },
	{ & pos_sensitivity, 0.00f,1.00f,  8, {11,13,14}, {1.00f, 0.10f, 0.01f} },
	{ &hori_play_radius, 0.00f,9.00f,  9, { 6, 8, 9}, {1.00f, 0.10f, 0.01f} },
	{ &hori_sensitivity, 0.00f,1.00f,  9, {11,13,14}, {1.00f, 0.10f, 0.01f} },
	{ &dist_play_radius, 0.00f,9.00f, 10, { 6, 8, 9}, {1.00f, 0.10f, 0.01f} },
	{ &dist_sensitivity, 0.00f,1.00f, 10, {11,13,14}, {1.00f, 0.10f, 0.01f} },
	{ & acc_play_radius, 0.00f,9.00f, 11, { 6, 8, 9}, {1.00f, 0.10f, 0.01f} },
	{ & acc_sensitivity, 0.00f,1.00f, 11, {11,13,14}, {1.00f, 0.10f, 0.01f} },
	{ &repeat_delay_sec, 0.00f,9.00f, 14, { 6, 8, 9}, {1.00f, 0.10f, 0.01f} },
	{ &repeat_pulse_sec, 0.00f,9.00f, 14, {11,13,14}, {1.00f, 0.10f, 0.01f} },
} ;


/***************************************************************
	Variables
***************************************************************/
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    = 165.0f ;	// DPD object display position
static f32	dpd_disp_oy    =  65.0f ;

static BOOL aim_mode;

/*******************************************************************************
	Initialization Functions
*******************************************************************************/
void init_sample( void )
{
	//----- Controller initialization setting
	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 = 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 ;
	}
}


/*******************************************************************************
	Increase/decrease value process
*******************************************************************************/
static void push_cursor( void )
{
	EditData	*ep = &edit_data[ csr_i ] ;


	//----- Is 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 >= 10 ) {
		KPADSetBtnRepeat( 0, repeat_delay_sec, repeat_pulse_sec ) ;
	} else if ( csr_i >= 8 ) {
		KPADSetAccParam ( 0,  acc_play_radius,  acc_sensitivity ) ;
	} else if ( csr_i >= 6 ) {
		KPADSetDistParam( 0, dist_play_radius, dist_sensitivity ) ;
	} else if ( csr_i >= 4 ) {
		KPADSetHoriParam( 0, hori_play_radius, hori_sensitivity ) ;
	} else if ( csr_i >= 2 ) {
		KPADSetPosParam ( 0,  pos_play_radius,  pos_sensitivity ) ;
	} else if ( csr_i == 1 ) {
		KPADSetObjInterval( obj_interval ) ;
	}
}


/*******************************************************************************
	CPU process function
*******************************************************************************/
void work_sample( void )
{
	/***********************************************************************
		Calibration
	***********************************************************************/
	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 );
        }
    }

	/***********************************************************************
		Value edit process
	***********************************************************************/
	move_cursor() ;		// Cursor movement process
	push_cursor() ;		// Increase/decrease value process
}


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


	//------ +Control Pad
	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 ;	// Cross 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 ) ;
}


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( sx,sy, white_clr, 4 ) ;
}


/*******************************************************************************
	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 ) ;
	x2 = calc_kfont_x2( csr_x ) - 1.0f ;
	y2 = calc_kfont_y2( csr_y ) - 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 )
{
	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 ;
	}

	//----- 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
	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 starting from older items
	if ( i < 0 ) i += KPAD_RING_BUFS ;

	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 ;

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

		if ( ++i == KPAD_RING_BUFS ) i = 0 ;
	}
}


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


	/***********************************************************************
		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 = 2 ;
	cy = 3 ;
	draw_kfont_letter( cx,cy, gray_clr, "POINTING") ;
	(void)draw_kfont_s32( cx+12,cy, blue_clr, (s32)pointing_scale ) ;
	cy += 2 ;
	draw_kfont_letter( cx,cy, gray_clr, "INTERVAL") ;
	(void)draw_kfont_f32( cx+9,cy, peagreen_clr, obj_interval, 2 ) ;
	cy += 2 ;
	draw_kfont_letter( cx,cy, gray_clr, "    PLAY SENS") ;
	++ cy ;
	draw_kfont_letter( cx,cy, gray_clr, "POS" ) ;
	(void)draw_kfont_f32( cx+4,cy,   cyan_clr,  pos_play_radius, 2 ) ;
	(void)draw_kfont_f32( cx+9,cy,   cyan_clr,  pos_sensitivity, 2 ) ;
	++ cy ;
	draw_kfont_letter( cx,cy, gray_clr, "HOR" ) ;
	(void)draw_kfont_f32( cx+4,cy, yellow_clr, hori_play_radius, 2 ) ;
	(void)draw_kfont_f32( cx+9,cy, yellow_clr, hori_sensitivity, 2 ) ;
	++ cy ;
	draw_kfont_letter( cx,cy, gray_clr, "DIS" ) ;
	(void)draw_kfont_f32( cx+4,cy,  green_clr, dist_play_radius, 2 ) ;
	(void)draw_kfont_f32( cx+9,cy,  green_clr, dist_sensitivity, 2 ) ;
	++ cy ;
	draw_kfont_letter( cx,cy, gray_clr, "ACC" ) ;
	(void)draw_kfont_f32( cx+4,cy,    red_clr,  acc_play_radius, 2 ) ;
	(void)draw_kfont_f32( cx+9,cy,    red_clr,  acc_sensitivity, 2 ) ;
	cy += 2 ;
	draw_kfont_letter( cx,cy, gray_clr, "    DLAY PULS") ;
	++ cy ;
	draw_kfont_letter( cx,cy, gray_clr, "RPT") ;
	(void)draw_kfont_f32( cx+4,cy, orange_clr, repeat_delay_sec, 2 ) ;
	(void)draw_kfont_f32( cx+9,cy, orange_clr, repeat_pulse_sec, 2 ) ;

	//----- Various data title
	draw_kfont_letter( 26.0f, 3.0f, gray_clr, "ACC"  ) ;	// Acceleration information
	draw_kfont_letter( 18.7f,10.3f, gray_clr, "VALID") ;	// DPD enable flag
	draw_kfont_letter( 19.2f,13.4f, gray_clr, "DIST" ) ;	// Distance information
	draw_kfont_letter( 30.8f,10.1f, 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, 30.0f, 40.0f ) ;
	(void)draw_kfont_s32( 10.4f,5.8f, violet_clr, kpads[0][0].dpd_valid_fg ) ;
	(void)draw_kfont_f32(  8.8f,7.4f,  green_clr, kpads[0][0].dist, 2 ) ;

	/***********************************************************************
		Button bit information
	***********************************************************************/
	init_draw_kfont( rmode_p->fbWidth, rmode_p->efbHeight, 14.0f, 18.8f ) ;
	cx =  2 ;
	cy =  18 ;
	
    hold    = kpads[0][0].hold;
    trig    = kpads[0][0].trig;
    release = kpads[0][0].release;
	
	draw_kfont_letter( cx,cy, gray_clr, "HLD") ;
	draw_kfont_bit( cx+38,cy, orange_clr, hold   , 32 ) ;
	++ cy ;
	draw_kfont_letter( cx,cy, gray_clr, "TRG") ;
	draw_kfont_bit( cx+38,cy, orange_clr, trig   , 32 ) ;
	++ cy ;
	draw_kfont_letter( cx,cy, gray_clr, "REL") ;
	draw_kfont_bit( cx+38,cy, orange_clr, release, 32 ) ;

	if ( kpads[0][0].dev_type == WPAD_DEV_CLASSIC ) {
	    hold    = kpads[0][0].ex_status.cl.hold;
	    trig    = kpads[0][0].ex_status.cl.trig;
	    release = kpads[0][0].ex_status.cl.release;

    	++ cy ;
    	draw_kfont_letter( cx,cy, gray_clr, "HLD") ;
    	draw_kfont_bit( cx+38,cy, peagreen_clr, hold   , 32 ) ;
    	++ cy ;
    	draw_kfont_letter( cx,cy, gray_clr, "TRG") ;
    	draw_kfont_bit( cx+38,cy, peagreen_clr, trig   , 32 ) ;
    	++ cy ;
    	draw_kfont_letter( cx,cy, gray_clr, "REL") ;
    	draw_kfont_bit( cx+38,cy, peagreen_clr, release, 32 ) ;
    }

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

	x1 =   10.0f ;	// x-coordinate of left grid
	x2 =  185.0f ;	// x-coordinate of right grid
	y  = -100.0f ;	// 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 ) {
		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(kpads[0][cx].dev_type == WPAD_DEV_FREESTYLE) {
    		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 ) ;
    	}
	}

	init_draw_graphic( rmode_p->fbWidth, rmode_p->efbHeight ) ;
	cx = kpad_reads ;
	while ( --cx >= 0 ) {
		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) {
		    draw_stick( x1, y, r, kpads[0][cx].ex_status.cl.lstick.x, -kpads[0][cx].ex_status.cl.lstick.y, blue_clr,  FALSE ) ;
		    draw_stick( x1, y, r, kpads[0][cx].ex_status.cl.rstick.x, -kpads[0][cx].ex_status.cl.rstick.y, green_clr, FALSE ) ;
		
    	}
	}
    

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

	/***********************************************************************
		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
	}
}