--Assumes you have an Xbox style controller

joyID = -1; --Joystick ID to use. The script will attempt to find a suitable joystick, but you can manually set this if you wish
yaw = 0.0;
pitch = 0.0;
pitchoffs = 0.0; --used to correct for the initial pitch of the camera
xpos = 0.0; --coordinates in a virtual 3D space
ypos = 0.0; --^^^
zpos = 0.0; --^^^
matAdjustment = {1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1};
movscale = 1.0;
rotscale = 1.0;
cooldown = 0;

function MatrixMult4x4(m1, m2) --sorta copied/inspired from some lua matrix library
    local mtx = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}
    for i=1,4 do
        for j=1,4 do
            local num = 0
            for n=1,4 do
                num = num + m1[(i-1)*4+n] * m2[(n-1)*4+j]
            mtx[(i-1)*4+j] = num
            end
        end
    end
    return mtx;
end

function Rotate(angle,x,y,z)
    local s = math.sin(math.rad(angle));
    local c = math.cos(math.rad(angle));
    return {(1-c)*x*x+c, (1-c)*y*x-s*z, (1-c)*z*x+s*y, 0,
            (1-c)*x*y+s*z, (1-c)*y*y+c, (1-c)*z*y-s*x, 0,
            (1-c)*x*z-s*y, (1-c)*y*z+s*x, (1-c)*z*z+c, 0,
            0, 0, 0, 1};
end

function vecrotx(angle, vx, vy, vz) --used for pitch correction in the functions below
    return vx,
            vy * math.cos(math.rad(angle)) - vz * math.sin(math.rad(angle)),
            vy * math.sin(math.rad(angle)) + vz * math.cos(math.rad(angle));
end

function MoveForward(l) --sorta based off https://www.gamedev.net/forums/topic/415144-get-forward-right-up-vectors-from-pitch-yaw-roll/
    local fX = math.sin(math.rad(yaw)) * math.cos(math.rad(pitch+pitchoffs));
    local fY = -math.sin(math.rad(pitch+pitchoffs));
    local fZ = -math.cos(math.rad(yaw)) * math.cos(math.rad(pitch+pitchoffs));
    
    fX, fY, fZ = vecrotx(pitchoffs, fX, fY, fZ);
    
    local mag = math.sqrt(fX*fX + fY*fY + fZ*fZ);
    fX = fX / mag; fY = fY / mag; fZ = fZ / mag;
    fX = fX * l; fY = fY * l; fZ = fZ * l;
    xpos = xpos + fX; ypos = ypos + fY; zpos = zpos + fZ;
end

function MoveUp(l)
    local fX = math.sin(math.rad(yaw)) * math.cos(math.rad(pitch+pitchoffs+90));
    local fY = -math.sin(math.rad(pitch+pitchoffs+90));
    local fZ = -math.cos(math.rad(yaw)) * math.cos(math.rad(pitch+pitchoffs+90));
    
    fX, fY, fZ = vecrotx(pitchoffs, fX, fY, fZ);
    
    local mag = math.sqrt(fX*fX + fY*fY + fZ*fZ);
    fX = fX / mag; fY = fY / mag; fZ = fZ / mag;
    fX = fX * l; fY = fY * l; fZ = fZ * l;
    xpos = xpos + fX; ypos = ypos + fY; zpos = zpos + fZ;
end

function MoveRight(l)
    local fX = math.sin(math.rad(yaw+90));
    local fY = 0;
    local fZ = -math.cos(math.rad(yaw+90));
    
    fX, fY, fZ = vecrotx(pitchoffs, fX, fY, fZ);
    
    local mag = math.sqrt(fX*fX + fY*fY + fZ*fZ);
    fX = fX / mag; fY = fY / mag; fZ = fZ / mag;
    fX = fX * l; fY = fY * l; fZ = fZ * l;
    xpos = xpos + fX; ypos = ypos + fY; zpos = zpos + fZ;
end

function on3d()
    emu.set3dtransform(2,matAdjustment);
end
emu.register3devent(on3d);
emu.set3dtransform(1,nil);

if joyID == -1 then
    for i = 0, 15 do
        c = controller.get(i);
        if c.x and c.y and c.z and c.u and c.r then --simple check that only looks for all the needed axes
            joyID = i;
            print("Found suitable joystick, id " .. i);
            break;
        end
    end
else
    print("Using manually set joystick, id " .. joyID);
end
if joyID == -1 then
    print("No suitable joystick found!");
    while true do
        emu.frameadvance();
    end
end

while true do
    local key = controller.get(joyID)
    
    scalemult = 1;
    if key["5"] then --Left bumper
        scalemult = 0.1;
    elseif key["6"] then --Right bumper
        scalemult = 10;
    end
    if key.up and cooldown==0 then --Dpad
        cooldown = 10;
        movscale = movscale + (0.1 * scalemult);
        print("Movement scale: " .. movscale);
    end
    if key.down and cooldown==0 then --Dpad
        cooldown = 10;
        movscale = movscale - (0.1 * scalemult);
        print("Movement scale: " .. movscale);
    end
    if key.right and cooldown==0 then --Dpad
        cooldown = 10;
        rotscale = rotscale + (0.1 * scalemult);
        print("Rotation scale: " .. rotscale);
    end
    if key.left and cooldown==0 then --Dpad
        cooldown = 10;
        rotscale = rotscale - (0.1 * scalemult);
        print("Rotation scale: " .. rotscale);
    end
    
    if(math.abs(key.x)>0.25) then --Left Stick
        MoveRight(-key.x * movscale);
    end
    if(math.abs(key.y)>0.25) then --Left Stick
        MoveForward(key.y * movscale);
    end
    if(math.abs(key.z)>0.25) then --Triggers
        MoveUp(-key.z * movscale);
    end
    if(math.abs(key.u)>0.25) then --Right Stick
        yaw = (yaw + (key.u * rotscale)) % 360;
    end
    if(math.abs(key.r)>0.25) then --Right Stick
        pitch = (pitch + (key.r * rotscale)) % 360;
    end
    
    if key["1"] and cooldown==0 then --A button
        cooldown = 20;
        yaw = 0.0;
        pitch = 0.0;
        pitchoffs = 0.0;
        xpos = 0.0;
        ypos = 0.0;
        zpos = 0.0;
        movscale = 1.0;
        rotscale = 1.0;
        print("Reset everything");
    end
    if key["2"] and cooldown==0 then --B button
        cooldown = 20;
        yaw = 0.0;
        pitch = 0.0;
        pitchoffs = 0.0;
        print("Reset rotations");
    end
    if key["3"] then --X button
        pitchoffs = -pitch;
    end
    if key["4"] and cooldown==0 then --Y button
        cooldown = 20;
        movscale = 1.0;
        rotscale = 1.0;
        print("Movement/rotation scales reset");
    end
    
    local tmp = Rotate(-yaw, 0.0, math.cos(math.rad(pitchoffs)), math.sin(math.rad(pitchoffs)));
    tmp = MatrixMult4x4(tmp, Rotate(-pitch, 1.0, 0.0, 0.0));
    local translation = {1,0,0,0,0,1,0,0,0,0,1,0,xpos,ypos,zpos,1};
    matAdjustment = MatrixMult4x4(translation, tmp);
    
    if cooldown>0 then cooldown=cooldown-1 end
    emu.frameadvance();
end