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

  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/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 )	// Move cursor
#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 values
#define CMD_DEC		( (kpads[0][0].trig & KPAD_BUTTON_B    ) != 0 )

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


/***************************************************************
	Adjustable items
***************************************************************/
static f32	pointing_scale = 200.0f ;	// On-screen pointing scale

static f32	obj_interval     = 0.20f ;	// Interval between light-emitting points on the TV (in meters)

static f32	pos_play_radius  = 0.00f ;	// Set pos sensitivity
static f32	pos_sensitivity  = 1.00f ;
static f32	hori_play_radius = 0.00f ;	// Set horizon sensitivity
static f32	hori_sensitivity = 1.00f ;
static f32	dist_play_radius = 0.00f ;	// Set dist sensitivity
static f32	dist_sensitivity = 1.00f ;
static f32	acc_play_radius  = 0.00f ;	// Set acc sensitivity
static f32	acc_sensitivity  = 1.00f ;

static f32	repeat_delay_sec = 0.75f ;	// Set key repeat
static f32	repeat_pulse_sec = 0.25f ;

#define EDIT_DATA_MAX	12	// Number of edited data items

typedef struct {
	f32	*vp ;	// Pointer to variable
	f32	min ;	// Minimum value of variable
	f32	max ;	// Maximum value of variable
	u8	y ;	// Y cursor position
	u8	x[3] ;	// X cursor position of each of the digits
	f32	v[3] ;	// Amount of change in value of each of the digits
} 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 ;	// Cursor position for adjusting values
static s32	csr_i, csr_k ;	// Data instance being edited and digit position
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 ;


/*******************************************************************************
	Initialization function
*******************************************************************************/
void init_sample( void )
{
	//----- Initial settings for controller
	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 ) ;

	//----- Initialize cursor position
	csr_i = 0 ;		// Data instance being edited
	csr_k = 2 ;		// Position of digit being edited
	csr_x = edit_data[ csr_i ].x[ csr_k ] ;
	csr_y = edit_data[ csr_i ].y ;

	//----- Initialize the calibration message count
	calib_disp_ct = 0 ;	// Minus: Failed display, 0: No display, Plus: Successful display
}


/*******************************************************************************
	Processing for moving the cursor
*******************************************************************************/
static void move_cursor( void )
{
	EditData	*ep ;
	s32		vx,vy, wx,wy, fy ;
	s32		i,w,k, im,wm,km ;


	/***********************************************************************
		Processing for moving the cursor
	***********************************************************************/
	//----- Temporary motion 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 closest digit position in edit data
	wm = 1 << 30 ;		// Maximum distance value
	i = EDIT_DATA_MAX - 1 ;
	do {
		ep = &edit_data[i] ;

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

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

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

	//----- Move cursor if candidate has been 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 ;
	}
}


/*******************************************************************************
	Adjust values
*******************************************************************************/
static void push_cursor( void )
{
	EditData	*ep = &edit_data[ csr_i ] ;


	//----- Has a button been pushed?
	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 change in numeric values
	}

	//----- Reset because numeric values have 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 processing function
*******************************************************************************/
void work_sample( void )
{
	/***********************************************************************
		Calibrate
	***********************************************************************/
	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 ;	// Succeeded
		} else {
			calib_disp_ct = -180 ;	// Failed
		}
	}

	/***********************************************************************
		Processing for editing numeric values
	***********************************************************************/
	move_cursor() ;		// Processing for moving the cursor
	push_cursor() ;		// Adjust values
}


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


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


	//------ Cross
	pos.x = pointing_scale * sp->pos.x ;	// Coordinate of center
	pos.y = pointing_scale * sp->pos.y ;

	scale = 72.0f ;
	vec.x = scale * sp->horizon.x ;		// Right-facing 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 ;	// Downward-facing 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 ) ;
}


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


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


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


/*******************************************************************************
	Draw cursor for adjusting numeric values
*******************************************************************************/
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 ) ;
}


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


/*******************************************************************************
	Draw 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_CORE &&
	     kpads[0][0].dev_type != WPAD_DEV_FREESTYLE &&
	     kpads[0][0].dev_type != WPAD_DEV_CLASSIC) {
		return ;
	}

	//----- Calculate 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 200 Hz
	switch ( kpads[0][0].dev_type ) {
	    case WPAD_DEV_CORE:
            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 rate

	i -= n - 1 ;				// Draw beginning from oldest data
	if ( i < 0 ) i += KPAD_RING_BUFS ;

	while ( --n >= 0 ) {
	    switch( kpads[0][0].dev_type ) {
	        case WPAD_DEV_CORE:
                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 ;

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

	/***********************************************************************
		Adjustable items
	***********************************************************************/
	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 titles
	draw_kfont_letter( 26.0f, 3.0f, gray_clr, "ACC"  ) ;	// Acceleration data
	draw_kfont_letter( 18.7f,10.3f, gray_clr, "VALID") ;	// DPD enable flag
	draw_kfont_letter( 19.2f,13.4f, gray_clr, "DIST" ) ;	// Distance data
	draw_kfont_letter( 30.8f,10.1f, gray_clr, "OBJ"  ) ;	// Object

	/***********************************************************************
		Cursor for adjusting numeric values
	***********************************************************************/
	init_draw_graphic( rmode_p->fbWidth, rmode_p->efbHeight ) ;
	draw_cursor() ;

	/***********************************************************************
		DPD enable flag & distance data
	***********************************************************************/
	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 data
	***********************************************************************/
	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 data
	***********************************************************************/
	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 ;	// Shared Y coordinate
	r  =   60.0f ;	// Shared 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 ) ;
		
    	}
	}
    

	/***********************************************************************
		Display DPD object
	***********************************************************************/
	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 messages
	***********************************************************************/
	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  ") ;	// Message in case of failure
	} else if ( calib_disp_ct ) {
		draw_kfont_letter( 0.5f,1.4f, magenta_clr, "CALIBRATE") ;	// Message in case of success
	}
}