#include "joystick.h"

#include "board.h"
#include "print.h"

#define pressures   false
#define rumble      false

Joystick::Joystick()
{
}

bool Joystick::init()
{
	pb_printf("init joystick");
	//PS2_SetInit();
#if 0
	unsigned long start = Board::get()->get_tick_count();
	while(Board::get()->get_tick_count()-start>500){
		;
	}
    error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);
	if (error == 0)
	{
		pb_printf("Teleop start");
        return true;
	}

	if (error == 1)
		pb_printf("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");
	else if (error == 2)
		pb_printf("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");
	else if (error == 3)
		pb_printf("Controller refusing to enter Pressures mode, may not support it. ");
	else
		pb_printf("Teleop err");
#endif
    return false;
}

#define JOYSTICK_TEST
void Joystick::test()
{
#ifdef JOYSTICK_TEST

	unsigned char key=PS2_DataKey();

	//有按键按下
	if(key!=0) {
		switch (key) {
			case PSB_SELECT:
				pb_printf("SELECT");
				break;
			case PSB_L3:
				pb_printf("L3");
				break;
			case PSB_R3:
				pb_printf("R3");
				break;
			case PSB_START:
				pb_printf("START");
				break;
			case PSB_PAD_UP:
				pb_printf("UP");
				break;
			case PSB_PAD_RIGHT:
				pb_printf("RIGHT");
				break;
			case PSB_PAD_DOWN:
				pb_printf("DOWN");
				break;
			case PSB_PAD_LEFT:
				pb_printf("LEFT");
				break;
			case PSB_L2:
				pb_printf("L2");
				break;
			case PSB_R2:
				pb_printf("R2");
				break;
			case PSB_L1:
				pb_printf("L1");
				break;
			case PSB_R1:
				pb_printf("R1");
				break;
			case PSB_TRIANGLE:
				pb_printf("TRIANGLE");
				break;
			case PSB_CIRCLE:
				pb_printf("CIRCLE");
				break;
			case PSB_CROSS:
				pb_printf("CROSS");
				break;
			case PSB_SQUARE:
				pb_printf("SQUARE");
				break;
			default:
				pb_printf("UNKNOWN");
				break;
		}
	}

	pb_printf("(need to switch mode) %5d %5d %5d %5d",PS2_AnologData(PSS_LX),PS2_AnologData(PSS_LY), PS2_AnologData(PSS_RX),PS2_AnologData(PSS_RY) );
#endif
}

bool Joystick::check(short& liner_x, short liner_y, short& angular_z){
	#if JOYSTICK_FOR_HOLONOMIC
		return holonomic_check(liner_x, liner_y, angular_z);
	#else
		return nonholonomic_check(liner_x, liner_y, angular_z);
	#endif
}

bool Joystick::holonomic_check(short& liner_x, short liner_y, short& angular_z){
	bool rtn = false;

	unsigned char key=PS2_DataKey();
		
	liner_y = liner_y;
	//up down left right  for liner x, y
	if (key == PSB_PAD_UP) {
	#if JOYSTICK_DEBUG_ENABLE
      	pb_printf("UP");
	#endif
		liner_x = MAX_LINER_X;
		rtn = true;
	}

    if(key == PSB_PAD_DOWN) {
	#if JOYSTICK_DEBUG_ENABLE
      	pb_printf("DOWN");
	#endif
		liner_x = -MAX_LINER_X;
		rtn = true;
    }

    if(key == PSB_PAD_RIGHT) {
	#if JOYSTICK_DEBUG_ENABLE
		pb_printf("RIGHT");
	#endif
		liner_y = MAX_LINER_Y;
		rtn = true;
    }

    if(key == PSB_PAD_LEFT) {
	#if JOYSTICK_DEBUG_ENABLE
      	pb_printf("LEFT");
	#endif
		liner_y = -MAX_LINER_Y;
		rtn = true;
    }
	
	//triangle square circle cross  for angular z
	if (key == PSB_SQUARE) {
	#if JOYSTICK_DEBUG_ENABLE
      	pb_printf("SQUARE");
	#endif
		angular_z = MAX_ANGULAR_Z;
		rtn = true;
	}

    if(key == PSB_CIRCLE) {
	#if JOYSTICK_DEBUG_ENABLE
      	pb_printf("CIRCLE");
	#endif
		angular_z = -MAX_ANGULAR_Z;
		rtn = true;
    }

	if (key == PSB_L1) { //print stick values if either is TRUE
	#if JOYSTICK_DEBUG_ENABLE
		pb_printf("Stick Values:");
		pb_printf("%d", PS2_AnologData(PSS_LY));
		pb_printf(",");
		pb_printf("%d", PS2_AnologData(PSS_LX));
		pb_printf(",");
		pb_printf("%d", PS2_AnologData(PSS_RY));
		pb_printf(",");
		pb_printf("%d", PS2_AnologData(PSS_RX));
	#endif

		if (PS2_AnologData(PSS_LX) == 255 && PS2_AnologData(PSS_LX) == 255 &&
		PS2_AnologData(PSS_LX) == 255 && PS2_AnologData(PSS_LX) == 255) {
	#if JOYSTICK_DEBUG_ENABLE
		pb_printf("switch mode for use rocker");
	#endif
		} else {
			liner_x = ((255.0/2)-PS2_AnologData(PSS_LY))/(255.0/2)*MAX_LINER_X;
			liner_y = ((255.0/2)-PS2_AnologData(PSS_LX))/(255.0/2)*MAX_LINER_Y;
			angular_z = ((255.0/2)-PS2_AnologData(PSS_RX))/(255.0/2)*MAX_ANGULAR_Z;
			rtn = true;
		}
	}

	return rtn;
}

bool Joystick::nonholonomic_check(short& liner_x, short liner_y, short& angular_z){
	
	liner_y = 0;
	
	bool rtn = false;

	unsigned char key=PS2_DataKey();
		
	liner_y = liner_y;
	//will be TRUE if button was JUST pressed
	if (key == PSB_PAD_UP) {
	#if JOYSTICK_DEBUG_ENABLE
      	pb_printf("Up held this hard: ");
	#endif
		liner_x = MAX_LINER_X;
		rtn = true;
	}

    if(key == PSB_PAD_RIGHT) {
	#if JOYSTICK_DEBUG_ENABLE
		pb_printf("Right held this hard: ");
	#endif
		angular_z = MAX_ANGULAR_Z;
		rtn = true;
    }

    if(key == PSB_PAD_LEFT) {
	#if JOYSTICK_DEBUG_ENABLE
      	pb_printf("LEFT held this hard: ");
	#endif
		angular_z = -MAX_ANGULAR_Z;
		rtn = true;
    }

    if(key == PSB_PAD_DOWN) {
	#if JOYSTICK_DEBUG_ENABLE
      	pb_printf("DOWN held this hard: ");
	#endif
		liner_x = -MAX_LINER_X;
		rtn = true;
    }

	if (key == PSB_L1) { //print stick values if either is TRUE
	#if JOYSTICK_DEBUG_ENABLE
		pb_printf("Stick Values:");
		pb_printf("%d", PS2_AnologData(PSS_LY));
		pb_printf(",");
		pb_printf("%d", PS2_AnologData(PSS_LX));
		pb_printf(",");
		pb_printf("%d", PS2_AnologData(PSS_RY));
		pb_printf(",");
		pb_printf("%d", PS2_AnologData(PSS_RX));
	#endif

		if (PS2_AnologData(PSS_LX) == 255 && PS2_AnologData(PSS_LX) == 255 &&
		PS2_AnologData(PSS_LX) == 255 && PS2_AnologData(PSS_LX) == 255) {
	#if JOYSTICK_DEBUG_ENABLE
		pb_printf("switch mode for use rocker");
	#endif
		} else {
			liner_x = ((255.0/2)-PS2_AnologData(PSS_LY))/(255.0/2)*MAX_LINER_X;
			angular_z = ((255.0/2)-PS2_AnologData(PSS_LX))/(255.0/2)*MAX_ANGULAR_Z;
			rtn = true;
		}
	}

	return rtn;
}