Single threaded quadtree

using System.Collections.Generic;
using Unity.VisualScripting;
using UnityEngine;
using Unity.Mathematics;
using Particle = Simulation.Particle;

struct Node
{
    public Quad boundary;
    public float mass;
    public float2 centerOfMass;
    public int childIndex;
    public int next;
}
public struct Quad
{
    //Bottom left of rectangle
    public Vector2 position;
    public float size;

    public int FindNode(Vector2 pos)
    {
        bool wX = pos.x - position.x < size / 2;
        bool sY = pos.y - position.y < size / 2;

        if (wX && !sY) return 0;
        if (!wX && !sY) return 1;
        if (wX && sY) return 2;
        if(!wX && sY) return 3;

        return 0;
    }
}

public class QuadTree
{
    List<Node> quadTree;

    public QuadTree()
    {
        quadTree = new();
        Quad boundary = new() { position = new Vector2(-5, -5), size = 10 };
        quadTree.Add(new Node
        {
            boundary = boundary,
            next = 0
        });
    }

    public void DrawTree()
    {
        foreach (var node in quadTree)
        {
            DrawSquare(node.boundary.position, node.boundary.size);
        }
    }
    static void DrawSquare(Vector2 position, float size)
    {
        Debug.DrawLine(position, new Vector2(position.x, position.y + size), Color.red);
        Debug.DrawLine(new Vector2(position.x, position.y + size), new Vector2(position.x + size, position.y + size), Color.red);
        Debug.DrawLine(new Vector2(position.x + size, position.y + size), new Vector2(position.x + size, position.y), Color.red);
        Debug.DrawLine(new Vector2(position.x + size, position.y), new Vector2(position.x, position.y), Color.red);
    }

    public Vector2 CalculateAcceleration(Particle particle, float theta, float epsilon, float deltaTime)
    {
        Vector2 acceleration = new();
        float thetaSqr = theta * theta;
        float epsilonSqr = epsilon * epsilon;

        int nodeIndex = 0;
        while (true)
        {
            Node node = quadTree[nodeIndex];
            Vector2 diff = node.centerOfMass - particle.position;
            float sqrDst = Mathf.Max(diff.sqrMagnitude, 0.05f);

            if (node.childIndex == 0 || node.boundary.size * node.boundary.size < sqrDst * thetaSqr)
            {
                float denom = (sqrDst + epsilonSqr) * Mathf.Sqrt(sqrDst);
                acceleration += diff * node.mass / denom * deltaTime;

                if (node.next == 0) return acceleration;
                nodeIndex = node.next;
            }
            else nodeIndex = node.childIndex;
        }
    }

    public void Propagate()
    {
        for (int i = quadTree.Count - 1; i > 0; i--)
        {
            if(quadTree[i].childIndex == 0) continue;

            int child = quadTree[i].childIndex;
            Node node = quadTree[i];

            node.centerOfMass = quadTree[child].centerOfMass * quadTree[child].mass
                                + quadTree[child + 1].centerOfMass * quadTree[child + 1].mass
                                + quadTree[child + 2].centerOfMass * quadTree[child + 2].mass
                                + quadTree[child + 3].centerOfMass * quadTree[child + 3].mass;
            node.mass = quadTree[child].mass
                        + quadTree[child + 1].mass
                        + quadTree[child + 2].mass
                        + quadTree[child + 3].mass;
            node.centerOfMass /= node.mass;
            quadTree[i] = node;

        }
    }

    public void Insert(Particle particle)
    {
        int nodeIndex = 0;
        while (quadTree[nodeIndex].childIndex != 0)
        {
            int quad = quadTree[nodeIndex].boundary.FindNode(particle.position);
            nodeIndex = quad + quadTree[nodeIndex].childIndex;
        }

        if (quadTree[nodeIndex].mass == 0)
        {
            Node n = quadTree[nodeIndex];
            n.centerOfMass = particle.position;
            n.mass = particle.mass;
            quadTree[nodeIndex] = n;
            return;
        }

        if (quadTree[nodeIndex].centerOfMass == particle.position)
        {
            Node n = quadTree[nodeIndex];
            n.mass += particle.mass;
            quadTree[nodeIndex] = n;
            return;
        }

        Vector2 nodePos = quadTree[nodeIndex].centerOfMass;
        float nodeMass = quadTree[nodeIndex].mass;
        while (true)
        {
            Subdivide(nodeIndex);
            int childIndex = quadTree[nodeIndex].childIndex;
            int q1 = quadTree[nodeIndex].boundary.FindNode(nodePos);
            int q2 = quadTree[nodeIndex].boundary.FindNode(particle.position);

            if (q1 == q2)
            {
                nodeIndex = q1 + childIndex;
            }
            else
            {
                int n1 = q1 + childIndex;
                int n2 = q2 + childIndex;

                quadTree[n1] = new Node
                {
                    centerOfMass = nodePos,
                    mass = nodeMass,
                    boundary = quadTree[n1].boundary,
                    next = quadTree[n1].next
                };
                quadTree[n2] = new Node
                {
                    centerOfMass = particle.position,
                    mass = particle.mass,
                    boundary = quadTree[n2].boundary,
                    next = quadTree[n2].next
                };
                return;
            }
        }
    }

    void Subdivide(int parentIndex)
    {
        Node parent = quadTree[parentIndex];
        parent.childIndex = quadTree.Count;
        quadTree[parentIndex] = parent;
        Vector2 parentPos = parent.boundary.position;
        Vector2 nwPosition = new(parentPos.x, parentPos.y + parent.boundary.size / 2);
        Vector2 nePosition = new(parentPos.x + parent.boundary.size / 2, parentPos.y + parent.boundary.size / 2);
        Vector2 swPosition = new(parentPos.x, parentPos.y);
        Vector2 sePosition = new(parentPos.x + parent.boundary.size / 2, parentPos.y);

        Node nw = new()
        {
            boundary = new Quad { position = nwPosition, size = parent.boundary.size / 2 },
            next = parent.childIndex + 1
        };
        Node ne = new()
        {
            boundary = new Quad { position = nePosition, size = parent.boundary.size / 2 },
            next = parent.childIndex + 2
        };
        Node sw = new()
        {
            boundary = new Quad { position = swPosition, size = parent.boundary.size / 2 },
            next = parent.childIndex + 3
        };
        Node se = new()
        {
            boundary = new Quad { position = sePosition, size = parent.boundary.size / 2 },
            next = parent.next
        };

        quadTree.Add(nw);
        quadTree.Add(ne);
        quadTree.Add(sw);
        quadTree.Add(se);
    }
}