#include "koopatlas/pathmanager.h"
#include "koopatlas/core.h"
#include "koopatlas/hud.h"
#include "koopatlas/player.h"
#include <sfx.h>
#include <stage.h>

extern "C" void PlaySoundWithFunctionB4(void *spc, nw4r::snd::SoundHandle *handle, int id, int unk);

static u8 MaybeFinishingLevel[2] = {0xFF,0xFF};

void dWMPathManager_c::setup() {
	dScKoopatlas_c *wm = dScKoopatlas_c::instance;

	isMoving = false;
	isJumping = false;
	scaleAnimProgress = -1;
	timer = 0.0;
	currentPath = 0;
	reverseThroughPath = false;

	pathLayer = wm->mapData.pathLayer;

	SpammyReport("setting up PathManager\n");
	SaveBlock *save = GetSaveFile()->GetBlock(-1);

	mustComplainToMapCreator = false;

	SpammyReport("Unlocking paths\n");
	isEnteringLevel = false;
	levelStartWait = -1;
	unlockPaths();

	waitForAfterDeathAnim = -1;
	mustPlayAfterDeathAnim = false;
	if (LastPowerupStoreType == LOSE_LEVEL) {
		mustPlayAfterDeathAnim = true;
		daWMPlayer_c::instance->visible = false;
		LastPowerupStoreType = BEAT_LEVEL;
	}

	SpammyReport("done\n");

	// Figure out what path node to start at
	if (wm->settings & 0x10000000) {
		// Start off from a "Change"
		u8 changeID = (wm->settings >> 20) & 0xFF;
		SpammyReport("entering at Change ID %d\n", changeID);
		SpammyReport("Path layer: %p\n", pathLayer);
		SpammyReport("Node count: %d\n", pathLayer->nodeCount);

		bool found = false;

		for (int i = 0; i < pathLayer->nodeCount; i++) {
			dKPNode_s *node = pathLayer->nodes[i];
			SpammyReport("Checking node: %p\n", node);

			if (node->type == dKPNode_s::CHANGE && node->thisID == changeID) {
				found = true;
				currentNode = node;

				//OSReport("Found CHANGE node: %d %p\n", changeID, node);

				// figure out where we should move to
				dKPPath_s *exitTo = 0;

				for (int i = 0; i < 4; i++) {
					dKPPath_s *candidateExit = node->exits[i];
					//OSReport("Candidate exit: %p\n", candidateExit);
					if (!candidateExit)
						continue;

					// find out if this path is a candidate
					dKPNode_s *srcNode = node;
					dKPPath_s *path = candidateExit;

					while (true) {
						dKPNode_s *destNode = (path->start == srcNode) ? path->end : path->start;
						//OSReport("Path: %p nodes %p to %p\n", path, srcNode, destNode);
						int ct = destNode->getAvailableExitCount();
						//OSReport("Dest Node available exits: %d; type: %d\n", ct, destNode->type);
						if (destNode == node || ct > 2 || destNode->type == dKPNode_s::LEVEL || destNode->type == dKPNode_s::CHANGE) {
							exitTo = candidateExit;
							//OSReport("Accepting this node\n");
							break;
						}

						if (ct == 1)
							break;

						// where to next?
						path = destNode->getOppositeAvailableExitTo(path);
						srcNode = destNode;
					}

					if (exitTo)
						break;
				}

				if (!exitTo)
					exitTo = node->getAnyExit();
				startMovementTo(exitTo);
				break;
			}
		}

		if (!found) {
			currentNode = pathLayer->nodes[0];
			mustComplainToMapCreator = true;
		}

		waitAtStart = -1;
	} else {
		if (!countdownToFadeIn)
			waitAtStart = 50;
		else
			waitAtStart = -1;

		if (wm->currentMapID == 0 && wm->settings & 0x80000000)
			waitAtStart = 280;

		SpammyReport("saved path node: %d\n", save->current_path_node);
		if (save->current_path_node >= pathLayer->nodeCount) {
			SpammyReport("out of bounds (%d), using node 0\n", pathLayer->nodeCount);
			currentNode = pathLayer->nodes[0];
		} else {
			currentNode = pathLayer->nodes[save->current_path_node];
			SpammyReport("OK %p\n", currentNode);
		}
	}

	for (int i = 0; i < pathLayer->nodeCount; i++)
		if (pathLayer->nodes[i]->type == dKPNode_s::LEVEL)
			pathLayer->nodes[i]->setupNodeExtra();

	// did we just beat a level?
	if (MaybeFinishingLevel[0] != 0xFF) {
		SaveBlock *save = GetSaveFile()->GetBlock(-1);
		if (save->CheckLevelCondition(MaybeFinishingLevel[0], MaybeFinishingLevel[1], COND_NORMAL)) {
			shouldRequestSave = true;
		}

		MaybeFinishingLevel[0] = 0xFF;
	}
}

static u8 *PathAvailabilityData = 0;
static u8 *NodeAvailabilityData = 0;

dWMPathManager_c::~dWMPathManager_c() {
	if (PathAvailabilityData && !isEnteringLevel) {
		delete[] PathAvailabilityData;
		PathAvailabilityData = 0;

		delete[] NodeAvailabilityData;
		NodeAvailabilityData = 0;
	}

	if (isEnteringLevel) {
		SaveBlock *save = GetSaveFile()->GetBlock(-1);
		if ((enteredLevel->displayLevel >= 21 && enteredLevel->displayLevel <= 27 && enteredLevel->displayLevel != 26)
				|| (enteredLevel->displayLevel >= 29 && enteredLevel->displayLevel <= 42)) {
			if (!save->CheckLevelCondition(enteredLevel->worldSlot, enteredLevel->levelSlot, COND_NORMAL)) {
				MaybeFinishingLevel[0] = enteredLevel->worldSlot;
				MaybeFinishingLevel[1] = enteredLevel->levelSlot;
			}
		}
	}
}

void dWMPathManager_c::unlockPaths() {
	u8 *oldPathAvData = PathAvailabilityData;
	PathAvailabilityData = new u8[pathLayer->pathCount];

	u8 *oldNodeAvData = NodeAvailabilityData;
	NodeAvailabilityData = new u8[pathLayer->nodeCount];

	SpammyReport("Unlocking paths\n");

	// unlock all needed paths
	for (int i = 0; i < pathLayer->pathCount; i++) {
		dKPPath_s *path = pathLayer->paths[i];

		PathAvailabilityData[i] = path->isAvailable;

		//SpammyReport("Path %d: %d\n", i, path->isAvailable);
		// if this path is not "always available", then nuke its alpha
		path->setLayerAlpha((path->isAvailable == dKPPath_s::ALWAYS_AVAILABLE) ? 255 : 0);
	}

	SaveBlock *save = GetSaveFile()->GetBlock(-1);

	u8 *in = (u8*)dScKoopatlas_c::instance->mapData.data->unlockData;
	SpammyReport("UNLOCKING PATHS: Unlock data @ %p\n", in);

	int cmdID = 0;

	while (*in != 0) {
		UnlockCmdReport("[%p] Cmd %d: Evaluating condition\n", in, cmdID);
		// begin processing a block
		bool value = evaluateUnlockCondition(in, save, 0);
		UnlockCmdReport("[%p] Cmd %d: Condition evaluated, result: %d\n", in, cmdID, value);
		//UnlockCmdReport("Unlock condition: %d\n", value);

		// get what it's supposed to affect
		// for now we'll assume that it affects one or more paths
		u8 affectedCount = *(in++);
		UnlockCmdReport("[%p] Cmd %d: Affects %d path(s)\n", in, cmdID, affectedCount);

		for (int i = 0; i < affectedCount; i++) {
			u8 one = *(in++);
			u8 two = *(in++);
			u16 pathID = (one << 8) | two;
			UnlockCmdReport("[%p] Cmd %d: Affected %d: PathID: %d\n", in, cmdID, i, pathID);

			dKPPath_s *path = pathLayer->paths[pathID];
			UnlockCmdReport("[%p] Cmd %d: Affected %d: Path: %p\n", in, cmdID, i, path);
			path->isAvailable = value ? dKPPath_s::AVAILABLE : dKPPath_s::NOT_AVAILABLE;
			UnlockCmdReport("[%p] Cmd %d: Affected %d: IsAvailable written\n", in, cmdID, i);
			PathAvailabilityData[pathID] = value ? dKPPath_s::AVAILABLE : dKPPath_s::NOT_AVAILABLE;
			UnlockCmdReport("[%p] Cmd %d: Affected %d: AvailabilityData written\n", in, cmdID, i);
			// NEWLY_AVAILABLE is set later, when that stuff is figured out

			path->setLayerAlpha(value ? 255 : 0);
			UnlockCmdReport("[%p] Cmd %d: Affected %d: Layer alpha applied\n", in, cmdID, i);
		}

		UnlockCmdReport("[%p] Cmd %d: %d affected path(s) processed\n", in, cmdID, affectedCount);

		cmdID++;
	}

	SpammyReport("UNLOCKING PATHS: All complete @ %p\n", in);

	for (int i = 0; i < pathLayer->nodeCount; i++) {
		dKPNode_s *node = pathLayer->nodes[i];
		NodeAvailabilityData[i] = node->isUnlocked();

		if (node->type == node->LEVEL && node->isUnlocked() && node->levelNumber[1] != 99) {
			save->completions[node->levelNumber[0]-1][node->levelNumber[1]-1] |= COND_UNLOCKED;
		}
	}

	// did anything become newly available?!
	newlyAvailablePaths = 0;
	newlyAvailableNodes = 0;

	if (oldPathAvData) {
		for (int i = 0; i < pathLayer->pathCount; i++) {
			if ((PathAvailabilityData[i] > 0) && (oldPathAvData[i] == 0)) {
				dKPPath_s *path = pathLayer->paths[i];
				path->isAvailable = dKPPath_s::NEWLY_AVAILABLE;
				newlyAvailablePaths++;

				// set this path's alpha to 0, we'll fade it in later
				path->setLayerAlpha(0);
			}
		}
		delete[] oldPathAvData;

		// check nodes too
		for (int i = 0; i < pathLayer->nodeCount; i++) {
			if ((NodeAvailabilityData[i] > 0) && (oldNodeAvData[i] == 0)) {
				dKPNode_s *node = pathLayer->nodes[i];
				node->isNew = true;
				newlyAvailableNodes++;
			}
		}
		delete[] oldNodeAvData;
	}

	// now set all node alphas
	for (int i = 0; i < pathLayer->nodeCount; i++) {
		dKPNode_s *node = pathLayer->nodes[i];
		
		node->setLayerAlpha((node->isUnlocked() & !node->isNew) ? 255 : 0);
	}

	// if anything was new, set it as such
	if (newlyAvailablePaths || newlyAvailableNodes) {
		countdownToFadeIn = 30;
	}
	unlockingAlpha = -1;
}

bool dWMPathManager_c::evaluateUnlockCondition(u8 *&in, SaveBlock *save, int stack) {
	UnlockCmdReport("[%p] CondStk:%d begin\n", in, stack);
	u8 controlByte = *(in++);

	u8 conditionType = (controlByte >> 6);
	UnlockCmdReport("[%p] CondStk:%d control byte: %d; condition type: %d\n", in, stack, controlByte, conditionType);

	if (conditionType == 0) {
		UnlockCmdReport("[%p] CondStk:%d end, returning CONSTANT 1\n", in, stack);
		return true;
	}

	if (conditionType == 1) {
		// Simple level

		bool isSecret = (controlByte & 0x10);
		u8 worldNumber = controlByte & 0xF;
		u8 levelNumber = *(in++);
		UnlockCmdReport("[%p] CondStk:%d level, w:%d l:%d secret:%d\n", in, stack, worldNumber, levelNumber, isSecret);

		u32 conds = save->GetLevelCondition(worldNumber, levelNumber);
		UnlockCmdReport("[%p] CondStk:%d returning for level conditions: %d / %x\n", in, stack, conds, conds);

		if (isSecret)
			return (conds & COND_SECRET) != 0;
		else
			return (conds & COND_NORMAL) != 0;
	}

	// Type: 2 = AND, 3 = OR
	bool isAnd = (conditionType == 2);
	bool isOr = (conditionType == 3);

	bool value = isOr ? false : true;

	u8 termCount = (controlByte & 0x3F) + 1;
	UnlockCmdReport("[%p] CondStk:%d and:%d or:%d startValue:%d termCount:%d\n", in, stack, isAnd, isOr, value, termCount);

	for (int i = 0; i < termCount; i++) {
		bool what = evaluateUnlockCondition(in, save, stack+1);

		if (isOr)
			value |= what;
		else
			value &= what;
	}

	UnlockCmdReport("[%p] CondStk:%d end, returning %d\n", in, stack, value);
	return value;
}


bool dWMPathManager_c::doingThings() {
	if (isEnteringLevel || (waitAfterUnlock > 0) ||
			(waitAtStart > 0) || (waitForAfterDeathAnim > 0) ||
			(countdownToFadeIn > 0))
		return true;

	return false;
}

void dWMPathManager_c::execute() {
	if (isEnteringLevel) {
		if (levelStartWait > 0) {
			levelStartWait--;
			if (levelStartWait == 0) {
				dScKoopatlas_c::instance->startLevel(enteredLevel);
			}
		}
		return;
	}

	// handle path fading
	if (countdownToFadeIn > 0) {
		countdownToFadeIn--;
		if (countdownToFadeIn <= 0) {
			unlockingAlpha = 0;

			nw4r::snd::SoundHandle something;
			PlaySoundWithFunctionB4(SoundRelatedClass, &something, SE_OBJ_GEN_LOAD, 1);
		} else {
			return;
		}
	}

	if (unlockingAlpha != -1) {
		unlockingAlpha += 3;

		for (int i = 0; i < pathLayer->pathCount; i++) {
			dKPPath_s *path = pathLayer->paths[i];

			if (path->isAvailable == dKPPath_s::NEWLY_AVAILABLE)
				path->setLayerAlpha(unlockingAlpha);
		}

		for (int i = 0; i < pathLayer->nodeCount; i++) {
			dKPNode_s *node = pathLayer->nodes[i];
			
			if (node->isNew)
				node->setLayerAlpha(unlockingAlpha);
		}

		if (unlockingAlpha == 255) {
			// we've reached the end
			unlockingAlpha = -1;
			nw4r::snd::SoundHandle something;
			PlaySoundWithFunctionB4(SoundRelatedClass, &something, SE_OBJ_GEN_NEW_COURSE, 1);
			waitAfterUnlock = 15;

			for (int i = 0; i < pathLayer->nodeCount; i++) {
				dKPNode_s *node = pathLayer->nodes[i];

				if (node->isNew && node->type == dKPNode_s::LEVEL) {
					Vec efPos = {node->x, -node->y, 3300.0f};
					S16Vec efRot = {0x2000,0,0};
					Vec efScale = {0.8f,0.8f,0.8f};
					SpawnEffect("Wm_cs_pointlight", 0, &efPos, &efRot, &efScale);
				}
			}
		}

		return;
	}

	if (waitAfterUnlock > 0) {
		waitAfterUnlock--;
		return;
	}

	if (waitAtStart > 0) {
		waitAtStart--;
		if (waitAtStart == 0) {
			if (mustPlayAfterDeathAnim) {
				daWMPlayer_c::instance->visible = true;
				daWMPlayer_c::instance->startAnimation(ending_wait, 1.0f, 0.0f, 0.0f);
				waitForAfterDeathAnim = 60;

				nw4r::snd::SoundHandle something;
				PlaySoundWithFunctionB4(SoundRelatedClass, &something, SE_VOC_MA_CS_COURSE_MISS, 1);
			}
		}
		return;
	}

	if (waitForAfterDeathAnim > 0) {
		waitForAfterDeathAnim--;
		if (waitForAfterDeathAnim == 0)
			daWMPlayer_c::instance->startAnimation(wait_select, 1.0f, 0.0f, 0.0f);
		return;
	}

	if (shouldRequestSave) {
		dScKoopatlas_c::instance->showSaveWindow();
		shouldRequestSave = false;
	}

	int nowPressed = Remocon_GetPressed(GetActiveRemocon());

	if (isMoving) {
		moveThroughPath();
	} else {
		// Left, right, up, down
		int pressedDir = -1;
		if (nowPressed & WPAD_LEFT) pressedDir = 0;
		else if (nowPressed & WPAD_RIGHT) pressedDir = 1;
		else if (nowPressed & WPAD_UP) pressedDir = 2;
		else if (nowPressed & WPAD_DOWN) pressedDir = 3;
		else if (nowPressed & WPAD_TWO)
			activatePoint();

		if (pressedDir >= 0) {
			if (canUseExit(currentNode->exits[pressedDir])) {
				startMovementTo(currentNode->exits[pressedDir]);
			} else {
				// TODO: maybe remove this? got to see how it looks
				static u16 directions[] = {-0x4000,0x4000,-0x7FFF,0};
				daWMPlayer_c::instance->rot.y = directions[pressedDir];
			}
		}
	}
}


void dWMPathManager_c::startMovementTo(dKPPath_s *path) {
	SpammyReport("moving to path %p [%d,%d to %d,%d]\n", path, path->start->x, path->start->y, path->end->x, path->end->y);

	if (!path->isAvailable) { return; }
	if (currentNode && dWMHud_c::instance)
		dWMHud_c::instance->leftNode();

	calledEnteredNode = false;

	SpammyReport("a\n");
	isMoving = true;
	reverseThroughPath = (path->end == currentNode);
	SpammyReport("b\n");

	currentPath = path;
	SpammyReport("c\n");

	// calculate direction of the path
	short deltaX = path->end->x - path->start->x;
	short deltaY = path->end->y - path->start->y;
	SpammyReport("d\n");
	u16 direction = (u16)(atan2(deltaX, deltaY) / ((M_PI * 2) / 65536.0));
	SpammyReport("e\n");

	if (reverseThroughPath) {
	SpammyReport("e2\n");
		direction += 0x8000;
	}

	SpammyReport("f\n");
	daWMPlayer_c *player = daWMPlayer_c::instance;
	SpammyReport("g %p\n", player);


	// Consider adding these as options
	// wall_walk_l = 60,
	// wall_walk_r = 61,

	// hang_walk_l = 65,
	// hang_walk_r = 66,

	static const struct {
		PlayerAnim anim;
		float animParam1, animParam2;
		s16 forceRotation;
		float forceSpeed;
		SFX repeatSound, initialSound;
		const char *repeatEffect, *initialEffect;
	} Animations[] = {
		// Walking
		{run,2.0f,10.0f, -1,-1.0f, SE_PLY_FOOTNOTE_DIRT,SE_NULL, 0,0},
		{run,2.0f,10.0f, -1,-1.0f, SE_PLY_FOOTNOTE_CS_SAND,SE_NULL, "Wm_mr_foot_sand",0},
		{run,2.0f,10.0f, -1,-1.0f, SE_PLY_FOOTNOTE_CS_SNOW,SE_NULL, "Wm_mr_foot_snow",0},
		{run,2.0f,10.0f, -1,-1.0f, SE_PLY_FOOTNOTE_CS_WATER,SE_NULL, "Wm_mr_foot_water",0},

		// Jumping
		{jump,1.0f,1.0f, -1,2.5f, SE_NULL,SE_PLY_JUMP, 0,0},
		{jump,1.0f,10.0f, -1,2.5f, SE_NULL,SE_PLY_JUMP, 0,0},
		{jump,1.0f,10.0f, -1,2.5f, SE_NULL,SE_PLY_JUMP, 0,0},
		{jump,1.0f,10.0f, -1,2.5f, SE_NULL,SE_PLY_JUMP, 0,"Wm_mr_waterwave_out"},

		// Ladder up, left, right
		{pea_plant,1.2f,10.0f, -0x7FFF,1.5f, SE_PLY_FOOTNOTE_CS_ROCK_CLIMB,SE_NULL, 0,0},
		{tree_climb,1.2f,10.0f, -0x4000,1.5f, SE_PLY_FOOTNOTE_CS_ROCK_CLIMB,SE_NULL, 0,0},
		{tree_climb,1.2f,10.0f, 0x4000,1.5f, SE_PLY_FOOTNOTE_CS_ROCK_CLIMB,SE_NULL, 0,0},
		// Fall (default?)
		{run,2.0f,10.0f, -1,-1.0f, SE_PLY_FOOTNOTE_DIRT,SE_NULL, 0,0},

		// Swim
		{swim_wait,1.2f,10.0f, -1,2.0f, SE_PLY_SWIM,SE_NULL, "Wm_mr_waterswim",0},
		// Run
		{b_dash2,3.0f,10.0f, -1,5.0f, SE_PLY_FOOTNOTE_DIRT,SE_NULL, 0,0},
		// Pipe
		{wait,2.0f,10.0f, 0,1.0f, SE_NULL,SE_PLY_DOKAN_IN_OUT, 0,0},
		// Door
		{wait,2.0f,10.0f, -0x7FFF,0.2f, SE_NULL,SE_OBJ_DOOR_OPEN, 0,0},

		// TJumped
		{Tjumped,2.0f,0.0f, -1,-1.0f, SE_NULL,SE_NULL, 0,0},

		// Enter cave, this is handled specially
		{run,1.0f,10.0f, -1,1.0f, SE_NULL,SE_NULL, 0,0},
		{run,1.0f,10.0f, -1,1.0f, SE_NULL,SE_NULL, 0,0},

		// Invisible, this is handled specially
		{wait,2.0f,10.0f, -1,1.0f, SE_NULL,SE_NULL, 0,0},
	};

	isJumping = (path->animation >= dKPPath_s::JUMP && path->animation <= dKPPath_s::JUMP_WATER);

	float playerScale = 1.6f;

	if (path->animation == dKPPath_s::ENTER_CAVE_UP) {
		scaleAnimProgress = 60;
		// what direction does this path go in?
		isScalingUp = (deltaY < 0) ^ reverseThroughPath;

		if (!isScalingUp)
			playerScale = 0.0f;
	}

	player->visible = (path->animation != dKPPath_s::INVISIBLE);
	player->scale.x = player->scale.y = player->scale.z = playerScale;

	int id = (path->animation >= dKPPath_s::MAX_ANIM) ? 0 : (int)path->animation;

	player->startAnimation(Animations[id].anim, Animations[id].animParam1, Animations[id].animParam2, 0.0f);

	player->rot.y = (Animations[id].forceRotation != -1) ? Animations[id].forceRotation : direction;

	moveSpeed = (Animations[id].forceSpeed >= 0.0f) ? Animations[id].forceSpeed : 3.0f;
	moveSpeed = path->speed * moveSpeed;

	if (Animations[id].repeatEffect) {
		player->hasEffect = true;
		player->effectName = Animations[id].repeatEffect;
	} else {
		player->hasEffect = false;
	}

	if (Animations[id].repeatSound != SE_NULL) {
		player->hasSound = true;
		player->soundName = Animations[id].repeatSound;
	} else {
		player->hasSound = false;
	}

	if (Animations[id].initialEffect) {
		SpawnEffect(Animations[id].initialEffect, 0, &player->pos, 0, &player->scale);
	}

	if (Animations[id].initialSound != SE_NULL) {
		nw4r::snd::SoundHandle something;
		PlaySoundWithFunctionB4(SoundRelatedClass, &something, Animations[id].initialSound, 1);

		if (Animations[id].initialSound == SE_PLY_JUMP) {
			nw4r::snd::SoundHandle something2;
			PlaySoundWithFunctionB4(SoundRelatedClass, &something2, SE_VOC_MA_CS_JUMP, 1);
		}
	}
}

void dWMPathManager_c::moveThroughPath() {
	dKPNode_s *from, *to;

	from = reverseThroughPath ? currentPath->end : currentPath->start;
	to = reverseThroughPath ? currentPath->start : currentPath->end;

	daWMPlayer_c *player = daWMPlayer_c::instance;


	if (scaleAnimProgress >= 0) {
		float soFar = scaleAnimProgress * (1.6f / 60.0f);
		float sc = isScalingUp ? soFar : (1.6f - soFar);
		player->scale.x = player->scale.y = player->scale.z = sc;

		scaleAnimProgress--;
	}


	Vec move = (Vec){to->x - from->x, to->y - from->y, 0};
	VECNormalize(&move, &move);
	VECScale(&move, &move, moveSpeed);

	if (isJumping) {
		float ys = (float)from->y;
		float ye = (float)to->y;
		float midpoint = (from->y + to->y) / 2;

		float top, len;
		if (ys > ye) { len = ys - ye; top = ys - midpoint + 10.0; }
		else		 { len = ye - ys; top = ye - midpoint + 10.0; }

		if (len == 0.0) { len = 2.0; }

		float a;
		if (timer > 0.0) { a = -timer; }
		else			 { a =  timer; }

		player->jumpOffset = -sin(a * 3.14 / len) * top;
		timer -= move.y;
	}

	player->pos.x += move.x;
	player->pos.y -= move.y;

	// what distance is left?
	if (to->type == dKPNode_s::LEVEL && !calledEnteredNode) {
		Vec toEndVec = {to->x - player->pos.x, to->y + player->pos.y, 0.0f};
		float distToEnd = VECMag(&toEndVec);
		//OSReport("Distance: %f; To:%d,%d; Player:%f,%f; Diff:%f,%f\n", distToEnd, to->x, to->y, player->pos.x, player->pos.y, toEndVec.x, toEndVec.y);

		if (distToEnd < 64.0f && dWMHud_c::instance) {
			calledEnteredNode = true;
			dWMHud_c::instance->enteredNode(to);
		}
	}

	// Check if we've reached the end yet
	if (
			(((move.x > 0) ? (player->pos.x >= to->x) : (player->pos.x <= to->x)) &&
			((move.y > 0) ? (-player->pos.y >= to->y) : (-player->pos.y <= to->y)))
			||
			(from->x == to->x && from->y == to->y)
	   ) {

   		currentNode = to;
   		player->pos.x = to->x;
   		player->pos.y = -to->y;

   		isJumping = false;
   		timer = 0.0;

   		SpammyReport("reached path end (%p) with type %d\n", to, to->type);

   		bool reallyStop = false;

   		if (to->type == dKPNode_s::LEVEL) {
   			// Always stop on levels
   			reallyStop = true;
   		} else if (to->type == dKPNode_s::CHANGE || to->type == dKPNode_s::WORLD_CHANGE) {
			// Never stop on entrances or on world changes
			reallyStop = false;
   		} else if (to->type == dKPNode_s::PASS_THROUGH) {
			// If there's only one exit here, then stop even though
			// it's a passthrough node
			reallyStop = (to->getAvailableExitCount() == 1);
		} else {
			// Quick check: do we *actually* need to stop on this node?
			// If it's a junction with more than two exits, but only two are open,
			// take the opposite open one
			if (to->getExitCount() > 2 && to->getAvailableExitCount() == 2)
   				reallyStop = false;
			else
				reallyStop = true;
		}

		if (to->type == dKPNode_s::WORLD_CHANGE) {
			// Set the current world info
			SaveBlock *save = GetSaveFile()->GetBlock(-1);

			OSReport("Activating world change %d\n", to->worldID);
			const dKPWorldDef_s *world = dScKoopatlas_c::instance->mapData.findWorldDef(to->worldID);
			if (world) {
				bool visiblyChange = true;
				if (strncmp(save->newerWorldName, world->name, 32) == 0) {
					OSReport("Already here, but setting BGM track\n");
					visiblyChange = false;
				}

				OSReport("Found!\n");
				strncpy(save->newerWorldName, world->name, 32);
				save->newerWorldName[31] = 0;
				save->newerWorldID = world->worldID;
				save->currentMapMusic = world->trackID;

				for (int i = 0; i < 2; i++) {
					save->fsTextColours[i] = world->fsTextColours[i];
					save->fsHintColours[i] = world->fsHintColours[i];
					save->hudTextColours[i] = world->hudTextColours[i];
				}

				save->hudHintH = world->hudHintH;
				save->hudHintS = world->hudHintS;
				save->hudHintL = world->hudHintL;

				save->titleScreenWorld = world->titleScreenWorld;
				save->titleScreenLevel = world->titleScreenLevel;

				if (visiblyChange && dWMHud_c::instance)
					dWMHud_c::instance->showFooter();

				dKPMusic::play(world->trackID);

			} else if (to->worldID == 0) {
				OSReport("No world\n");
				save->currentMapMusic = 0;
				dKPMusic::play(0);
				save->newerWorldName[0] = 0;
				if (dWMHud_c::instance)
					dWMHud_c::instance->hideFooter();
			} else {
				OSReport("Not found!\n");
			}
		}

		if (to->type == dKPNode_s::CHANGE) {
			// Go to another map

			// should we continue moving?
			if (to->getAvailableExitCount() == 1) {
				OSReport("Stopping");
				isMoving = false;
			} else {
				OSReport("Continuing");
				startMovementTo(to->getOppositeAvailableExitTo(currentPath));
			}

			SaveBlock *save = GetSaveFile()->GetBlock(-1);
			SpammyReport("node: %x, %s", to->destMap, to->destMap);
			save->current_world = dScKoopatlas_c::instance->getIndexForMapName(to->destMap);

			SpammyReport("Change to map ID %d (%s), entrance ID %d\n", save->current_world, to->destMap, to->foreignID);

			dScKoopatlas_c::instance->keepMusicPlaying = true;
			ActivateWipe(to->transition);
			DoSceneChange(WORLD_MAP, 0x10000000 | (to->foreignID << 20), 0);

		} else if (reallyStop) {
	   		// Stop here
	   		player->startAnimation(0, 1.2, 10.0, 0.0);
	   		player->hasEffect = false;
	   		player->hasSound = false;

	   		SpammyReport("stopping here\n");

	   		isMoving = false;

			SaveBlock *save = GetSaveFile()->GetBlock(-1);
			save->current_path_node = pathLayer->findNodeID(to);
			if (!calledEnteredNode && dWMHud_c::instance)
				dWMHud_c::instance->enteredNode();
		} else {
			startMovementTo(to->getOppositeAvailableExitTo(currentPath));
			SpammyReport("passthrough node, continuing to next path\n");
		}
	}
}

void dWMPathManager_c::activatePoint() {
	if (levelStartWait >= 0)
		return;

	if (currentNode->type == dKPNode_s::LEVEL) {
		int w = currentNode->levelNumber[0] - 1;
		int l = currentNode->levelNumber[1] - 1;

		if (l == 98) {
			dWMShop_c::instance->show(w);
			dScKoopatlas_c::instance->state.setState(&dScKoopatlas_c::instance->StateID_ShopWait);
			return;
		}

		if ((l >= 29) && (l <= 36)) {
			SaveBlock *save = GetSaveFile()->GetBlock(-1);
			u32 conds = save->GetLevelCondition(w, l);

			SpammyReport("Toad House Flags: %x", conds);
			if (conds & 0x30) { 
				nw4r::snd::SoundHandle something;
				PlaySoundWithFunctionB4(SoundRelatedClass, &something, SE_SYS_INVALID, 1);
				return;
			}
		}

		nw4r::snd::SoundHandle something;
		PlaySoundWithFunctionB4(SoundRelatedClass, &something, SE_SYS_GAME_START, 1);

		nw4r::snd::SoundHandle something2;
		PlaySoundWithFunctionB4(SoundRelatedClass, &something2, (Player_Powerup[0] == 3) ? SE_VOC_MA_PLAYER_DECIDE_MAME: SE_VOC_MA_CS_COURSE_IN, 1);

		daWMPlayer_c::instance->startAnimation(course_in, 1.2, 10.0, 0.0);
		daWMPlayer_c::instance->rot.y = 0;

		isEnteringLevel = true;
		levelStartWait = 40;
		enteredLevel = dLevelInfo_c::s_info.searchBySlot(w, l);

		dKPMusic::stop();
	}
}


void dWMPathManager_c::unlockAllPaths(char type) {

	// Unlocks ALL paths, regular and secret
	if (type == 0) {
		for (int i = 0; i < pathLayer->pathCount; i++) {
			dKPPath_s *path = pathLayer->paths[i];
			path->isAvailable = true;

			SaveBlock *save = GetSaveFile()->GetBlock(-1);
			for (int j = 0; j < 10; j++) {
				for (int h = 0; h < 0x2A; h++) {
					save->completions[j][h] = 0x30;
				}
			}
			unlockPaths();
		}
	}

	// Unlocks ALL paths, regular only
	if (type == 1) {
		for (int i = 0; i < pathLayer->pathCount; i++) {
			dKPPath_s *path = pathLayer->paths[i];
			path->isAvailable = true;

			SaveBlock *save = GetSaveFile()->GetBlock(-1);
			for (int j = 0; j < 10; j++) {
				for (int h = 0; h < 0x2A; h++) {
					save->completions[j][h] = 0x10;
				}
			}
			unlockPaths();
		}
	}

	// Unlocks current path, regular and secret
	if (type == 2) {
		if (currentNode->type == dKPNode_s::LEVEL) {
			int w = currentNode->levelNumber[0] - 1;
			int l = currentNode->levelNumber[1] - 1;

			SaveBlock *save = GetSaveFile()->GetBlock(-1);
			save->completions[w][l] = 0x30;
			unlockPaths();
		}
	}

	// Can't change node models - the price we pay for not using anims
	// for (int i = 0; i < pathLayer->nodeCount; i++) {
	// 	dKPNode_s *node = pathLayer->nodes[i];
	// 	node->setupNodeExtra();
	// }
	
}