package nz.ac.massey.javaecs.examples.Systems;

import java.util.ArrayList;
import java.util.BitSet;
import java.util.HashSet;
import java.util.List;
import java.util.Set;
import java.util.stream.Stream;

import nz.ac.massey.javaecs.ECSSystem;
import nz.ac.massey.javaecs.Engine;
import nz.ac.massey.javaecs.Entity;
import nz.ac.massey.javaecs.examples.Components.Collider;
import nz.ac.massey.javaecs.examples.Components.Gravity;
import nz.ac.massey.javaecs.examples.Components.RidgidBody;
import nz.ac.massey.javaecs.examples.Components.Vec2D;

public class CollisionSystem extends ECSSystem {
    Engine gameEngine;

    public CollisionSystem(Engine gameEngine) {
        this.gameEngine = gameEngine;
        // Set registrations
        registrationSet = new BitSet();
        registrationSet.set(gameEngine.getComponentIndex(Vec2D.class));
        registrationSet.set(gameEngine.getComponentIndex(Collider.class));
        registrationSet.set(gameEngine.getComponentIndex(RidgidBody.class));
    }

    @Override
    public void init() {
        // TODO Auto-generated method stub

    }
    public void updateNew() {
        Set<Entity> processed = new HashSet<>();
        // Gets all items we have collided with
        for (Entity entity : entities) {
            RidgidBody ridgidBody = (RidgidBody) gameEngine.getComponentData(entity, RidgidBody.class);
            // Only considering elements with mass just now
            if (ridgidBody.mass > 0){
                Vec2D pos = (Vec2D) gameEngine.getComponentData(entity, Vec2D.class);
                Collider collider = (Collider) gameEngine.getComponentData(entity, Collider.class);

                // The pos of each bounding line of the entity
                // i.e.
                //        
                //   -------------
                //   |###########|
                //   |###########|
                //   |###########|
                //   |###########|
                //   -------------
                //        
                double eleft = pos.x;
                double ebottom = pos.y;
                double eright = pos.x + collider.x; 
                double etop = pos.y + collider.y;

                double evectorAngle = calcAngle(ridgidBody.xVel, ridgidBody.yVel);


                if (ridgidBody.mass > 0){
                    // Dont't recalc for entities that aren't static
                    processed.add(entity);
                }

                for (Entity entity2 : entities) {
                    if (!processed.contains(entity2)) {
                        // skip entities that have already been processed
                        Vec2D oPos = (Vec2D) gameEngine.getComponentData(entity2, Vec2D.class);
                        RidgidBody oRidgidBody = (RidgidBody) gameEngine.getComponentData(entity2, RidgidBody.class);
                        Collider oCollider = (Collider) gameEngine.getComponentData(entity2, Collider.class);

                        double oleft = oPos.x;
                        double obottom = oPos.y;
                        double oright = oPos.x + oCollider.x;
                        double otop = oPos.y + oCollider.y;

                        // Check if a collision occured
                        // Few conditions required, that is, 
                        // - oS2 must be greater than eS0, else they aren't close
                        // - eS2 must be greater than oS0, for the same reason
                        // - either 
                        if (!(eleft > oright || oleft > eright || etop < obottom || otop < ebottom)){
                            // Collided
                            // Grab the angle the other is moving, to figure the quadrant to move to
                            double ovectorAngle = calcAngle(oRidgidBody.xVel, oRidgidBody.yVel);
                            // Grab the vector sums
                            double xVelSum = ridgidBody.xVel + oRidgidBody.xVel;
                            double yVelSum = ridgidBody.yVel + oRidgidBody.yVel;
                            // Collision occurred
                        }
                    }
                }
            }
            
        }
    }

    @Override
    public void update() {
        // Parallelising this loop ()
        Set<Entity> processed = new HashSet<>();
        // Gets all items we have collided with
        for (Entity entity : entities) {
            Vec2D pos = (Vec2D) gameEngine.getComponentData(entity, Vec2D.class);
            RidgidBody ridgidBody = (RidgidBody) gameEngine.getComponentData(entity, RidgidBody.class);
            Collider collider = (Collider) gameEngine.getComponentData(entity, Collider.class);
            if (ridgidBody.mass > 0){
                // Only ignore non-static colliders
                processed.add(entity);
            }
            for (Entity entity2 : entities) {
                if (!processed.contains(entity2)) {
                    // skip entities that have already been processed
                    Vec2D otherPos = (Vec2D) gameEngine.getComponentData(entity2, Vec2D.class);
                    RidgidBody otherRidgidBody = (RidgidBody) gameEngine.getComponentData(entity2, RidgidBody.class);
                    Collider otherCollider = (Collider) gameEngine.getComponentData(entity2, Collider.class);
                

                    if (ridgidBody.mass >= 0 || otherRidgidBody.mass >= 0) {
                        // Evaluate a collision in the top-left
                        boolean tlCollide = pos.x > otherPos.x && pos.x < otherPos.x + otherCollider.x
                                && pos.y > otherPos.y && pos.y < otherPos.y + otherCollider.y;
                        // Evaluate a collision in the bottom-left
                        boolean blCollide = pos.x > otherPos.x && pos.x < otherPos.x + otherCollider.x
                                && pos.y + collider.y > otherPos.y && pos.y + collider.y < otherPos.y + otherCollider.y;
                        // Evaluate a collision in the bottom-right
                        boolean brCollide = pos.x + collider.x > otherPos.x
                                && pos.x + collider.x < otherPos.x + otherCollider.x && pos.y + collider.y > otherPos.y
                                && pos.y + collider.y < otherPos.y + otherCollider.y;
                        // Evaluate a collision in the top-right
                        boolean trCollide = pos.x + collider.x > otherPos.x
                                && pos.x + collider.x < otherPos.x + otherCollider.x && pos.y > otherPos.y
                                && pos.y < otherPos.y + otherCollider.y;
                        if (tlCollide || blCollide || brCollide || trCollide) {
                            // Determine collision nature
                            if (tlCollide) {
                                int k = 0;
                            }
                            if (trCollide) {
                                int k = 0;
                            }
                            if (blCollide) {
                                int k = 0;
                            }
                            if (brCollide) {
                                int k = 0;
                            }
                            // Get the vels
                            double totalXVel = Math.abs(ridgidBody.xVel) + Math.abs(otherRidgidBody.xVel);
                            double totalYVel = Math.abs(ridgidBody.yVel) + Math.abs(otherRidgidBody.yVel);
                            double totalMass = ridgidBody.mass + otherRidgidBody.mass;
                            int xMult = 0;
                            int yMult = 0;
                            boolean fullEdge = false;
                            // left-hand collision
                            if (tlCollide && blCollide) {
                                fullEdge = true;
                                xMult = 2;
                            }
                            // Bottom collision
                            if (blCollide && brCollide) {
                                fullEdge = true;
                                yMult = -2;
                            }
                            // right-hand collision
                            if (brCollide && trCollide) {
                                fullEdge = true;
                                xMult = -2;
                            }
                            // top collision
                            if (trCollide && tlCollide) {
                                fullEdge = true;
                                yMult = 2;
                            }
                            if (fullEdge) {
                                // Finally, actuate the calculation
                                if (ridgidBody.mass < 0) {
                                    // entity 1 is immovable
                                    otherRidgidBody.xVel += totalXVel * (-1 * xMult);
                                    otherRidgidBody.yVel += totalYVel * (-1 * yMult);

                                    Object grav = gameEngine.getComponentData(entity, Gravity.class);
                                    if (grav != null) {
                                        Gravity gravity = (Gravity) grav;
                                        otherRidgidBody.xAcc += gravity.x * -1.;
                                        otherRidgidBody.yAcc += gravity.y * -1.;
                                    }

                                } else if (otherRidgidBody.mass < 0) {
                                    // entity 2 is immovable; full acceleration applied to the current entity
                                    ridgidBody.xVel += totalXVel * xMult;
                                    ridgidBody.yVel += totalYVel * yMult;
                                    Object grav = gameEngine.getComponentData(entity, Gravity.class);
                                    if (grav != null) {
                                        // negate gravity while collided
                                        Gravity gravity = (Gravity) grav;
                                        ridgidBody.xAcc += gravity.x * -1.;
                                        ridgidBody.yAcc += gravity.y * -1.;
                                    }
                                } else {
                                    // collision is elastic; add the forces x & y and the masses, and share between
                                    // objects
                                    /*ridgidBody.xVel += (ridgidBody.mass / totalMass) * totalXVel * xMult;
                                    otherRidgidBody.xVel += (otherRidgidBody.mass / totalMass) * totalXVel
                                            * (-1 * xMult);

                                    ridgidBody.yVel += (ridgidBody.mass / totalMass) * totalYVel * yMult;
                                    otherRidgidBody.yVel += (otherRidgidBody.mass / totalMass) * totalYVel
                                            * (-1 * yMult);*/
                                }
                            }
                            else{
                            }
                        }
                    }
                }
            } 
        }
    }

    /**
     * Zero degrees is straight up,
     * 90 east
     * -90 (270) = west
     * 180 south
     * @param xVel
     * @param yVel
     * @return
     */
    public static double calcAngle(double xVel, double yVel){
        if (xVel == 0){ // Div zero; = +- 90
            return yVel < 0 ? 270 : 90;
        }
        else if (xVel < 0 && yVel < 0){ // bottom-left quad (180 - 270)
            return 180. + Math.tanh(yVel/xVel);
        }
        else if (xVel < 0){ // Top left quad (270 - 360)
            return 360 - Math.tanh(yVel/xVel);
        }
        else if (yVel < 0){
            return 180 - Math.tanh(yVel/xVel);
        }
        else{
            return Math.tanh(yVel/xVel);
        }
        
    }

}
