-- Controls an elevator on a prismatic joint with two buttons


-- States
IDLE          = 1  -- Stationary and awaiting input
NUMPAD        = 2  -- Floor number is being entered into numpad
BUTTON_DOWN   = 3  -- Button has been pressed down
DOORS_CLOSING = 4  -- Doors closing
ACCELERATING  = 5  -- Moving up or down
DECELERATING  = 6  -- Slowing down, approaching destination
DOORS_OPENING = 7  -- Doors opening

-- Door commands
OPEN  = 1
CLOSE = 2


elevatorId = GetStringParam("id", "NO_ID_SET")

-- Constants
MAX_SPEED = 6          -- Meters/second.
ACCEL_TIME = 5         -- Seconds. Time needed to accelerate up to full speed.
DECEL_DIST = 5         -- Meters. Start to decelerate this distance away from destination.
MIN_DECEL_SPEED = 0.1  -- Meters/second. Don't go slower than this when decelerating.
STOP_THRESHOLD = 0.05  -- Meters. When cabin is +/- this distance it will be considered to have stopped.
BUTTON_DOWN_TIME = 0.3 -- Seconds. Amount of time that button should stay pressed.
DOOR_SPEED = 1         -- Meters/second. How fast to open/close the doors
DOOR_STRENGTH = 400    -- Strength parameter when setting joint motor.
DOOR_THRESHOLD = 0.05  -- Meters. When door position is less than this it will be considered closed.


function init()
	debug = false
	firstTime = true
	time = 0
	currentState = IDLE
	currentButtonPressed = nil
	currentFloorPressed = 0
	speed = 0
	oldSpeed = 0
	direction = 1  -- 1 for up, -1 for down
	decelPos = 0   -- Position at which to start decellerating before reaching destination
	
	setCurrentFloor(0)
	setDestinationFloor(0)

	clickUp = LoadSound("clickup.ogg")
	clickDown = LoadSound("clickdown.ogg")
	chime = LoadSound("elevator-chime.ogg")
	motorSound = LoadLoop("MOD/elevator/motor.ogg")
	
	stopButton = FindShape("emergency-stop")
	motor = FindJoint("motor")
	cabin = GetShapeBody(FindShape("cabin"))
	numpad = FindShape("numpad")
	numpadScreen = FindScreen("numpad")
	upButtons, downButtons = findButtons()
	if #upButtons == 0 or #downButtons == 0 then
		selfDebugPrint("Cannot find buttons")
	end
	floorPositions = findFloors()
	if #floorPositions == 0 then
		selfDebugPrint("Cannot find floors")
	end
	cabinDoors, doors = findDoors()
	if #doors == 0 then
		selfDebugPrint("Cannot find doors")
	end
	
	if debug then
		selfDebugPrint("Found " .. #upButtons .. " up buttons and " .. #downButtons .. " down buttons")
		selfDebugPrint("Found " .. #floorPositions .. " floor positions")
		selfDebugPrint("Found " .. #doors .. " door pairs")
	end

	SetTag(stopButton, "interact", "Stop")
	for i=1, #upButtons do
		SetTag(upButtons[i], "interact", "Up")
		SetTag(downButtons[i], "interact", "Down")
		buttonLightOff(upButtons[i])
		buttonLightOff(downButtons[i])
	end
end



---------------------------
--                       --
--   State functions     --
--                       --
---------------------------

function changeState(newState)
	time = 0     -- Reset time counter
	currentState = newState
end

function doIdle()
	stop()
	SetTag(numpad, "interact", "Enter floor")

	if InputPressed("interact") then
		local thing = GetPlayerInteractShape()
		if thing == numpad then
			RemoveTag(numpad, "interact")
			SetPlayerScreen(numpadScreen)
			changeState(NUMPAD)
		else
			for i=1, #upButtons do
				if thing == upButtons[i] or thing == downButtons[i] then
					currentButtonPressed = thing
					currentFloorPressed = i - 1
					press(currentButtonPressed)
					changeState(BUTTON_DOWN)
				end
			end
		end
	end
end

function doNumpad()
	stop()
	
	-- Check if player is done with numpad screen.
	if GetPlayerScreen() == 0 then
		local destFloor = getDestinationFloor()
		if destFloor ~= getCurrentFloor() and destFloor < #floorPositions then
			playChimeSound()
			changeState(DOORS_CLOSING)
		else
			changeState(IDLE)
		end
	end
end

function doButtonDown()
	if time > BUTTON_DOWN_TIME then
		unpress(currentButtonPressed)
		playChimeSound()
		if currentFloorPressed == getCurrentFloor() then
			changeState(DOORS_OPENING)
		else
			setDestinationFloor(currentFloorPressed)
			changeState(DOORS_CLOSING)
		end
		currentButtonPressed = nil
		currentFloorPressed = 0
	end
end

function doDoorsOpening()
	moveDoors(OPEN)
	changeState(IDLE)
end

function doDoorsClosing()
	moveDoors(CLOSE)
	local leftDoorPos = GetJointMovement(GetShapeJoints(cabinDoors.left)[1])
	local rightDoorPos = GetJointMovement(GetShapeJoints(cabinDoors.right)[1])
	if debug then
		selfDebugWatch("cabin door (left)", leftDoorPos)
		selfDebugWatch("cabin door (right)", rightDoorPos)
	end
	if leftDoorPos < DOOR_THRESHOLD and rightDoorPos < DOOR_THRESHOLD then
		startAccelerating()
		changeState(ACCELERATING)
	elseif time > 5 then     -- If doors cannot close for some reason
		changeState(DOORS_OPENING)
	end
end

function doAccelerating()
	SetValue("speed", MAX_SPEED, "linear", ACCEL_TIME)
	move()
	if sign(decelPos - getCabinBottomPos()) ~= direction then
		oldSpeed = speed
		changeState(DECELERATING)
	end
end

function doDecelerating()
	local destPos = getDestinationPos()
	local currentPos = getCabinBottomPos()
	if speed > MIN_DECEL_SPEED then
		speed = oldSpeed * ((destPos - currentPos) / (destPos - decelPos))
	end
	move()
	if math.abs(destPos - currentPos) < STOP_THRESHOLD then
		stop()
		playChimeSound()
		changeState(DOORS_OPENING)
	end
end


function stop()
	speed = 0
	oldSpeed = 0
	direction = 1
	SetJointMotor(motor, 0)
end

function startAccelerating()
	local destPos = getDestinationPos()
	local currentPos = getCabinBottomPos()
	direction = sign(destPos - currentPos)
	
	-- Start to decelerate near the destination.
	-- If destination is closer than that point, then start to decelerate at the half way point.
	if math.abs(destPos - currentPos) < DECEL_DIST then
		decelPos = (currentPos + destPos) / 2
	else
		decelPos = destPos - (DECEL_DIST * direction)
	end
end

function move()
	SetJointMotor(motor, speed * direction)
	PlayLoop(motorSound, GetBodyTransform(cabin).pos, (speed / MAX_SPEED / 2) + 0.5)
	setCurrentFloor(findClosestFloor())
	if debug then
		selfDebugWatch("dest position", getDestinationPos())
		selfDebugWatch("cabin position", getCabinBottomPos())
		selfDebugWatch("speed", speed * direction)
	end
end


function moveDoor(door, direction)
	local motor = GetShapeJoints(door)[1]
	local min, max = GetJointLimits(motor)
	if direction == OPEN then
		SetJointMotorTarget(motor, max, DOOR_SPEED, DOOR_STRENGTH)
	elseif direction == CLOSE then
		SetJointMotorTarget(motor, min, DOOR_SPEED, DOOR_STRENGTH)
	end
end

function moveDoors(direction)
	local currentFloorDoors = doors[getCurrentFloor() + 1]
	if debug then
		local dirString = "Opening"
		if direction == CLOSE then dirString = "Closing" end
		selfDebugPrint(dirString .. " doors on floor " .. tostring(getCurrentFloor()))
	end
	moveDoor(currentFloorDoors.left, direction)
	moveDoor(currentFloorDoors.right, direction)
	moveDoor(cabinDoors.left, direction)
	moveDoor(cabinDoors.right, direction)
end


function press(button)
	local t = GetShapeLocalTransform(button)
	PlaySound(clickDown, TransformToParentPoint(GetBodyTransform(GetShapeBody(button)), t.pos))
	buttonLightOn(button)
end

function unpress(button)
	local t = GetShapeLocalTransform(button)
	PlaySound(clickUp, TransformToParentPoint(GetBodyTransform(GetShapeBody(button)), t.pos))
	buttonLightOff(button)
end



---------------------------
--                       --
--   Getters/Setters     --
--                       --
---------------------------

function getCurrentFloor()
	return GetInt("level.elevator" .. elevatorId .. ".currentFloor")
end

function getDestinationFloor()
	return GetInt("level.elevator" .. elevatorId .. ".destinationFloor")
end

function setCurrentFloor(num)
	SetInt("level.elevator" .. elevatorId .. ".currentFloor", num)
end

function setDestinationFloor(num)
	SetInt("level.elevator" .. elevatorId .. ".destinationFloor", num)
end



---------------------------
--                       --
--   Misc functions      --
--                       --
---------------------------

-- Returns two arrays of buttons (up and down), sorted from lowest to highest
function findButtons()
	local upButtons = FindShapes("up")
	local downButtons = FindShapes("down")
	table.sort(upButtons, sortShapeByPos)
	table.sort(downButtons, sortShapeByPos)
	return upButtons, downButtons
end

-- Returns an array of vertical positions of each floor, sorted from lowest to highest
function findFloors()
	local locations = FindLocations("floor")
	local positions = {}
	for i=1, #locations do
		positions[i] = GetLocationTransform(locations[i]).pos[2]
	end
	table.sort(positions)
	return positions
end

-- Returns an array of tables representing the left and right doors for each floor
function findDoors()
	local allDoors = FindShapes("elevator-door")
	table.sort(allDoors, sortShapeByPos)
	
	-- After sorting, each floor's left and right doors should be next to each other.
	-- Just need to check which one is left and which is right.
	local doorArray = {}
	for i=1, #allDoors, 2 do
		local door1 = allDoors[i]
		local door2 = allDoors[i+1]
		if HasTag(door1, "left") then
			table.insert(doorArray, { left = door1, right = door2 })
		else
			table.insert(doorArray, { left = door2, right = door1 })
		end
	end
	
	local allCabinDoors = FindShapes("elevator-cabin-door")
	local door1 = allCabinDoors[1]
	local door2 = allCabinDoors[2]
	local cabinObj
	if HasTag(door1, "left") then
		cabinObj = { left = door1, right = door2 }
	else
		cabinObj = { left = door2, right = door1 }
	end

	return cabinObj, doorArray
end

-- Find floor closest to current cabin position
function findClosestFloor()
	local i = 1
	while i < #floorPositions and getCabinBottomPos() > (floorPositions[i] + floorPositions[i+1]) / 2 do
		i = i + 1
	end
	return i - 1  -- Floor numbers are zero-based (ground is zero)
end


function sortShapeByPos(a, b)
	return GetShapeWorldTransform(a).pos[2] < GetShapeWorldTransform(b).pos[2]
end


function getDestinationPos()
	return floorPositions[getDestinationFloor() + 1]
end

function getCabinBottomPos()
	local min, max = GetBodyBounds(cabin)
	return min[2]
end

function buttonLightOn(button)
	SetShapeEmissiveScale(button, 1)
end
function buttonLightOff(button)
	SetShapeEmissiveScale(button, 0.02)
end

function playChimeSound()
	PlaySound(chime, GetBodyTransform(cabin).pos)
end

function sign(number)
	return (number > 0 and 1) or (number == 0 and 0) or -1
end


function selfDebugPrint(message)
	DebugPrint("Elevator " .. elevatorId .. ": " .. message)
end

function selfDebugWatch(name, value)
	DebugWatch("Elevator " .. elevatorId .. ": " .. name, value)
end



---------------------------
--                       --
--   Main loop           --
--                       --
---------------------------

stateFuncMap = {
	[IDLE]          = doIdle,
	[NUMPAD]        = doNumpad,
	[BUTTON_DOWN]   = doButtonDown,
	[DOORS_CLOSING] = doDoorsClosing,
	[ACCELERATING]  = doAccelerating,
	[DECELERATING]  = doDecelerating,
	[DOORS_OPENING] = doDoorsOpening
}

function tick(timeDelta)
	time = time + timeDelta

	if debug then
		selfDebugWatch("time", string.format("%.2f", time))
		selfDebugWatch("state", currentState)
		selfDebugWatch("current floor", getCurrentFloor())
		selfDebugWatch("destination floor", getDestinationFloor())
	end

	if InputPressed("interact") and GetPlayerInteractShape() == stopButton then
		changeState(IDLE)
	end

	local func = stateFuncMap[currentState]
	if func then
		func()
	else
		-- Somehow got into trouble, reset
		DebugPrint("Elevator "..elevatorId.." got into an Unknown state: "..currentState)
		changeState(IDLE)
	end
end
