//#include <termios.h>
#include <stdio.h>
//#include <unistd.h>
//#include <fcntl.h>
//#include <dirent.h>
//#include <string.h>
/*far puntare a dove risiede la libreria G*/
//#include "../AISColorSeg/AISAiboYCrCbColorSeg.h"
#include "info_ball.h"
#include <sys/signal.h>
#include <sys/types.h>
#include <sys/stat.h>
#include<../lib/iuriFit/CircleFittingClass.h>
//#include <../../EventMgr/EventMgr.h>
//******************     hough     *****************
#include <stdlib.h>
//#include<../iuriFit/CircleFittingClass.h>

double* info_ball (const unsigned int* ptr){
  
  punto** matr  = (punto**) calloc(alt,sizeof(punto*)); //alloco la memoria per una colonna
  if (matr == NULL) while(1) printf("2"); //exit (2);
  for(int t=0;t<alt;t++){ //alloco la memoria per ogni riga
    matr[t] = (punto*) calloc(lar,sizeof(punto));
    if (matr[t] == NULL) while(1) printf("3"); //exit (2);
  }

  punto** mat  = (punto**) calloc(alt,sizeof(punto*)); //alloco la memoria per una colonna
  if (mat == NULL) while(1) printf("2"); //exit (2);
  for(int t=0;t<alt;t++){ //alloco la memoria per ogni riga
    mat[t] = (punto*) calloc(lar,sizeof(punto));
    if (mat[t] == NULL) while(1) printf("3"); //exit (2);
  }
	colore ne, bi, ro, az, gi, ver;		//variabili per il calcolo delle occorrenze dei colori

/*inizializzazione variabili riconoscimento colori*/	
	ne.tipo = 0;
	az.tipo = 1;
	gi.tipo = 2;
	
/*ciclo per la pulizia dell'immagine: applico un elemento strutturale a stella per eliminare 
tutto quello che c'è in eccesso*/

	for (int h=1; h<al; h++){
	  for (int k=1; k<la; k++){
		//inizializzazione variabili locali
		  ne.occorrenza = 0;
		  az.occorrenza = 0;
		  gi.occorrenza = 0;

/*eliminazione pixel isolati*/
/*controllo degli 8_vicini di un pixel: assegno al pixel il valore di colore più presente nell'elemento
 strutturale a "stella"*/
		if (matr[h][k].val_col != 0){
		 corr_col (matr, mat, h, k, ne, az, gi);
		}
	  }
	}


  int z = 0;		//variabile ausiliaria per la copia dell'immagine su matrice statica (alt X lar)
  double diam = 0;
  int xcentroid = 0;
  int ycentroid = 0;
  //printf("creazione della matrice matr dalla segmentazione\n");
  //printf("*****************ptr[0] >> %d *****************\n",ptr[0]);	
  //printf("ptr -  %d  *******************************",ptr);
  for (int r=0; r<alt; r++){
    for (int c=0; c<lar; c++){
      //copia valori ottenuti dalla segmentazione nella matrice che reppresenta l'immagine
      if(r == al || c == la || r == 0 || c==0){
	matr[r][c].val_col = 0; //metti 0 per evitare che cercando la successiva parte del bordo esca dall'array
	matr[r][c].regione = 0;	//regione ancora incognita, cioe' == 0
      }else {
	matr[r][c].val_col = (ptr[z]);
	matr[r][c].regione = 0;	//regione ancora incognita, cioe' == 0
      }
      z++;
    }
  }

  int i = 0;
  int maxY = 0;
  int maxX = 0;
  int minY = alt;
  int minX = lar;

  for (int x=1; x<al; x++){
    for (int y=1; y<la; y++){
      if((matr[x][y].val_col == 2)&&(i==0)){
		i = 1;
 		maxY = x;
 		minY = x;
 		maxX = y;
 		minX = y;
		contorno (matr, x, y, &diam, &xcentroid, &ycentroid, &maxX, &minY, &minX);
		break; 
      }
    }
    if (i==1) break;
  }

  int i2 = 0;
  int maxY2 = 0;
  int maxX2 = 0;
  int minY2 = alt;
  int minX2 = lar;
  double diam2 = 0;
  int xcentroid2 = 0;
  int ycentroid2 = 0;

  /*for (int x2=40; x2<130; x2++){
    for (int y2=20; y2<160; y2++){
      if((matr[x2][y2].val_col == 1)&&(i2==0)){
		i2 = 1;
 		maxY = x2;
 		minY = x2;
 		maxX = y2;
 		minX = y2;
		contorno (matr, x2, y2, &diam2, &xcentroid2, &ycentroid2, &maxX2, &minY2, &minX2);
		
		break; 
      }
    }
    if (i2==1) break;
  }
  // printf("+++ Fine elaborazione info_ball+++\n");*/

  if ((i == 1)&&(xcentroid != 0)){
	//if (diam > 45){
		/*conversione Y*/
		maxY = (159 - maxY);
		minY = (159 - minY);
		printf("#rettangolo minimo coord x y\n");
		printf("%d %d\n", minX, maxY);
		printf("%d %d\n", maxX, maxY);
		printf("%d %d\n", maxX, minY);
		printf("%d %d\n", minX, minY);
		printf("%d %d\n", minX, maxY);
		printf("\n");
		printf("%d %d\n", xcentroid, ycentroid); 
	//	printf("yc=%d xc=%d (maxY, maxX) = (%d, %d) - (minY, minX) = (%d, %d)*/ \n", ycentroid, xcentroid/*, maxY, maxX, minY, minX*/);
	//}
  }

/*  if (i2 == 1){
	if ((diam2 > 45)||(xcentroid2 != 0)){
  		printf("yc=%d xc=%d (maxY, maxX) = (%d, %d) - (minY, minX) = (%d, %d) \n", ycentroid2, xcentroid2, maxY2, maxX2, minY2, minX2);
	}
  }*/

  for(int t=0;t<alt;t++) free(matr[t]);
  free(matr); 

  for(int t=0;t<alt;t++) free(mat[t]);
  free(mat); 

  double* tmp = (double*) calloc(3,sizeof(double)); //alloco la memoria 
  if (tmp == NULL) while(1) printf("5");
  //ha invertito le x con le y, e io in unscita metto prima y poi x, cos= le riporto giuste
  if ((i!=0) && (xcentroid != NULL)){
    tmp[0]=diam;
    tmp[1]=(unsigned int)ycentroid+0.5;
    tmp[2]=(unsigned int)xcentroid+0.5;
	if (diam<5){
		
	}
    return(NULL);
  }
  else {
    return(NULL);
  }
}//end main

/*filtro l'immagine per eliminare i pixel singoli e armonizzare il resto*/
void corr_col (punto** m, punto** mat, int h, int k, colore ne, colore az, colore gi){
/*applico un elemento strutturale a stella di ampiezza 5*/ 
	colore max;								//conterranno il max per il colore a seconda dei due el. strutt.

	max.tipo = 0;
	max.occorrenza = 0;

/*pixel sopra*/
	switch (m[h+1][k].val_col){
		case(0):
			ne.occorrenza = ne.occorrenza +1;
		break;
		case(1):
			az.occorrenza = az.occorrenza +1;
		break;
		case(2):
			gi.occorrenza = gi.occorrenza +1;
		break;
	}

/*pixel laterali*/
/*sx*/
	switch (m[h][k-1].val_col){
		case(0):
			ne.occorrenza = ne.occorrenza +1;
		break;
		case(1):
			az.occorrenza = az.occorrenza +1;
		break;
		case(2):
			gi.occorrenza = gi.occorrenza +1;
		break;
	}

/*dx*/
	switch (m[h][k+1].val_col){
		case(0):
			ne.occorrenza = ne.occorrenza +1;
		break;
		case(1):
			az.occorrenza = az.occorrenza +1;
		break;
		case(2):
			gi.occorrenza = gi.occorrenza +1;
		break;
	}

/*pixel sotto*/
	switch (m[h+1][k].val_col){
		case(0):
			ne.occorrenza = ne.occorrenza +1;
		break;
		case(1):
			az.occorrenza = az.occorrenza +1;
		break;
		case(2):
			gi.occorrenza = gi.occorrenza +1;
		break;
	}

/*pixel diagonali*/
/*alto sx*/
	switch (m[h-1][k-1].val_col){
		case(0):
			ne.occorrenza = ne.occorrenza +1;
		break;
		case(1):
			az.occorrenza = az.occorrenza +1;
		break;
		case(2):
			gi.occorrenza = gi.occorrenza +1;
		break;
	}

/*alto dx*/
	switch (m[h-1][k+1].val_col){
		case(0):
			ne.occorrenza = ne.occorrenza +1;
		break;
		case(1):
			az.occorrenza = az.occorrenza +1;
		break;
		case(2):
			gi.occorrenza = gi.occorrenza +1;
		break;
	}

/*basso sx*/
	switch (m[h+1][k-1].val_col){
		case(0):
			ne.occorrenza = ne.occorrenza +1;
		break;
		case(1):
			az.occorrenza = az.occorrenza +1;
		break;
		case(2):
			gi.occorrenza = gi.occorrenza +1;
		break;
	}

/*basso dx*/
	switch (m[h+1][k+1].val_col){
		case(0):
			ne.occorrenza = ne.occorrenza +1;
		break;
		case(1):
			az.occorrenza = az.occorrenza +1;
		break;
		case(2):
			gi.occorrenza = gi.occorrenza +1;
		break;
	}

	max = maggiore (ne, az, gi);
	//if (max.tipo != 0){
    //	printf("%d, %d, %d ", ne.occorrenza, az.occorrenza, gi.occorrenza);
	//	printf("colore = %d\n", mat[h][k].val_col);
	//}
	mat[h][k].val_col = max.tipo;
	mat[h][k].regione = 0;

}//end corr_col


/*calcola il maggiore tra 6 valori di tipo colore e lo restituisce */
colore maggiore (colore ne, colore az, colore gi){
	colore max1, max2;

/*controllo ne e bi*/
  if (ne.occorrenza > az.occorrenza){
  	max1.occorrenza = ne.occorrenza;
  	max1.tipo = 0;
  }
  else {
	max1.occorrenza = az.occorrenza;
	max1.tipo = 1;
  }	
/*controllo maggiore tra max1 e max2 salvato in max5*/
  if (max1.occorrenza > gi.occorrenza){
	max2.occorrenza = max1.occorrenza;
	max2.tipo = max1.tipo;
  }
  else {
	max2.occorrenza = gi.occorrenza;
	max2.tipo = gi.tipo;
  }
 return (max2);
}//end procedura

/*calcola il perimetro in numero di pixel della palla e restituisce il diametro*/ 
void contorno (punto** m, int h, int k, double* drd, int* xc, int* yc, int* maxX, int* minY, int* minX){
  
  /*assegno il valore delle coordinate del punto a queste var ausiliarie come da testo dell'algoritmo*/
  CircleFittingClass* oCircleFitting;
  oCircleFitting = new CircleFittingClass();
  oCircleFitting->mResetPoints();
  
  
  coord position[8];//array con le posizioni da scandire 
  //int, maxX,minX,maxY,minY; //calcolo il raggio medio
  double dr =0;
  int search_pos = 0; //posizione corrente nella ricerca del prox pixel
  int x, y;
  x=h;
  y=k;
  
  int a, b, a1, b1;
  a=h;
  b=k;
  a1=h;
  b1=k;
  
  int po = 1;
  int pos = 0;//identifica la posizione del pixel precedente
  int l=0;
  int c=0;
  int xsum = 0; //variabili per il calcolo del centroide
  int ysum = 0;
  int npixels = 0;
  
  
  /*se no mi manca il primo*/
  m[a][b].regione = po;
  //printf("ricerca dei componenti del perimetro\n"); 
  
  /*passo preliminare: 
1- ho il primo pixel della regione trovato per scansione sequenziale, quindi devo controllare
solo i pixel dal vicino di destra.
2- la scansione avviene in senso orario sui pixel 8 vicini del mio non esclusi dal passo 
preliminare (dx, sotto dx, sotto e sotto sx)
3- al ciclo dovrà passare il primo pixel dello stesso colore che trovo e il suo predecessore
*/
  
  /*controllo i vicini */
  /*vicino dx*/
  if (m[a][b].val_col == m[a][b+1].val_col){
    pos = 2;
    po++;
    m[a][b+1].regione = po;
    //printf("pixel dx\n");
    dr = dr +1;
    a=a;
    b=b+1;
    xsum +=a;
    ysum +=b;
    npixels++;
	if (*maxX<b){
		*maxX = b;
	}
	if (*minX>b){
		*minX = b;
	}
	if (*minY<a){
		*minY = a;
	}
    oCircleFitting->mAddPoint((double)a,(double)b);
    //printf("%d %d\n",a,b);
  }
  else {
    /*vicino sotto dx*/
    if (m[a][b].val_col == m[a+1][b+1].val_col){
      pos = 2;
      po++;
      m[a+1][b+1].regione = po;
      //printf("pixel basso dx\n");	
      dr = dr +1;
      a=a+1;
      b=b+1;
      xsum +=a;
      ysum +=b;
      npixels++;
	  if (*maxX<b){
		*maxX = b;
	  }
      if (*minX>b){
		*minX = b;
	  }
	  if (*minY<a){
		*minY = a;
	  }
      oCircleFitting->mAddPoint((double)a,(double)b);
      // printf("%d %d\n",a,b);
    }
    else {
      /*vicino sotto*/
      if (m[a][b].val_col == m[a+1][b].val_col){
	pos = 4;
	po++;
	m[a+1][b].regione = po;
	//printf("pixel basso\n");
	dr = dr +1;
	a=a+1;
	b=b;
	xsum +=a;
	ysum +=b;
	npixels++;
	if (*maxX<b){
		*maxX = b;
	}
    if (*minX>b){
		*minX = b;
	}
	if (*minY<a){
		*minY = a;
	}
	oCircleFitting->mAddPoint((double)a,(double)b);
	//printf("%d %d\n",a,b);
      }
      else {
	/*vicino sotto sx*/
	if (m[a][b].val_col == m[a+1][b-1].val_col){
	  pos = 4;
	  po++;
	  m[a+1][b-1].regione = po;
	  //printf("pixel basso sx\n");	
	  dr = dr +1;
	  a=a+1;
	  b=b-1;
	  xsum +=a;
	  ysum +=b;
	  npixels++;
	  if (*maxX<b){
		*maxX = b;
	  }
	  if (*minX>b){
		*minX = b;
	  }
	  if (*minY<a){
		*minY = a;
	  }
	  oCircleFitting->mAddPoint((double)a,(double)b);
	  // printf("%d %d\n",a,b);
	}
      }
    }
  }
  
  /*n.b.: per scopi sperimentali setto la regione dei pixel di contorno a un valore
    diverso da 2 per poi riportarli su di un file (region.txt) per vedere se il perimetro che traccia e' quello esatto*/
  
  /*n.b.: pos è il pixel precedente!!!*/
  // printf("il pixel COLORATO, passato dal passaggio preliminare, da cui parto x=%d, y=%d e posizione=%d\n", a, b, pos);
  while (a!=h || b!=k){ //era in or...provaiamo l'end
    //printf("il pixel COLORATO, passato dal passaggio preliminare, da cui parto x=%d, y=%d e posizione=%d\n", a, b, pos);
    //inizializzazione posizioni
    position[0].line = a;
    position[0].column = b-1;
    position[1].line = a-1;
    position[1].column = b-1;
    position[2].line = a-1;
    position[2].column = b;
    position[3].line = a-1;
    position[3].column = b+1;
    position[4].line = a;
    position[4].column = b+1;
    position[5].line = a+1;
    position[5].column = b+1;
    position[6].line = a+1;
    position[6].column = b;
    position[7].line = a+1;
    position[7].column = b-1;
    
    for (int i=0; i<=7; i++){
      //printf("pos=%d\n, a=%d, b%d =>", pos, a, b);
      search_pos = ((pos+i)%8);	//n.b.: search pos contiene la posizione del primo pixel uguale trovato durante la scansione
      
      //printf("search pos=%d\n", search_pos);
      l = position[search_pos].line;

      c = position[search_pos].column;
      //printf("pixel che controllo per stesso colore l=%d, c=%d\n", l, c);
      //if(!(a == 0 || b == 0 || a == 208 || b == 160)
      if (m[a][b].val_col == m[l][c].val_col){
	//per la visualizzazione 
	//po++;
	m[l][c].regione = po;
	//printf("trovato pixel con colore uguale\n");
	dr = dr+1;
	a=l;
	b=c;
	//devo trovare il numero del pixel precdente nella scansione
	if ((search_pos==4)||(search_pos==5)){
	  pos = 2;
	}
	else {
	  if ((search_pos==2)||(search_pos==3)){
	    pos = 0;
	  }
	  else {
	    if ((search_pos==6)||(search_pos==7)){
	      pos = 4;
	    }
	    else {
	      if ((search_pos==0)||(search_pos==1)){
		pos = 6;
	      }
	    }
	  }
	}
	
	xsum +=a;
	ysum +=b;
	npixels++;
	if (*maxX<b){
		*maxX = b;
	}
	if (*minX>b){
		*minX = b;
	}
	if (*minY<a){
		*minY = a;
	}
	oCircleFitting->mAddPoint((double)a,(double)b);
	//printf("%d %d\n",a,b);
	break;
      }
      
    }
    //printf("esco dal ciclo for - trovato vicino colorato - inizio scansione per il prox\n");
    //printf("nuovo pixel centrale a=%d, b=%d, pos=%d\n", a, b, pos);
  }//end while

  
 
  if (npixels!=0){
    oCircleFitting->mEdgeBaricenterCircleFitting();
    //printf("RAGGIO-baricentro> %f",oCircleFitting->mGetRadius());
    oCircleFitting->mOMaMaCircleFitting_DownhillSimplex(0.5); //default 0.5
    *xc  = oCircleFitting->mGetCenterX();//(int)(((double)xsum) / ((double)npixels)); 
    *yc  = oCircleFitting->mGetCenterY();//(int)(((double)ysum) / ((double)npixels));
    *drd = oCircleFitting->mGetRadius()*2;

    //printf("CENTROIDE %lf %lf\n",*xc,*yc);
    //printf("RAGGIO %lf\n",*drd);
    
    // printf("CENTROIDE %lf %lf\n",oCircleFitting->mGetCenterX(),oCircleFitting->mGetCenterY());
    //printf("RAGGIO-media %lf -  ",oCircleFitting->mGetRadius());
    //oCircleFitting->mOMaMaCircleFitting_DownhillSimplex(0.5); //default 0.5
    //printf("CENTROIDE-fit %lf %lf\n",oCircleFitting->mGetCenterX(),oCircleFitting->mGetCenterY());
    // printf("%lf\n",oCircleFitting->mGetRadius());
    //dr = dr/PI;
    //*drd = dr;
    //*xc = (int)(((double)xsum) / ((double)npixels)); 
    //*yc = (int)(((double)ysum) / ((double)npixels));
  }
  else{
    *xc = NULL;
    *yc = NULL;
    *drd = NULL;
  }
//  delete oCircleFitting;
}

