Pushable blocks - dynamic puzzle

Hi. I want to make pushable block puzzles, like you'd see in a Zelda game. My code kind of works but there's always a weird bug/s going on.

The example below is an example of how I'd like it to work. AKA push 1 and 2 up to allow lateral movement with 3

I'd like it that a player might push block 2 to the right but obviously the block wouldn;t move. Yet that wouldn't then stop them pushing up next. Currently this happens.

I also want to make the block commit to one movement along the relevant axis and not be changed by the player after the push.

I currently get these kinds of issues:

If a player hits the middle of the block perfectly, it moves as it should but if the corner is hit, bugs happen.

This is my code: public class PushableOneSpace : MonoBehaviour { private Rigidbody2D objectsBody; private float moveSpeed = 2; private Vector3 directionChange; private Vector3 pushFromPosition; private bool playerTouchingBlock; private bool IAmStatic = true; private Vector3 currentPosition; private Vector3 addXPosition; private Vector3 subXPosition; private Vector3 addYPosition; private Vector3 subYPosition; public bool pushable; private bool movingSoNowUntouchable; private float maxPushRange = 1; private void Start() { objectsBody = GetComponent<Rigidbody2D>(); currentPosition = GetComponent<Transform>().position; addXPosition = currentPosition + new Vector3(maxPushRange, 0, 0); addYPosition = currentPosition + new Vector3(0, maxPushRange, 0); subXPosition = currentPosition + new Vector3(-maxPushRange, 0, 0); subYPosition = currentPosition + new Vector3(0, -maxPushRange, 0); } public void MoveBlock() { UpLeftRightDown(transform.position, pushFromPosition); directionChange.Normalize(); objectsBody.MovePosition(transform.position + directionChange * moveSpeed * Time.deltaTime); movingSoNowUntouchable = true; } private void FixedUpdate() { currentPosition = transform.position; if (!IAmStatic) { if (!pushable) { //Works but do you want to keep? objectsBody.bodyType = RigidbodyType2D.Static; return; } if (playerTouchingBlock && pushable) { objectsBody.bodyType = RigidbodyType2D.Dynamic; MoveBlock(); //BELOW here is only applicable when the player has finished moving, not during! if (currentPosition.x >= addXPosition.x) { Debug.Log(">= add X"); pushable = false; } else if (currentPosition.x <= subXPosition.x) { Debug.Log("<= sub X"); pushable = false; } else if (currentPosition.y >= addYPosition.y) { Debug.Log(">= add Y"); pushable = false; } else if (currentPosition.y <= subYPosition.y) { Debug.Log("<= sub Y"); pushable = false; } } else { Debug.Log("Want To Return RETURN"); return; } } else { objectsBody.bodyType = RigidbodyType2D.Static; } } private void OnTriggerEnter2D(Collider2D other) { if (other.CompareTag("PlayerPush")) { if (!movingSoNowUntouchable) { IAmStatic = false; pushFromPosition = other.transform.position; playerTouchingBlock = true; } } } private void OnTriggerStay2D(Collider2D other) { if (other.CompareTag("PlayerPush")) { if (!movingSoNowUntouchable) { IAmStatic = false; pushFromPosition = other.transform.position; playerTouchingBlock = true; } } } private Vector3 UpLeftRightDown(Vector3 positionOne, Vector3 postionTwo) { float diffferenceY = positionOne.y - postionTwo.y; float diffferenceX = positionOne.x - postionTwo.x; if (Mathf.Abs(diffferenceY) > Mathf.Abs(diffferenceX)) { if (diffferenceY > 0) { objectsBody.constraints = RigidbodyConstraints2D.FreezePositionX; objectsBody.freezeRotation = true; Debug.Log("Touching Bottom"); return directionChange = transform.position + new Vector3(0, 1, 0); } else if (diffferenceY < 0) { objectsBody.constraints = RigidbodyConstraints2D.FreezePositionX; objectsBody.freezeRotation = true; Debug.Log("Touching Up"); return directionChange = transform.position + new Vector3(0, -1, 0); } else { Debug.Log("Touching Nothing UPDown"); objectsBody.constraints = RigidbodyConstraints2D.None; objectsBody.freezeRotation = true; return directionChange = transform.position + new Vector3(0, 0, 0); } } else { if (diffferenceX > 0) { Debug.Log("Touching Left"); objectsBody.constraints = RigidbodyConstraints2D.FreezePositionY; objectsBody.freezeRotation = true; return directionChange = transform.position + new Vector3(1, 0, 0); } else if (diffferenceX < 0) { Debug.Log("Touching Right"); objectsBody.constraints = RigidbodyConstraints2D.FreezePositionY; objectsBody.freezeRotation = true; return directionChange = transform.position + new Vector3(-1, 0, 0); } else { Debug.Log("Touching Nothing Sides"); objectsBody.constraints = RigidbodyConstraints2D.None; objectsBody.freezeRotation = true; return directionChange = transform.position + new Vector3(0, 0, 0); } } } } Thanks