/********************
	pccont.c
 ********************/

#include	"ot_header.h"


/*******************************************************
	VARIABLE
*******************************************************/
OSContStatus		pad_status[MAXCONTROLLERS] ;
OSContPad		pad_data[MAXCONTROLLERS] ;
Controller		pad[MAXCONTROLLERS] ;
int			pad_no[MAXCONTROLLERS] ;
uchar			pad_pattern ;
int			nofpad ;

MotorPack		motor[MAXCONTROLLERS] ;


/*******************************************************************************
	CONTROLLER PROCESS
*******************************************************************************/
void ControllerProcess( void *arg )
{
	OSMesg		mesg ;
	int		i, n ;
	uchar		fg ;
	ushort		us ;
	Controller	*cp ;
	OSContPad	*dp ;
	MotorPack	*mp ;


	/***********************************************************************
		INITIATE CONTROLLER
	***********************************************************************/
 	osContInit( &MesgQ_PAD, &pad_pattern, &pad_status[0] ) ;

	#if  WINDOW_PRINT_SW
	WINDOW_PRINT("pattern 0x%X\n", pad_pattern ) ;
	#endif

	fg = 1 ;
	for ( i = n = 0; i < MAXCONTROLLERS; ++ i ) {

		motor[i].request_sw = motor[i].insert_sw = motor[i].move_sw = motor[i].reset_sw = 0 ;
		pad_no[i] = 0 ;

		if ( pad_pattern & fg ) {
			pad_no[n++] = i ;
			if ( osMotorInit( &MesgQ_PAD, &motor[i].pfs, i ) == 0 ) {
				osMotorStop( &motor[i].pfs ) ;
				#if WINDOW_PRINT_SW
				WINDOW_PRINT( "INSERT VIB No.%d\n", i ) ;
				#endif
				motor[i].insert_sw = 1 ;
			}
		}

		#if  WINDOW_PRINT_SW
		WINDOW_PRINT("controller:%d  error:0x%X  type:0x%X  status:0x%X\n",
		i, pad_status[i].errno, pad_status[i].type, pad_status[i].status ) ;
		#endif

		fg <<= 1 ;
	}
	nofpad = n ;


	/***********************************************************************
		LOOP CONTROLLER
	***********************************************************************/
	while (1) {
		osRecvMesg( &MesgQ_X2C_etc, &mesg, OS_MESG_BLOCK ) ;

		if ( mesg == MESG_X2C_TIMING ) {

			for ( i = 0, fg = 1 ; i < MAXCONTROLLERS ; ++ i, fg <<= 1 ) {
				if ( ( pad_pattern & fg ) == 0 )  continue ;

				mp = &motor[i] ;

				if ( mp->reset_sw ) {
					if ( osMotorInit( &MesgQ_PAD, &mp->pfs, i ) == 0 ) {
						osMotorStop( &mp->pfs ) ;
						#if WINDOW_PRINT_SW
						WINDOW_PRINT( "INSERT VIB No.%d\n", i ) ;
						#endif
						mp->insert_sw = 1 ;
						mp->move_sw = 0 ;
						mp->count = 0 ;
						mp->count_add = 0 ;
					}
					mp->reset_sw = 0 ;
				} else if ( mp->request_sw == 0 || prenmi_sw != 0 ) {
					if ( mp->insert_sw == 1 ) {
						if ( mp->move_sw == 1 || ( frame_count & 63 ) == 0) {
							if ( osMotorStop( &mp->pfs ) == 0 ) {
								mp->move_sw = 0 ;
							} else {
								mp->insert_sw = 0 ;
							}
						}
					}
				} else {
					if ( mp->insert_sw == 1 ) {
						mp->count += mp->count_add ;
						if ( ( mp->count_time -= mp->count_add_minus ) < 0 ) {
							mp->count_time = mp->count_add = 0 ;
						}
						if ( mp->count > 255 ) {
							mp->count -= 256 ;
							if ( mp->move_sw == 0 ) {
								if ( osMotorStart( &mp->pfs ) == 0 ) {
									mp->move_sw = 1 ;
								} else {
									mp->insert_sw = 0 ;
								}
							}
						} else if ( mp->move_sw == 1 ) {
							if ( osMotorStop( &mp->pfs ) == 0 ) {
								mp->move_sw = 0 ;
							} else {
								mp->insert_sw = 0 ;
							}
						}
					} else if ( ( frame_count & 63 ) == 0 ) {
						mp->reset_sw = 1 ;
					}
				}
			}

		} else if ( mesg == MESG_X2C_CONTREAD ) {

			osContStartReadData( &MesgQ_PAD ) ;
			osRecvMesg( &MesgQ_PAD, &mesg, OS_MESG_BLOCK ) ;
			osContGetReadData( &pad_data[0] ) ;

			for ( i = 0, fg = 1 ; i < MAXCONTROLLERS ; ++ i, fg <<= 1 ) {
				if ( ( pad_pattern & fg ) == 0 )  continue ;

				cp = &pad[i] ;
				dp = &pad_data[i] ;

				cp->old  = cp->hold   ;
				cp->hold = dp->button ;

				us          = pad[i].old ^ cp->hold ;
				cp->trig    = us         & cp->hold ;
				cp->release = us         & cp->old  ;

				cp->x = dp->stick_x ;
				cp->y = dp->stick_y ;
			}

			osSendMesg( &MesgQ_C2G_contdone, MESG_C2G_CONTDONE, OS_MESG_NOBLOCK ) ;

		} else {
			n = (int)mesg >> 16 ;
			i = (int)mesg & 255 ;
			if ( i > motor[n].count_add ) {
				motor[n].count_add = motor[n].count_time = i ;
				motor[n].count_add_minus = (int)mesg >> 8 & 255 ;
			}
		}
	}
}