module game_core.collision;

import bubel.ecs.attributes;
import bubel.ecs.block_allocator;
import bubel.ecs.core;
import bubel.ecs.std;
import bubel.ecs.vector;

import ecs_utils.math.vector;
import ecs_utils.utils;

import game_core.basic;


void registerCollisionModule(EntityManager* manager)
{
    manager.registerDependency(ShootGridDependency);
    manager.registerDependency(BVHDependency);
    manager.registerDependency(StaticBVHDependency);

    manager.registerComponent!CShootGrid;
    manager.registerComponent!CShootGridMask;
    manager.registerComponent!CColliderScale;
    manager.registerComponent!CBVH;
    manager.registerComponent!CAABB;
    manager.registerComponent!CStatic;

    manager.registerSystem!ShootGridManager(-80);
    manager.registerSystem!ShootGridCleaner(-101);
    manager.registerSystem!BVHBuilder(-80);
    manager.registerSystem!StaticBVHBuilder(-80);
    //manager.registerSystem!BVHBuilder2(-79);
    manager.registerSystem!AABBUpdater(-81);
}

enum ShootGridDependency = "ShootGridDependency";
enum BVHDependency = "BVHDependency";
enum StaticBVHDependency = "StaticBVHDependency";

struct CShootGrid
{
    mixin ECS.Component;
}

struct CBVH
{
    mixin ECS.Component;

    uint index;
}

struct CStatic
{
    mixin ECS.Component;
}

struct CAABB
{
    mixin ECS.Component;

    alias bounding this;

    AABB bounding;
}

struct CShootGridMask
{
    mixin ECS.Component;

    alias value this;

    ubyte value;
}

struct CColliderScale
{
    mixin ECS.Component;

    alias value this;

    vec2 value = vec2(16,16);
}


struct ShootGrid
{

    ~this() @nogc nothrow
    {
        if(nodes)Mallocator.dispose(nodes);
        if(masks)Mallocator.dispose(masks);
    }

    struct Node
    {
        alias entity this;

        EntityID entity;
    }

    void create(ivec2 nodes_count, vec2 node_size)
    {
        this.size = nodes_count;
        this.node_size = node_size;
        inv_node_size = vec2(1.0/node_size.x, 1.0/node_size.y);
        nodes = Mallocator.makeArray!Node(nodes_count.x * nodes_count.y);
        masks = Mallocator.makeArray!ubyte(nodes.length);
    }

    void mark(EntityID id, vec2 beg, vec2 end, ubyte mask)
    {
        ivec2 ibeg = cast(ivec2)(beg * inv_node_size);
        ivec2 iend = cast(ivec2)(end * inv_node_size + 0.5);
        if(ibeg.x < 0)ibeg.x = 0;
        if(ibeg.y < 0)ibeg.y = 0;
        if(iend.x > size.x)iend.x = size.x;
        if(iend.y > size.y)iend.y = size.y;
        foreach(i; ibeg.y .. iend.y)
        {
            foreach(j; ibeg.x .. iend.x)
            {
                nodes[i * size.x + j] = id;
                masks[i * size.x + j] = mask;
            }
        }
    }

    void clear()
    {
        size_t size = nodes.length * EntityID.sizeof;
        memset(nodes.ptr, 0, size);
        memset(masks.ptr, 0, masks.length);
    }

    bool test(out EntityID id, vec2 beg, vec2 end, ubyte mask)
    {
        ivec2 ibeg = cast(ivec2)(beg * inv_node_size);
        ivec2 iend = cast(ivec2)(end * inv_node_size + 0.5);
        if(ibeg.x < 0)ibeg.x = 0;
        if(ibeg.y < 0)ibeg.y = 0;
        if(iend.x > size.x)iend.x = size.x;
        if(iend.y > size.y)iend.y = size.y;
        foreach(i; ibeg.y .. iend.y)
        {
            foreach(j; ibeg.x .. iend.x)
            {
                uint index = i * size.x + j;
                if(nodes[index].id != 0)
                {
                    if((masks[index] & mask) == 0)continue;
                    id = nodes[index];
                    return true;
                }
            }
        }
        return false;
    }

    bool test(out EntityID id, vec2 pos, ubyte mask)
    {
        ivec2 ipos = cast(ivec2)(pos * inv_node_size - 0.5);
        if(ipos.x < 0)ipos.x = 0;
        if(ipos.y < 0)ipos.y = 0;
        if(ipos.x >= size.x)ipos.x = size.x - 1;
        if(ipos.y >= size.y)ipos.y = size.y - 1;
        size_t index = ipos.y * size.x + ipos.x;
        if((masks[index] & mask) == 0)return false;
        if(nodes[index].id != 0)
        {
            id = nodes[index];
            return true;
        }
        return false;
    }

    vec2 inv_node_size;
    ivec2 size;
    vec2 node_size;
    Node[] nodes;
    ubyte[] masks;
}

struct ShootGridCleaner
{
    mixin ECS.System!1;

    struct EntitiesData
    {

    }

    ShootGrid* grid;

    bool onBegin()
    {
        grid = gEM.getSystem!ShootGridManager().grid;
        if(grid != null)return true;
        else return false;
    }

    void onUpdate(EntitiesData data)
    {
        if(grid)grid.clear();
    }
}

struct ShootGridManager
{
    mixin ECS.System!128;

    mixin ECS.WritableDependencies!(ShootGridDependency);

    struct EntitiesData
    {
        uint length;
        //uint thread_id;
        const (Entity)[] entity;
        @readonly CLocation[] locations;
        @readonly CShootGrid[] grid_flag;
        //@readonly CGuild[] guild;
        @optional @readonly CShootGridMask[] mask;
        @optional @readonly CScale[] scale;
        @optional @readonly CColliderScale[] collider_scale;
    }

    ShootGrid* grid;

    void onCreate()
    {
        //grid = space_invaders.shoot_grid;
        grid = Mallocator.make!ShootGrid;
        grid.create(ivec2(80,60), vec2(5,5));
    }

    void onDestroy()
    {
        Mallocator.dispose(grid);
    }

    // bool onBegin()
    // {
    //     //if(!grid)return false;
    //     //grid.clear();
    //     return true;
    // }

    void onUpdate(EntitiesData data)
    {
        vec2[] scale;
        if(data.collider_scale)scale = cast(vec2[])data.collider_scale;
        else if(data.scale)scale = cast(vec2[])data.scale;
        else return;
        if(data.mask is null)
        {
            foreach(i; 0..data.length)
            {
                vec2 half_scale = scale[i] * 0.5;
                grid.mark(data.entity[i].id, data.locations[i] - half_scale, data.locations[i] + half_scale, ubyte.max);//cast(ubyte)(1 << data.guild[i].guild));
            }
        }
        else foreach(i; 0..data.length)
        {
            vec2 half_scale = scale[i] * 0.5;
            grid.mark(data.entity[i].id, data.locations[i] - half_scale, data.locations[i] + half_scale, data.mask[i]);//cast(ubyte)(1 << data.guild[i].guild));
        }

    }
}

struct AABB
{
    vec2 size() 
    {
        return max-min;
    }

    vec2 center()
    {
        return (max+min) * 0.5;
    }

    float area()
    {
        return size.x * size.y;
    }

    void set(ref AABB base, vec2 position, float angle, vec2 scale)
    {
        import std.algorithm.comparison : max;

        float sr = sinf(angle);
		float cr = cosf(angle);
		/*mat2 m = mat2(cr,-sr,
					sr,cr);*/

        
		//vec2 pos = ;//m * ((base.max + base.min)*0.5*scale);
		vec2 size = (base.max - base.min)*scale;
		vec2[2] axis = [vec2(cr*size.x,sr*size.y),vec2(-sr*size.x,cr*size.y)];
		
	    this.max.x = max(fabs(axis[0].x),fabs(axis[1].x));
		this.max.y = max(fabs(axis[0].y),fabs(axis[1].y));
		
		this.min = -this.max;
		
		this.min += center + position;
		this.max += center + position;
    }

    void set(ref AABB base, vec2 position, vec2 scale)
    {
        vec2 size = (base.max - base.min)*scale;
		
		this.min = -size;
		this.max = size;
		
		this.min += center + position;
		this.max += center + position;
    }

    void set(ref AABB base, vec2 position, float angle)
    {
        import std.algorithm.comparison : max;

        float sr = sinf(angle);
		float cr = cosf(angle);
		/*mat2 m = mat2(cr,-sr,
					sr,cr);*/

        
		//vec2 pos = ;//m * ((base.max + base.min)*0.5*scale);
		vec2 size = (base.max - base.min);//*scale;
		vec2[2] axis = [vec2(cr*size.x,sr*size.y),vec2(-sr*size.x,cr*size.y)];
		
	    this.max.x = max(fabs(axis[0].x),fabs(axis[1].x));
		this.max.y = max(fabs(axis[0].y),fabs(axis[1].y));
		
		this.min = -this.max;
		
		this.min += center + position;
		this.max += center + position;
    }

    void set(ref AABB base, vec2 position)
    {
		min = base.min + position;
		max = base.max + position;
    }

    vec2 min;
    vec2 max;
}

bool test(AABB a, AABB b)
{
    if((a.max.x>b.min.x && a.max.y>b.min.y) &&
		(a.min.x<b.max.x && a.min.y<b.max.y))return true;
	else return false;
}

byte intersectTest(AABB a, AABB b)
{
	if(a.min.x < b.min.x && a.min.y < b.min.y &&
	a.max.x > b.max.x && a.max.y > b.max.y)return 2;
	else if((a.max.x>b.min.x && a.max.y>b.min.y) &&
		(a.min.x<b.max.x && a.min.y<b.max.y))return 1;
	return 0;
}

bool test(vec2 point, AABB b)
{
	if((point.x>b.min.x && point.y>b.min.y) &&
		(point.x<b.max.x && point.y<b.max.y))return true;
	else return false;
}

AABB merge(AABB a, AABB b)
{
    import std.algorithm.comparison: min, max;
    return AABB(vec2(min(a.min.x,b.min.x),min(a.min.y,b.min.y)),vec2(max(a.max.x,b.max.x),max(a.max.y,b.max.y)));
}

struct Quadtree
{

    Node* add(EntityID id, AABB aabb)
    {   
        ubyte depth = void;
        vec2 ratio = aabb.size / this.aabb.size;
        //if(ratio.x < ratio.y)depth = log2f();
        //else depth = 0;
        return null;
        //2^x = size2/size;
    }

    struct Node
    {

        AABB aabb;
        Node*[4] nodes;
        MemoryBlock* block;
        MemoryBlock* last_block;
    }

    struct MemoryBlock
    {
        EntityID[10] entities;
        MemoryBlock* next_block;
        void* Node;
    }
 
    struct MetaBlock
    {
        union 
        {
            MemoryBlock _alignment;
            struct 
            {
                Quadtree* quadtree;
            }
        }
    }

    AABB aabb;
    
    Node main_node;
    uint max_depth;

    BlockAllocator allocator;
}

struct BVHTree
{

    void generateTopDown()
    {

    }

    void generateBottomUp()
    {
        clearNodes();
        uint index = 0;
        while(index < nodes.length - 1)
        {
            Node* node = &nodes[index];
            if(node.parent != uint.max)
            {
                index++;
                continue;
            }
            uint best_index = 0;
            float best_cost = float.max;
            foreach(i;index+1 .. nodes.length)
            {
                Node* test_node = &nodes[i];
                if(test_node.parent != uint.max)continue;
                // vec2 rel_pos = node.bounding.center - test_node.bounding.center;
                // float cost = fabs(rel_pos.x) + fabs(rel_pos.y);
                // float cost = rel_pos.length;
                // float cost = rel_pos.length2;
                float cost = merge(node.bounding, test_node.bounding).area();
                if(cost < best_cost)
                {
                    best_cost = cost;
                    best_index = cast(uint)i;
                }
            }

            uint new_index = getNode();
            Node* new_node = &nodes[new_index];
            Node* best_node = &nodes[best_index];
            
            new_node.childs[0] = index;
            new_node.childs[1] = best_index;
            new_node.bounding = merge(best_node.bounding, node.bounding);

            best_node.parent = new_index;
            node.parent = new_index;
        }

        root = cast(uint)nodes.length - 1;
    }

    uint addIncrementally(AABB bounding, EntityID id)
    {
        if(root == uint.max)
        {
            root = getNode();
            Node* new_node = &nodes[root];
            new_node.parent = uint.max;
            new_node.entity = id;
            new_node.bounding = bounding;
            //new_node.childs = [uint.max, uint.max].staticArray;
            new_node.childs[0] = uint.max;
            new_node.childs[1] = uint.max;
            return root;
        }

        float cost = float.max;
        uint best_index = 0;
        findBest(bounding, root, best_index, cost, 0);

        uint new_index = getNode();
        uint leaf_index = getNode();

        Node* new_node = &nodes[new_index];
        Node* node = &nodes[best_index];
        Node* parent = &nodes[node.parent];
        Node* leaf_node = &nodes[leaf_index];

        leaf_node.entity = id;
        leaf_node.bounding = bounding;
        leaf_node.parent = new_index;

        new_node.parent = node.parent;
        new_node.childs[0] = best_index;
        new_node.childs[1] = leaf_index;
        new_node.bounding = merge(bounding, node.bounding);

        if(node.parent != uint.max)
        {
            if(parent.childs[0] == best_index)
            {
                parent.childs[0] = new_index;
            }
            else
            {
                parent.childs[1] = new_index;
            }
        }
        else
        {
            root = new_index;
        }

        node.parent = new_index;

        uint index = new_node.parent;

        while(index != uint.max)
        {
            Node* lnode = &nodes[index];

            recalculate(lnode);

            rotate(lnode);

            index = lnode.parent;
        }

        return leaf_index;
    }

    void clearNodes()
    {
        root = uint.max;
        uint i = 0;
        while(i < nodes.length)
        {
            Node* node = &nodes[i];
            if(node.childs[0] == uint.max)
            {
                node.parent = uint.max;
                i++;
            }
            else 
            {
                removeNode(i);
            }
        }
    }

    void findBest(AABB bounding, uint node_index, ref uint best_index, ref float best_cost, float cost)
    {
        Node* node = &nodes[node_index];
        //float area = nodes.bounding.area;
        AABB new_bounding = merge(node.bounding, bounding);
        float new_area = new_bounding.area;
        if(new_area + cost < best_cost)
        {
            best_index = node_index;
            best_cost = cost + new_area;
        }

        if(node.childs[0] == uint.max)return;
        float area_delta = new_area - node.bounding.area;

        if(bounding.area + area_delta + cost < best_cost)
        {
            findBest(bounding, node.childs[0], best_index, best_cost, cost + area_delta);
            findBest(bounding, node.childs[1], best_index, best_cost, cost + area_delta);
        }
    }

    void add(AABB bounding, EntityID id)
    {
        Node* node = &nodes[getNode()];
        node.entity = id;
        node.bounding = bounding;
        // node.childs = [uint.max,uint.max];
        node.childs[0] = uint.max;
        node.childs[1] = uint.max;
    }

    void test(AABB bounding, bool delegate(EntityID id) callback)
    {
        bool traverse(Node* node)
        {
            if(.test(bounding, node.bounding))
            {
                if(node.childs[0] == uint.max)
                {
                    return callback(node.entity);
                }
                if(!traverse(&nodes[node.childs[0]]))return false;
                if(!traverse(&nodes[node.childs[1]]))return false;
            }
        /*node.bounding.max.x = max(nodes[node.childs[0]].bounding.max.x,nodes[node.childs[1]].bounding.max.x);
        node.bounding.max.y = max(nodes[node.childs[0]].bounding.max.y,nodes[node.childs[1]].bounding.max.y);
        node.bounding.min.x = min(nodes[node.childs[0]].bounding.min.x,nodes[node.childs[1]].bounding.min.x);
        node.bounding.min.y = min(nodes[node.childs[0]].bounding.min.y,nodes[node.childs[1]].bounding.min.y);*/
            return true;
        }

        if(root < nodes.length)traverse(&nodes[root]);
    }

    float computeCost()
    {
        float cost = 0;
        foreach(ref Node node;nodes)
        {
            if(node.childs[0] != uint.max)
            {
                cost += node.bounding.area();
            }
        }
        return cost;
    }

    void recalculate(Node* node)
    {
        import std.algorithm.comparison: min, max;
        node.bounding = merge(nodes[node.childs[0]].bounding, nodes[node.childs[1]].bounding);
    }

    void rotate(Node* node)
    {
        import std.algorithm.comparison: min, max;
        if(node.parent == uint.max)return;
        Node* parent = &nodes[node.parent];
        Node* child1 = &nodes[node.childs[0]];
        Node* child2 = &nodes[node.childs[1]];
        
        uint child_index = void;
        if(parent.childs[0] == child1.parent)
        {
            child_index = 1;
        }
        else
        {
            child_index = 0;
        }
        Node* to_rotate = &nodes[parent.childs[child_index]];

        float cost = node.bounding.area();
        AABB bounding1 = merge(child1.bounding, to_rotate.bounding);
        AABB bounding2 = merge(child2.bounding, to_rotate.bounding);
        float area1 = bounding1.area;
        float area2 = bounding2.area;
        if(area1 < area2)
        {
            if(area1 < cost)
            {
                to_rotate.parent = child1.parent;
                child2.parent = node.parent;
                uint swap_index = node.childs[1];
                node.childs[1] = parent.childs[child_index];
                parent.childs[child_index] = swap_index;
                node.bounding = bounding1;
            }
        }
        else
        {
            if(area2 < cost)
            {
                to_rotate.parent = child1.parent;
                child1.parent = node.parent;
                uint swap_index = node.childs[0];
                node.childs[0] = parent.childs[child_index];
                parent.childs[child_index] = swap_index;
                node.bounding = bounding2;
            }
        }
    }

    void remove(uint i)
    {
        //foreach(i, ref Node node; nodes)
        //{
        // if(node.entity == id)
        // {
        Node* node = &nodes[i];
        if(node.parent != uint.max)
        {
            ///parent isn't root, most common beaviour
            Node* parent = &nodes[node.parent];
            if(parent.parent == uint.max)
            {
                //delete leaf attached to root
                if(parent.childs[0] == i)
                {
                    root = parent.childs[1];
                    nodes[parent.childs[1]].parent = uint.max;
                }
                else 
                {
                    root = parent.childs[0];
                    nodes[parent.childs[0]].parent = uint.max;
                }
            }
            else 
            {
                ///remove node from inside of tree
                Node* grand_parent = &nodes[parent.parent];
                uint remain_index = void;
                if(parent.childs[0] == i)remain_index = parent.childs[1];
                else remain_index = parent.childs[0];
                if(grand_parent.childs[0] == node.parent)
                {
                    grand_parent.childs[0] = remain_index;
                    nodes[remain_index].parent = parent.parent;
                }
                else 
                {
                    grand_parent.childs[1] = remain_index;
                    nodes[remain_index].parent = parent.parent;
                }

                uint index = parent.parent;

                while(index != uint.max)
                {
                    Node* lnode = &nodes[index];

                    recalculate(lnode);

                    rotate(lnode);

                    index = lnode.parent;
                }
            }
            removeNode(node.parent);
        }
        else root = uint.max;
        removeNode(cast(uint)i);
                //return;
            //}
        //}
    }

    void clear()
    {
        last_node = uint.max;
        nodes.clear();
        root = uint.max;
    }

    uint getNode()
    {
        if(last_node == uint.max)
        {
            //nodes.length = nodes.length + 1;
            nodes.add(Node());
            return cast(uint)nodes.length - 1;
        }
        else 
        {
            uint ret = last_node;
            last_node = nodes[last_node].parent;
            nodes[ret] = Node();
            return ret;
        }
    }

    void removeNode(uint index)
    {
        Node* node = &nodes[index];
        node.parent = last_node;
        node.ptr = null;
        last_node = index;
    }

    /*void create()
    {
        root = getNode();
    }*/

    struct Node
    {
        union 
        {
            EntityID entity;
            void* ptr;
        }   
        AABB bounding;
        uint parent = uint.max;
        union 
        {
            struct //workaround betterC compilation issue with _memset
            {
                uint _c1 = uint.max;
                uint _c2 = uint.max;
            }
            uint[2] childs;
        }
        
    }

    Vector!Node nodes;
    //uint nodes_count;
    uint last_node = uint.max;
    uint root = uint.max;
}

struct StaticBVHBuilder
{
    mixin ECS.System!1;

    mixin ECS.WritableDependencies!(StaticBVHDependency);

    struct EntitiesData
    {
        uint length;
        //uint thread_id;
        const (Entity)[] entity;
        CBVH[] bvh;
        @readonly CStatic[] static_flag;
        @readonly CLocation[] locations;
        @readonly CAABB[] bounding;
    }

    BVHTree* tree;

    void onCreate()
    {
        tree = Mallocator.make!BVHTree;
    }

    void onDestroy()
    {
        Mallocator.dispose(tree);
    }

    // bool onBegin()
    // {
    //     tree.clear();
    //     return true;
    //     // return false;
    // }

    void onAddEntity(EntitiesData data)
    {
        foreach(i;0..data.length)
        {
            data.bvh[i].index = tree.addIncrementally(data.bounding[i], data.entity[i].id);
        }
    }

    void onRemoveEntity(EntitiesData data)
    {
        foreach(i;0..data.length)
        {
            tree.remove(data.bvh[i].index);
        }
    }

    // void onUpdate(EntitiesData data)
    // {
    //     foreach(i; 0..data.length)
    //     {
    //         // tree.add(data.bounding[i], data.entity[i].id);
    //         tree.addIncrementally(data.bounding[i], data.entity[i].id);
    //     }

    //     import std.stdio;
    //     writeln("Cost: ",tree.computeCost());
    // }
}

struct BVHBuilder
{
    mixin ECS.System!1;

    mixin ECS.WritableDependencies!(BVHDependency);

    struct EntitiesData
    {
        uint length;
        //uint thread_id;
        const (Entity)[] entity;
        CBVH[] bvh;
        @readonly CLocation[] locations;
        @readonly CAABB[] bounding;
    }

    mixin ECS.ExcludedComponents!(CStatic);

    BVHTree* tree;

    void onCreate()
    {
        tree = Mallocator.make!BVHTree;
    }

    void onDestroy()
    {
        Mallocator.dispose(tree);
    }

    bool onBegin()
    {
        tree.clear();
        return true;
        // return false;
    }

    // void onAddEntity(EntitiesData data)
    // {
    //     foreach(i;0..data.length)
    //     {
    //         tree.add(data.bounding[i], data.entity[i].id);
    //     }
    // }

    // void onRemoveEntity(EntitiesData data)
    // {
    //     foreach(i;0..data.length)
    //     {
    //         tree.remove(data.entity[i].id);
    //     }
    // }

    void onUpdate(EntitiesData data)
    {
        foreach(i; 0..data.length)
        {
            // tree.add(data.bounding[i], data.entity[i].id);
            data.bvh[i].index = tree.addIncrementally(data.bounding[i], data.entity[i].id);
        }

        // import std.stdio;
        // writeln("Cost: ",tree.computeCost());
    }
}
/*
struct BVHBuilder2
{
    mixin ECS.System!1;

    mixin ECS.WritableDependencies!(BVHDependency);

    struct EntitiesData
    {
    }

    BVHTree* tree;

    void onCreate()
    {
        tree = gEM.getSystem!BVHBuilder().tree;
    }

    bool onBegin()
    {
        //if(tree.nodes.length-1 != tree.root)tree.clear();
        return true;
    }

    void onUpdate(EntitiesData data)
    {
        if(tree.nodes.length-1 != tree.root)
        {
            tree.generateBottomUp();
            import std.stdio;
            writeln("Cost: ",tree.computeCost());
        }
    }
}//*/

struct AABBUpdater
{
    mixin ECS.System!64;

    struct EntitiesData
    {
        uint length;
        //uint thread_id;
        const (Entity)[] entity;
        CAABB[] bounding;

        @readonly CLocation[] location;
        @readonly CScale[] scale;
        @optional @readonly CRotation[] rotation;
    }

    void onAddEntity(EntitiesData data)
    {
        foreach(i; 0..data.length)
        {
            data.bounding[i] = AABB(data.location[i]-data.scale[i],data.location[i]+data.scale[i]);
        }
    }

    void onUpdate(EntitiesData data)
    {
        foreach(i; 0..data.length)
        {
            data.bounding[i] = AABB(data.location[i]-data.scale[i],data.location[i]+data.scale[i]);
        }
    }
}