//#include <CGAL/Cartesian.h> 
//#include <CGAL/convex_hull_2.h> 
//#include <CGAL/Polygon_2.h> 
//#include <list> 
#include "imvideocapture.h"
extern "C" {
	#include "tsai.h"
	#include "kalman.h"
};
#include ".\vimage.h"
#include <math.h>
#include <conio.h>
#include <stdio.h>


#define  MAX_CORNERS 30000
#define  Phi   2*1.57079632679489661923132169164F

#define NPTSX 8
#define NPTSY 8
static double xwn[NPTSX*NPTSY], ywn[NPTSX*NPTSY], xfn[NPTSX*NPTSY], yfn[NPTSX*NPTSY];

static int np;
static double *xw, *yw, *xf, *yf;
//static double mtsaiopengl_modelview[16], mtsaiopengl_projection[16];

static double f, T[3], r[9];
static int ni = 0, npos;
static double xfarray[1000], yfarray[1000];
static KalmanStruct *kf, *kfc;
static float dt = 1;
static double dfocal;




void load_data(char *arq, int *np, double **xw, double **yw, double **xf, double **yf)
{
 int i;
 int npaux;
 double *xwaux, *ywaux, *xfaux, *yfaux;
 FILE *fp = fopen (arq, "r");
 fscanf(fp, "%d", &npaux);
 xwaux = (double *)malloc(npaux*sizeof(double));
 ywaux = (double *)malloc(npaux*sizeof(double));
 xfaux = (double *)malloc(npaux*sizeof(double));
 yfaux = (double *)malloc(npaux*sizeof(double));
 for (i=0; i<npaux; i++) {
  fscanf (fp, "%lf %lf %lf %lf", &(xwaux[i]), &(ywaux[i]), &(xfaux[i]), &(yfaux[i]));
//  xfaux[i] -= IMAGE_W/2.0;
//  yfaux[i] -= IMAGE_H/2.0;
 }
 fclose(fp);
 *np = npaux;
 *xw = xwaux;
 *yw = ywaux;
 *xf = xfaux;
 *yf = yfaux;
}

ConexComp::ConexComp()
{
	num_extremos=0;
	num_pontos=0;
	num_pontos_f=0;
	label=-1;
	pmin_x.x=9999;
	pmin_x.y=9999;
	pmin_x.data=0;
	this->Pontos=NULL;
	this->Fecho=NULL;
	this->Extremos=NULL;
	center.x=999;
	center.y=999;
}

ConexComp::~ConexComp()
{
/*	delete []this->Fecho;
	delete []this->Pontos;
	delete []this->Extremos;
*/}

SetConexComp::SetConexComp()
{
	Components=NULL;
	num_component=0;
}

SetConexComp::~SetConexComp()
{
//	delete [] this->Components;
}

Point2D::Point2D()
{
	x=0;
	y=0;
	fx=0;
	fy=0;
	data=-1;
}

Point2D::~Point2D()
{
}
	

VImage::VImage(void)
{
    
	pImage=NULL;
	this->Proc_Image=NULL;
	this->yDPI=0;
	this->xDPI=0;
	this->Height=0;
	this->Width=0;
	ImageEo=NULL;
	ImageEs=NULL;
	ImageGrax=NULL;
	ImageGray=NULL;
	Corn_List=new Point2D[MAX_CORNERS];
	n_Corners=0;
	Cn_Es=NULL;
	Cn_Eo=NULL;
	Cn_In=NULL;
    Lines_List = new Point2D[20];
	houghMap=NULL;
	HMap_Height=0;
	HMap_Width=360;
	Distances=NULL;
	step_Distance=1;
	MaxDistance=-1;
	MaxAngle=360;
	Bin_Image=NULL;
	Labels=NULL;
	labels_cont=NULL;
	fechados= new int [100];
	valid_labels=NULL;
	num_valid_labels=0;
	ref_points=NULL;
	valid_labels_acc=NULL;
	this->Componentes=NULL;
    C_pontos=NULL;
	num_calpontos=10;
	recognition=false;
	fpoint=false;
	dfocal=0;
	fcos = new float[360];
    fsin = new float[360];
    for (int i=0 ; i<360; i++) { 
        fsin[i]= (float) sin(((3.141516)*i)/180.0F);
	    fcos[i]= (float) cos(((3.141516)*i)/180.0F);
    }
}

VImage::VImage(BYTE * pInfo,long XDpi,long YDpi,int width,int Height)
{
	this->pImage=pInfo;			
	this->xDPI=XDpi;
	this->yDPI=YDpi;
	this->Height=Height;
	this->Width=width;
}

VImage::~VImage(void)
{

}

void VImage:: Filter_Gaussian()
{
	float new_r,new_g,new_b;
	if  (this->Proc_Image==NULL)
	{        this->Proc_Image=new BYTE[3*this->Height*this->Width];
	
				for (int i=0; i < this->Height;i++ )
						for (int j=1; j < this->Width-1;j++ )
					{
						new_r =((2*pImage[3*((i*this->Width)+j)])+pImage[3*((i*this->Width)+j-1)]+pImage[3*((i*this->Width)+j+1)])/4;
						new_g =((2*pImage[3*((i*this->Width)+j)+1])+pImage[3*((i*this->Width)+j-1)+1]+pImage[3*((i*this->Width)+j+1)+1])/4;
						new_b =((2*pImage[3*((i*this->Width)+j)+2])+pImage[3*((i*this->Width)+j-1)+2]+pImage[3*((i*this->Width)+j+1)+2])/4;
			
						Proc_Image[3*((i*this->Width)+j)]=(BYTE)new_r;
						Proc_Image[3*((i*this->Width)+j)+1]=(BYTE)new_g;
						Proc_Image[3*((i*this->Width)+j)+2]=(BYTE)new_b;
			
					}	

				for (int j=0; j < this->Width;j++ )
						for (int i=1; i < this->Height-1;i++ )
	
					{
						new_r =((2*pImage[3*((i*this->Width)+j)])+pImage[3*(((i-1)*this->Width)+j)]+pImage[3*(((i+1)*this->Width)+j)])/4;
						new_g =((2*pImage[3*((i*this->Width)+j)+1])+pImage[3*(((i-1)*this->Width)+j)+1]+pImage[3*(((i+1)*this->Width)+j)+1])/4;
						new_b =((2*pImage[3*((i*this->Width)+j)+2])+pImage[3*(((i-1)*this->Width)+j)+2]+pImage[3*(((i+1)*this->Width)+j)+2])/4;
			
					Proc_Image[3*((i*this->Width)+j)]=(BYTE)new_r;
					Proc_Image[3*((i*this->Width)+j)+1]=(BYTE)new_g;
					Proc_Image[3*((i*this->Width)+j)+2]=(BYTE)new_b;
			
					}
	}
	else
	{
				for (int i=0; i < this->Height;i++ )
						for (int j=1; j < this->Width-1;j++ )
					{
						new_r =((2*Proc_Image[3*((i*this->Width)+j)])+Proc_Image[3*((i*this->Width)+j-1)]+Proc_Image[3*((i*this->Width)+j+1)])/4;
						new_g =((2*Proc_Image[3*((i*this->Width)+j)+1])+Proc_Image[3*((i*this->Width)+j-1)+1]+Proc_Image[3*((i*this->Width)+j+1)+1])/4;
						new_b =((2*Proc_Image[3*((i*this->Width)+j)+2])+Proc_Image[3*((i*this->Width)+j-1)+2]+Proc_Image[3*((i*this->Width)+j+1)+2])/4;
			
						Proc_Image[3*((i*this->Width)+j)]=(BYTE)new_r;
						Proc_Image[3*((i*this->Width)+j)+1]=(BYTE)new_g;
						Proc_Image[3*((i*this->Width)+j)+2]=(BYTE)new_b;
			
					}	

				for (int j=0; j < this->Width;j++ )
						for (int i=1; i < this->Height-1;i++ )
	
					{
						new_r =((2*Proc_Image[3*((i*this->Width)+j)])+Proc_Image[3*(((i-1)*this->Width)+j)]+Proc_Image[3*(((i+1)*this->Width)+j)])/4;
						new_g =((2*Proc_Image[3*((i*this->Width)+j)+1])+Proc_Image[3*(((i-1)*this->Width)+j)+1]+Proc_Image[3*(((i+1)*this->Width)+j)+1])/4;
						new_b =((2*Proc_Image[3*((i*this->Width)+j)+2])+Proc_Image[3*(((i-1)*this->Width)+j)+2]+Proc_Image[3*(((i+1)*this->Width)+j)+2])/4;
			
					Proc_Image[3*((i*this->Width)+j)]=(BYTE)new_r;
					Proc_Image[3*((i*this->Width)+j)+1]=(BYTE)new_g;
					Proc_Image[3*((i*this->Width)+j)+2]=(BYTE)new_b;
			
					}	
	}
}

void VImage::GrayScale()
{
	for (int i=0; i < this->Height*this->Width;i++ ){
		this->Proc_Image[3*i]=((this->Proc_Image[(3*i)+2])*117 + (this->Proc_Image[(3*i)+1])*601 + (this->Proc_Image[3*i])*306) >> 10;
		this->Proc_Image[(3*i)+1]=this->Proc_Image[(3*i)+2]=this->Proc_Image[3*i];
	
	}
}


void VImage::Filter_Soubel(int threshold)
{
	if (this->ImageEo==NULL)
		this->ImageEo=new int[this->Height*this->Width];
	if (this->ImageEs==NULL)
		this->ImageEs=new int[this->Height*this->Width];

	if (this->Bin_Image==NULL)
		this->Bin_Image=new BYTE[this->Height*this->Width];

	for (int i=1; i < this->Height-1;i++ )
		for (int j=1; j < this->Width-1;j++ )
		ImageEo[(i*this->Width)+j]=-Proc_Image[3*(((i-1)*this->Width)+j-1)]-2*Proc_Image[3*(((i-1)*this->Width)+j)]-Proc_Image[3*(((i-1)*this->Width)+j+1)]+Proc_Image[3*(((i+1)*this->Width)+j-1)]+2*Proc_Image[3*(((i+1)*this->Width)+j)]+Proc_Image[3*(((i+1)*this->Width)+j+1)];
		

	for (i=1; i < this->Height-1;i++ )
		for (int j=1; j < this->Width-1;j++ )
		ImageEs[(i*this->Width)+j]=-Proc_Image[3*(((i-1)*this->Width)+j-1)]-2*Proc_Image[3*((i*this->Width)+j-1)]-Proc_Image[3*(((i+1)*this->Width)+j-1)]+Proc_Image[3*(((i-1)*this->Width)+j+1)]+2*Proc_Image[3*((i*this->Width)+j+1)]+Proc_Image[3*(((i+1)*this->Width)+j+1)];
		
	for ( i=1; i < this->Height-1;i++ )
		for (int j=1; j < this->Width-1;j++ ){
		
			if (sqrt((float)(ImageEo[(i*this->Width)+j]*ImageEo[(i*this->Width)+j]+ImageEs[(i*this->Width)+j]*ImageEs[(i*this->Width)+j]))<threshold)
			{
				Proc_Image[3*((i*this->Width)+j)]=255;
				this->Bin_Image[(i*this->Width)+j]=1;
			}
			else
			{
				Proc_Image[3*((i*this->Width)+j)]=0;
				this->Bin_Image[(i*this->Width)+j]=0;
			}
			Proc_Image[3*((i*this->Width)+j)+2]=Proc_Image[3*((i*this->Width)+j)+1]=Proc_Image[3*((i*this->Width)+j)];
			
		}

}


void VImage::Filter_Canny(int Up_Thr, int Down_Thr)
{
	int Jx;
	int Jy;
	float aprox;
	float d1=0;
	float d2=45;
	float d3=90;
	float d4=135;
	
	if (this->Cn_Eo==NULL)
		this->Cn_Eo=new float[this->Height*this->Width];
	if (this->Cn_Es==NULL)
		this->Cn_Es=new float[this->Height*this->Width];
	if (this->Cn_In==NULL)
		this->Cn_In=new float[this->Height*this->Width];

/*Parte 1 CANNY-ENHANCER */
	for (int i=1; i < this->Height-1;i++ )
		for (int j=1; j < this->Width-1;j++ ){
		
			Jx = Proc_Image[3*((i*this->Width)+j-1)]-Proc_Image[3*((i*this->Width)+j+1)];
			Jy = Proc_Image[3*(((i-1)*this->Width)+j)]-Proc_Image[3*(((i+1)*this->Width)+j)];
			this->Cn_Es[(i*this->Width)+j]=sqrt((float)(Jx*Jx + Jy*Jy));
			if ( Jx==0 )
			this->Cn_Eo[(i*this->Width)+j]=0;
			else
			this->Cn_Eo[(i*this->Width)+j]=atan((float)(Jy/Jx));
		}

/*Parte 2 Nonmax_Supression*/
	
	for ( i=1; i < this->Height-1;i++ )
		for (int j=1; j < this->Width-1;j++ ){
				aprox=d1;
				if (abs(this->Cn_Eo[(i*this->Width)+j]-aprox) > abs(this->Cn_Eo[(i*this->Width)+j]-d2))
				aprox=d2;
				if (abs(this->Cn_Eo[(i*this->Width)+j]-aprox) > abs(this->Cn_Eo[(i*this->Width)+j]-d3))
				aprox=d3;
				if (abs(this->Cn_Eo[(i*this->Width)+j]-aprox) > abs(this->Cn_Eo[(i*this->Width)+j]-d4))
				aprox=d4;

				if (aprox==d1)
				{
					if (((this->Cn_Es[(i*this->Width)+j])<(this->Cn_Es[(i*this->Width)+j-1]))||((this->Cn_Es[(i*this->Width)+j])<(this->Cn_Es[(i*this->Width)+j+1])))
					this->Cn_In[(i*this->Width)+j]=0;
					else
					this->Cn_In[(i*this->Width)+j]=this->Cn_Es[(i*this->Width)+j];
				}

				if (aprox==d2)
				{
					if (((this->Cn_Es[(i*this->Width)+j])<(this->Cn_Es[((i+1)*this->Width)+j-1]))||((this->Cn_Es[(i*this->Width)+j])<(this->Cn_Es[((i-1)*this->Width)+j+1])))
					this->Cn_In[(i*this->Width)+j]=0;
					else
					this->Cn_In[(i*this->Width)+j]=this->Cn_Es[(i*this->Width)+j];
				}

				if (aprox==d3)
				{
					if (((this->Cn_Es[(i*this->Width)+j])<(this->Cn_Es[((i-1)*this->Width)+j]))||((this->Cn_Es[(i*this->Width)+j])<(this->Cn_Es[((i+1)*this->Width)+j])))
					this->Cn_In[(i*this->Width)+j]=0;
					else
					this->Cn_In[(i*this->Width)+j]=this->Cn_Es[(i*this->Width)+j];
				}

				if (aprox==d4)
				{
					if (((this->Cn_Es[(i*this->Width)+j])<(this->Cn_Es[((i-1)*this->Width)+j-1]))||((this->Cn_Es[(i*this->Width)+j])<(this->Cn_Es[((i+1)*this->Width)+j+1])))
					this->Cn_In[(i*this->Width)+j]=0;
					else
					this->Cn_In[(i*this->Width)+j]=this->Cn_Es[(i*this->Width)+j];
				}

		}	

		/*HYSTERESIS_THRESH*/
		for ( i=1; i < this->Height-1;i++ )
			for (int j=1; j < this->Width-1;j++ ){
				/*if ((this->Cn_In[(i*this->Width)+j] > 0)&&(this->Cn_In[(i*this->Width)+j] >Up_Thr )){
					 
				}
			*/	if (this->Cn_In[(i*this->Width)+j]< Up_Thr)
				Proc_Image[3*((i*this->Width)+j)]=255;
				else
				Proc_Image[3*((i*this->Width)+j)]=0;
				Proc_Image[3*((i*this->Width)+j)+2]=Proc_Image[3*((i*this->Width)+j)+1]=Proc_Image[3*((i*this->Width)+j)];
			}
}

/*ALGORITMO DE CORNERS*/

void VImage::Corners(int threshold, int janela)
{
	int half=(int)(janela/2);
	int a,b,c,d;
	int lamd_2;
	
	if (this->ImageGrax==NULL)
		this->ImageGrax=new int[this->Height*this->Width];
	if (this->ImageGray==NULL)
		this->ImageGray=new int[this->Height*this->Width];
	
	for (int j=1; j < this->Width-1;j++ )
	    for (int i=0; i < this->Height;i++ )
		this->ImageGray[(i*this->Width)+j]=Proc_Image[3*((i*this->Width)+j-1)]-Proc_Image[3*((i*this->Width)+j+1)];
		
	for ( j=0; j < this->Width;j++ )
		for (int i=1; i < this->Height-1;i++ )
		 this->ImageGrax[(i*this->Width)+j]=Proc_Image[3*(((i-1)*this->Width)+j)]-Proc_Image[3*(((i+1)*this->Width)+j)];	
	
	this->n_Corners=0;	
	for (int i=half+1; i < this->Height-half-1;i++ )
		for (int j=half+1; j < this->Width-half-1;j++ ){
		    	if( Proc_Image[3*((i*this->Width)+j)]==0){
					//if (!((Proc_Image[3*(((i-1)*this->Width)+j-1)]==0)&&(Proc_Image[3*(((i-1)*this->Width)+j)]==0)&&(Proc_Image[3*(((i-1)*this->Width)+j+1)]==0)&&(Proc_Image[3*(((i)*this->Width)+j-1)]==0)&&(Proc_Image[3*(((i)*this->Width)+j+1)]==0)&&(Proc_Image[3*(((i+1)*this->Width)+j-1)]==0)&&(Proc_Image[3*(((i+1)*this->Width)+j)]==0) &&(Proc_Image[3*(((i+1)*this->Width)+j+1)]==0))){
						if(!((Proc_Image[3*(((i-1)*this->Width)+j-1)]==255)&&(Proc_Image[3*(((i-1)*this->Width)+j)]==255)&&(Proc_Image[3*(((i-1)*this->Width)+j+1)]==255)&&(Proc_Image[3*(((i)*this->Width)+j-1)]==255)&&(Proc_Image[3*(((i)*this->Width)+j+1)]==255)&&(Proc_Image[3*(((i+1)*this->Width)+j-1)]==255)&&(Proc_Image[3*(((i+1)*this->Width)+j)]==255) &&(Proc_Image[3*(((i+1)*this->Width)+j+1)]==255))){
						a=0;b=0;c=0;d=0;
						for (int k=0; k < janela ; k++)
							for (int w=0;w < janela ; w++)
							{
								a+=this->ImageGrax[((i-half+ k)*this->Width) + (j-half)+w]*this->ImageGrax[((i-half+ k)*this->Width) + (j-half)+w];
								d+=this->ImageGray[((i-half+ k)*this->Width) + (j-half)+w]*this->ImageGray[((i-half+ k)*this->Width) + (j-half)+w];
							}
							lamd_2=a;
							//if (b<lamd_2)
							//lamd_2=b;
							//if (c<lamd_2)
							//lamd_2=c;
							if (d<lamd_2)
							lamd_2=d;

							if (lamd_2 > threshold){
								if (this->n_Corners < MAX_CORNERS)
								 {
									 this->Corn_List[this->n_Corners].x=i;
									 this->Corn_List[this->n_Corners].y=j;
									this->Corn_List[this->n_Corners].data=lamd_2;
									this->n_Corners++;
								}	
							}
						}
				   }
				//}
			}
		

	//ordenção
		quicksort(this->Corn_List,0, this->n_Corners-1);
		Extract_Corners(janela);
				
		for (int k=0 ; k < this->n_Corners;k++)
		{	int i=this->Corn_List[k].x;
			int j=this->Corn_List[k].y;
			for (int w = -2 ; w < 2; w++)
				for( int z =-2 ; z < 2 ; z++){ 
					Proc_Image[3*(((i+w)*this->Width)+(j+z))]=255;
					Proc_Image[3*(((i+w)*this->Width)+(j+z))+1]=0;
					Proc_Image[3*(((i+w)*this->Width)+(j+z))+2]=0;
				}
			
		}

}

/*ALGORITMO DE IDENTIFICAÇÃO DE LINHAS (TRANSFORMADA DE HOUGH)*/
void VImage::Transform_Hough (int HMap_Height ,int threshold)
{
	int HMapsize;
	int position;
	float distance;
	int index;
	int max;
	float m;
	int value,aux, oldPosition;

	if (this->MaxDistance==-1)
		this->MaxDistance=sqrt(float(this->Height*this->Height +this->Width*this->Width));
	if (this->Distances==NULL)
	{
		this->Distances= new float[HMap_Height];
        for (int i=0; i < HMap_Height ;i++)
			this->Distances[i]=i*(this->MaxDistance/(float)HMap_Height);
	}

	if (this->houghMap==NULL)
	{	this->HMap_Height=HMap_Height;
		this->houghMap= new int[this->HMap_Height*this->HMap_Width];
	}
	HMapsize=this->HMap_Height*this->HMap_Width;
	for (int i=0; i<HMapsize;i++)
		this->houghMap[i]=0;
	
	for ( i=0; i<20;i++)
	{
		Lines_List[i].x=0;
		Lines_List[i].y=0;
		Lines_List[i].data=0;
	}
	
	position=0;
	for ( i=1; i < this->Height-1;i++ )
			for (int j=1; j < this->Width-1;j++ ){
				if (Proc_Image[3*((i*this->Width)+j)]==0){
					for (int k=0 ; k < this->MaxAngle ;k++){
						distance =(i*this->fcos[k])+ (j*this->fsin[k]);
						index =(int)fabs(distance)/*/(this->MaxDistance/(float)HMap_Height)))*/; 
						if (index > 0)
							this->houghMap[index*this->MaxAngle+k]++;
						else{
								if (k > 180)
								this->houghMap[(k-180)*HMap_Height-index]++;
								else
                                this->houghMap[(k+180)*HMap_Height-index]++;
						}
					}
				}
				position++;
			}

position=0;
index=0;
	for ( i=0; i<360; i++) {
	  for (int j=0; j<400; j++) {

		  if ( this->houghMap[i*400+j]>= 100 ) {
			  DrawLine(  j,i);
		  }
		  
	  }
	  
	}
	max=this->houghMap[0];	
		for ( i=0; i < HMapsize;i++ )
		{
			if(this->houghMap[i]>max)
				max=this->houghMap[i];
		}

		for ( i=0; i < this->Height;i++ )
			for (int j=0; j <this->Width ;j++ )
			{
				if (this->houghMap[i*this->HMap_Width +j]  >10){ 
			        if (max==0)
			        this->Proc_Image[3*((i*this->Width)+j)]=0;
			        else
			        this->Proc_Image[3*((i*this->Width)+j)]=(BYTE)(255*this->houghMap[i*this->HMap_Width +j]/max);
			        this->Proc_Image[3*((i*this->Width)+j)+1]=this->Proc_Image[3*((i*this->Width)+j)+2]=this->Proc_Image[3*((i*this->Width)+j)];
				}
				else
					this->Proc_Image[3*((i*this->Width)+j)+1]=this->Proc_Image[3*((i*this->Width)+j)+2]=this->Proc_Image[3*((i*this->Width)+j)]=0;
			}
}


/*DESENHA UMA LINHA NO FORMATO DA SAIDA DA TRANSFORMADA DE HOUGH*/

void VImage::DrawLine(float rho,int angle)
{

     int i=0, x, y;
	float cosAngle, sinAngle, radAngle;

	radAngle = (angle*(3.141516/2))/90.0F;

    cosAngle = (float)cos(radAngle);
	sinAngle = (float)sin(radAngle);

    if ( (angle>45  && angle<135) ||
		 (angle>225 && angle<315)   ) {
			 for (; i<this->Width; i++) {
	    x=i;
		y=(int)((rho-x*cosAngle)/sinAngle);
		if (y>=0 && y<this->Height)
		{
			this->Proc_Image[3*(((y)*this->Width)+(x))]=255;
			this->Proc_Image[3*(((y)*this->Width)+(x))+1]=0;
			this->Proc_Image[3*(((y)*this->Width)+(x))+2]=0;
		}
	  }
	}
	else {
		for (; i<this->Height; i++) {
	    y=i;
		x=(int)((rho-y*sinAngle)/cosAngle);
		if(x>=0 && x<this->Width){
		 this->Proc_Image[3*(((y)*this->Width)+(x))]=255;
			this->Proc_Image[3*(((y)*this->Width)+(x))+1]=0;
			this->Proc_Image[3*(((y)*this->Width)+(x))+2]=0;
		
		}
		}
	}



}

/*ALGORITMO DE ORDENAçÃO PARA OS CORNERS*/

void VImage::quicksort(Point2D * input_array, int bottom, int top) // uses recursion-calling itself
{
     int middle;
     if(top > bottom)
    {
          middle = partition(input_array, bottom, top);
          quicksort(input_array, bottom, middle-1);   //sort top partition
          quicksort(input_array, middle + 1, top);    //sort bottom partition
     }
     return;
}

//Function to determine the partitions
//partitions input_array, returning middle index. Used by procedure quicksort
int VImage::partition(Point2D* list, const int first, const int last)
{  
	int x = list[first].data; 
	int i = first;  
	int j = last + 1;  
	while (true) {       
		while (list[--j].data < x) { }
		while (list[++i].data > x) { }  
		if (i < j) exchange(list,i,j);       
		else {  
		exchange (list,first, j);             
		return j;        
		}  
	}
}


void VImage::exchange(Point2D* input_array,int i, int j)
{
	 int temp = input_array[i].data;    //switch elements at positions i and j
	 input_array[i].data = input_array[j].data;
	 input_array[j].data = temp;

	temp = input_array[i].x;    //switch elements at positions i and j
	input_array[i].x = input_array[j].x;
	input_array[j].x = temp;

	temp= input_array[i].y;    //switch elements at positions i and j
	input_array[i].y = input_array[j].y;
	input_array[j].y = temp;

}


void VImage::Extract_Corners(int janela)
{
	int position=0;
	int tam=(int)(janela/2);
	Point2D corrent=this->Corn_List[position];
	for (int i=1; i<this->n_Corners;i++)
	{
		if ((abs(corrent.x-this->Corn_List[i].x)>tam)&&(abs(corrent.y-this->Corn_List[i].y)>tam))
		{   position++;
			this->Corn_List[position]=this->Corn_List[i];
			corrent=this->Corn_List[i];
		}
	}

	this->n_Corners=position;
}



/* 
 * Cria uma imagem sem distorção radial (usando interpolação linear) 
 * e distorçao radiométrica a partir de uma imagem
 */
void VImage::IRRCorrection () 
{

  int i, j, posPixel, posPixelH, posPixelV, position=0, position2=0;
  float red, green, blue;
  if (this->Proc_Image==NULL)
		this->Proc_Image=new BYTE[3*this->Height*this->Width];

  for (i=0; i<this->Height; i++) {
	  for (j=0; j<this->Width; j++) {

	  //mapeamento do pixel
	  posPixel = mapa[position];
      posPixelH = mapaH[position];
      posPixelV = mapaV[position];

	  blue = this->pImage[posPixel++]  * iFactor[position]  +
		     this->pImage[posPixelH++] * iFactorH[position] +
		     this->pImage[posPixelV++] * iFactorV[position] ;
      
	  green = this->pImage[posPixel++]  *iFactor[position]  +
		      this->pImage[posPixelH++] *iFactorH[position] +
		      this->pImage[posPixelV++] *iFactorV[position] ;

	  red = this->pImage[posPixel]  * iFactor[position]  +
		    this->pImage[posPixelH] * iFactorH[position] +
			this->pImage[posPixelV] * iFactorV[position] ;

      //cor do pixel
	  blue *= radioFactor[position];
	  green *= radioFactor[position];
	  red *= radioFactor[position];

	  if (red>255.0F)	red=255.0F;
	  if (green>255.0F) green=255.0F;
	  if (blue>255.0F)  blue=255.0F;

	  //pinta pixel (i,j)
	  this->Proc_Image[position2++] = (unsigned char)blue;
      this->Proc_Image[position2++] = (unsigned char)green;
	  this->Proc_Image[position2++] = (unsigned char)red;
     
      //fGrey[position++]=red; //(blue + green + red)*0.3F;
      //GreyFlag=1;

	  position++;
	}
  }
}


void VImage::CameraParametersInit() 
{

  //variáveis auxiliares
  int gSize;
int i, j, aux;
  int xim, yim, position=0;
  float x, y, xd, yd, sqDistance, alpha;

  //mensagem inicial
  printf("\n\nParametros da camera Creative WebCam PRO:");

  //verifica tamanho da imagem
  gSize = this->Height*this->Width;

  //centro da câmera
  int ox = (int)(286 * (this->Width/640.0F));
  int oy = (int)(270 * (this->Height/480.0F));
 printf("\nox = %i", ox);
  printf("\noy = %i", oy);

  //tamanho efetivo dos pixels em milímetros
  float sx = 0.129F * (640.0F/this->Width);
  float sy = 0.127F * (480.0F/this->Height);
  printf("\nsx = %f", sx);
  printf("\nsy = %f", sy);

  //foco da câmera
  float f = 100.0F;
  printf("\nf = %f", f);

  //parâmetro m1
  float m1 = -0.000025F;

  //inicializa arrays
  mapa = new int[gSize];
  mapaH =new int[gSize]; 
  mapaV = new int[gSize];
  radioFactor = new float[gSize];
  iFactor = new float[gSize];
  iFactorH = new float[gSize];
  iFactorV = new float[gSize];

  //fator de correção da tangente de alpha
float   tanAlphaCorrection=0.85F;

  for (i=0; i<this->Height; i++) {
	for (j=0; j<this->Width; j++) {
      //coordenada do ponto da imagem final
	  x=(j-ox)*sx;
	  y=(i-oy)*sy;

	  //distância quadrada
      sqDistance = (float)(pow(x,2)+pow(y,2));

	  //coordenadas do ponto distorcido
	  xd=x*(1+sqDistance*m1);
      yd=y*(1+sqDistance*m1);

	  //coordenadas do ponto distorcido em pixel;
	  xim=(int)((xd/sx)+ox);
      yim=(int)((yd/sy)+oy);

	  //coordenadas do vizinho horizontal mais próximo
      aux = (int)(((xd/sx)+ox)*1000);
	  aux %= 1000;
	  aux -= 500;
	  if (aux >= 0) {
	    mapaH[position]=3*((xim+1)+this->Width*yim);
		iFactorH[position]= ((float)aux)/1000.0F;
	  }
	  else {
        mapaH[position]=3*((xim-1)+this->Width*yim);
        iFactorH[position]= ((float)-aux)/1000.0F;
	  }

      //coordenadas do vizinho vertical mais próximo
	  aux = (int)(((yd/sy)+oy)*1000);
	  aux %= 1000;
	  aux -= 500; 
	  if (aux >= 0) {
	    mapaV[position]=3*(xim+this->Width*(yim+1));
		iFactorV[position]= ((float)aux)/1000.0F;
	  }
	  else {
        mapaV[position]=3*(xim+this->Width*(yim-1));
		iFactorV[position]= ((float)-aux)/1000.0F;
	  }

	  //fator de interpolação 
	  iFactor[position]= 1.0F - iFactorH[position] - iFactorV[position];

	  //armazena coordenada
	  mapa[position]=3*(xim+this->Width*yim);

	  //calcula o ângulo alpha
	  alpha = (float)atan2(tanAlphaCorrection*sqrt(sqDistance), f);

	  //armazena (1/cos4)
	  radioFactor[position] = 1.0F/(float)pow(cos(alpha),4);

	  position++;
	}
  }

}

/**/

void  VImage::find_conexcomponents()
{
	int cores[10][3];
cores [0][0]=255;
cores [0][1]=0;
cores [0][2]=0;

cores [1][0]=0;
cores [1][1]=0;
cores [1][2]=255;

cores [2][0]=0;
cores [2][1]=255;
cores [2][2]=0;

cores [3][0]=255;
cores [3][1]=0;
cores [3][2]=255;

cores [4][0]=0;
cores [4][1]=255;
cores [4][2]=255;
    
int  max_labels[2];
max_labels[0]=0;
max_labels[1]=0;
    Point2D max1_x;
	max1_x.x=-1;
	max1_x.y=-1;
	Point2D min1_x;
	min1_x.x=1000;
	min1_x.y=1000;
	Point2D max1_y;
	max1_y.x=-1;
	max1_y.y=-1;
	Point2D min1_y;
	min1_y.x=1000;
	min1_y.y=1000;
	
	Point2D max2_x;
	max2_x.x=-1;
	max2_x.y=-1;
	Point2D min2_x;
	min2_x.x=1000;
	min2_x.y=1000;
	Point2D max2_y;
	max2_y.x=-1;
	max2_y.y=-1;
	Point2D min2_y;
	min2_y.x=1000;
	min2_y.y=1000;
	
	int label=0;
	this->labels_cont=NULL;

	if (this->Labels==NULL)
		this->Labels=new int [this->Height*this->Width];
    
	for (int i=0; i<this->Height*this->Width; i++)
		this->Labels[i]=0;
	
	for ( i=1; i<this->Height-1; i++)
			for (int j=1; j<this->Width-1;j++)
		       if ((Proc_Image[3*((i*this->Width)+j)]==0)&&(this->Labels[i*this->Width+j]==0))
			   {   conex(i,j,++label);
				  //printf("primero ponto: %d ,%d \n",i,j);
	               //getch();
			   }
	        
	if (this->labels_cont==NULL)
		this->labels_cont=new int[label+1];

	for (int i=0; i<label+1; i++)
		labels_cont[i]=0;

	
	for (int i=0; i<this->Height*this->Width; i++)
	{    labels_cont[Labels[i]]++;
	     
	
		Proc_Image[3*i ]=255;
	    Proc_Image[3*i +1]=255;
	    Proc_Image[3*i +2]=255;
	}
    int count=0;
	for (int l=1; l < label+1 ;l++){
	 
		if (labels_cont[l] > 300){
			//max_labels[count+1]=l;
			for ( i=1; i<this->Height-1; i++)
				for (int j=1; j<this->Width-1;j++)
				
					if (this->Labels[i*this->Width+j]== l)
					{
					   Proc_Image[3*((i*this->Width)+j)]=cores[(l+1)%4][0];
					   Proc_Image[3*((i*this->Width)+j)+1]=cores[(l+1)%4][1];
					   Proc_Image[3*((i*this->Width)+j)+2]=cores[(l+1)%4][2];
					}
		count++;			
		}
	}
   
	/*if ( count <=2)
	{
		for ( i=1; i<this->Height-1; i++)
				for (int j=1; j<this->Width-1;j++)
	        if ()     		
	}*/
	//free(this->labels_cont);
   //delete(this->labels_cont);
	
   printf("%d\n",count);
			
}




void VImage::conex(int i, int j, int label)
{

if ((i<this->Height) && (j < this->Width))
	{
		     //printf("ultimo ponto: %d ,%d \n",i,j);
		     this->Labels[i*this->Width+j]=label;
            //Proc_Image[3*((i*this->Width)+j)]=cores[label%4][0];
			//Proc_Image[3*((i*this->Width)+j)+1]=cores[label%4][1];
			//Proc_Image[3*((i*this->Width)+j)+2]=cores[label%4][2];
			//getch();
			//printf("ultimo ponto: %d ,%d \n",label,label);
				for (int w = -2 ; w < 2; w++)
					for( int z =-2 ; z < 2 ; z++){ 
						if ((i+w< this->Height)&&(j+z<this->Width) &&(i+w>=0)&&(j+z>=0))
						{
							if ( (Proc_Image[3*(((i+w)*this->Width)+(j+z))]==0)&& (this->Labels[(i+w)*this->Width+(j+z)]==0))
							{	//this->Labels[(i+w)*this->Width+(j+z)]=label;
								conex(i+w,j+z,label); break;
							}
						}
					}
	}
}


void  VImage::find_conexcomponents2()
{
	int cores[10][3];
cores [0][0]=255;
cores [0][1]=0;
cores [0][2]=0;

cores [1][0]=0;
cores [1][1]=0;
cores [1][2]=255;

cores [2][0]=0;
cores [2][1]=255;
cores [2][2]=0;

cores [3][0]=255;
cores [3][1]=0;
cores [3][2]=255;

cores [4][0]=0;
cores [4][1]=255;
cores [4][2]=255;
    
int  max_labels[2];
max_labels[0]=0;
max_labels[1]=0;
    Point2D max1_x;
	max1_x.x=-1;
	max1_x.y=-1;
	Point2D min1_x;
	min1_x.x=1000;
	min1_x.y=1000;
	Point2D max1_y;
	max1_y.x=-1;
	max1_y.y=-1;
	Point2D min1_y;
	min1_y.x=1000;
	min1_y.y=1000;
	
	Point2D max2_x;
	max2_x.x=-1;
	max2_x.y=-1;
	Point2D min2_x;
	min2_x.x=1000;
	min2_x.y=1000;
	Point2D max2_y;
	max2_y.x=-1;
	max2_y.y=-1;
	Point2D min2_y;
	min2_y.x=1000;
	min2_y.y=1000;
	
	int label=0;
	this->labels_cont=NULL;
    valid_labels=NULL;
	valid_labels_acc=NULL;

	//this->labels_ref=NULL;
	//this->ConecComp==NULL;

	if (this->Labels==NULL)
		this->Labels=new int [this->Height*this->Width];
    
	for (int i=0; i<this->Height*this->Width; i++)
		this->Labels[i]=0;
	for (int i=0; i<100;i++)
		this->fechados[i]=0;

	for ( i=1; i<this->Height-1; i++)
			for (int j=1; j<this->Width-1;j++)
		       if ((Proc_Image[3*((i*this->Width)+j)]==0)&&(this->Labels[i*this->Width+j]==0))
			   {	this->init.x=i;
                   	this->init.y=j;
					conex2(i,j,++label, 0);
				}
	
	if (this->labels_cont==NULL)
		this->labels_cont=new int[label+1];
	if (this->valid_labels_acc==NULL)
		this->valid_labels_acc=new Point2D[label+1];
    
	for (int i=0; i<label+1; i++)
	{	labels_cont[i]=0;
	    this->valid_labels_acc[i].x=0;
		this->valid_labels_acc[i].y=0;
	}
	
	for ( i=0; i<this->Height; i++)
				for (int j=0; j<this->Width;j++)
			{
				labels_cont[Labels[i*this->Width +j]]++;
				valid_labels_acc[Labels[i*this->Width +j]].x+=i;
				valid_labels_acc[Labels[i*this->Width +j]].y+=j;
	   			 Proc_Image[3*(i*this->Width +j) ]=255;
				 Proc_Image[3*(i*this->Width +j) +1]=255;
	             Proc_Image[3*(i*this->Width +j) +2]=255;
	
			}		
	
	for (int i=0; i<label+1 ;i++)
	{
		this->valid_labels_acc[i].x = this->valid_labels_acc[i].x/this->labels_cont[i];
		this->valid_labels_acc[i].y = this->valid_labels_acc[i].y/this->labels_cont[i];
	}
	/*
	for (int i=0; i<this->Height*this->Width; i++)
	{    labels_cont[Labels[i]]++;
	     
	
		Proc_Image[3*i ]=255;
	    Proc_Image[3*i +1]=255;
	    Proc_Image[3*i +2]=255;
	}
*/
    int count=0;
	for (int l=1; l < label+1 ;l++){
	 
		if (labels_cont[l] > 100){
			//max_labels[count+1]=l;
			if (this->fechados[l]>=1){
			for ( i=1; i<this->Height-1; i++)
				for (int j=1; j<this->Width-1;j++)
				
					if (this->Labels[i*this->Width+j]== l)
					{
					   Proc_Image[3*((i*this->Width)+j)]=cores[(l+1)%4][0];
					   Proc_Image[3*((i*this->Width)+j)+1]=cores[(l+1)%4][1];
					   Proc_Image[3*((i*this->Width)+j)+2]=cores[(l+1)%4][2];
					}
			count++;
			}
					
		}
	}

	if (this->valid_labels==NULL)
		this->valid_labels=new int[count];
			
	count=0;
	for (int l=1; l < label+1 ;l++){
	 
		if (labels_cont[l] > 100){
						if (this->fechados[l]>=1)
						{
							valid_labels[count]=l;
			        		count++;
			            }
					
		}
	}
  this->num_valid_labels=count;
  printf("%d\n",count);
			
}

void VImage::conex2(int i, int j, int label, int level)
{
	int lax_x, lax_y;
	bool flag_achou=false;
	
	if ((i<this->Height) && (j < this->Width))
	{
			this->Labels[i*this->Width+j]=label;
			//Proc_Image[3*((i*this->Width)+j)]=cores[label%4][0];
			//Proc_Image[3*((i*this->Width)+j)+1]=cores[label%4][1];
			//Proc_Image[3*((i*this->Width)+j)+2]=cores[label%4][2];
              for (int w = -2 ; w < 3; w++)
					for( int z =-2 ; z < 3 ; z++){ 
						if ((i+w< this->Height)&&(j+z<this->Width)&&(j+z>=0)&&(i+w>=0) )
						{    
							if ((level > 190)&&(abs(this->init.x-(i+w))<=8)&&(abs(this->init.y-(j+z))<=8)&&(w!=0)&&(z!=0))
								this->fechados[label]++;
					
						if ( (Proc_Image[3*(((i+w)*this->Width)+(j+z))]==0)&& (this->Labels[(i+w)*this->Width+(j+z)]==0))
							{	//this->Labels[(i+w)*this->Width+(j+z)]=label;
								conex2(i+w,j+z,label,++level); 
							}
						}
					}
						
	}

}

void VImage::find_interesting_points()
{

	this->ref_points=NULL;	

	if (this->ref_points==NULL)
		this->ref_points= new Point2D[4*this->num_valid_labels];

	for (int i=0 ;i<this->num_valid_labels;i++)
	{
		this->ref_points[4*i].x=999;	this->ref_points[4*i].y=999;	
		this->ref_points[4*i+1].x=-1;	this->ref_points[4*i+1].y=-1;	
		this->ref_points[4*i+2].x=-1;	this->ref_points[4*i+2].y=999;	
		this->ref_points[4*i+3].x=999;	this->ref_points[4*i+3].y=-1;	
	
	}

	int label_pos;

	for ( i=1; i<this->Height-1; i++)
		  for (int j=1; j<this->Width-1;j++)
				{  
					if (this->Labels[i*this->Width+j] > 0)
					{					
							label_pos=find_label(this->Labels[i*this->Width+j]);
 							if (label_pos!= -1)
							{
							if (  (i  <= this->ref_points[label_pos*4].x))// && (j < this->ref_points[label_pos*4].y))
								{	
										this->ref_points[label_pos*4].x=i;
		        						this->ref_points[label_pos*4].y=j;
										this->ref_points[label_pos*4].data=this->Labels[i*this->Width+j];
								}
							
							if ( (i > this->ref_points[label_pos*4+1].x))//&& (j > this->ref_points[label_pos*4+1].y))
								{	
										this->ref_points[label_pos*4+1].x=i;
		        						this->ref_points[label_pos*4+1].y=j;
										this->ref_points[label_pos*4+1].data=this->Labels[i*this->Width+j];
								}

						
							if ( (j < this->ref_points[label_pos*4+2].y))// && (i > this->ref_points[label_pos*4+2].x))
								{	
									this->ref_points[label_pos*4+2].x=i;
		        					this->ref_points[label_pos*4+2].y=j;
								
								}

						if ( (j >= this->ref_points[label_pos*4+3].y))// && j < this->ref_points[label_pos*4+3].y)
								{	
									this->ref_points[label_pos*4+3].x=i;
		        					this->ref_points[label_pos*4+3].y=j;
								}

							}
					}
				}
/*int x,y,data;
 for (int i=0; i<this->num_valid_labels; i++)
	 {
		 x=this->ref_points[4*i].x;
		 y=this->ref_points[4*i].y;
		 data=this->ref_points[4*i].data;

     /*   for (int fila =1 ; fila >=0; fila--)
		   for (int j = y - 50 ; j >0 ;j--)
			 if (this->Labels[(x+fila)*Width+j]==this->ref_points[4*i].data)
				{
					this->ref_points[4*i].x=x+fila;
					this->ref_points[4*i].y=j;
					this->ref_points[4*i].data=this->Labels[(x+fila)*Width+j];
				}
/*
		 x=this->ref_points[4*i+1].x;
		 y=this->ref_points[4*i+1].y;
		 data=this->ref_points[4*i+1].data;

         for (int fila =2 ; fila >=0; fila--)
		   for (int j = y + 50 ; j < this->Width;j++)
			 if (this->Labels[(x+fila)*Width+j]==this->ref_points[4*i].data)
				{
					this->ref_points[4*i].x=x+fila;
					this->ref_points[4*i].y=j;
					this->ref_points[4*i].data=this->Labels[(x+fila)*Width+j];
				}		
				
				
				/*

		for (int j = this->ref_points[4*i + 1].y - 5 ; j > 0;j--)
			 if (this->Labels[(i-1)*this->Width+j]==this->ref_points[4*i].data)
				{
					this->ref_points[4*i].x=i-1;
					this->ref_points[4*i].y=j;
					this->ref_points[4*i].data=this->Labels[(i-1)*this->Width+j];
				}
	 }
*/
		for (int p=0 ;p < 4*this->num_valid_labels;p++)
		{    i=this->ref_points[p].x;
		     int j=this->ref_points[p].y;
			 if ((i>=0) &&(j>=0)&&(i<this->Height) &&(j<this->Width)){
                for (int w = -2 ; w < 2; w++)
				    for( int z =-2 ; z < 2 ; z++){ 
				     	pImage[3*(((i+w)*this->Width)+(j+z))]=0;
					    pImage[3*(((i+w)*this->Width)+(j+z))+1]=0;
					    pImage[3*(((i+w)*this->Width)+(j+z))+2]=255;
				    }
				}
		}

  /*  for (int i=0; i<50; i++)
		for (int j=0; j<100; j++)
		{
			 pImage[3*(((i)*this->Width)+(j))]=255;
			 pImage[3*(((i)*this->Width)+(j))+1]=255;
			 pImage[3*(((i)*this->Width)+(j))+2]=255;
		}
*/  
  delete []this->ref_points;
/*
if ( (i  <= this->ref_points[label_pos*4].x)&&(j > this->ref_points[label_pos*4].y))
								{	
										this->ref_points[label_pos*4].x=i;
		        						this->ref_points[label_pos*4].y=j;
										this->ref_points[label_pos*4].data=this->Labels[i*this->Width+j];
								}
							
							if ( (i > this->ref_points[label_pos*4+1].x))// && (j > this->ref_points[label_pos*4+1].y))
								{	
										this->ref_points[label_pos*4+1].x=i;
		        						this->ref_points[label_pos*4+1].y=j;
										this->ref_points[label_pos*4+1].data=this->Labels[i*this->Width+j];
								}

						
							if ( (j<= this->ref_points[label_pos*4+2].y) && (i > this->ref_points[label_pos*4+2].x))
								{	
									this->ref_points[label_pos*4+2].x=i;
		        					this->ref_points[label_pos*4+2].y=j;
								
								}

						if ( (j > this->ref_points[label_pos*4+3].y))// && j < this->ref_points[label_pos*4+3].y)
								{	
									this->ref_points[label_pos*4+3].x=i;
		        					this->ref_points[label_pos*4+3].y=j;
								
								}

*/
}

int VImage::find_label(int label)
{
	int resp=-1;
	for (int i=0 ; i< this->num_valid_labels;i++)
		if (this->valid_labels[i]==label)
			resp=i;

	return resp;
}


void VImage::find_interesting_points2()
{

	this->ref_points=NULL;	

	if (this->ref_points==NULL)
		this->ref_points= new Point2D[4*this->num_valid_labels];

	for (int i=0 ;i<4*this->num_valid_labels;i++)
	{
		this->ref_points[i].x=0; this->ref_points[i].y=0; this->ref_points[i].data=0;	
	}

	int cx,cy;
    float distance;
	for (int l=0 ; l<this->num_valid_labels;l++)
	{
		cx=this->valid_labels_acc[this->valid_labels[l]].x;
		cy=this->valid_labels_acc[this->valid_labels[l]].y;
		
		for ( i=1; i<cx; i++)
		    for (int j=1; j<cy;j++)
			{
				if (this->Labels[i*this->Width+j]==this->valid_labels[l])
				{
					distance=(float)sqrt((i-cx)*(i-cx)+(j-cy)*(j-cy));
					if ( (int)distance > this->ref_points[l*4].data)
					{
						this->ref_points[l*4].x=i;
						this->ref_points[l*4].y=j;
						this->ref_points[l*4].data=(int)distance;
					}
				}
			}
	
	}
	
for (int p=0 ;p < 4*this->num_valid_labels;p++)
		{    i=this->ref_points[p].x;
		     int j=this->ref_points[p].y;
			 if ((i>=0) &&(j>=0)&&(i<this->Width) &&(j<this->Height)){
                for (int w = -2 ; w < 2; w++)
				    for( int z =-2 ; z < 2 ; z++){ 
				     	pImage[3*(((i+w)*this->Width)+(j+z))]=0;
					    pImage[3*(((i+w)*this->Width)+(j+z))+1]=0;
					    pImage[3*(((i+w)*this->Width)+(j+z))+2]=255;
				    }
				}
		}

delete []this->ref_points;

}

/* acha o conjunto de componentes conexas fechadas da imagem dada*/

void  VImage::find_conexcomponents3()
{
	int label=0;
	this->labels_cont=NULL;
    this->valid_labels=NULL;
	this->Componentes=NULL;
	
   if (this->Labels==NULL)
		this->Labels=new int [this->Height*this->Width];
    
   for (int i=0; i<this->Height*this->Width; i++)
		this->Labels[i]=0;

   for (int i=0; i<100;i++)
		this->fechados[i]=0;

	for ( i=1; i<this->Height-1; i++)
			for (int j=1; j<this->Width-1;j++)
		       if ((Proc_Image[3*((i*this->Width)+j)]==0)&&(this->Labels[i*this->Width+j]==0))
			   {	this->init.x=i;
                   	this->init.y=j;
					conex2(i,j,++label, 0);
				}
	
	if (this->labels_cont==NULL)
		this->labels_cont=new int[label+1];
	
	for (int i=0; i<label+1; i++)
		labels_cont[i]=0;
	
	for (int i=0; i<this->Height*this->Width; i++)
	{   labels_cont[Labels[i]]++;
		Proc_Image[3*i ]=255;
	    Proc_Image[3*i +1]=255;
	    Proc_Image[3*i +2]=255;
	}				
	
	int count=0;
	for (int l=1; l < label+1 ;l++){
	   		if (labels_cont[l] > 100){
						if (this->fechados[l]>=1){
						count++;
						}
				}
	}

	
	if (this->valid_labels==NULL)
		this->valid_labels=new int[count];
			
	count=0;
	for (int l=1; l < label+1 ;l++){
	 
		if (labels_cont[l] > 100){
						if (this->fechados[l]>=1)
						{
							valid_labels[count]=l;
			        		count++;
			            }
					
		}
	}
  this->num_valid_labels=count;

  if (this->Componentes==NULL)
		this->Componentes= new SetConexComp();

	this->Componentes->num_component=count;
	this->Componentes->Components= new ConexComp[count];
	
	for (int l=0; l<this->num_valid_labels;l++)
		this->Componentes->Components[l].Pontos= new Point2D[this->labels_cont[this->valid_labels[l]]];
	
	int index;
	int acum;
	
	
	for (int l=0; l<this->num_valid_labels;l++)
	{
			for ( int i=1; i<this->Height-1; i++)
				for ( int j=1; j<this->Width-1;j++){
				   acum=0;
					if (this->Labels[i*this->Width+j]== this->valid_labels[l])
					{  
						 for (int w = -1 ; w < 2; w++)
				             for( int z =-1 ; z < 2 ; z++){ 
								 if (this->Labels[(i+w)*this->Width+(j+z)]== this->valid_labels[l])
								 acum++;
							 }
							 if (acum >= 6)
							 {
									index= this->Componentes->Components[l].num_pontos;
									this->Componentes->Components[l].Pontos[index].x=i;
									this->Componentes->Components[l].Pontos[index].y=j;
									this->Componentes->Components[l].Pontos[index].data=this->valid_labels[l];
									this->Componentes->Components[l].num_pontos++;
									
									if (i<this->Componentes->Components[l].pmin_x.x)
									{
										this->Componentes->Components[l].pmin_x.x=i;
										this->Componentes->Components[l].pmin_x.y=j;
										this->Componentes->Components[l].pmin_x.data=this->valid_labels[l];
									}
							 }
					}
				}
	}
  
  delete [] this->labels_cont;
  delete [] this->valid_labels;
  
  find_convexHull();
  interesting_points();
  recogquad_points();
  //printf("%d\n",count);
			
}

void VImage::Desenha_Componentes()
{
	Point2D aux;
	int cores[10][3];
    cores [0][0]=255;
    cores [0][1]=0;
    cores [0][2]=0;

    cores [1][0]=0;
    cores [1][1]=0;
    cores [1][2]=255;

    cores [2][0]=0;
    cores [2][1]=255;
    cores [2][2]=0;

    cores [3][0]=255;
    cores [3][1]=0;
    cores [3][2]=255;

    cores [4][0]=0;
    cores [4][1]=255;
    cores [4][2]=255;

	for (int l=0; l<this->Componentes->num_component; l++)
	{
		if (this->Componentes->Components[l].num_extremos==4)
		 { 
			 for (int i=0; i<this->Componentes->Components[l].num_pontos; i++)
		     {
				aux.x=this->Componentes->Components[l].Pontos[i].x;
				aux.y=this->Componentes->Components[l].Pontos[i].y;
				aux.data=this->Componentes->Components[l].Pontos[i].data;
				Proc_Image[3*((aux.x*this->Width)+aux.y)]=cores[(aux.data+1)%4][0];
				Proc_Image[3*((aux.x*this->Width)+aux.y)+1]=cores[(aux.data+1)%4][1];
				Proc_Image[3*((aux.x*this->Width)+aux.y)+2]=cores[(aux.data+1)%4][2];
			}
		 }
	}
	
	

	delete [] this->Componentes->Components;
	delete this->Componentes;
}


void VImage::find_convexHull()
{
    Point2D menor_p;
	for (int l=0; l< this->Componentes->num_component; l++)
	{
		this->Componentes->Components[l].Fecho=new Point2D[this->Componentes->Components[l].num_pontos];
		this->Componentes->Components[l].num_pontos_f=this->chainHull_2D(this->Componentes->Components[l].Pontos, this->Componentes->Components[l].num_pontos, this->Componentes->Components[l].Fecho);
					
			
	//	printf("num_pontos %d : %d \n", l,this->Componentes->Components[l].num_pontos_f );
	/*	for (int i=0; i<this->Componentes->Components[l].num_pontos_f; i++)
		{
			menor_p.x=this->Componentes->Components[l].Fecho[i].x;
			menor_p.y=this->Componentes->Components[l].Fecho[i].y;
			//printf("(%d , % d )\n", menor_p.x, menor_p.y);
			for (int w = -1 ; w < 1; w++)
				    for( int z =-1 ; z < 1 ; z++) 
						{     	pImage[3*(((menor_p.x+w)*this->Width)+(menor_p.y+z))]=0;
					            pImage[3*(((menor_p.x+w)*this->Width)+(menor_p.y+z))+1]=0;
					           pImage[3*(((menor_p.x+w)*this->Width)+(menor_p.y+z))+2]=255;
						}
		}
		//getch();
	//printf("termina\n");
*/
	}
	
}

void VImage::interesting_points()
{
   Point2D base;
   Point2D meio;
   Point2D corrente;
   Point2D vetor_a;
   Point2D vetor_b;

   for (int l=0; l< this->Componentes->num_component; l++)
   {
	   if ( this->Componentes->Components[l].num_pontos_f >=4)
	   {for (int i=1; i<4; i++)
		{
			 this->Componentes->Components[l].Fecho[this->Componentes->Components[l].num_pontos_f]= this->Componentes->Components[l].Fecho[i];
			 this->Componentes->Components[l].num_pontos_f++;
			}
	   }
	   this->Componentes->Components[l].Extremos=new Point2D[this->Componentes->Components[l].num_pontos_f];
	   this->Componentes->Components[l].num_extremos=0;
	   base.x= this->Componentes->Components[l].Fecho[0].x;
	   base.y= this->Componentes->Components[l].Fecho[0].y;

	   meio.x= this->Componentes->Components[l].Fecho[1].x;
	   meio.y= this->Componentes->Components[l].Fecho[1].y;

	   for (int i=2 ; i<this->Componentes->Components[l].num_pontos_f; i++)
	   {
		   corrente.x=this->Componentes->Components[l].Fecho[i].x;
		   corrente.y=this->Componentes->Components[l].Fecho[i].y;
		   vetor_a = this->Unit(this->Sub(base,meio));
		   vetor_b = this->Unit(this->Sub(corrente,meio));
		   if (this->dot(vetor_a, vetor_b)  > -0.75)
		   {
			   this->Componentes->Components[l].Extremos[this->Componentes->Components[l].num_extremos].x=meio.x;
			   this->Componentes->Components[l].Extremos[this->Componentes->Components[l].num_extremos].y=meio.y;
			   this->Componentes->Components[l].num_extremos++;
			   base.x=meio.x; base.y=meio.y;
			   meio.x=corrente.x; meio.y=corrente.y;
		   }
		   else
		   {
			   meio.x=corrente.x; meio.y=corrente.y;
		   }
	   
	   }
	   
	  int count=1;
	  Point2D current;
	  Point2D actual;
	  actual= this->Componentes->Components[l].Extremos[0];
	  for (int i=1; i< this->Componentes->Components[l].num_extremos; i++)
	  {
		  current= this->Componentes->Components[l].Extremos[i];
		  if (!((abs(current.x-actual.x)<=21)&&(abs(current.y-actual.y)<=21)))
		  {
			  this->Componentes->Components[l].Extremos[count]=current;
			  actual=current;
			  count++;
		  }
	  }
	  
	  if ((abs(this->Componentes->Components[l].Extremos[0].x-this->Componentes->Components[l].Extremos[count-1].x)<=21)&&(abs(this->Componentes->Components[l].Extremos[0].y-this->Componentes->Components[l].Extremos[count-1].y) <= 21))
	  this->Componentes->Components[l].num_extremos=count-1;
	  else
	  this->Componentes->Components[l].num_extremos=count;
	  //printf(" refer: %d \n",this->Componentes->Components[l].num_extremos );
	  /*Point2D aux;
	   for (int i=0; i< this->Componentes->Components[l].num_extremos; i++)
	   {  
		   
		   aux.x=this->Componentes->Components[l].Extremos[i].x;
		   aux.y=this->Componentes->Components[l].Extremos[i].y;
		  
		   for (int w = -1 ; w < 2; w++)
				    for( int z =-1 ; z < 2 ; z++) 
						{     	pImage[3*(((aux.x+w)*this->Width)+(aux.y+z))]=0;
					            pImage[3*(((aux.x+w)*this->Width)+(aux.y+z))+1]=0;
					           pImage[3*(((aux.x+w)*this->Width)+(aux.y+z))+2]=255;
						}
           
	   }
	 */
   }
 
}

void VImage::recogquad_points()
{
	Point2D center;
	Point2D A,B,C,D;

    for (int l=0; l< this->Componentes->num_component; l++)
   {  
	   if ((this->Componentes->Components[l].num_extremos==4))
	   {	
		     A=this->Componentes->Components[l].Extremos[0];
			 B=this->Componentes->Components[l].Extremos[2];
			 C=this->Componentes->Components[l].Extremos[1];
			 D=this->Componentes->Components[l].Extremos[3];
			 center=this->inter_line(A,B,C,D);
			 if (center.data==0)
			{
				this->Componentes->Components[l].center=center;
				//this->Componentes->Components[l].Extremos[this->Componentes->Components[l].num_extremos]=center;
				//this->Componentes->Components[l].num_extremos++;
			/*	for (int w = -1 ; w < 2; w++)
				     for( int z =-1 ; z < 2 ; z++) 
				      		{     	pImage[3*(((center.x+w)*this->Width)+(center.y+z))]=0;
					                pImage[3*(((center.x+w)*this->Width)+(center.y+z))+1]=0;
					                pImage[3*(((center.x+w)*this->Width)+(center.y+z))+2]=255;
      								
  						    }
              */   
              
			}

	   }
 }  

}


void VImage::Identifing_pattern1()
{
	this->recognition=false;
	int count;
	Point2D vetor_c;
	Point2D vetor_a;
	Point2D vetor_b;
	float distance;
	float distance2;
	bool achou=false; 
    int init=-1;
	int cores[10][3];
    
	cores [0][0]=255;
    cores [0][1]=0;
    cores [0][2]=0;

    cores [1][0]=0;
    cores [1][1]=0;
    cores [1][2]=255;

    cores [2][0]=0;
    cores [2][1]=255;
    cores [2][2]=0;

    cores [3][0]=255;
    cores [3][1]=0;
    cores [3][2]=255;

    cores [4][0]=0;
    cores [4][1]=255;
    cores [4][2]=255;


	if (this->C_pontos==NULL)
		this->C_pontos= new Point2D[10];
	
	if (this->Componentes->num_component==2)
    recognition=true;
	if (this->Componentes->num_component > 2)
	{   count=0;
		for (int k=0; k<this->Componentes->num_component;k++ )
		{
			if (this->Componentes->Components[k].num_extremos==4)
			{
				this->Componentes->Components[count]=this->Componentes->Components[k];
				count++;
			}
		}
		if (count==2)
		recognition=true;
	}
	

	if (recognition)
	{
		distance=this->norma(this->Sub(this->Componentes->Components[0].center,this->Componentes->Components[0].Extremos[0])); 
		distance2=this->norma(this->Sub(this->Componentes->Components[1].center,this->Componentes->Components[1].Extremos[0])); 		
		
		if ( distance > distance2 )
		{
			achou= false;
			vetor_c=this->Sub(this->Componentes->Components[1].center, this->Componentes->Components[0].center);
			for (int i=0 ; i<this->Componentes->Components[1].num_extremos-1;i++)
				{
					vetor_a =this->Sub(this->Componentes->Components[1].Extremos[i],this->Componentes->Components[0].center);
					vetor_b =this->Sub(this->Componentes->Components[1].Extremos[i+1],this->Componentes->Components[0].center);
				
					if ((this->ccw(vetor_c,vetor_a)>0)&&(this->ccw(vetor_b,vetor_c)>0))
					{
						achou=true;
						init=i;
					}
				}
				if (!achou)
					{
						vetor_a =this->Sub(this->Componentes->Components[1].Extremos[this->Componentes->Components[1].num_extremos-1],this->Componentes->Components[0].center);
						vetor_b =this->Sub(this->Componentes->Components[1].Extremos[0],this->Componentes->Components[0].center);
				
						if ((this->ccw(vetor_c,vetor_a)>0)&&(this->ccw(vetor_b,vetor_c)>0))
						{
							achou=true;
							init=this->Componentes->Components[1].num_extremos-1;
						}
				    }

					if (init==0)
					{
						this->C_pontos[0]=this->Componentes->Components[1].Extremos[3];
						this->C_pontos[1]=this->Componentes->Components[1].Extremos[2];
						this->C_pontos[2]=this->Componentes->Components[1].Extremos[1];
						this->C_pontos[3]=this->Componentes->Components[1].Extremos[0];
						this->C_pontos[4]=this->Componentes->Components[1].center;
					}

					if (init==1)
					{
						this->C_pontos[0]=this->Componentes->Components[1].Extremos[0];
						this->C_pontos[1]=this->Componentes->Components[1].Extremos[3];
						this->C_pontos[2]=this->Componentes->Components[1].Extremos[2];
						this->C_pontos[3]=this->Componentes->Components[1].Extremos[1];
						this->C_pontos[4]=this->Componentes->Components[1].center;
					}

					if (init==2)
					{
						this->C_pontos[0]=this->Componentes->Components[1].Extremos[1];
						this->C_pontos[1]=this->Componentes->Components[1].Extremos[0];
						this->C_pontos[2]=this->Componentes->Components[1].Extremos[3];
						this->C_pontos[3]=this->Componentes->Components[1].Extremos[2];
						this->C_pontos[4]=this->Componentes->Components[1].center;
					}

					if (init==3)
					{
						this->C_pontos[0]=this->Componentes->Components[1].Extremos[2];
						this->C_pontos[1]=this->Componentes->Components[1].Extremos[1];
						this->C_pontos[2]=this->Componentes->Components[1].Extremos[0];
						this->C_pontos[3]=this->Componentes->Components[1].Extremos[3];
						this->C_pontos[4]=this->Componentes->Components[1].center;
					}
			
            achou= false;
			vetor_c=this->Sub(this->Componentes->Components[1].center, this->Componentes->Components[0].center);
			for (int i=0 ; i<this->Componentes->Components[0].num_extremos-1;i++)
				{
					vetor_a =this->Sub(this->Componentes->Components[0].Extremos[i],this->Componentes->Components[0].center);
					vetor_b =this->Sub(this->Componentes->Components[0].Extremos[i+1],this->Componentes->Components[0].center);
				
					if ((this->ccw(vetor_c,vetor_a)>0)&&(this->ccw(vetor_b,vetor_c)>0))
					{
						achou=true;
						init=i;
					}
				}
				if (!achou)
					{
						vetor_a =this->Sub(this->Componentes->Components[0].Extremos[this->Componentes->Components[0].num_extremos-1],this->Componentes->Components[0].center);
						vetor_b =this->Sub(this->Componentes->Components[0].Extremos[0],this->Componentes->Components[0].center);
				
						if ((this->ccw(vetor_c,vetor_a)>0)&&(this->ccw(vetor_b,vetor_c)>0))
						{
							achou=true;
							init=this->Componentes->Components[0].num_extremos-1;
						}
				    }

					if (init==0)
					{
						this->C_pontos[5]=this->Componentes->Components[0].Extremos[3];
						this->C_pontos[6]=this->Componentes->Components[0].Extremos[2];
						this->C_pontos[7]=this->Componentes->Components[0].Extremos[1];
						this->C_pontos[8]=this->Componentes->Components[0].Extremos[0];
						this->C_pontos[9]=this->Componentes->Components[0].center;
					}

					if (init==1)
					{
						this->C_pontos[5]=this->Componentes->Components[0].Extremos[0];
						this->C_pontos[6]=this->Componentes->Components[0].Extremos[3];
						this->C_pontos[7]=this->Componentes->Components[0].Extremos[2];
						this->C_pontos[8]=this->Componentes->Components[0].Extremos[1];
						this->C_pontos[9]=this->Componentes->Components[0].center;
					}

					if (init==2)
					{
						this->C_pontos[5]=this->Componentes->Components[0].Extremos[1];
						this->C_pontos[6]=this->Componentes->Components[0].Extremos[0];
						this->C_pontos[7]=this->Componentes->Components[0].Extremos[3];
						this->C_pontos[8]=this->Componentes->Components[0].Extremos[2];
						this->C_pontos[9]=this->Componentes->Components[0].center;
					}

					if (init==3)
					{
						this->C_pontos[5]=this->Componentes->Components[0].Extremos[2];
						this->C_pontos[6]=this->Componentes->Components[0].Extremos[1];
						this->C_pontos[7]=this->Componentes->Components[0].Extremos[0];
						this->C_pontos[8]=this->Componentes->Components[0].Extremos[3];
						this->C_pontos[9]=this->Componentes->Components[0].center;
					}

		}
		else{
			 
			achou= false;
			vetor_c=this->Sub(this->Componentes->Components[0].center, this->Componentes->Components[1].center);
			for (int i=0 ; i<this->Componentes->Components[0].num_extremos-1;i++)
				{
					vetor_a =this->Sub(this->Componentes->Components[0].Extremos[i],this->Componentes->Components[1].center);
					vetor_b =this->Sub(this->Componentes->Components[0].Extremos[i+1],this->Componentes->Components[1].center);
				
					if ((this->ccw(vetor_c,vetor_a)>0)&&(this->ccw(vetor_b,vetor_c)>0))
					{
						achou=true;
						init=i;
					}
				}
				if (!achou)
					{
						vetor_a =this->Sub(this->Componentes->Components[0].Extremos[this->Componentes->Components[0].num_extremos-1],this->Componentes->Components[1].center);
						vetor_b =this->Sub(this->Componentes->Components[0].Extremos[0],this->Componentes->Components[1].center);
				
						if ((this->ccw(vetor_c,vetor_a)>0)&&(this->ccw(vetor_b,vetor_c)>0))
						{
							achou=true;
							init=this->Componentes->Components[0].num_extremos-1;
						}
				    }

					if (init==0)
					{
						this->C_pontos[0]=this->Componentes->Components[0].Extremos[3];
						this->C_pontos[1]=this->Componentes->Components[0].Extremos[2];
						this->C_pontos[2]=this->Componentes->Components[0].Extremos[1];
						this->C_pontos[3]=this->Componentes->Components[0].Extremos[0];
						this->C_pontos[4]=this->Componentes->Components[0].center;
					}

					if (init==1)
					{
						this->C_pontos[0]=this->Componentes->Components[0].Extremos[0];
						this->C_pontos[1]=this->Componentes->Components[0].Extremos[3];
						this->C_pontos[2]=this->Componentes->Components[0].Extremos[2];
						this->C_pontos[3]=this->Componentes->Components[0].Extremos[1];
						this->C_pontos[4]=this->Componentes->Components[0].center;
					}

					if (init==2)
					{
						this->C_pontos[0]=this->Componentes->Components[0].Extremos[1];
						this->C_pontos[1]=this->Componentes->Components[0].Extremos[0];
						this->C_pontos[2]=this->Componentes->Components[0].Extremos[3];
						this->C_pontos[3]=this->Componentes->Components[0].Extremos[2];
						this->C_pontos[4]=this->Componentes->Components[0].center;
					}

					if (init==3)
					{
						this->C_pontos[0]=this->Componentes->Components[0].Extremos[2];
						this->C_pontos[1]=this->Componentes->Components[0].Extremos[1];
						this->C_pontos[2]=this->Componentes->Components[0].Extremos[0];
						this->C_pontos[3]=this->Componentes->Components[0].Extremos[3];
						this->C_pontos[4]=this->Componentes->Components[0].center;
					}
			
            achou= false;
			vetor_c=this->Sub(this->Componentes->Components[1].center, this->Componentes->Components[1].center);
			for (int i=0 ; i<this->Componentes->Components[1].num_extremos-1;i++)
				{
					vetor_a =this->Sub(this->Componentes->Components[1].Extremos[i],this->Componentes->Components[1].center);
					vetor_b =this->Sub(this->Componentes->Components[1].Extremos[i+1],this->Componentes->Components[1].center);
				
					if ((this->ccw(vetor_c,vetor_a)>0)&&(this->ccw(vetor_b,vetor_c)>0))
					{
						achou=true;
						init=i;
					}
				}
				if (!achou)
					{
						vetor_a =this->Sub(this->Componentes->Components[1].Extremos[this->Componentes->Components[1].num_extremos-1],this->Componentes->Components[1].center);
						vetor_b =this->Sub(this->Componentes->Components[1].Extremos[0],this->Componentes->Components[1].center);
				
						if ((this->ccw(vetor_c,vetor_a)>0)&&(this->ccw(vetor_b,vetor_c)>0))
						{
							achou=true;
							init=this->Componentes->Components[1].num_extremos-1;
						}
				    }

					if (init==0)
					{
						this->C_pontos[5]=this->Componentes->Components[1].Extremos[3];
						this->C_pontos[6]=this->Componentes->Components[1].Extremos[2];
						this->C_pontos[7]=this->Componentes->Components[1].Extremos[1];
						this->C_pontos[8]=this->Componentes->Components[1].Extremos[0];
						this->C_pontos[9]=this->Componentes->Components[1].center;
					}

					if (init==1)
					{
						this->C_pontos[5]=this->Componentes->Components[1].Extremos[0];
						this->C_pontos[6]=this->Componentes->Components[1].Extremos[3];
						this->C_pontos[7]=this->Componentes->Components[1].Extremos[2];
						this->C_pontos[8]=this->Componentes->Components[1].Extremos[1];
						this->C_pontos[9]=this->Componentes->Components[1].center;
					}

					if (init==2)
					{
						this->C_pontos[5]=this->Componentes->Components[1].Extremos[1];
						this->C_pontos[6]=this->Componentes->Components[1].Extremos[0];
						this->C_pontos[7]=this->Componentes->Components[1].Extremos[3];
						this->C_pontos[8]=this->Componentes->Components[1].Extremos[2];
						this->C_pontos[9]=this->Componentes->Components[1].center;
					}

					if (init==3)
					{
						this->C_pontos[5]=this->Componentes->Components[1].Extremos[2];
						this->C_pontos[6]=this->Componentes->Components[1].Extremos[1];
						this->C_pontos[7]=this->Componentes->Components[1].Extremos[0];
						this->C_pontos[8]=this->Componentes->Components[1].Extremos[3];
						this->C_pontos[9]=this->Componentes->Components[1].center;
					}

		}
	
	/*	Point2D aux;
		for (int i=0; i<this->num_calpontos; i++)
		{
			aux=this->C_pontos[i];
			for (int w = -1 ; w < 2; w++)
				    for( int z =-1 ; z < 2 ; z++) 
					{         if (( aux.x < this->Height-2)&&(aux.y<this->Width-2))
							   pImage[3*(((aux.x+w)*this->Width)+(aux.y+z))]=cores[i%5][0];
					           pImage[3*(((aux.x+w)*this->Width)+(aux.y+z))+1]=cores[i%5][1];
					           pImage[3*(((aux.x+w)*this->Width)+(aux.y+z))+2]=cores[i%5][2];
						}
           
		}
*/
	}

	
}

void VImage::Camera_Calibration(/*Point2D *pImagem , int num_pontos, Point2D *pWorld*/)
{
	
	double *xw,*yw,*xf,*yf;
	double f, T[3],R[9];
    int count=0, np;

	printf("componentes: %d\n", this->Componentes->num_component);
	
	if (this->recognition)
	{
		
		
		np=this->num_calpontos-2;	
  
		xw= new double[np];
		yw=new double[np];
		xf= new double[np];
		yf=new double[np];
		
        xw[0]=56.0;yw[0]=56.0;
		xw[1]=-68.0;yw[1]=56.0;
		xw[2]=-68.0;yw[2]=-18.0;
		xw[3]=56.0;yw[3]=-18.0;
		//xw[4]=19.0;yw[4]=-6.5;
		xw[4]=91.0;yw[4]=80.0;
		xw[5]=-91.0;yw[5]=80.0;
		xw[6]=-91.0;yw[6]=-80.0;
		xw[7]=91.0;yw[7]=-80.0;
        //xw[9]=0.0;yw[9]=0.0;
		
		   

		for (int i=0; i< 4; i++)
			{
				yf[i]=(double)(this->C_pontos[i].x - (this->Height/2));
				xf[i]=(double)(this->C_pontos[i].y - (this->Width/2));
			}

				
			count =4;
		for (int i=5; i< 9; i++)
			{
				yf[count]=(double)(this->C_pontos[i].x - (this->Height/2));
				xf[count]=(double)(this->C_pontos[i].y - (this->Width/2));
				count++;
			}

 tsaiCalibration2D(np, xw, yw, xf, yf,&f, T,  R);
	double xw_1=0.0;
	double yw_1=0.0;
	double zw_1=0.0;
	
	double xf_1;
	double yf_1;

	double xw_2=-91.0;
	double yw_2=-80.0;
	double zw_2=0;
	
	double xf_2;
	double yf_2;

   tsaiProject(xw_1, yw_1, zw_1, &xf_1, &yf_1,f,T,R);
   tsaiProject(xw_2, yw_2, zw_2, &xf_2, &yf_2,f,T,R);
 
   xf_1+=(double)(this->Height/2);
   yf_1+=(double)(this->Width/2);

   xf_2+=(double)(this->Height/2);
   yf_2+=(double)(this->Width/2);

   tsaiOGLModelViewMatrix(T,R,this->modelview);
   tsaiOGLProjectionMatrix(f, 0.1, 5000, 240, 320, this->projection);

   // initialize_photometrics_parms();
	//coplanar_calibration();
	//coplanar_calibration_with_full_optimization ();
    //world_coord_to_image_coord (xw_1, yw_1, zw_1, &Xf_1, &Yf_1);
	//world_coord_to_image_coord (xw_2, yw_2, zw_2, &Xf_2, &Yf_2);
/*
	for (int w = -1 ; w < 2; w++)
				     for( int z =-1 ; z < 2 ; z++) 
				      		{     	pImage[3*((((int)xf_1+w)*this->Width)+((int)yf_1+z))]=255;
					                pImage[3*((((int)xf_1+w)*this->Width)+((int)yf_1+z))+1]=0;
					                pImage[3*((((int)xf_1+w)*this->Width)+((int)yf_1+z))+2]=0;
						    }
/*
	for (int w = -1 ; w < 2; w++)
				     for( int z =-1 ; z < 2 ; z++) 
				      		{     	pImage[3*((((int)xf_2+w)*this->Width)+((int)yf_2+z))]=255;
					                pImage[3*((((int)xf_2+w)*this->Width)+((int)yf_2+z))+1]=0;
					                pImage[3*((((int)xf_2+w)*this->Width)+((int)yf_2+z))+2]=0;
						    }
*/
	
	delete []xw;
	delete []yw;
	delete []xf;
	delete []yf;

	}
	//getch();

}

void VImage::init_Kalman()
{
   FILE *ff;
   int i;
   npos = 0;
   load_data("pontos1.txt", &np, &xw, &yw, &xf, &yf);
   tsaiCalibration2D(np, xw, yw, xf, yf, &f, T, r);
   dfocal=f;
	// iniciar filtros
	kf = init_kf_points(np, (float*)xf, (float*)yf, 0.5);
	kfc = init_kf_camera(2, 2, r, T);
}


void VImage::Camera_Calibration1(/*Point2D *pImagem , int num_pontos, Point2D *pWorld*/)
{
	
	double *xw,*yw,*xf,*yf;
	double f, T[3],R[9];
    int count=0, np;
	int i, ok;
	//printf("componentes: %d\n", this->Componentes->num_component);
	
	if (this->recognition)
	{
		
		np=this->num_calpontos-2;	
  
		xw= new double[np];
		yw=new double[np];
		xf= new double[np];
		yf=new double[np];
		
        xw[0]=56.0;yw[0]=56.0;
		xw[1]=-68.0;yw[1]=56.0;
		xw[2]=-68.0;yw[2]=-18.0;
		xw[3]=56.0;yw[3]=-18.0;
		//xw[4]=19.0;yw[4]=-6.5;
		xw[4]=91.0;yw[4]=80.0;
		xw[5]=-91.0;yw[5]=80.0;
		xw[6]=-91.0;yw[6]=-80.0;
		xw[7]=91.0;yw[7]=-80.0;
        //xw[9]=0.0;yw[9]=0.0;
		
		   

		for (int i=0; i< 4; i++)
			{
				yf[i]=(double)(this->C_pontos[i].x - (this->Height/2));
				xf[i]=(double)(this->C_pontos[i].y - (this->Width/2));
			}

				
			count =4;
		for (int i=5; i< 9; i++)
			{
				yf[count]=(double)(this->C_pontos[i].x - (this->Height/2));
				xf[count]=(double)(this->C_pontos[i].y - (this->Width/2));
				count++;
			}

	  //tsaiCalibration2D(np, xw, yw, xf, yf,&f, T,  R);

	
	/* if (fpoint==false)
	  {		  tsaiCalibration2D(NPTSX*NPTSY, xwn, ywn, xfn, yfn, &f, T, R);
	          this->dfocal=f;
			  fpoint=true;
	  }
	  */
	  //tsaiCalibration2D(NPTSX*NPTSY, xwn, ywn, xfn, yfn, &f, T, R);
	  //dfocal=f;
/*	  tsaiCalibration2D_Tr(NPTSX*NPTSY, xwn, ywn, xfn, yfn,f, T, R);

/*
*/
 // filtrar pontos
		//ok = step_kf_points(np,  (float*)xf, (float*)yf, 0.1,  dt, kf);
		//tsaiCalibration2D_Tr(np, xw, yw, xf, yf, dfocal, T,R);
		tsaiBuildNewData(np, xw, yw, xf, yf, NPTSX, NPTSY, xwn, ywn, xfn, yfn);
		tsaiCalibration2D_Tr(NPTSX*NPTSY, xwn, ywn, xfn, yfn,dfocal, T, R);
		// filtrar parametros de camera
		ok = step_kf_camera(0.05, 0.05, R, T, dt, kfc);
/*
double xw_1=0.0;
	double yw_1=0.0;
	double zw_1=0.0;
	
	double xf_1;
	double yf_1;

	double xw_2=-91.0;
	double yw_2=-80.0;
	double zw_2=0;
	
	double xf_2;
	double yf_2;

   tsaiProject(xw_1, yw_1, zw_1, &xf_1, &yf_1,f,T,R);
   tsaiProject(xw_2, yw_2, zw_2, &xf_2, &yf_2,f,T,R);
 
   xf_1+=(double)(this->Height/2);
   yf_1+=(double)(this->Width/2);

   xf_2+=(double)(this->Height/2);
   yf_2+=(double)(this->Width/2);
*/
   tsaiOGLModelViewMatrix(T,R,this->modelview);
   tsaiOGLProjectionMatrix(dfocal, 0.1, 5000, 320, 240, this->projection);
  
   // initialize_photometrics_parms();
	//coplanar_calibration();
	//coplanar_calibration_with_full_optimization ();
    //world_coord_to_image_coord (xw_1, yw_1, zw_1, &Xf_1, &Yf_1);
	//world_coord_to_image_coord (xw_2, yw_2, zw_2, &Xf_2, &Yf_2);
/*
	for (int w = -1 ; w < 2; w++)
				     for( int z =-1 ; z < 2 ; z++) 
				      		{     	pImage[3*((((int)xf_1+w)*this->Width)+((int)yf_1+z))]=255;
					                pImage[3*((((int)xf_1+w)*this->Width)+((int)yf_1+z))+1]=0;
					                pImage[3*((((int)xf_1+w)*this->Width)+((int)yf_1+z))+2]=0;
						    }
/*
	for (int w = -1 ; w < 2; w++)
				     for( int z =-1 ; z < 2 ; z++) 
				      		{     	pImage[3*((((int)xf_2+w)*this->Width)+((int)yf_2+z))]=255;
					                pImage[3*((((int)xf_2+w)*this->Width)+((int)yf_2+z))+1]=0;
					                pImage[3*((((int)xf_2+w)*this->Width)+((int)yf_2+z))+2]=0;
						    }
*/
	
	delete []xw;
	delete []yw;
	delete []xf;
	delete []yf;

	}
	//getch();

}


Point2D VImage::inter_line(Point2D A, Point2D B, Point2D C,Point2D D)
{
		Point2D resp;
		resp.data=-1;
        float	Dt;
		 
		 Dt= A.x*(D.y - C.y) + B.x*(C.y - D.y) + D.x*(B.y - A.y) + C.x*(A.y - B.y);
		
		 if (Dt == 0)
		 return resp;

		 float lambda = A.x*(D.y - C.y) + 	C.x*(A.y - D.y) + D.x*(C.y - A.y);
		lambda /= Dt;

		resp.x = (int)((1 - lambda)*A.x + lambda*B.x);
		resp.y = (int)((1 - lambda)*A.y + lambda*B.y);
		resp.data=0;

		return resp;
}

//typedef CGAL::Cartesian<int>                     K; 
//typedef K::Point_2                               Point_2; 
///typedef CGAL::Polygon_2<K, std::list<Point_2> >  Polygon_2; 


bool VImage::find_point_fecho(Point2D point, int component)
{
	bool resp=false;
	for (int i=0 ; i< this->Componentes->Components[component].num_pontos_f; i++)
	{
		if ((point.x==this->Componentes->Components[component].Fecho[i].x)&&(point.y==this->Componentes->Components[component].Fecho[i].y))
			resp=true;
	}
 return resp;
}
float VImage::norma(Point2D vetor)
{
	return (float)sqrt(vetor.x*vetor.x + vetor.y*vetor.y);
}

Point2D VImage::Sub(Point2D a,Point2D b )  //a-b
{  Point2D resp;
     resp.x= a.x-b.x;
	 resp.y=a.y-b.y;
	 resp.data=0;
  return resp;
}

Point2D VImage::Unit(Point2D vetor)
{
	 Point2D resp;
	float norma = this->norma(vetor);
	resp.fx= ((float)vetor.x)/norma;
	resp.fy= ((float)vetor.y)/norma;
	return resp;
}
float VImage::ccw(Point2D a,Point2D b)   //(a)X(b)
{
	return (a.x*b.y - b.x*a.y);
}

float VImage::dot(Point2D a,Point2D b)
{
	return (a.fx*b.fx + a.fy*b.fy);
}


// Copyright 2001, softSurfer (www.softsurfer.com)
// This code may be freely used and modified for any purpose
// providing that this copyright notice is included with it.
// SoftSurfer makes no warranty for this code, and cannot be held
// liable for any real or imagined damage resulting from its use.
// Users of this code must verify correctness for their application.


// Assume that a class is already given for the object:
//    Point with coordinates {float x, y;}
//===================================================================


// isLeft(): tests if a point is Left|On|Right of an infinite line.
//    Input:  three points P0, P1, and P2
//    Return: >0 for P2 left of the line through P0 and P1
//            =0 for P2 on the line
//            <0 for P2 right of the line
//    See: the January 2001 Algorithm on Area of Triangles
inline float VImage::isLeft( Point2D P0, Point2D P1, Point2D P2 )
{
    return (float)(P1.x - P0.x)*(P2.y - P0.y) - (P2.x - P0.x)*(P1.y - P0.y);
}
//===================================================================
 

// chainHull_2D(): Andrew's monotone chain 2D convex hull algorithm
//     Input:  P[] = an array of 2D points 
//                   presorted by increasing x- and y-coordinates
//             n = the number of points in P[]
//     Output: H[] = an array of the convex hull vertices (max is n)
//     Return: the number of points in H[]
int  VImage::chainHull_2D( Point2D* P, int n, Point2D* H )
{
    // the output array H[] will be used as the stack
    int    bot=0, top=(-1);  // indices for bottom and top of the stack
    int    i;                // array scan index

    // Get the indices of points with min x-coord and min|max y-coord
    int minmin = 0, minmax;
    float xmin = P[0].x;
    for (i=1; i<n; i++)
        if (P[i].x != xmin) break;
    minmax = i-1;
    if (minmax == n-1) {       // degenerate case: all x-coords == xmin
        H[++top] = P[minmin];
        if (P[minmax].y != P[minmin].y) // a nontrivial segment
            H[++top] = P[minmax];
        H[++top] = P[minmin];           // add polygon endpoint
        return top+1;
    }

    // Get the indices of points with max x-coord and min|max y-coord
    int maxmin, maxmax = n-1;
    float xmax = P[n-1].x;
    for (i=n-2; i>=0; i--)
        if (P[i].x != xmax) break;
    maxmin = i+1;

    // Compute the lower hull on the stack H
    H[++top] = P[minmin];      // push minmin point onto stack
    i = minmax;
    while (++i <= maxmin)
    {
        // the lower line joins P[minmin] with P[maxmin]
        if (isLeft( P[minmin], P[maxmin], P[i]) >= 0 && i < maxmin)
            continue;          // ignore P[i] above or on the lower line

        while (top > 0)        // there are at least 2 points on the stack
        {
            // test if P[i] is left of the line at the stack top
            if (isLeft( H[top-1], H[top], P[i]) > 0)
                break;         // P[i] is a new hull vertex
            else
                top--;         // pop top point off stack
        }
        H[++top] = P[i];       // push P[i] onto stack
    }

    // Next, compute the upper hull on the stack H above the bottom hull
    if (maxmax != maxmin)      // if distinct xmax points
        H[++top] = P[maxmax];  // push maxmax point onto stack
    bot = top;                 // the bottom point of the upper hull stack
    i = maxmin;
    while (--i >= minmax)
    {
        // the upper line joins P[maxmax] with P[minmax]
        if (isLeft( P[maxmax], P[minmax], P[i]) >= 0 && i > minmax)
            continue;          // ignore P[i] below or on the upper line

        while (top > bot)    // at least 2 points on the upper stack
        {
            // test if P[i] is left of the line at the stack top
            if (isLeft( H[top-1], H[top], P[i]) > 0)
                break;         // P[i] is a new hull vertex
            else
                top--;         // pop top point off stack
        }
        H[++top] = P[i];       // push P[i] onto stack
    }
    if (minmax != minmin)
        H[++top] = P[minmin];  // push joining endpoint onto stack

    return top+1;
}


