package Projet;
import java.util.*;
import java.lang.Math;


public class Heuristique2 extends ProblemZ2
{
	private LinkedList<Vector2> states;
	private LinkedList<Vector2> exploredStates;
	private LinkedList<Vector2> toExploreState;
	private LinkedList<Vector2> nextIteration;
	private LinkedList<Vector2> actions;
	private LinkedList<LinkedList<Combination>> comb;
	private LinkedList<Vector2> path;
	private Vector2 startingState;
	private Vector2 finalState;
	private LinkedList<Jump> jumps;

	//Constructor
	public Heuristique2(Vector2 startingState, Vector2 finalState, LinkedList<Vector2> actions)
	{
		this.states = new LinkedList<Vector2>();
		this.exploredStates = new LinkedList<Vector2>();
		this.toExploreState = new LinkedList<Vector2>();
		this.nextIteration = new LinkedList<Vector2>();
		this.actions = new LinkedList<Vector2>();
		this.comb = new LinkedList<LinkedList<Combination>>();
		this.path = new LinkedList<Vector2>();
		this.startingState = startingState;
		this.finalState = finalState;
		this.states.add(startingState);
		this.toExploreState.add(startingState);
		this.actions = actions;
		this.jumps = new LinkedList<Jump>();
	}

	public LinkedList<Vector2> solveBFS(int maxIteration)
	{

		float computeLimit = 0;
		for(int j = 0 ; j < this.actions.size() ; j++)
			computeLimit += this.actions.get(j).magnitude();

		int limit = (int) (Math.ceil(computeLimit));

		computeCombination();
		createJump(limit);

		boolean firstIter = true;
		for(int i = 1 ; i<= maxIteration ; i++)
		{
			while(this.toExploreState.size() != 0)
			{
				Vector2 e = this.toExploreState.get(0);

				//Check if final state
				if(e.equals(this.finalState))
				{
					LinkedList<Vector2> endPath = new LinkedList<Vector2>();
					while(e.parent() != this.startingState)
					{
						endPath.add(0,e);
						e = e.parent();
					}
					for(int j = 0 ; j < this.jumps.size() ; j++)
					{
						if(this.jumps.get(j).end().equals(e))
						{
							this.path.addAll(this.jumps.get(j).computePath());
							break;
						}
					}
					this.path.addAll(endPath);
					return this.path;
				}
			
				//On firest iteration, make jump instead of generic search
				if(firstIter)
				{
					//Create successor using jumps
					for(int j = 0 ; j < this.jumps.size() ; j++)
					{
						Vector2 s = this.jumps.get(j).end();
						this.states.add(s);

						if((!contains(this.exploredStates,s) && !contains(this.toExploreState,s)))
						{
							if(s.distance(this.finalState) <= limit)
							{
								this.toExploreState.add(s);
							}
							else
							{
								this.nextIteration.add(s);
							}
						}
					}
					firstIter = false;
				}
				else
				{
					//Create successor
					for(int j = 0 ; j < this.actions.size() ; j++)
					{
						Vector2 s = suc(e,this.actions.get(j));
						this.states.add(s);
						if((!contains(this.exploredStates,s) && !contains(this.toExploreState,s)))
						{
							if(s.distance(this.finalState) <= limit)
							{
								this.toExploreState.add(s);
							}
							else
							{
								this.nextIteration.add(s);
							}
						}
					}					
				}
				this.exploredStates.add(e);
				this.toExploreState.remove(0);
			}
			//Dichotomy
			limit *= 2;
			this.toExploreState.addAll(this.nextIteration);
			firstIter = true;
		}
		return null;
	}

	//Create linear combination using actions
	private void computeCombination()
	{
		LinkedList<Vector2> temp;
		LinkedList<Combination> newSize = new LinkedList<Combination>();

		//Create size 1 commbination
		for(int i = 0 ; i < this.actions.size() ; i++)
		{
			temp = new LinkedList<Vector2>();
			temp.add(this.actions.get(i));
			newSize.add(new Combination(temp));
		}
		this.comb.add(newSize);

		//Create size n combination
		for(int i = 1 ; i < this.actions.size() ; i++)
		{
			this.comb.add(new LinkedList<Combination>());
			for(int j = 0 ; j < this.comb.size()/2 ; j++)
			{
				for(int n = 0 ; n < this.comb.get(j).size() ; n++)
				{

					for(int m = 0 ; m < this.comb.get(this.comb.size()-j-2).size(); m++)
					{
						temp = new LinkedList<Vector2>();
						temp.addAll(this.comb.get(j).get(n).element());
						temp.addAll(this.comb.get(this.comb.size()-j-2).get(m).element());
						this.comb.get(i).add(new Combination(temp));
					}
				}
			}
			

		}
	}

	//Create possible jumps
	private void createJump(int limit)
	{
		for(int i = 0 ; i < this.comb.size() ; i++)
		{
			for(int j = 0 ; j < this.comb.get(i).size() ; j++)
			{
				LinkedList<Integer> alpha = new LinkedList<Integer>();
				alpha = jump(limit, this.comb.get(i).get(j).value());
				if(alpha != null)
				{					
					for(int k = 0 ; k < alpha.size() ; k++)
					{
						this.jumps.add(new Jump(this.startingState, alpha.get(k), this.comb.get(i).get(j)));
					}
				}
			} 
		}
	}

	//Compute the possible alpha to jump
	private LinkedList<Integer> jump(int limit,Vector2 a)
	{
		double m;
		double p;
		double coefa;
		double coefb;
		double coefc;
		if(a.y() == 0)
		{
			m = 0;
			p = this.startingState.y();
			coefa = m*m +1;
			coefb = (-2*this.finalState.x()) + (2*m*p - 2) * (this.finalState.y() * m);
			coefc = (this.finalState.x() * this.finalState.x()) + (p * p) - (2* this.finalState.y() * p) + (this.finalState.y() * this.finalState.y()) - (limit * limit);
		}
		else if(a.x() == 0)
		{
			m = 0;
			p = this.startingState.x();
			coefa = 1;
			coefb = (-2*this.finalState.y());
			coefc = (p * p) - (2* this.finalState.x() * p) + (this.finalState.y() * this.finalState.y()) - (limit * limit) + (this.finalState.x() * this.finalState.x());
		}
		else
		{
			m = a.x()/a.y();
			p = this.startingState.y()/a.y();			
			coefa = m*m +1;
			coefb = (-2*this.finalState.x()) + (2*m*p - 2) * (this.finalState.y() * m);
			coefc = (this.finalState.x() * this.finalState.x()) + (p * p) - (2* this.finalState.y() * p) + (this.finalState.y() * this.finalState.y()) - (limit * limit);
		}
		LinkedList<Integer> alpha = new LinkedList<Integer>();

		double delta = Math.pow(coefb,2) - 4 * coefa * coefc;

		//Can't reach the circle
		if(delta < 0)
		{
			return alpha;
		}
		//Have 1 intersection with cirlce
		if(delta == 0)
		{
			double x = - (coefb + Math.sqrt(delta))/(2*coefa);
			double y = m*x + p;
			if(x % 1 == 0 && y % 1 == 0)
			{
				double alpha1 = new Vector2((int)Math.floor(x),(int)Math.floor(y)).distance(this.startingState) / a.magnitude();
				if(alpha1 % 1 ==0)
					alpha.add((int)Math.floor(alpha1));
			}
			return alpha;
		}
		//Have 2 intersection
		else 
		{
			if(a.x() != 0)
			{
				double x2 = (-coefb + Math.sqrt(delta))/(2*coefa);
				double x1 = (-coefb - Math.sqrt(delta))/(2*coefa);
				double y1 = m * x1 + p;
				double y2 = m * x2 + p;
				LinkedList<Vector2> validPoints = new LinkedList<Vector2>();
				for(int i = (int)Math.ceil(x1) ; i <= (int)Math.floor(x2) ; i++)
				{
					if(m * i + p >= y1 && m * i + p <= y2)
						validPoints.add(new Vector2(i,(int)Math.floor(m*i+p)));
				}
				for(int i = 0 ; i < validPoints.size() ; i++)
				{
					double alpha1 = validPoints.get(i).distance(this.startingState) / a.magnitude();
					if(alpha1 % 1 == 0 && alpha1 > 0)
						alpha.add((int)alpha1);
				}				
			}
			//Particular case for x = k line 
			else 
			{
				double y1 = (-coefb - Math.sqrt(delta))/(2*coefa);
				double y2 = (-coefb + Math.sqrt(delta))/(2*coefa);
				LinkedList<Vector2> validPoints = new LinkedList<Vector2>();
				for(int i = (int)Math.ceil(y1) ; i <= (int)Math.floor(y2) ; i++)
				{
					validPoints.add(new Vector2((int)Math.floor(p),i));
				}
				for(int i = 0 ; i < validPoints.size() ; i++)
				{
					double alpha1 = validPoints.get(i).distance(this.startingState) / a.magnitude();
					if(alpha1 % 1 == 0)
						alpha.add((int)alpha1);
				}				
			}
			return alpha;
		}
	}

	//Definiton of combination
	private class Combination
	{
		int size;
		LinkedList<Vector2> element;

		//Constructor
		private Combination(LinkedList<Vector2> element)
		{
			this.element = new LinkedList<Vector2>();
			this.element.addAll(element);
			this.size = this.element.size();
		}

		//Getter
		private int size()
		{
			return this.size;
		}

		private LinkedList<Vector2> element()
		{
			LinkedList<Vector2> temp = new LinkedList<Vector2>();
			temp.addAll(element);
			return temp;
		}

		//Value of the vector created by the combination
		private Vector2 value()
		{
			int x = 0;
			int y = 0;
			for(int i = 0 ; i < this.size ; i++)
			{
				x += this.element.get(i).x();
				y += this.element.get(i).y();
			}
			return new Vector2(x,y);
		}

		@Override
		public String toString()
		{
			String r = "";
			for(int i = 0 ; i < this.size ; i++)
			{
				r+= this.element.get(i).toString();
			}
			return r;
		}
	}

	//Definition of jump
	private class Jump
	{
		Vector2 start;
		int alpha;
		Combination comb;

		//Constructor
		private Jump(Vector2 start, int alpha, Combination comb)
		{
			this.comb = comb;
			this.alpha = alpha;
			this.start = start;
		}

		//Give the end point 
		private Vector2 end()
		{
			int x = this.comb.value().x() * this.alpha;
			int y = this.comb.value().y() * this.alpha;
			
			return new Vector2(x,y,start);
		}

		//Compute the path for printing
		private LinkedList<Vector2> computePath()
		{
			LinkedList<Vector2> path = new LinkedList<Vector2>();
			path.add(this.start);
			for(int j = 0 ; j < this.comb.size() ; j++)
			{
				for(int i = 0 ; i < this.alpha ; i++)
				{
					path.add(suc(path.get(i + j * this.alpha),this.comb.element().get(j)));
				}
			}
			return path;
		}

		@Override
		public String toString()
		{
			String r = "";
			r += this.alpha * this.comb.value().x() + "," + this.alpha * this.comb.value().y();
			return r;
		}
	}

}