//#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);
  }


  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++;
    }
  }
  //rilevamento contorno
  int i = 0;
  int x,y;
  int maxY = 0;
  int maxX = 0;
  int minY = alt;
  int minX = lar;
  //printo tutti i pixel della palla
  // for (x=1; x<al; x++) for (y=1; y<la; y++) if(matr[x][y].val_col == 1) printf("%d %d\n",x, y);;
  //printf("******************* FINE PALLA **********************************************");

  for (x=1; x<al; x++){
    for (y=1; y<la; y++){
      //printf("x=%d, y=%d\n", x, y);
      if((matr[x][y].val_col == 2)&&(i==0)){
	i = 1;
 	maxY = x;
	//printf("********* contorno eseguito \n");
	contorno (matr, x, y, &diam, &xcentroid, &ycentroid, &maxX, &minY, &minX);
	//printf("centroide x=%d, y=%d\n", xcentroid, ycentroid);
	break; 
      }
    }
    if (i==1) break;
  }
  
  // printf("+++ Fine elaborazione info_ball+++\n");

  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){
		printf("yc=%d xc=%d diam=%f maxY=%d maxX=%d minY=%d minX=%d\n", ycentroid, xcentroid, diam, maxY, maxX, minY, minX);
	}
    return(NULL);
  }
  else {
    return(NULL);
  }
}//end main

/*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;
}

