package particlefiltering;

import java.awt.BasicStroke;
import java.awt.Color;
import java.awt.Font;
import java.awt.Graphics2D;
import java.awt.image.BufferedImage;
import java.util.Arrays;
import java.util.Random;

import parameters.tracking_arguments;
import preprocessing.Util;
import tracking.Atributes;

public class particles {
	
	/* standard deviations for gaussian sampling in transition model */
	/*final static double TRANS_X_STD = 1.0;
	final static double TRANS_Y_STD = 0.5;*/ 
	
	/*final static double TRANS_X_STD = 2.0;
	final static double TRANS_Y_STD = 2.0;
	
	final static double TRANS_S_STD = 0.001;

	// autoregressive dynamics parameters for transition model 
	final static double A1 = 2.0;
	final static double A2 = -1.0;
	final static double B0 = 1.0000; */
	
	
	/*
	  Creates an initial distribution of particles at specified locations
	  
	  
	  @param regions an array of regions describing player locations around
	    which particles are to be sampled
	  @param histos array of histograms describing regions in \a regions
	  @param n the number of regions in \a regions
	  @param p the total number of particles to be assigned
	  
	  @return Returns an array of \a p particles sampled from around regions in
	    \a regions
	*/
	
	public particle[] init_distributionAA (pRegion regions[], pHistogram histos[], int n, int p)
	{
		particle[] particles = new particle[p];
		int k = 0;
		int np = p / n;

		// create particles at the centers of each of n regions 
		for(int i = 0; i < p; i++ ) {
			particles[i] = new particle();
			particles[i].histo = new pHistogram();
		}
		
		for(int i = 0; i < n; i++ ) {
	
			int width = regions[i].get_width();
			int height = regions[i].get_height();
			double x = regions[i].get_x() + width / 2;
			double y = regions[i].get_y() + height / 2;
			for(int j = 0; j < np; j++ )
			{
				particles[k].set_x(x);
				particles[k].set_xp(x);
				particles[k].set_x0(x);
				
				particles[k].set_y(y);
				particles[k].set_yp(y);
				particles[k].set_y0(y);
				
				particles[k].set_sp(1.0);
				particles[k].set_s(1.0);
				
				particles[k].set_width(width);
				particles[k].set_height(height);
				copy_histo (particles[k], histos[i]);
				particles[k++].set_weight(0.0);
			}
		}
		// make sure to create exactly p particles 
		int i = 0;
		while( k < p )
		{
			int width = regions[i].get_width();
			int height = regions[i].get_height();
			int x = regions[i].get_x() + width / 2;
			int y = regions[i].get_y() + height / 2;
			
			particles[k].set_x(x);
			particles[k].set_xp(x);
			particles[k].set_x0(x);
			
			particles[k].set_y(y);
			particles[k].set_yp(y);
			particles[k].set_y0(y);
			
			particles[k].set_sp(1.0);
			particles[k].set_s(1.0);
			
			particles[k].set_width(width);
			particles[k].set_height(height);
			copy_histo (particles[k], histos[i]);
			particles[k++].set_weight(0.0);
	
			i = ( i + 1 ) % n;
		}

		return particles;
	} 
	
	
	public particle[] init_distribution (pRegion region, pHistogram histos, int p)
	{
		particle[] particles = new particle[p];
		int k = 0;
		int np = p;

		/* create particles at the centers of each of n regions */
		for(int i = 0; i < p; i++ ) {
			particles[i] = new particle();
			particles[i].histo = new pHistogram();
		}
		
		int width = region.get_width();
		int height = region.get_height();
		double x = region.get_x() + width / 2;
		double y = region.get_y() + height / 2;
			for(int j = 0; j < np; j++ )
			{
				particles[k].set_x(x);
				particles[k].set_xp(x);
				particles[k].set_x0(x);
				
				particles[k].set_y(y);
				particles[k].set_yp(y);
				particles[k].set_y0(y);
				
				particles[k].set_sp(1.0);
				particles[k].set_s(1.0);
				
				particles[k].set_width(width);
				particles[k].set_height(height);
				particles[k].valid = true;
				copy_histo (particles[k], histos);
				particles[k++].set_weight(0.0);
			}
	
		/* make sure to create exactly p particles */
		while( k < p )
		{
			particles[k].set_x(x);
			particles[k].set_xp(x);
			particles[k].set_x0(x);
			
			particles[k].set_y(y);
			particles[k].set_yp(y);
			particles[k].set_y0(y);
			
			particles[k].set_sp(1.0);
			particles[k].set_s(1.0);
			
			particles[k].set_width(width);
			particles[k].set_height(height);
			particles[k].valid = true;
			copy_histo (particles[k], histos);
			particles[k++].set_weight(0.0);

		}

		return particles;
	}

	void copy_histo (particle target, pHistogram histo)
	{
		target.histo.set_length (histo.get_length());		
		
		target.histo.copy_vector(histo.get_vector());
		
		/*for (int i = 0; i < histo.get_length(); i++) {
			target.histo.set_data(i, histo.get_data(i));
		}*/
	}
	
	void copy_particle_histo (particle source, particle target)
	{
		target.histo.set_length (source.histo.get_length());	
		
		target.histo.copy_vector(source.histo.get_vector());
		
		/*for (int i = 0; i < source.histo.get_length(); i++) {
			target.histo.set_data(i, source.histo.get_data(i));
		}*/
	}
	
	void copy_particle (particle source, particle target)
	{
		target.set_x(source.get_x());
		target.set_y(source.get_y());
		target.set_s(source.get_s());
		target.set_xp(source.get_xp());
		target.set_yp(source.get_yp());
		target.set_sp(source.get_sp());
		target.set_x0(source.get_x0());
		target.set_y0(source.get_y0());
		target.set_weight(source.get_weight());
		target.set_width(source.get_width());
		target.set_height(source.get_height());
		target.valid = source.valid;
		copy_particle_histo (source, target);
	}
		
	/*
	  Normalizes particle weights so they sum to 1
	  
	  @param particles an array of particles whose weights are to be normalized
	  @param n the number of particles in \a particles
	*/
	//void normalize_weights (ArrayList<particle> particles, int n)
	public void normalize_weights (particle particles[], int n, int box)
	{
	  float sum = 0;
	  int i, bad_particles = 0;
	  double best = -Double.MAX_VALUE;

	  double sum2 = 0.0;
	  for( i = 0; i < n; i++ ) {
			double weigth = particles[i].get_weight();
			if (weigth <= 0.0) {
			    bad_particles++;
			}
			if (weigth > best) {
				best = weigth;
			}
			sum2 += particles[i].get_weight();
      }
	  
	  //System.out.printf("Text region : %d, bad : %d, mean weight : %f, best : %f\n", box, bad_particles, sum2/((double)n), best);
	  
	  for( i = 0; i < n; i++ ) 
	    sum += particles[i].get_weight();
	  for( i = 0; i < n; i++ ) {
		double weigth = particles[i].get_weight();
		particles[i].set_weight(weigth/sum);
	  }
	  
	  if (sum2/((double)n) < 0.000001) {
		  for( i = 0; i < n; i++ ) {
			  particles[i].valid = false;
			  /*if (i == 0 || i == (n-1)) {
				  System.out.printf("Setei false Text region : %d, bad : %d, mean weight : %f\n", box, bad_particles, sum2/((double)n));
			  }*/
		  }
	  }
	  
	}

	/*
	  Re-samples a set of particles according to their weights to produce a
	  new set of unweighted particles
	  
	  @param particles an old set of weighted particles whose weights have been
	    normalized with normalize_weights()
	  @param n the number of particles in \a particles
	  
	  @return Returns a new set of unweighted particles sampled from \a particles
	*/
	public particle[] resample (particle particles[], int n )
	{
	  particle new_particles[] = new particle[n];
	  
	  /* create particles at the centers of each of n regions */
		for(int i = 0; i < n; i++ ) {
			new_particles[i] = new particle();
			new_particles[i].histo = new pHistogram();
		}

	  comparator c = new comparator(); 
	  
	  Arrays.sort(particles, c);  
		
	  /*for(int j = 0; j < n; j++ )
      {
		  copy_particle(particles[0], new_particles[j]); 
      }*/
	  
	  int k = 0;
	  for(int i = 0; i < n; i++ )
	    {
	      
		  int np = Math.round((float)(particles[i].get_weight() * n));
	      
	      for(int j = 0; j < np; j++ )
	        {
	    	  copy_particle(particles[i], new_particles[k++]); 
	          
	          if( k == n )
	        	  return new_particles;
	        }
	    }
	  while( k < n )
		  copy_particle(particles[0], new_particles[k++]); 

	  return new_particles;
	}
	
	double gsl_ran_gaussian (Random rng, double sigma)
	{
	  double x, y, r2;
	  

	  do
	    {
	      /* choose x,y in uniform square (-1,-1) to (+1,+1) */

	      x = -1 + 2 * rng.nextDouble();
	      y = -1 + 2 * rng.nextDouble();

	      /* see if it is in the unit circle */
	      r2 = x * x + y * y;
	    }
	  while (r2 > 1.0 || r2 == 0);

	  /* Box-Muller transform */
	  return sigma * y * Math.sqrt (-2.0 * Math.log (r2) / r2);
	}

	
	
	/*
	  Samples a transition model for a given particle
	  
	  @param p a particle to be transitioned
	  @param w video frame width
	  @param h video frame height
	  @param rng a random number generator from which to sample

	  @return Returns a new particle sampled based on <EM>p</EM>'s transition
	    model
	*/
	public particle transition (particle p, int w, int h, Random rng, tracking_arguments tracking_parameters)
	{
	  particle pn = new particle();

	  /* sample new state using second-order autoregressive dynamics */
	  //double x = A1 * ( p.get_x() - p.get_x0() ) + A2 * ( p.get_xp() - p.get_x0() ) + B0 * Math.pow(rng.nextGaussian(), TRANS_X_STD) + p.get_x0();
	  double x = tracking_parameters.A1 * ( p.get_x()  - p.get_x0() ) + 
	             tracking_parameters.A2 * ( p.get_xp() - p.get_x0() ) + 
	             tracking_parameters.B0 * ( rng.nextGaussian() * tracking_parameters.TRANS_X_STD ) + p.get_x0();
	  //double x = A1 * ( p.get_x() - p.get_x0() ) + A2 * ( p.get_xp() - p.get_x0() ) + B0 * gsl_ran_gaussian( rng, TRANS_X_STD ) + p.get_x0();
	  //System.err.println("trans_x_std: " + Math.pow(rng.nextGaussian(), TRANS_X_STD) + ", ou : " + rng.nextGaussian()*TRANS_X_STD);
	  //System.err.println("x: " + x);
	  pn.set_x( Math.max ( 0.0, Math.min ( (double)w - 1.0, x ) ) );
	  
	  //double y = A1 * ( p.get_y() - p.get_y0() ) + A2 * ( p.get_yp() - p.get_y0() ) + B0 * Math.pow(rng.nextGaussian(), TRANS_Y_STD) + p.get_y0();
	  double y = tracking_parameters.A1 * ( p.get_y()  - p.get_y0() ) + 
	             tracking_parameters.A2 * ( p.get_yp() - p.get_y0() ) + 
	             tracking_parameters.B0 * ( rng.nextGaussian() * tracking_parameters.TRANS_Y_STD ) + p.get_y0();
	  //double y = A1 * ( p.get_y() - p.get_y0() ) + A2 * ( p.get_yp() - p.get_y0() ) + B0 * gsl_ran_gaussian( rng, TRANS_Y_STD) + p.get_y0();
	  //System.err.println("y: " + y);
	  pn.set_y( Math.max ( 0.0, Math.min ( (double)h - 1.0, y ) ) );
	  
	  //double s = A1 * ( p.get_s() - 1.0 ) + A2 * ( p.get_sp() - 1.0 ) + B0 * Math.pow(rng.nextGaussian(), TRANS_S_STD) + 1.0;
	  double s =  tracking_parameters.A1 * ( p.get_s()  - 1.0 ) + 
	              tracking_parameters.A2 * ( p.get_sp() - 1.0 ) + 
	              tracking_parameters.B0 * ( rng.nextGaussian() * tracking_parameters.TRANS_S_STD ) + 1.0;
	  //double s = A1 * ( p.get_s() - 1.0 ) + A2 * ( p.get_sp() - 1.0 ) + B0 * gsl_ran_gaussian (rng, TRANS_S_STD) + 1.0;
	  pn.set_s( Math.max (0.1, s));
	  //System.out.printf("x : %f, y : %f, s : %f, gx : %f, gy : %f, gs : %f\n", x, y, s, rng.nextGaussian() * tracking_parameters.TRANS_X_STD, rng.nextGaussian() * tracking_parameters.TRANS_Y_STD, rng.nextGaussian() * tracking_parameters.TRANS_S_STD);
	  //System.out.printf("x : %f, x0 : %f, xp : %f, y : %f, y0 : %f, yp : %f\n", p.get_x(), p.get_x0(), p.get_xp(), p.get_y(), p.get_y0(), p.get_yp());
	  //System.out.printf("s : %f %f\n", s, rng.nextGaussian() * tracking_parameters.TRANS_S_STD); 
	  
	  pn.set_xp(p.get_x());
	  pn.set_yp(p.get_y());
	  pn.set_sp(p.get_s());
	  pn.set_x0(p.get_x0());
	  pn.set_y0(p.get_y0());
	  pn.set_width(p.get_width());
	  pn.set_height(p.get_height());
	  pn.set_weight(0.0);
	  pn.histo = new pHistogram();
	  copy_particle_histo (p, pn);
	  
	  return pn;
	}
	
	/*
	  Displays a particle on an image as a rectangle around the region specified
	  by the particle
	  
	  @param img the image on which to display the particle
	  @param p the particle to be displayed
	  @param color the color in which \a p is to be displayed
	*/
	public void display_particle (BufferedImage image, particle p, int nparticle, String label)
	{
		int x0 = Math.round((float)(p.get_x() - 0.5 * p.get_s() * p.get_width()));
		int y0 = Math.round((float)(p.get_y() - 0.5 * p.get_s() * p.get_height()));
		int x1 = Math.round((float)(p.get_s() * p.get_width()) );
		int y1 = Math.round((float)(p.get_s() * p.get_height()) );

		int x2 = x0 - 2;
		int y2 = y0 - 2;
		int x3 = x1 + 4;
		int y3 = y1 + 4;

		Graphics2D g1 = image.createGraphics();
		g1.setStroke(new BasicStroke(2.0f));
		g1.setColor(Color.black);

		Graphics2D g2 = image.createGraphics();
		g2.setStroke(new BasicStroke(2.0f));
		g2.setColor(Color.white);

		g1.drawRect (x0, y0, x1, y1);	
		g2.drawRect (x2, y2, x3, y3);

		Font big = new Font("SansSerif", Font.BOLD, 18);
		
		g1.setFont(big);
		g1.drawString(label, x0-2, y0-2);

		//g2.setFont(big);
		//g2.drawString(label, x0+x1-15, y0-2);

		g1.dispose();
		g2.dispose();
	}
	
	public void display_particle3 (BufferedImage image, particle p, int nparticle, String label)
	{
	  int x0, y0, x1, y1;

	  x0 = Math.round((float)(p.get_x() - 0.5 * p.get_s() * p.get_width()));
	  y0 = Math.round((float)(p.get_y() - 0.5 * p.get_s() * p.get_height()));
	  //x1 = x0 + Math.round((float)(p.get_s() * p.get_width()) );
	  //y1 = y0 + Math.round((float)(p.get_s() * p.get_height()) );
	  x1 = Math.round((float)(p.get_s() * p.get_width()) );
	  y1 = Math.round((float)(p.get_s() * p.get_height()) );
	  
	  Graphics2D g = image.createGraphics();
		
	  g.setColor(Color.green);

	  g.setStroke(new BasicStroke(2.0f));
	  
	  g.drawRect (x0, y0, x1, y1);
  	
	  //char[] characters = {'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H', 'I', 'J', 'K', 'L', 'M', 'N', 'O', 'P', 'Q', 'R', 'S', 'T', 'U', 'V', 'W', 'X', 'Y', 'Z'}; 
	  //g.drawChars(characters, nparticle, 1, x0, y0);
  
	  Font big = new Font("SansSerif", Font.BOLD, 16);

	  g.setFont(big);
	  
	  g.drawString(label, x0, y0);
	  
  	  g.dispose();

	}
	
	public void display_particle4 (BufferedImage image, particle p, int nparticle, String label)
	{
	  int x0, y0, x1, y1;

	  x0 = Math.round((float)(p.get_x() - 0.5 * p.get_s() * p.get_width()));
	  y0 = Math.round((float)(p.get_y() - 0.5 * p.get_s() * p.get_height()));
	  //x1 = x0 + Math.round((float)(p.get_s() * p.get_width()) );
	  //y1 = y0 + Math.round((float)(p.get_s() * p.get_height()) );
	  x1 = Math.round((float)(p.get_s() * p.get_width()) );
	  y1 = Math.round((float)(p.get_s() * p.get_height()) );
	  
	  Graphics2D g = image.createGraphics();
		
	  g.setColor(Color.BLACK);

	  g.setStroke(new BasicStroke(2.0f));
	  
	  g.drawRect (x0, y0, x1, y1);
  	
	  //char[] characters = {'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H', 'I', 'J', 'K', 'L', 'M', 'N', 'O', 'P', 'Q', 'R', 'S', 'T', 'U', 'V', 'W', 'X', 'Y', 'Z'}; 
	  //g.drawChars(characters, nparticle, 1, x0, y0);
  
	  Font big = new Font("SansSerif", Font.BOLD, 16);

	  g.setFont(big);
	  
	  g.drawString(label, x0, y0);
	  
  	  g.dispose();

	}

	public void display_particle2 (BufferedImage image, particle p, int nparticle, String label)
	{
	  int x0, y0, x1, y1;

	  x0 = Math.round((float)(p.get_x() - 0.5 * p.get_s() * p.get_width()));
	  y0 = Math.round((float)(p.get_y() - 0.5 * p.get_s() * p.get_height()));
	  //x1 = x0 + Math.round((float)(p.get_s() * p.get_width()) );
	  //y1 = y0 + Math.round((float)(p.get_s() * p.get_height()) );
	  x1 = Math.round((float)(p.get_s() * p.get_width()) );
	  y1 = Math.round((float)(p.get_s() * p.get_height()) );
	  
	  Graphics2D g = image.createGraphics();
		
	  g.setColor(Color.blue);

	  g.setStroke(new BasicStroke(1.0f));
	  
	  g.drawRect (x0, y0, x1, y1);

	  //char[] characters = {'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H', 'I', 'J', 'K', 'L', 'M', 'N', 'O', 'P', 'Q', 'R', 'S', 'T', 'U', 'V', 'W', 'X', 'Y', 'Z'}; 
	  //g.drawChars(characters, nparticle, 1, x0, y0);
	  
	  /*Font big = new Font("SansSerif", Font.BOLD, 18);

	  g.setFont(big);
  	
	  g.drawString(label, x0, y0);*/
	  
  	  g.dispose();

	}
	
	public void display_atribute (BufferedImage image, Atributes A, int label)
	{
	  int x0, y0, x1, y1;

	  x0 = (int) A.get_x();
	  y0 = (int) A.get_y();
	  x1 = A.get_width();
	  y1 = A.get_height();
	  
	  Graphics2D g = image.createGraphics();
		
	  g.setColor(Color.GREEN);

	  g.setStroke(new BasicStroke(1.0f));
	  
	  g.drawRect (x0, y0, x1, y1);

	  //char[] characters = {'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H', 'I', 'J', 'K', 'L', 'M', 'N', 'O', 'P', 'Q', 'R', 'S', 'T', 'U', 'V', 'W', 'X', 'Y', 'Z'}; 
	  //g.drawChars(characters, nparticle, 1, x0, y0);
	  
	  Font big = new Font("SansSerif", Font.BOLD, 18);

	  g.setFont(big);
	  
	  String slabel = String.format("%d", label);
  	
	  g.drawString(slabel, x0, y0);
	  
  	  g.dispose();

	}
	

}
