'$INCLUDE: 'QB.BI'

DECLARE SUB centerPrint (msg AS STRING)
DECLARE SUB drawCrosshair (x AS INTEGER, y AS INTEGER)
DECLARE SUB rangeSet (index AS INTEGER, dist AS SINGLE)
DECLARE SUB drawRange ()
DECLARE SUB rangeReset ()
DECLARE SUB rangeUpdate (x AS INTEGER, y AS INTEGER)
DECLARE FUNCTION ATN2! (x AS SINGLE, y AS SINGLE)
DECLARE SUB getJoy (x AS INTEGER, y AS INTEGER, z AS INTEGER, t AS INTEGER, b AS INTEGER)
DECLARE SUB drawArea ()

CONST areaX = 128
CONST areaY = 128

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, b AS INTEGER
DIM z AS INTEGER, t AS INTEGER
DIM msg AS STRING

SCREEN 13
rangeReset
DO
	getJoy x, y, z, t, b
	rangeUpdate x, y

	LOCATE 1: PRINT SPACE$(80): LOCATE 1
	centerPrint "Coords:" + STR$(x) + "," + STR$(y)
	centerPrint "Trigger:" + STR$(t)
	msg = "Buttons:"
	FOR i% = 0 TO 3
		msg = msg + STR$(SGN(b AND 2 ^ i%))
	NEXT i%
	centerPrint msg

	drawArea
	drawCrosshair x, y
	drawRange
LOOP UNTIL LEN(INKEY$)

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 , (40 - LEN(msg)) \ 2: PRINT msg
END SUB

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

	x = (319 - areaX) \ 2: y = (199 - areaY) \ 2
	w = areaX \ 2: h = areaY \ 2
	LINE (x - 1, y - 1)-STEP(areaX + 1, areaY + 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

SUB getJoy (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

SUB rangeReset
	FOR i% = 0 TO UBOUND(range)
		rangeSet i%, 1
	NEXT i%
END SUB

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

SUB rangeUpdate (x AS INTEGER, y AS INTEGER)
	DIM a AS SINGLE, aR AS SINGLE
	DIM d AS SINGLE, bestD AS SINGLE
	DIM bestR AS INTEGER

	' get closest node in that direction
	IF ((x = 0) AND (y = 0)) THEN EXIT SUB
	a = ATN2!(CSNG(x), CSNG(y))
	bestR = -1
	bestD = 32767
	FOR i% = 0 TO UBOUND(range)
		aR = (i% / (UBOUND(range) + 1) * 360) * DEGTORAD!
		d = ABS(aR - a)
		IF (d < bestD) THEN
			bestD = d
			bestR = i%
		END IF
	NEXT i%

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