#include "world.h"
#include <math.h>
#include "config.h"
#include "collision.h"
#include "player.h"
#include <stdio.h>
void checkCollision(){
    for(int u=0; u<=MAX_ENTITIES-1; u++){
        Entity *en1 = &entities[u];
        if(!en1->active){ 
        continue;
        }
        en1->headgrounded = false;
        for(int j=u+1; j<=MAX_ENTITIES-1; j++){
            if(u!=j){
                Entity *en2 = &entities[j];
                if(!en2->active){ 
                continue;
                }
                //calculate overlap
                float half = (en1->size + en2->size) * 0.5f;
                float overlapXcheck = half - fabsf(en1->position.x - en2->position.x);
                float overlapXapply = ((en1->size + en2->size) * 0.43f) - fabsf(en1->position.x - en2->position.x);
                float overlapY = half - fabsf(en1->position.y - en2->position.y);
                // check if any overlap
                if (overlapXcheck > 0 && overlapY > 0)
                {
                    if (overlapY < overlapXapply)
                    {
                        if (en1->position.y < en2->position.y)
                        {
                            // if the first entity is above the second put norml force on first entity
                            en1->position.y = en2->position.y - half;
                            en1->velocity.y = 0;
                            en1->velocity.x *= en2->drag;
                            en1->headgrounded = true;
                        }
                        else
                        {
                            // if the second entity is above the fisrt put norml force on second entity
                            en2->position.y = en1->position.y - half;
                            en2->velocity.y = 0;
                            en2->velocity.x *= en1->drag;
                            en2->headgrounded = true;
                        }
                    }
                    else
                    {
                        // push apart calculation
                        float forcex = fabsf(((en1->mass+en2->mass)/2)*((((en1->velocity.x)+(scale))+((en2->velocity.x)+(scale)))/2))*(scale*1.5);
                        //direction resolution
                 if(en1->position.x < en2->position.x){
                    //kill velocity so they cant go throughe achother
                        en1->velocity.x=0;
                        en2->velocity.x=0;
                        ApplyForce(
                            en1,
                            (Vector2){-forcex, 0}
                        );
                        ApplyForce(
                            en2,
                            (Vector2){forcex, 0}
                        );
                    }
                    else{
                        en1->velocity.x=0;
                        en2->velocity.x=0;
                        ApplyForce(
                            en1,
                            (Vector2){forcex, 0}
                        );
                        ApplyForce(
                            en2,
                            (Vector2){-forcex, 0}
                        );
                    }
                        }
                }
            }
        }
    } 
}