package asteroids.model;

import be.kuleuven.cs.som.annotate.Basic;
import be.kuleuven.cs.som.annotate.Immutable;
import be.kuleuven.cs.som.annotate.Raw;

public class Entity {

    /**
     * Initialize this new entity with given x- and y-coordinates for the position, x-and y-coordinate
     * for the velocity and the given radius.
     *
     * @param  x
     *         the x-coordinate of the position for this new entity.
     * @param  y
     *         the y-coordinate of the position for this new entity.
     * @param  velocityx
     *         The x-coordinate of the velocity for this new entity.
     * @param  velocityy
     *         The y-coordinate of the velocity for this new entity.
     * @param  radius
     *         The radius for this new entity.
     * @throws IllegalNumberException
     *        The given number is not valid
     *         | ! isValidNumber(number)
     *@throw   IllegalNumberException
     *        the given radius is not valid.
     *        | ! canHaveAsRadius(radius)
     * @post   The position of this new entity is equal to a double[] with coordinates the given x and y.
     *         |new.getPosition()=={x,y}.
     * @post   If the given x-coordinate and y-coordinate are valid coordinates for the velocity of any entity
     *        and the speed is valid for any entity, the velocity of this new entity is equal to
     *        a double[] with coordinates the given x and y.
     *         Otherwise, the velocity of this new entity is equal to {300000/sqrt(2) , 300000/sqrt(2)}.
     *         | if (isValidNumber(velocityx) && IsValidNumber(velocityy) && isValidSpeed(new.speed) )
     *         |    then new.getVelocity() == {velocityx , velocityy}
     *         | else new.getVelocity() == {300000/Math.sqrt(2) , 300000/Math.sqrt(2)}
     * @post   The radius of this new entity is equal to the given radius.
     *         | new.getRadius()== radius
     */
    public Entity(double x, double y, double velocityx, double velocityy, double radius)
            throws IllegalNumberException {
        setPosition(x,y);
        setVelocity(velocityx,velocityy);

        this.radius = radius;
    }

    /**
     * Terminate this entity.
     *
     * @post   This entity  is terminated.
     *       | new.isTerminated()
     * @post   ...
     *       | ...
     */
     public void terminate() {
         this.isTerminated = true;
     }

     /**
      * Return a boolean indicating whether or not this entity
      * is terminated.
      */
     @Basic @Raw
     public boolean isTerminated() {
         return this.isTerminated;
     }

     /**
      * Variable registering whether this person is terminated.
      */
     private boolean isTerminated = false;



    /**
    * Check whether the given number is a valid number for any entity
    * @param number
    *        The number to check.
    * @return true if the number is not infinitive and not NaN.
    *              |result == (number != infinity && number!= -infinity && number!=NaN)
    */
    public static boolean isValidNumber(double number) {
        return number != Double.POSITIVE_INFINITY && number != Double.NEGATIVE_INFINITY && ! Double.isNaN(number);
    }


    //Methods about the position of the entity

    /**
     * @return the position of this entity.
     */

    @Basic @Raw
    public double[] getPosition() {
        return this.position;
    }

    /**
     * set the position of the entity to {x,y} with x and y the given coordinates.
     * @param  x
     *         the new x-coordinate for this entity.
     * @param  y
     *         the new y-coordinate for this entity.
     * @post   If the x- and y-coordinates are valid, the position is equal to the double[] {x,y}.
     *         | new.position=Double[] {x,y}
     * @throws IllegalNumberException
     *         The given x- or y-coordinate is not valid for the position of this entity.
     *         | ! isValidNumber(x) || ! isValidNumber(y)
     */


    public void setPosition(double x,double y) throws IllegalNumberException {
        if ( !isValidNumber(x))
            throw new IllegalNumberException(x);
        if ( !isValidNumber(y))
            throw new IllegalNumberException(y);
        double[] loc = {x,y};
        this.position = loc;
    }

    /**
     * variable registering the position of this entity.
     */
    private double[] position;


    //methods about the velocity of the entity

    /**
     * @return the x-coordinate of the velocity of this entity.
     */
    @Basic @Raw
    public double getVelocityx() {
        return this.xvelocity;
    }

    /**
     *
     * @return the y-coordinate of the velocity of this entity.
     */
    @Basic @Raw
    public double getVelocityy() {
        return this.yvelocity;
    }

    /**
     * Set the x-coordinate of the velocity of this entity to the given coordinate.
     *
     * @param  velocityx
     *         The new x-coordinate for the velocity for this entity.
     * @post   If the given x-coordinate is a valid coordinate for any entity,
     *          the x-coordinate of the velocity of this new entity is equal to the given
     *          coordinate.
     *          | if ( isValidNumber(velocityx))
     *          |   then new.getVelocityx() == velocityx
     */
    @Raw
    public void setVelocityx(double velocityx) {
        if (! isValidNumber(velocityx))
            this.xvelocity =  0;
        else
            this.xvelocity= velocityx;
    }
    /**
     * Set the y-coordinate of the velocity of this entity to the given coordinate.
     *
     * @param  velocityy
     *         The new y-coordinate for the velocity for this entity.
     * @post   If the given y-coordinate is a valid coordinate for any entity,
     *         the y-coordinate of the velocity of this new entity is equal to the given
     *         coordinate.
     *       | if ( isValidNumber(velocityy))
     *       |   then new.getVelocityy() == velocityy
     */
    @Raw
    public void setVelocityy(double velocityy) {
        if ( ! isValidNumber(velocityy))
            this.yvelocity =  0;
        else
            this.yvelocity=velocityy;
    }

    /**
     * variable registering the x-coördinate of the velocity of the entity
     */
    private double xvelocity;

    /**
     * variable registering the y-coördinate of the velocity of the entity
     */
    private double yvelocity;


    /**
     * Check whether the given speed is a valid speed for
     * any entity.
     *
     * @param  speed
     *         The speed to check.
     * @return true if speed<=300000
     *         |result == (velocity<=300000)
     */
    public static boolean isValidSpeed(double speed) {
        return speed<=300000;
    }

    /**
     * Variable registering the speed of this entity.
     */
    private double speed;

    /**
     * @Return the velocity of this entity.
     */
    @Basic @Raw
    public double getSpeed() {
        return this.speed;
    }

    /**
     * @Return the velocity of this entity.
     */
    @Basic @Raw
    public double[] getVelocity() {
        return this.velocity;
    }



    /**
     * Set the velocity of this entity to a double[] {x,y} with x and y
     * the given coordinates of the velocity.
     *
     * @param velocityx
     *        The new x-coordinate of the velocity for this entity.
     * @param velocityy
     *        The new y-coordinate of the velocity for this entity.
     * @post  If the calculated speed is a valid speed for any entity,
     *        the velocity of this new entity is equal to the the double[] {x,y}
     *        with x and y the given coordinates.
     *        Otherwise, the velocity is equal to {3000000/sqrt(2), 300000/sqrt(2)}
     *       | if (isValidSpeed(Speed))
     *       |   then new.getVelocity() == double[] {velocityx, velocityy}
     *       | else new.getVelocity() == double[] {3000000/Math.sqrt(2), 300000/Math.sqrt(2)}
     */
    @Raw
    public void setVelocity(double velocityx,double velocityy) {

        this.speed =  Math.sqrt(Math.pow(velocityx, 2) + Math.pow(velocityy, 2));
        if (!isValidSpeed(this.speed)){
            setVelocityx(300000/Math.sqrt(2));
            setVelocityy(300000/Math.sqrt(2));
            this.speed=300000;}
        else
            {setVelocityx(velocityx);
            setVelocityy(velocityy);}
        double[] velocityList= {this.getVelocityx(),this.getVelocityy()};
        this.velocity = velocityList;

    }



    /**
     * variable registering the velocity of this entity.
     */
    private double[] velocity;

    //methods about the radius of the entity

    /**
     * Return the radius of this entity.
     */
    @Basic @Raw @Immutable
    public double getRadius() {
        return this.radius;
    }

    /**
     * Variable registering the radius of this entity.
     */
    private final double radius;

    //method getDistanceBetween

    /**
     * Get the distance between this entity and a given entity.
     * @param  entity
     *         the other entity of which we need to get the distance from this entity.
     * @return 0
     *         If the given entity is equal to this entity, the distance is zero.
     *         | distance == zero
     * @return distance
     *         returns the calculated distance between this entity and the given entity.
     *         | distance == Math.sqrt(Math.pow(this.getPosition().get(0)-entity.getPosition().get(0), 2)
     *           +Math.pow(this.getPosition().get(1)-entity.getPosition().get(1),2))-this.getRadius()-entity.getRadius();
     * @throws IllegalNumberException
     *         The given distance isn't a valid number for any entity.
     *         ( ! isValidNumber(distance))
     */
    public double getDistanceBetween(Entity entity) throws IllegalNumberException {
        double distance=Math.sqrt(Math.pow(this.getPosition()[0]-entity.getPosition()[0], 2)
                +Math.pow(this.getPosition()[1]-entity.getPosition()[1],2))-this.getRadius()-entity.getRadius();
        if (! isValidNumber(distance))
            throw new IllegalNumberException(distance);
        if (this==entity)
            return 0;
        return distance;
    }

   //method overlap
    /**
     * Determine if this entity overlaps with the given entity.
     * @param   entity
     *          The given entity which may or may not overlap with this entity.
     * @return  true if the distance between this entity and the given entity is less than 0
     *          or if this entity is equal to the given entity.
     *          | (this.getDistanceBetween(entity) <= 0 )
     * @throws  IllegalNumberException
     *          The distance between this entity and a given entity is not a valid number.
     *          | (! isValidNumber(getDistanceBetween(entity))
     */
    public boolean overlap(Entity entity) throws IllegalNumberException {
        return (getDistanceBetween(entity) <= 0);
    }

    //methods concerning getTimeCollision

    /**
     * Return when, if ever, two space entitys collide.
     * @param entity
     * @return Positive infinity
     *         if the scalar product of the velocity vector and the position vector
     *         is greater than or equal to 0,
     *         meaning the entitys won't ever collide.
     * @return Positive infinity
     *         if the scalar product of the velocity vector and the position vector squared
     *         - the product of the length of the velocity vector squared and (the length of the position vector squared
     *         - the sum of the radii squared), is less than or equal to 0,
     *         meaning the entitys won't ever collide.
     * @return The time needed for the two entitys to collide,
     *         by collide meaning that the distance between this entity and a given entity is equal to the sum of the two radii.
     *         |timeToCollision == -(this.scalarProductVeloPos(entity)
     *         | + Math.sqrt(this.calculatedD(entity))) / this.lengthVelocitySquared(entity)
     * @throws IllegalArgumentException
     *         if the two entitys already overlap.
     *         if (overlap(entity))
     * @throws IllegalNumberException

     */
    public double getTimeToCollision(Entity entity) throws IllegalNumberException,IllegalArgumentException {
        if (this.overlap(entity))
            throw new IllegalArgumentException();
        if (this.scalarProductVeloPos(entity) >= 0)
            return Double.POSITIVE_INFINITY;
        if (( this.calculatedD(entity) <= 0))
            return Double.POSITIVE_INFINITY;
        return -( this.scalarProductVeloPos(entity) + Math.sqrt(this.calculatedD(entity))) / this.lengthVelocitySquared(entity);
    }

    /**
     * calculate the position vector between the two positions
     * @param entity
     * @return  a double[] list {x,y} where x is the position vector between the x-coordinates of this entity and a given entity
     *          and y is the position vector between the y-coordinates of this entity and a given entity.
     *          | newposition == (entity.getPosition()[0] - this.getPosition()[0], entity.getPosition()[1] - this.getPosition[1])
     */
    public double[] positionVector(Entity entity) {
        double [] positionVector = {entity.getPosition()[0] - this.getPosition()[0],entity.getPosition()[1] - this.getPosition()[1]};
        return positionVector;
    }

    /**
     * calculate the velocity vector between the two velocities
     * @param  entity
     * @return a double[] (x,y) where x is the velocity vector between this entity and a given entity in x direction
     *         and y is the velocity vector between this entity and a given entity in the y direction.
     *         | newvelocity == (entity.getVelocityx() - this.getVelocityx(), entity.getVelocityy() - this.getVelocityy())
     */
    public double[] velocityVector(Entity entity) {
        double[] velocityVector=new double[2];
        velocityVector[0] = entity.getVelocityx() - this.getVelocityx();
        velocityVector[1] = entity.getVelocityy() - this.getVelocityy();
        return velocityVector;
    }

    /**
     * calculate the scalar product of the position vector with itself
     * @param entity
     * @return the length of the vector between the center of this entity and a given entity squared
     *         | length == Math.pow(differenceInPosition(entity)[0], 2) + Math.pow(differenceInPosition(entity)[1], 2);
     * @throws IllegalNumberException
     *         if the calculated length is not valid.
     *         | ( ! isValidNumber(length))
     */
    public double lengthPositionSquared(Entity entity) throws IllegalNumberException {
        double length=Math.pow(positionVector(entity)[0], 2) + Math.pow(positionVector(entity)[1], 2);
        if  ( ! isValidNumber(length))
            throw new IllegalNumberException(length);
        return  length;
    }

    /**
     * calculate the scalar product of the velocity vector with itself
     * @param entity
     * @return the length of the velocity vector between the center of this entity and a given entity squared
     *         | differenceVeSq == Math.pow(differenceInVelocity(entity)[0], 2) + Math.pow(differenceInVelocity(entity)[1], 2);
     * @throws IllegalNumberException
     *         if the calculated length is not valid.
     *         | ( ! isValidNumber(number))
     */
    public double lengthVelocitySquared(Entity entity) throws IllegalNumberException {
            lengthVeSq = Math.pow(this.velocityVector(entity)[0], 2) + Math.pow(this.velocityVector(entity)[1], 2);
        if (!isValidNumber(lengthVeSq))
                throw new IllegalNumberException(lengthVeSq);
        return lengthVeSq;
    }

    /**
     * variable registering the the length of the velocity vector squared
     */
    private double lengthVeSq;


    /**
     * calculate the scalar product of the velocity vector and the position vector
     * @param  entity
     * @return the velocity vector in the x-direction times the position vector on the x-axis
     *         + the velocity vector in the y-direction times the position vector on the y-axis
     *         |differenceVeLo == ((differenceInVelocity(entity)[0] * differenceInPosition(entity)[0])
     *         |+ (differenceInVelocity(entity)[1] * differenceInPosition(entity)[1]))
     * @throws IllegalNumberException
     *         if the calculated number is not valid.
     *         | ( ! isValidNumber(number))
     */
    public double scalarProductVeloPos(Entity entity) throws IllegalNumberException{
        differenceVePos = ((this.velocityVector(entity)[0] * this.positionVector(entity)[0])
                + (this.velocityVector(entity)[1] * this.positionVector(entity)[1]));
        if (!isValidNumber(differenceVePos))
                throw new IllegalNumberException(differenceVePos);
        return differenceVePos;
    }

    /**
     * variable registering the product of the velocity vector and the position vector.
     */
    private double differenceVePos;

    /**
     * calculate the sum of the radii squared
     * @param entity
     * @return the sum of the radii squared.
     *         | sigmasquared == Math.pow(this.getRadius() + entity.getRadius(),2);
     * @throws IllegalNumberException
     *         if the calculated number is not valid.
     *         | ( ! isValidNumber(number))
     */
    public double sigmasquared(Entity entity)throws IllegalNumberException{
        sigmasquared = Math.pow(this.getRadius() + entity.getRadius(),2);
        if (!isValidNumber(sigmasquared))
                throw new IllegalNumberException(sigmasquared);
        return sigmasquared;
    }

    /**
     * variable registering the sum of the radii squared.
     */
    private double sigmasquared;

    /**
     * calculate d
     * @param entity
     * @return ( the scalar product of the velocity vector and the position vector ) squared
     *         - the product of length of the velocity vector squared
     *         and (the length of the position vector squared - the sum of the radii squared)
     *         |calculatedD == Math.pow((differenceVeloLoc(entity)),2)
     *         |- ((differenceVelocitySquared(entity)) * ((differenceLocationSquared(entity) - sigmaSquared(entity))))
     * @throws IllegalNumberException
     *         if the calculated number is not valid.
     *         | ( ! isValidNumber(number))
     */
    public double calculatedD(Entity entity) throws IllegalNumberException {

        try{
            calculatedD = Math.pow((this.scalarProductVeloPos(entity)),2) -
                    ((this.lengthVelocitySquared(entity)) * ((this.lengthPositionSquared(entity) - this.sigmasquared(entity))));
        }catch (IllegalNumberException e){
            throw new IllegalNumberException(calculatedD);
            }
        return calculatedD;
    }

    /**
     * variable registering the calculated D.
     */
    private double calculatedD;

    //methods concerning getCollisionPosition

    /**
     * variable registering the center of this entity while colliding
     */
    private double[] thisCoordCollision;

    /**
     * variable registering the center of the other entity while colliding
     */
    private double[] entityCoordCollision;

    /**
     * calculate the center of the circles at the time of collision
     * @param  entity
     * @post   set the double[] thisCoordCollision to this.getPosition() + (getTimeToCollision(entity) * this.getVelocity());
     *         |this.thisCoordCollision==this.getPosition() + (getTimeToCollision(entity) * this.getVelocity())
     * @post   set the double[] entityCoordCollision to entity.getPosition() + (getTimeToCollision(entity) * entity.getVelocity());
     *         |this.entityCoordCollision==entity.getPosition() + (getTimeToCollision(entity) * entity.getVelocity())
     * @throws IllegalNumberException
     */
    public void calculateCollision(Entity entity) throws IllegalNumberException {
        double[] coordcollthis=new double[2];
        double[] coordcollentity=new double[2];
        coordcollthis[0] = this.getPosition()[0] + (getTimeToCollision(entity) * this.getVelocityx());
        coordcollthis[1] = this.getPosition()[1] + (getTimeToCollision(entity) * this.getVelocityy());
        coordcollentity[0] = entity.getPosition()[0] + (getTimeToCollision(entity) * entity.getVelocityx());
        coordcollentity[1] = entity.getPosition()[1] + (getTimeToCollision(entity) * entity.getVelocityy());
        this.thisCoordCollision=coordcollthis;
        this.entityCoordCollision=coordcollentity;
    }

    /**
     * get the position of the collision
     * @param  entity
     * @return null
     *         if the time before the collision is infinity.
     *         |(getTimeToCollision(entity)==Double.POSITIVE_INFINITY)
     * @return collisionPosition
     *         the position where the two entitys collide
     *         | collisionPosition= thisCoordCollision+direction * (this.getRadius()/(this.getRadius()+entity.getRadius()));
     * @throws IllegalArgumentException
     *         if the two entitys already overlap
     *         | (this.overlap(entity))
     * @throws IllegalNumberException
     *         if one of the elements of the position of the collision is negative or NaN
     *         | (collisionPosition[0]==Double.NEGATIVE_INFINITY || collisionPosition[0]==Double.NaN)
     *         | (collisionPosition[1]==Double.NEGATIVE_INFINITY || collisionPosition[1]==Double.NaN)
     */
    public double[] getCollisionPosition(Entity entity) throws IllegalArgumentException,IllegalNumberException{
        if (overlap(entity))
            throw new IllegalArgumentException();
        if (this.getTimeToCollision(entity)==Double.POSITIVE_INFINITY)
            return null;
        calculateCollision(entity);
        double[] direction=new double[2];
        direction[0]=entityCoordCollision[0]-thisCoordCollision[0];
        direction[1]=entityCoordCollision[1]-thisCoordCollision[1];
        double[] colposition= new double[2];
        colposition[0]= thisCoordCollision[0]+direction[0] * (this.getRadius()/(this.getRadius()+entity.getRadius()));
        colposition[1]= thisCoordCollision[1]+direction[1] * (this.getRadius()/(this.getRadius()+entity.getRadius()));
        if (colposition[0]==Double.NEGATIVE_INFINITY || colposition[0]==Double.NaN)
            throw new IllegalNumberException(colposition[0]);
        if (colposition[0]==Double.NEGATIVE_INFINITY || colposition[0]==Double.NaN)
            throw new IllegalNumberException(colposition[1]);
        this.collisionPosition=colposition;
        return this.collisionPosition;
    }

   /**
    * variable registering the position of the collision.
    */
    private double[] collisionPosition;


    //methods associating with world

    /**
     * Return the world to which this entity belongs.
     */
    @Basic @Raw
    public World getWorld() {
        return this.world;
    }

    /**
     * Check whether this entity can have the given world as
     * its world.
     *
     * @param  world
     *         The world to check.
     * @return If this entity is terminated, true if and only if the
     *         given world is not effective.
     *       | if (this.isTerminated())
     *       |   then result == (world == null)
     * @return If this entity is not terminated, true if and only if the given
     *         world is effective and not yet terminated.
     *       | if (! this.isTerminated())
     *       |   then result ==
     *       |     (world != null) &&
     *       |     (! world.isTerminated())
     */
    @Raw
    public boolean canHaveAsWorld(World world) {
        if (this.isTerminated())
            return world == null;
        if (this.getWorld()!=world)
            return false;
        return (world != null)
                && (!world.isTerminated());
    }

    /**
     * Check whether this entity has a proper world.
     *
     * @return True if and only if this entity can have its world as its
     *         world, and if the world of this entity is either not effective
     *         or if it has this entity as one of its entitys.
     *       | result ==
     *       |   canHaveAsworld(getworld()) &&
     *       |   ( (getworld() == null) || getworld().hasAsentity(this))
     * @note   At this point we do not impose the condition that the world
     *         may not have another entity of the same share. That condition
     *         is taken care of in the invariants of world.
     */
    @Raw
    public boolean hasProperWorld() {
        return canHaveAsWorld(getWorld())
                && ((getWorld() == null) || (getWorld().hasAsEntity(this)));
    }

    /**
     * Set the world of this entity to the given world.
     *
     * @param  world
     *         The new world for this entity.
     * @post   The world of this entity is the same as the
     *         given world.
     *       | new.getworld() == world
     * @throws IllegalArgumentException
     *         This entity cannot have the given world as its world.
     *       | ! canHaveAsworld(world)
     */
    @Raw
    public void setWorld(World world) throws IllegalArgumentException {
        if (!canHaveAsWorld(world))
            throw new IllegalArgumentException("Inappropriate world!");
        this.world = world;
    }

    /**
     * Variable referencing the world to which this entity belongs.
     */
    private World world = null;


}