'$INCLUDE: 'QB.BI'

DECLARE SUB drawCrosshair (x AS INTEGER, y AS INTEGER)
DECLARE SUB drawRange ()
DECLARE SUB drawArea ()

DECLARE SUB rangeSet (index AS INTEGER, dist AS SINGLE)
DECLARE SUB rangeReset ()
DECLARE SUB rangeUpdate (x AS INTEGER, y AS INTEGER)
DECLARE FUNCTION rangeNearest% (x AS INTEGER, y AS INTEGER)

DECLARE SUB deadzoneUpdate (x AS INTEGER, y AS INTEGER)

DECLARE SUB joyGet (x AS INTEGER, y AS INTEGER, z AS INTEGER, t AS INTEGER, b AS INTEGER)
DECLARE SUB joyFilter (x AS INTEGER, y AS INTEGER, xScale AS SINGLE, yScale AS SINGLE)

DECLARE SUB centerprint (msg AS STRING)
DECLARE FUNCTION ATN2! (x AS SINGLE, y AS SINGLE)

CONST PI = 3.141593
CONST PI2 = 6.283185
CONST DEGTORAD! = .017453293#

TYPE rngStruct
	x AS INTEGER   ' on-screen X
	y AS INTEGER   ' on-screen Y
	d AS SINGLE    ' distance average
	trs1 AS SINGLE ' distance 1
	trs2 AS SINGLE ' distance 2
	trs3 AS SINGLE ' distance 3
	trs4 AS SINGLE ' distance 4
END TYPE

REDIM SHARED range(0 TO 15) AS rngStruct

DIM x AS INTEGER, y AS INTEGER    ' stick X, Y
DIM b AS INTEGER                  ' pressed buttons
DIM z AS INTEGER, t AS INTEGER    ' second stick Y axis and triggers
DIM x2 AS SINGLE, y2 AS SINGLE    ' scaled X, Y

DIM joyPrevB AS INTEGER           ' previously pressed buttons
DIM SHARED joyReach AS SINGLE     ' minimal distance from center
DIM SHARED joyDeadzone AS SINGLE  ' maximal distance in deadzone

joyGet x, y, z, t, b
IF ((x = -128) AND (y = -128)) THEN
	PRINT "No joystick found."
	END
END IF

SCREEN 13

' set range and deadzone
rangeReset
DO
	joyPrevB = b
	joyGet x, y, z, t, b
	LOCATE 1

	SELECT CASE phase
		' get range (area that can be easily covered)
		CASE 0
			centerprint "Phrase 1: range"
			centerprint "Spin the analog stick around to"
			centerprint "cover as much area as possible."
			rangeUpdate x, y
			IF (b <> 0) AND (joyPrevB = 0) THEN
				' get minimal range rechable
				joyReach = range(0).d
				FOR i% = 1 TO UBOUND(range)
					IF (range(i%).d < joyReach) THEN joyReach = range(i%).d
				NEXT i%
				joyReach = joyReach * 2
				phase = phase + 1
				rangeReset
			END IF

		CASE 1
			centerprint "Phrase 2: deadzone"
			centerprint "Nudge and slowly release the analog"
			centerprint "stick near the center to find the"
			centerprint "deadzone, which will be ignored."
			deadzoneUpdate x, y
			IF (b <> 0) AND (joyPrevB = 0) THEN
				' get deadzone max range
				joyDeadzone = range(0).d
				FOR i% = 1 TO UBOUND(range)
					IF (range(i%).d > joyDeadzone) THEN joyDeadzone = range(i%).d
				NEXT i%
				joyDeadzone = joyDeadzone * 2
				EXIT DO
			END IF
	END SELECT
	LOCATE 23
	centerprint "Press any button when you're done."

	drawArea
	drawCrosshair x, y
	drawRange
LOOP UNTIL (INKEY$ = CHR$(27))

' These are the values I got with one of my XBox 360 controllers.
' Ideally, you should store such results in a configuration file,
' so you don't need to go through the calibration process each
' and every time the program is loaded.
'   joyReach = 123.1863
'   joyDeadzone = 23.34524
CLS
DO
	joyGet x, y, z, t, b
	joyFilter x, y, x2, y2
	LOCATE 1, 1
	PRINT LTRIM$(STR$(x)) + ", " + STR$(y) + "      "
	FOR i% = 0 TO 3
		PRINT SGN(b AND 2 ^ i%);
	NEXT i%

	drawArea
	drawCrosshair x, y
LOOP UNTIL (INKEY$ = CHR$(27))

FUNCTION ATN2! (x AS SINGLE, y AS SINGLE)
	DIM v AS SINGLE

	SELECT CASE x
		CASE IS > 0
			v = ATN(y / x)
		CASE IS < 0
			IF (y >= 0) THEN
				v = PI + ATN(y / x)
			ELSE
				v = -PI + ATN(y / x)
			END IF
		CASE ELSE
			IF (y > 0) THEN
				v = PI / 2
			ELSEIF (y < 0) THEN
				v = -PI / 2
			ELSE
				' Throw an error if both {x} and {y} are 0 (undefined angle)
			END IF
	END SELECT
	IF (v < 0) THEN v = v + PI2

	ATN2! = v
END FUNCTION

SUB centerprint (msg AS STRING)
	msg = LTRIM$(RTRIM$(msg))
	LOCATE , (42 - LEN(msg)) \ 2: PRINT msg
END SUB

''
'' update nodes for deadzone (the area around the center)
''
SUB deadzoneUpdate (x AS INTEGER, y AS INTEGER)
	DIM d AS SINGLE
	DIM bestR AS INTEGER

	IF ((x = 0) AND (y = 0)) THEN EXIT SUB
	bestR = rangeNearest%(x, y)

	' update distance
	d = SQR(x ^ 2 + y ^ 2) / 2
	range(bestR).trs1 = range(bestR).trs2
	range(bestR).trs2 = range(bestR).trs3
	range(bestR).trs3 = range(bestR).trs4
	range(bestR).trs4 = d
	IF (range(bestR).trs1 = range(bestR).trs2) THEN
		IF (range(bestR).trs2 = range(bestR).trs3) THEN
			IF (range(bestR).trs3 = range(bestR).trs4) THEN
				IF (range(bestR).trs4 = range(bestR).trs1) THEN
					rangeSet bestR, d
				END IF
			END IF
		END IF
	END IF
END SUB

SUB drawArea
	DIM x AS INTEGER, y AS INTEGER
	DIM w AS INTEGER, h AS INTEGER

	x = (319 - 128) \ 2: y = (199 - 128) \ 2
	w = 128 \ 2: h = 128 \ 2
	LINE (x - 1, y - 1)-STEP(128 + 1, 128 + 1), 4, B
	LINE (x, y)-STEP(w - 1, h - 1), 0, BF
	LINE (x + w, y)-STEP(w - 1, h - 1), 7, BF
	LINE (x, y + h)-STEP(w - 1, h - 1), 8, BF
	LINE (x + w, y + h)-STEP(w - 1, h - 1), 15, BF
END SUB

SUB drawCrosshair (x AS INTEGER, y AS INTEGER)
	DIM cx AS INTEGER, cY AS INTEGER
	cx = (319 + x) \ 2
	cY = (199 + y) \ 2

	LINE (cx - 7, cY)-STEP(14, 0), 4
	LINE (cx, cY - 7)-STEP(0, 14), 4
END SUB

SUB drawRange
	DIM prevX AS INTEGER, prevY AS INTEGER

	prevX = range(UBOUND(range)).x
	prevY = range(UBOUND(range)).y
	FOR i% = 0 TO UBOUND(range)
		LINE (prevX, prevY)-(range(i%).x, range(i%).y), 4
		prevX = range(i%).x
		prevY = range(i%).y
	NEXT i%
END SUB

''
'' Adjust X, Y coordinates and also returns normalized X, Y values
''
SUB joyFilter (x AS INTEGER, y AS INTEGER, xScale AS SINGLE, yScale AS SINGLE)
	DIM d AS SINGLE, a AS SINGLE

	' get distance from center
	d = SQR((x * x) + (y * y))

	' out of deadzone
	IF (d > joyDeadzone) THEN
		d = (d - joyDeadzone) / (joyReach - joyDeadzone) ' normalize distance from center
		IF (ABS(d) > 1) THEN d = SGN(d) ' cap to -1.0, 1.0
		a = ATN2!(CSNG(x), CSNG(y))     ' get angle of X,Y coordinates
		xScale = COS(a) * d             ' get scaled X (-1.0 to 1.0)
		yScale = SIN(a) * d             ' get scaled Y (-1.0 to 1.0)
		x = xScale * 127                ' get filtered X (-127 to 127)
		y = yScale * 127                ' get filtered Y (-127 to 127)

	' inside deadzone
	ELSE
		x = 0: y = 0                    ' cancel X,Y
		xScale = 0: yScale = 0          ' cancel scaled X,Y
	END IF
END SUB

SUB joyGet (x AS INTEGER, y AS INTEGER, z AS INTEGER, t AS INTEGER, b AS INTEGER)
	DIM reg AS RegTypeX

	' get buttons (bitflags)
	reg.ax = &H8400
	reg.dx = 0
	CALL INTERRUPTX(&H15, reg, reg)
	b = &HF XOR (((reg.ax AND &HFF) \ 16) AND &HF)

	' get sticks
	reg.ax = &H8400
	reg.dx = 1
	CALL INTERRUPTX(&H15, reg, reg)
	x = reg.ax - 128
	y = reg.bx - 128
	t = reg.cx
	z = reg.dx
END SUB

''
'' get node in that nearest direction
''
FUNCTION rangeNearest% (x AS INTEGER, y AS INTEGER)
	DIM a AS SINGLE, d AS SINGLE
	DIM bestD AS SINGLE

	a = ATN2!(CSNG(x), CSNG(y))
	rangeNearest% = -1
	bestD = 32767
	FOR i% = 0 TO UBOUND(range)
		d = ABS(((i% / (UBOUND(range) + 1) * 360) * DEGTORAD!) - a)
		IF (d < bestD) THEN
			bestD = d
			rangeNearest% = i%
		END IF
	NEXT i%
END FUNCTION

''
'' reset all nodes
''
SUB rangeReset
	FOR i% = 0 TO UBOUND(range)
		range(i%).trs2 = 0
		range(i%).trs3 = 0
		range(i%).trs4 = 0
		rangeSet i%, 0
	NEXT i%
END SUB

''
'' set specified node
''
SUB rangeSet (index AS INTEGER, dist AS SINGLE)
	DIM angle AS SINGLE

	angle = (index / (UBOUND(range) + 1) * 360) * DEGTORAD!
	range(index).trs1 = range(index).trs2
	range(index).trs2 = range(index).trs3
	range(index).trs3 = range(index).trs4
	range(index).trs4 = dist
	range(index).d = (range(index).trs1 + range(index).trs2 + range(index).trs3 + range(index).trs4) / 4
	range(index).x = 159 + COS(angle) * range(index).d
	range(index).y = 99 + SIN(angle) * range(index).d
END SUB

''
'' update nodes for range (how far the stick can reach)
''
SUB rangeUpdate (x AS INTEGER, y AS INTEGER)
	DIM d AS SINGLE
	DIM bestR AS INTEGER

	' get closest node in that direction
	IF ((x = 0) AND (y = 0)) THEN EXIT SUB

	bestR = rangeNearest%(x, y)

	' update distance
	d = SQR(x ^ 2 + y ^ 2) / 2
	IF (d >= range(bestR).d - 5) THEN ' provide some threshold
		rangeSet bestR, d
	END IF
END SUB