/********************************************************************************
*         CS 791E Programming Assignment # 2 (2.1)
*              Canny Algorithm Application
*
*  
*  Name:   hough.c
*  Usage:  1). make -f snakeMakefile
*          2). make <input-image-file-name> <output-image-FILEBASE-name>
*                    <rstart> <rend> 
*  Autor:  Beifang Yi
*  Date:   3/16/03
*
*
********************************************************************************/

#ifdef _CH_
#pragma package <opencv>
#endif

#ifndef _EiC
#include "cv.h"
#include "highgui.h"
#endif

#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <iostream.h>
#include <fstream.h>
#include <unistd.h>
#include <list>

using namespace std;

//parameters for "houghing"
#define HOUGH_LINE_COLOR 128

//for quantize the parameter space
int quantum = 3;
int rStart;
int rEnd;
//int rQuantum;

//for thresholding
int cTRatioX100 = 80;     //for hough voting threshold
int bTRatioX100 = 90;     //for thresholding of "Sobeled" image

//image structures
IplImage *sourceI = 0;
IplImage *houghedI = 0;
IplImage *tempI = 0;

//file name for saved image
char destIF[80];

//create a new image with a circle
void newCircleImage(void);

//Sobel image
void sobelImage(void);

//for hough tranformation
#define MAX_NUM_CIRCLES 30
bool houghProcessed = false;
int ***houghSpace;
list <CvPoint> ***houghList;

typedef struct {
  int size;
  int step;
  int *dist;
  int start;
  int end;
} hBin;
hBin xBin;
hBin yBin;
hBin rBin;

typedef struct {
  int cx;
  int cy;
  int r;
  int count;
  float fittingError;
} hCircle;
hCircle foundCircles[MAX_NUM_CIRCLES];
int localMaximaCircleFlag[MAX_NUM_CIRCLES];
int numCircles = 20;

//allocate and deallocate memory
void buildHoughMem(void);
void deleteHoughMem(void);

//get the bin number from the current value v
int binNumber(int v, hBin &hb);
//give one edge point (x,y),
//increment the corresponding voting cells
void onePixelHough(int x, int y, int r);
void houghInitial();
void imageHough(IplImage *);
//test circles in the H-Space
void siftCircles(int x, int y, int r);
void drawCircles(void);
void findLocalMaximaCircles(void);

//for canny edge detection
int cannyLowThreshold = 70;
int cannyHighThreshold = 140;
int cannyMS = 5;
IplImage *cannyI = 0;
void canny_image(int h);

//calculate fitting errors
void calculateFE(void);

int main( int argc, char** argv )
{
  int i, j, k, m, key;
  int ci, cj, ck;
  char tempC[20];
  static int fileNameC = 0;

  double minVal, maxVal;
  CvPoint *minLoc, *maxLoc;
  minLoc = new CvPoint; maxLoc = new CvPoint;

  
  if (argc < 4) {
    cout << " To run the program, please type: "<<endl;
    cout << " ./snake <input-image-file-name> <output-image-FILEBASE-name>"<<endl;
    cout << "                <r_start> <r_end> "<<endl;
    exit(0);
  }
    
  // check and load source image
  if( (sourceI = cvLoadImage(argv[1], 0)) == 0 ) {
    cout << " Can not load image. " << endl;
    return -1;
  }
  houghedI = cvCloneImage(sourceI);
  tempI = cvCloneImage(sourceI);
  cannyI = cvCloneImage(sourceI);
  
  //initialize the houghing parameter/space
  rStart = atoi(argv[3]);
  rEnd = atoi(argv[4]);
  //rQuantum = atoi(argv[5]);
  
  cvMinMaxLoc( houghedI, &minVal, &maxVal, minLoc, maxLoc);

  //The instruction for using this "houghing" program
  cout<<endl;
  cout<<"-----------Welcome to Beifang's 'Houghing' program!!--------"<<endl;
  cout<<"This program interacts with you via key and mouse and terminal."<<endl;
  cout<<"----------------------------------------------------------"<<endl;
  cout<<"       FIRST: CLICK ON THE IMAGE/INTERFACE and then....   "<<endl;
  cout<<"type 'B' or 'b'-----to do Sobel detection on the input image. "<<endl;
  cout<<"type 'C' or 'c'-----to do Canny detection on the input image. "<<endl;
  cout<<"type 'H' or 'h'-----to do Hough transform on the current image."<<endl;
  cout<<"type 'L' or 'l'-----to local maxima on the clussters of circles."<<endl;
  cout<<"type 'E' or 'e'-----to calculate the fitting errors."<<endl;
  cout<<"type 'S' or 's'-----to save the current image."<<endl;
  cout<<"type 'D' or 'd'-----to find and draw found circle(s). "<<endl;
  cout<<"type 'N' or 'n'-----to create new image with a circle."<<endl;
  cout<<"type  SPACEBAR -----to quit this program.  "<<endl;
  cout<<"    And click/move the sliderbar to get new values.   "<<endl;
  cout<<"-----------------------------------------------------------"<<endl;
  cout<<"   ALWAYS MAKE the image/interface the current environment."<<endl;
  cout<<"      WHILE typing the above commands NOT in the shell window,"<<endl;
  cout<<"      BUT on the image/interface window!!!! "<<endl;
  cout<<"   Images saved in 'jpg' format. "<<endl;
  

  // Create windows for displaying  the effect
  cvNamedWindow("Source", CV_WINDOW_AUTOSIZE);
  cvNamedWindow("HoughedImage", CV_WINDOW_AUTOSIZE);

  // Show the source image.
  cvShowImage("Source", sourceI);
  cvShowImage( "HoughedImage", houghedI );

  // Create toolbars: for low/high threshold adjustment
  cvCreateTrackbar( "X_Y_R_Quantum", "HoughedImage", &quantum, 20, NULL);
  cvCreateTrackbar( "No. of Circles (in clusters)", "HoughedImage", &numCircles, MAX_NUM_CIRCLES, NULL);
  cvCreateTrackbar( "sobelThresholdRatioX100", "HoughedImage", &bTRatioX100, 100, NULL);
  //cvCreateTrackbar( "votingThresholdRatioX100", "HoughedImage", &cTRatioX100, 100, NULL);
  cvCreateTrackbar( "Canny Low Threshold", "HoughedImage", &cannyLowThreshold, 
		    ((int) maxVal)*cannyMS*cannyMS, canny_image );
  cvCreateTrackbar( "Canny High Threshold", "HoughedImage", &cannyHighThreshold, 
		    ((int) maxVal)*cannyMS*cannyMS, canny_image );
  
  
  // Wait for a key stroke, and process the image
  key = cvWaitKey(0);
  while (key != 32) {

    //save the snaked image
    if (key == 83 || key == 115) {      //key == 'S' or 's': save image
      strcpy(destIF, "houghed_");
      strcat(destIF, argv[2]);
      sprintf(tempC, "_%02d", fileNameC);
      strcat(destIF, tempC);
      strcat(destIF, ".jpg");
      cvSaveImage(destIF, houghedI);
      fileNameC++;
      cvShowImage("HoughedImage", houghedI);
    }

    //to do Sobel on the image
    else if (key == 'B' || key == 'b') {
      cout<<"----To do Sobel on the image "<<endl;
      sobelImage();
    }

    //to do Sobel on the image
    else if (key == 'C' || key == 'c') {
      cout<<"----To do Canny edge detection on the image "<<endl;
      canny_image(0);
    }

    //to do Hough on the image
    else if (key == 'H' || key == 'h') { 
      cout<<"----Now to do Hough transformation..."<<endl;
      houghInitial();
      imageHough(houghedI);
      houghProcessed = true;
      cout<<"----Hough transformation finished."<<endl<<endl;
    }

    //Threshold the voting on parameter space
    else if (key == 'L' || key == 'l' ) {
      cout<<endl<<"---Find the local maxima on clusters of circles... "<<endl;
      for(i = 0; i < MAX_NUM_CIRCLES; i++)
	localMaximaCircleFlag[i] = 0;
      findLocalMaximaCircles();
      drawCircles();
      cout<<endl<<"---Finished in finding the circles."<<endl<<endl;
      

    } 
    //calculate the fitting errors
    else if (key == 'E' || key == 'e') {
      calculateFE();
    } 

    //draw the FOUND circles
    else if (key == 'D' || key == 'd') {
      for(k = 0; k < rBin.size; k++)
	for (i = 0; i < xBin.size; i++)
	  for (j = 0; j < yBin.size; j++)
	    siftCircles(i, j, k);
      for(i = 0; i < MAX_NUM_CIRCLES; i++)
	localMaximaCircleFlag[i] = 1;
      drawCircles();
    }

    //create new image with a circle
    else if (key == 'N' || key == 'n') { 
      cout<<"Create an image with a circle."<<endl;
      newCircleImage();
    }

    //for recording the VERY PROCESS OF SNAKING---NO!!!!
    else if (key == 85 || key == 117) {    //key = 'U' or 'u'
      //get specifics about the input image
      cvMinMaxLoc( houghedI, &minVal, &maxVal, minLoc, maxLoc);
      cout<<"image width and height: "<<houghedI->width;
      cout<<"   "<<houghedI->height<<endl;
      cout<<" min-max  "<<minVal<<" "<<maxVal;
      cout<<"  minLoc  "<<minLoc->x<<" "<<minLoc->y<<endl;
      cout<<" ---maxLoc "<<maxLoc->x<<"  "<<maxLoc->y<<endl;
      onePixelHough(maxLoc->x, maxLoc->y, rBin.dist[0]);

    }
    
    key = cvWaitKey(0);
  }

  //release the images
  cvReleaseImage(&houghedI);
  cvDestroyWindow("Source");
  cvDestroyWindow("HoughedImage");

  return 0;
}

void newCircleImage()
{
  int i;
  
  cvReleaseImage(&houghedI);
  houghedI = new IplImage;
  houghedI = cvCreateImage( cvSize(sourceI->width, sourceI->height), 
			    IPL_DEPTH_8U, 1 );
  for (i = 0; i < houghedI->width * houghedI->height; i++)
    cvSetReal1D( houghedI, i, 255 );
  cvCircle( houghedI, cvPoint(houghedI->width/2, houghedI->height/2),
	    houghedI->height/3, 128, 2); 
  cvShowImage("HoughedImage", houghedI);
  
}


void sobelImage()
{

  cvReleaseImage(&tempI);
  tempI = new IplImage;
  tempI = cvCreateImage( cvSize(houghedI->width, houghedI->height), 
			 IPL_DEPTH_16S, 1 );
  cvSobel( houghedI, tempI, 1, 0);
  
  IplImage *img = new IplImage;
  img = cvCreateImage( cvSize(houghedI->width, houghedI->height), 
			 IPL_DEPTH_16S, 1 );
  cvReleaseImage(&houghedI);
  houghedI = cvCreateImage( cvSize(sourceI->width, sourceI->height), 
			    IPL_DEPTH_8U, 1 );
  cvConvertScale( tempI, houghedI);
  cvSobel( houghedI, img, 0, 1);
  
  cvReleaseImage(&houghedI);
  houghedI = cvCreateImage( cvSize(sourceI->width, sourceI->height), 
			    IPL_DEPTH_8U, 1 );
  cvConvertScale( img, houghedI);
  
  //threshold:
  cvReleaseImage(&tempI);
  tempI = cvCloneImage(houghedI);
  cvThreshold(tempI, houghedI, 200, 255, CV_THRESH_BINARY);
  
  cvShowImage("HoughedImage", houghedI);
  cvReleaseImage(&img);

}


//find the bin number in the x/y/r bin via the value v
int binNumber(int v, hBin &hb)
{
  int i,j;
  

  if (v < hb.start) {
    cout<<"!!!! No bin number: smaller than the first element. !!!!"<<endl;
    cout<<"   v , hb.start = " <<v<<"  "<<hb.start<<endl;
    exit(0);
  }
  if (v > hb.end) {
    cout<<"!!!! No bin number: larger than the last element. !!!!"<<endl;
    
    exit(0);
  }
  
  j = hb.start;
  for (i = 0; i < hb.size; i++) {
    j += hb.step;
    if (v < j) return i;
  }
  if (v <= hb.end)
    return hb.size - 1;

  cout<<"!!!! Crazy thing in binNumber()!!!!"<<endl;
 
  exit(0);

}
  
//give one edge point (x,y) and the radius
//increment the corresponding voting cells
void onePixelHough(int x, int y, int r)
{
  int i,j,k;
  int rBinNum, xBinNum, yBinNum, r2p, x2p, y2p, x2r, y2r;
  int xStart, xEnd, yStart, yEnd;       // x, y ranges
  int xBinStart, xBinEnd, yBinStart, yBinEnd;  //x, y bin number ranges

  //check x , y
  if (x < 0 || y < 0) {
    cout<<"!!! Crazy pixel location at (x, y) = "<<x<<"  "<<y<<endl;
    exit(0);
  }

  //get r's bin number
  // cout<<"rBinnum---"<<endl;
  rBinNum = binNumber(r, rBin);
  //cout<<"rbinnnn+++"<<endl;

  //get (x, y)'s ranges
  xStart = (int) (x - ((double) r ) / sqrt(2.0));
  xEnd = (int) (x + ((double) r ) / sqrt(2.0));
  yStart = (int) (y - ((double) r ) / sqrt(2.0));
  yEnd = (int) (y + ((double) r ) / sqrt(2.0)); 
  
  r2p = r * r;
  
 
    //X-Directional voting
    for (i = xStart; i <= xEnd; i += xBin.step) {
      if (i < xBin.start || i > xBin.end) continue;
      xBinNum = binNumber(i, xBin);
      x2p = (x - i) * (x - i);
      if (r2p >= x2p) {
	y2r = (int) sqrt( (double) (r2p - x2p) );
	j = y - y2r;
	if (j >= yBin.start && j <= yBin.end) {
	  yBinNum = binNumber(j, yBin);
	  houghSpace[xBinNum][yBinNum][rBinNum]++;
	  houghList[xBinNum][yBinNum][rBinNum].push_back(cvPoint(x, y));
	}
	j = y + y2r;
	if (j >= yBin.start && j <= yBin.end) {
	  yBinNum = binNumber(j, yBin);
	  houghSpace[xBinNum][yBinNum][rBinNum]++;
	  houghList[xBinNum][yBinNum][rBinNum].push_back(cvPoint(x, y));
	}	
      }
    }
    //Y-Directional voting
    for (i = yStart; i <= yEnd; i += yBin.step) {
      if (i < yBin.start || i > yBin.end) continue;
      yBinNum = binNumber(i, yBin);
      y2p = (y - i) * (y - i);
      if (r2p >= y2p) {
	x2r = (int) sqrt( (double) (r2p - y2p) );
	j = x - x2r;
	if (j >= xBin.start && j <= xBin.end) {
	  xBinNum = binNumber(j, xBin);
	  houghSpace[xBinNum][yBinNum][rBinNum]++;
	  houghList[xBinNum][yBinNum][rBinNum].push_back(cvPoint(x, y));
	  
	}
	j = x + x2r;
	if (j >= xBin.start && j <= xBin.end) {
	  xBinNum = binNumber(j, xBin);
	  houghSpace[xBinNum][yBinNum][rBinNum]++;
	  houghList[xBinNum][yBinNum][rBinNum].push_back(cvPoint(x, y));
	}	
      }
    }
    //}
}


void imageHough(IplImage *img)
{
  int i, j, k;
  int x, y, r;
  
  for (r = rBin.start; r <= rBin.end; r += rBin.step)
    for (j = 0; j < img->height; j++)
      for (i = 0; i < img->width; i++) {
	k = (int) cvGetReal1D(img, j * img->width + i);
	if (k > 128) {
	  onePixelHough(i, j, r);
	  // cout<<"hhhhhh: pixel (x, y, r) houhed: "<<i<<" "<<j<<" "<<r<<endl;
	}
      }
}

void houghInitial()
{

  int i, j, k, l, m;

  //if already hough processed, first free space and then allocate new space
  if (houghProcessed) 
    {
      //free memory first
      
      for (i = 0; i < xBin.size; i++)
	for (j = 0; j < yBin.size; j++) {
	  for (k = 0; k < rBin.size; k++)
	    houghList[i][j][k].clear();
	  delete [] houghSpace[i][j];
	  delete [] houghList[i][j];
	}
      for (i = 0; i < xBin.size; i++) {
	delete [] houghSpace[i];
	delete [] houghList[i];
      }
      delete [] houghSpace;
      delete [] houghList;
      delete [] xBin.dist;
      delete [] yBin.dist;
      delete [] rBin.dist;

      //for rBin initialization
      rBin.step = quantum;
      rBin.size = (int) ceil( (double) (rEnd - rStart + 1) / 
			      (double)(rBin.step) );
      xBin.size = (int) ceil( (double)houghedI->width / (double) quantum);
      yBin.size = (int) ceil( (double) houghedI->height / (double) quantum );

      xBin.dist = new int [xBin.size];
      yBin.dist = new int [yBin.size];
      rBin.dist = new int [rBin.size];
      
      if (rBin.size == 0) rBin.size = 1;
      rBin.start = rStart;
      rBin.end = rEnd;
      for (i = 0; i < rBin.size; i++)
	rBin.dist[i] = 0;
      rBin.dist[0] = rStart;
      
      for (i = 1; i < rBin.size; i++){
	rBin.dist[i] = rBin.step + rBin.dist[i-1];
      }
       //allocate hough space memory
      houghSpace = new (int **) [xBin.size];
      for (i = 0; i < xBin.size; i++)
	houghSpace[i] = new (int *) [yBin.size];
      for (i = 0; i < xBin.size; i++)
	for (j = 0; j < yBin.size; j++)
	  houghSpace[i][j] = new int [rBin.size];

      //allocate list -link space 
      houghList = new (list<CvPoint> **) [xBin.size];
      for (i = 0; i < xBin.size; i++)
	houghList[i] = new (list<CvPoint> *) [yBin.size];
      for (i = 0; i < xBin.size; i++)
	for (j = 0; j < yBin.size; j++)
	  houghList[i][j] = new list<CvPoint> [rBin.size];
    
    
  }
  else 
    //for the first time: intialization
    {
      //for rBin initialization
      rBin.step = quantum;
      rBin.size = (int) ceil( (double) (rEnd - rStart + 1) / 
			      (double)(rBin.step) );
      xBin.size = (int) ceil( (double)houghedI->width / (double) quantum);
      yBin.size = (int) ceil( (double) houghedI->height / (double) quantum );

      xBin.dist = new int [xBin.size];
      yBin.dist = new int [yBin.size];
      rBin.dist = new int [rBin.size];
      
      if (rBin.size == 0) rBin.size = 1;
      rBin.start = rStart;
      rBin.end = rEnd;
      for (i = 0; i < rBin.size; i++)
	rBin.dist[i] = 0;
      rBin.dist[0] = rStart;
      
      for (i = 1; i < rBin.size; i++){
	rBin.dist[i] = rBin.step + rBin.dist[i-1];
      }
      
      //allocate hough space memory
      houghSpace = new (int **) [xBin.size];
      for (i = 0; i < xBin.size; i++)
	houghSpace[i] = new (int *) [yBin.size];
      for (i = 0; i < xBin.size; i++)
	for (j = 0; j < yBin.size; j++)
	  houghSpace[i][j] = new int [rBin.size];

      //allocate list -link space 
      houghList = new (list<CvPoint> **) [xBin.size];
      for (i = 0; i < xBin.size; i++)
	houghList[i] = new (list<CvPoint> *) [yBin.size];
      for (i = 0; i < xBin.size; i++)
	for (j = 0; j < yBin.size; j++)
	  houghList[i][j] = new list<CvPoint> [rBin.size];
    }


  for (i = 0; i < xBin.size; i++)
    for (j = 0; j < yBin.size; j++)
      for (k = 0; k < rBin.size; k++)
	houghSpace[i][j][k] = 0;

  for (i = 0; i < MAX_NUM_CIRCLES; i++) {
    foundCircles[i].cx = -1;
    foundCircles[i].cy = -1;
    foundCircles[i].r = -1;
    foundCircles[i].count = 0;
    foundCircles[i].fittingError = 0.0;
  }

  xBin.start = 0;
  xBin.end = houghedI->width - 1;
  xBin.step = quantum;
  for (i = 0; i < xBin.size; i++)
    xBin.dist[i] = 0;
  xBin.dist[0] = (int) (quantum / 2);
  for (i = 1; i < xBin.size; i++) {
    xBin.dist[i] = xBin.step + xBin.dist[i-1];
  }
  xBin.dist[xBin.size - 1] = xBin.end;

  yBin.start = 0;
  yBin.end = houghedI->height - 1;
  yBin.step = quantum;
  for (i = 0; i < yBin.size; i++)
    yBin.dist[i] = 0;
  yBin.dist[0] = (int) (quantum / 2 );
  for (i = 1; i < yBin.size; i++){
    yBin.dist[i] = yBin.step + yBin.dist[i-1];
  }
  yBin.dist[yBin.size - 1] = yBin.end;



  j = min(xBin.size, yBin.size);
  if (j < 3) {
    cout<<"!!!!meaningless hough space size---too small!!!"<<endl;
    exit(0);
  }

  xBin.dist[xBin.size - 1] = houghedI->width - 1;
  yBin.dist[yBin.size - 1] = houghedI->height - 1;
  

}


// CALLBACK: when either the threshold changes,
//           this function will be called.
//           FOR Canny edge detection based on the changed parameters
void canny_image(int h)
{

  // Canny detection
  cvCanny(cannyI, houghedI, cannyLowThreshold, cannyHighThreshold, cannyMS);
  // Show image. HighGUI use.
  cvShowImage( "HoughedImage", houghedI );

}


//test circles in the H-Space
void siftCircles(int x, int y, int r)
{
  int i, j;

  for (i = 0; i < numCircles; i++)
    if (x == foundCircles[i].cx &&
	y == foundCircles[i].cy &&
	r == foundCircles[i].r)
      return;

  for (i = 0; i < numCircles; i++)
    if (houghSpace[x][y][r] > foundCircles[i].count) {
      for (j = numCircles - 1; j > i; j--) {
	foundCircles[j].cx = foundCircles[j - 1].cx;
	foundCircles[j].cy = foundCircles[j - 1].cy;
	foundCircles[j].r = foundCircles[j - 1].r;
	foundCircles[j].count = foundCircles[j - 1].count;
      }
      foundCircles[i].cx = x;
      foundCircles[i].cy = y;
      foundCircles[i].r = r;
      foundCircles[i].count = houghSpace[x][y][r];

      break;
    }

}


void drawCircles()
{

  int i, j, k, r;

  cvReleaseImage(&houghedI);
  houghedI = cvCloneImage(sourceI);

  for (k = 0; k < numCircles; k++) 
    if (localMaximaCircleFlag[k] == 1)
    {
    i = foundCircles[k].cx;
    j = foundCircles[k].cy;
    r = foundCircles[k].r;
   
    cout<<" Found Circle at (x, y): "<<xBin.dist[i];
    cout<<" "<<yBin.dist[j]<<" radius = "<<rBin.dist[r]<<endl;
    cout<<" The counting at that is: "<<houghSpace[i][j][r]<<endl;
      
    cvCircle( houghedI, cvPoint(xBin.dist[i], yBin.dist[j]),
	      rBin.dist[r], 255, 1); 
    cvShowImage("HoughedImage", houghedI);
  }

}

// local maxima-circles
void findLocalMaximaCircles()
{

  int i, j, f = 3;
  int k = 0;

  for (i = 0; i < MAX_NUM_CIRCLES; i++)
    localMaximaCircleFlag[i] = 0;

  localMaximaCircleFlag[0] = 1;

  
  for (i = 1; i < numCircles; i++) {
    for (j = 0; j < i; j++) 
      if (( abs(foundCircles[i].cx - foundCircles[j].cx) <= f * quantum &&
	   abs(foundCircles[i].cy - foundCircles[j].cy) <= f * quantum &&
	    abs(foundCircles[i].r - foundCircles[j].r) <= f * quantum ) &&
	  (localMaximaCircleFlag[j] == 1) ) {
	k = 1;
	break;
      }
    if ( k == 0)
      localMaximaCircleFlag[i] = 1;
    k = 0;
  }
  

}

//calculate the fitting error for each found circle
void calculateFE()
{

  int i, j, r, k, q, ls;
  int di, dj, dr, dx, dy;
  float e = 0.0;
  double d;

  for (k = 0; k < numCircles; k++) 
   if (localMaximaCircleFlag[k] == 1){
    i = foundCircles[k].cx;
    j = foundCircles[k].cy;
    r = foundCircles[k].r;

    e = 0;

    di = xBin.dist[i];
    dj = yBin.dist[j];
    dr = rBin.dist[r];
    
    list<CvPoint>::iterator p = houghList[i][j][r].begin();
    ls = houghList[i][j][r].size();
    if (ls <= 0) {
      cout<<" !!!Crazy, list size <=0 !!!!"<<endl;
      exit(0);
    }
    for (q = 0; q < ls; q++) {
      dx = p->x - di;
      dy = p->y - dj;
      dx = dx * dx; dy = dy * dy;
      d = sqrt((double) (dx + dy));
      d = d - dr;
      e += (float) (d * d);
      
      p++;
    }
    e = e / (float) ls;
    
    foundCircles[k].fittingError = e;
    //xxx
    cout<<" circle "<<k<<"'s fitting error = "<<e<<endl;
  }

}