quadtree draft

using System.Collections.Generic;
using Unity.VisualScripting;
using UnityEngine;
using Unity.Mathematics;
using Unity.Collections;
using Unity.Jobs;
using Unity.Burst;
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);
        float halfScreenSize = Simulation.HalfScreenSize;
        Quad boundary = new() { position = new Vector2(-halfScreenSize, -halfScreenSize), size = halfScreenSize * 2 };
        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()
    {
        int count = nodes.Length - 1;
        for (int i = count; i > 0; i--)
        {
            int child = nodes[i].childIndex;
            if(child == 0) continue;

            Node node = nodes[i];
            Node nw = nodes[child];
            Node ne = nodes[child + 1];
            Node sw = nodes[child + 2];
            Node se = nodes[child + 3];
            node.centerOfMass = nw.centerOfMass * nw.mass
                                + ne.centerOfMass * ne.mass
                                + sw.centerOfMass * sw.mass
                                + se.centerOfMass * se.mass;
            node.mass = nw.mass
                        + ne.mass
                        + sw.mass
                        + se.mass;
            node.centerOfMass /= node.mass;
            nodes[i] = node;

        }
    }

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

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

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

        Vector2 nodePos = node.centerOfMass;
        float nodeMass = node.mass;
        while (true)
        {
            Subdivide(nodeIndex);
            node = nodes[nodeIndex];
            int childIndex = node.childIndex;
            int q1 = node.boundary.FindNode(nodePos);
            int q2 = node.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);
    }
}