QuadTree draft

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

public 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
{
    public NativeList<Node> nodes;

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

    ~QuadTree()
    {
        nodes.Dispose();
    }

    public void DrawTree()
    {
        foreach (var node in nodes)
        {
            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 void Propagate()
    {
        for (int i = nodes.Length - 1; i > 0; i--)
        {
            if(nodes[i].childIndex == 0) continue;

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

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

        }
    }

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

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

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

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

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

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

    void Subdivide(int parentIndex)
    {
        Node parent = nodes[parentIndex];
        parent.childIndex = nodes.Length;
        nodes[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
        };

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