#include <stdio.h>
#include <ctype.h>
#include <stdlib.h>
#include <errno.h>
#include <math.h>
#include "fitting.h"

/*
 *__get_data - read data from a file and store them 
 *into the given target structure,it is dimension related
 */
static int __get_data(FILE *fp,struct fitting_target *target,int dim){
	double	*x;
	double	*y;
	double	*z=NULL;
	int		size=100;
	int		count=0;
	int		i;
	char	buffer[80];

	if(!fp)
		return -ENOFILE;
	if(!target)
		return -ENOMEM;
	if(dim!=2 && dim!=3)
		return -EINVAL;

	x = (double *)calloc(size, sizeof(double));
	if(!x)
		return -ENOMEM;
   
	y = (double *)calloc(size, sizeof(double));
	if(!y){
		free(x);
		return -ENOMEM;
	}

	if(dim==3){
		z=(double *)calloc(size, sizeof(double));
		if(!z){
			free(x);
			free(y);
			return -ENOMEM;
		}
	}		

	for(i=0;!feof(fp);i++){
         
		 fgets(buffer,80,fp);

         if(isdigit(buffer[0])||(buffer[0] == '-')){
			if(dim==2)
				sscanf(buffer, "%lf%lf",x+i,y+i);
			else
				sscanf(buffer, "%lf%lf%lf",x+i,y+i,z+i);

            count++;

            if(!(count%100)){               
				x = realloc(x,(size+100)*sizeof(double));
				if(!x)
					return -ENOMEM;
                y = realloc(y,(size+100)*sizeof(double));
				if(!y){
					free(x);
					return -ENOMEM;
				}
                if(dim==3){
					z = realloc(z,(size+100)*sizeof(double));
					if(!z){
						free(x);
						free(y);
						return -ENOMEM;
					}
				}
				size=size+100;
			}
         }else  
			 break;
    }

	target->data.x=x;
	target->data.y=y;
	target->data.z=z;
	target->data.count=count;
	return 0;
}

	
/*
 *get_data - read data from a file and store them 
 *into the given target structure 
 */
static int get_data(char *fname,struct fitting_target *target){
	FILE *fp;
	int	 ret;

	fp=fopen(fname,"r");
	if(!fp)
		return -ENOFILE;

	if(!target)
		return -ENOMEM;

	switch(target->type){
		case TYPE_2D_CIRCLE	:
			ret=__get_data(fp,target,2); //read two dimension data
			break;
		default :
			ret=__get_data(fp,target,3); //read three dimension data
	}

	return ret;
}

/*
 *put_data - to free the data that stored in the target
 */
static void put_data(struct fitting_target *target){
	if(!target)
		return;
	if(target->data.x)
		free(target->data.x);
	if(target->data.y)
		free(target->data.y);
	if(target->data.z)
		free(target->data.z);
}


/*
 *matrix_alloc - to allocate an empty matrix given 
 *row number and column number,the data type is double
 */
static matrix_t *matrix_alloc(int rows,int cols){
	matrix_t *matrix;
	double	 *array,*temp;
	int		 i;

	if(rows<=0 || cols<=0){
		return NULL;
	}

	matrix=(matrix_t *)malloc(sizeof(matrix_t));
	if(!matrix)
		return NULL;
	matrix->rows=rows;
	matrix->cols=cols;

	array=(double *)malloc(rows*cols*sizeof(double));
	if(!array){
		free(matrix);
		return NULL;
	}

	matrix->data=(double **)malloc(rows*sizeof(double *));
	if(!matrix->data){
		free(matrix);
		free(array);
		return NULL;
	}

	for(temp=array,i=0;i<rows;i++){
		matrix->data[i]=temp;
		temp+=cols;
	}

	return matrix;
}
/*
 *vector_normalize - to normalize a vector
 */
static int vector_normalize(matrix_t *matrix){
	int i,cnt;
	double mod,*array;

	if(!matrix)
		return -ENOMEM;
	if(matrix->rows!=1 && matrix->cols!=1)
		return -EINVAL;
	
	cnt=matrix->rows==1 ? matrix->cols : matrix->rows;
	array=matrix->data[0];

	mod=0.0;
	for(i=0;i<cnt;i++)
		mod+=array[i]*array[i];
	
	mod=sqrt(mod);

	for(i=0;i<cnt;i++)
		array[i]/=mod;

	return 0;
}
/*
 *matrix_destroy - free a matrix
 */
static void matrix_destroy(matrix_t *matrix){
	if(matrix && matrix->data && *matrix->data) 
		free(*matrix->data);

	if(matrix && matrix->data)
		free(matrix->data);

	if(matrix)
		free(matrix);
}

int matrix_trans (matrix_t *src, matrix_t *dst) {
	int i, j;
	if(src->rows!=dst->cols || src->cols!=dst->rows)
		return -EINVAL;

	for(i=0; i<src->rows; i++)
		for(j=0; j<src->cols; j++)
			(dst->data)[j][i] = (src->data)[i][j];
	return 0;
}

static void matrix_inverse(matrix_t *m){
	int i,j;

	for(i=0;i<m->rows;i++)
		for(j=0;j<m->cols;j++)
			m->data[i][j]*=-1.0;
}


/*
 *matrix_multiply - to multiply two matrixes
 */
static int matrix_multiply(matrix_t *m1,matrix_t *m2,matrix_t *result){
	int i,j,k,l;
	double item;

	if(!m1->data || !m2->data || !result->data){
		return -ENOMEM;
	}

	if(m1->cols!=m2->rows){
		fprintf(stderr,"invalid matrix arguments!\n");
		return -EINVAL;
	}

	if(result->rows!=m1->rows||result->cols!=m2->cols){
		fprintf(stderr,"invalid matrix arguments!\n");
		return -EINVAL;
	}

	for(i=0;i<m1->rows;i++){
		for(j=0;j<m2->cols;j++){
			item=0;
			l=0;
			for(k=0;k<m1->cols;k++){
				item+=m1->data[i][k]*m2->data[l++][j];
			}
			result->data[i][j]=item;
		}
	}

	return 0;
}

static int matrix_add(matrix_t *m1,matrix_t *m2,matrix_t *result){
	int i,j;

	if(m1->rows!=m2->rows || 
	   m1->cols!=m2->cols ||
	   m1->rows!=result->rows ||
	   m1->cols!=result->cols)
		return -EINVAL;

	for(i=0;i<m1->rows;i++)
		for(j=0;j<m1->cols;j++)
			result->data[i][j]=m1->data[i][j]+m2->data[i][j];

	return 0;
}

static int matrix_cpy(matrix_t *sou, matrix_t *des) {
	int i, j;
	if(((des->cols)!=(sou->cols)) || ((des->rows)!=(sou->rows)))
		return -1;
	for(i=0; i<des->rows; i++)
		for(j=0; j<des->cols; j++)
			(des->data)[i][j] = (sou->data)[i][j];
	return 0;
}

/*
near 0
*/

int matrix_near(matrix_t *m_1, matrix_t *m_2) {
	int i, j;
	if(((m_1->cols)!=(m_2->cols)) || ((m_1->rows)!=(m_2->rows)))
		return -1;
	for(i=0; i<m_1->rows; i++)
		for(j=0; j<m_1->cols; j++) {
			if(fabs((m_1->data)[i][j]-(m_2->data)[i][j]) > MIN_NUM)
				return -1;
		}
	return 0;
}

static int resolve(matrix_t *A, matrix_t *b, matrix_t *x){ 
	int *js,l,k,i,j,is;
	int n = A->rows;
    double d,t;
    js=malloc(n*sizeof(int));
    l=1;
    for (k=0;k<=n-2;k++)
      { d=0.0;
        for (i=k;i<=n-1;i++)
          for (j=k;j<=n-1;j++)
            { t=fabs(A->data[i][j]);
              if (t>d) { d=t; js[k]=j; is=i;}
            }
        if (d+1.0==1.0) l=0;
        else
          { if (js[k]!=k)
              for (i=0;i<=n-1;i++)
                {
                  t=A->data[i][k]; A->data[i][k]=A->data[i][js[k]]; A->data[i][js[k]]=t;
                }
            if (is!=k)
              { for (j=k;j<=n-1;j++)
                  { 
                    t=A->data[k][j]; A->data[k][j]=A->data[is][j]; A->data[is][j]=t;
                  }
                t=b->data[k][0]; b->data[k][0]=b->data[is][0]; b->data[is][0]=t;
              }
          }
        if (l==0)
          { free(js); printf("fail\n");
            return -1;
          }
        d=A->data[k][k];
        for (j=k+1;j<=n-1;j++)
          { A->data[k][j]=A->data[k][j]/d;}
        b->data[k][0]=b->data[k][0]/d;
        for (i=k+1;i<=n-1;i++)
          { for (j=k+1;j<=n-1;j++)
              { 
                A->data[i][j]=A->data[i][j]-A->data[i][k]*A->data[k][j];
              }
            b->data[i][0]=b->data[i][0]-A->data[i][k]*b->data[k][0];
          }
      }
    d=A->data[n-1][n-1];
    if (fabs(d)+1.0==1.0)
      { free(js); printf("fail\n");
        return -1;
      }
    b->data[n-1][0]=b->data[n-1][0]/d;
    for (i=n-2;i>=0;i--)
      { t=0.0;
        for (j=i+1;j<=n-1;j++)
          t=t+A->data[i][j]*b->data[j][0];
        b->data[i][0]=b->data[i][0]-t;
      }
    js[n-1]=n-1;
    for (k=n-1;k>=0;k--)
      if (js[k]!=k)
        { t=b->data[k][0]; b->data[k][0]=b->data[js[k]][0]; b->data[js[k]][0]=t;}
    free(js);
	for(i=0; i<n; i++)
		x->data[i][0] = b->data[i][0];
    return 0;
 }


static int get_init_guess_2d_circle(struct fitting_target *target, matrix_t *matrix) {
	int count, decount;
	int i;
	struct data_set *data = &(target->data);
	matrix_t *A = matrix_alloc(2, 2);
	matrix_t *b = matrix_alloc(2, 1);
	matrix_t *x = matrix_alloc(2, 1);
	
	double sum_x = 0;
	double sum_y = 0;
	double sum_r = 0;
	double tmp_x, tmp_y;

	count = (data->count)/3;
	if(count > MAX_GUESS_COUNT)
		count = MAX_GUESS_COUNT;
	decount = 0;

	for(i=0; i<count; i++) {
		(A->data)[0][0] = ((data->x)[i*3+1]-(data->x)[i*3+0])*2;	//2(x2-x1)
		(A->data)[1][0] = ((data->x)[i*3+2]-(data->x)[i*3+0])*2;	//2(x3-x1)
		(A->data)[0][1] = ((data->y)[i*3+1]-(data->y)[i*3+0])*2;	//2(y2-y1)
		(A->data)[1][1] = ((data->y)[i*3+2]-(data->y)[i*3+0])*2;	//2(y3-y1)
		(b->data)[0][0] = ((data->x)[i*3+1]*(data->x)[i*3+1]-(data->x)[i*3+0]*(data->x)[i*3+0])+((data->y)[i*3+1]*(data->y)[i*3+1]-(data->y)[i*3+0]*(data->y)[i*3+0]);	//(x2*x2-x1*x1)+(y2*y2-y1*y1)
		(b->data)[1][0] = ((data->x)[i*3+2]*(data->x)[i*3+2]-(data->x)[i*3+0]*(data->x)[i*3+0])+((data->y)[i*3+2]*(data->y)[i*3+2]-(data->y)[i*3+0]*(data->y)[i*3+0]);	//(x3*x3-x1*x1)+(y3*y3-y1*y1)
		decount++;
		if(resolve(A, b, x) != -1) {
			tmp_x = (x->data)[0][0];
			tmp_y = (x->data)[1][0];
			sum_x += tmp_x;
			sum_y += tmp_y;
			sum_r += sqrt((tmp_x-(data->x)[i*3+1])*(tmp_x-(data->x)[i*3+1])+(tmp_y-(data->y)[i*3+1])*(tmp_y-(data->y)[i*3+1]));
			decount--;
		}
		(x->data)[0][0] = 0;
		(x->data)[1][0] = 0;
	}
	matrix->data[0][0] = sum_x/(count-decount);
	matrix->data[1][0] = sum_y/(count-decount);
	matrix->data[2][0] = sum_r/(count-decount);

	matrix_destroy(A);
	matrix_destroy(b);
	matrix_destroy(x);

	return(0);
}

static double obj_func_2d_circle(struct data_set *data,matrix_t *p){
	double *x,*y;
	double x0,y0,r0;
	double ret=0.0;
	int i;

	if(!data)
		return (double)(-ENOMEM);

	if(p->rows!=3 || p->cols!=1)
		return (double)(-EINVAL);

	x=data->x;
	y=data->y;
	x0=p->data[0][0];
	y0=p->data[1][0];
	r0=p->data[2][0];

	#define D(i) (sqrt((x[i]-x0)*(x[i]-x0)+(y[i]-y0)*(y[i]-y0)))
	
	for(i=0;i<data->count;i++)
		ret+=(D(i)-r0)*(D(i)-r0);

	#undef D(i)

	return ret;
}

static int fit_2d_circle(struct fitting_target *target){
	matrix_t	*P0;
	matrix_t	*F0;
	matrix_t	*F0T;
	matrix_t	*quot;
	matrix_t	*Di;
	matrix_t	*F0TDi;
	matrix_t	*result;
	matrix_t	*Pnew;

	double		*x,*y;
	double		x0,y0,r0;
	double		J0,Jnew;
	double		factor=INIT_FACTOR_VALUE;
	int			i,j,count;
	int			error,inner_it_cnt,outer_it_cnt;
	char		converged=0;

	if(target->type!=TYPE_2D_CIRCLE)
		return -EINVAL;

	if(!(target->data.x) || !(target->data.y))
		return -ENOMEM;

	P0=matrix_alloc(3,1);
	if(!P0)
		return -ENOMEM;

	F0=matrix_alloc(target->data.count,3);
	if(!F0){
		error=-ENOMEM;
		goto F0_err;
	}

	F0T=matrix_alloc(3,target->data.count);
	if(!F0T){
		error=-ENOMEM;
		goto F0T_err;
	}

	quot=matrix_alloc(3,3);
	if(!quot){
		error=-ENOMEM;
		goto quot_err;
	}

	Di=matrix_alloc(target->data.count,1);
	if(!Di){
		error=-ENOMEM;
		goto Di_err;
	}
	
	F0TDi=matrix_alloc(3,1);
	if(!F0TDi){
		error=-ENOMEM;
		goto F0TDi_err;
	}

	result=matrix_alloc(3,1);
	if(!result){
		error=-ENOMEM;
		goto result_err;
	}

	Pnew=matrix_alloc(3,1);
	if(!Pnew){
		error=-ENOMEM;
		goto Pnew_err;
	}

	/*
	 *lev_marq iteration goes here
	 */
	x=target->data.x;
	y=target->data.y;
	count=target->data.count;
	get_init_guess_2d_circle(target,P0);

	printf("MyGuess: x0=%lf y0=%lf r0=%lf\n",P0->data[0][0],P0->data[1][0],P0->data[2][0]);
	
	inner_it_cnt=0;
	outer_it_cnt=0;

	//calculate J0
	J0=obj_func_2d_circle(&target->data,P0);
	
	#define D(i) (sqrt((x[i]-x0)*(x[i]-x0)+(y[i]-y0)*(y[i]-y0)))

	//converged by initial guess? if so, we are lucky!
	if(J0>CONVERGE_VALUE){
		//not lucky enough,have to start iteration
		do{
			//decrease numda
			factor-=DECREMENT_FACTOR;

			x0=P0->data[0][0];
			y0=P0->data[1][0];
			r0=P0->data[2][0];

			//calculate F0
			for(i=0;i<count;i++){
				j=0;
				//must be careful here! 
				//square(xi-x0)+square(yi-y0) may equal to 0
				if(D(i)==0)
					continue;
				F0->data[i][j++]=(-1.0)*(x[i]-x0)/D(i);
				F0->data[i][j++]=(-1.0)*(y[i]-y0)/D(i);
				F0->data[i][j]=-1.0;
			}

			//F0T=F0 trans
			error=matrix_trans(F0,F0T);
			if(error<0)
				goto free_all;

			//quot=F0T * F0
			error=matrix_multiply(F0T,F0,quot);
			if(error<0)
				goto free_all;

			//calculate di(P0)
			for(i=0;i<Di->rows;i++)
				Di->data[i][0]=D(i)-r0;

			//v=F0T * di(P0)
			error=matrix_multiply(F0T,Di,F0TDi);
			if(error<0)
				goto free_all;
	
			//v->-v
			matrix_inverse(F0TDi);

			do{
				//increse numda
				factor+=INCREMENT_FACTOR;

				//H=F0T*F0 + numda*I
				for(i=0;i<quot->rows;i++)
					quot->data[i][i]+=factor;

				//resolve: Hx=-v
				if((error=resolve(quot,F0TDi,result))<0){
					goto free_all;
				}

				//Pnew=P0+x
				if((error=matrix_add(result,P0,Pnew))<0){
					goto free_all;
				}

				//calculate Jnew
				Jnew=obj_func_2d_circle(&target->data,Pnew);
			
				//converged in this iteration?
				if(Jnew<CONVERGE_VALUE){
					converged=1;
					//Pnew->P0
					if((error=matrix_cpy(Pnew,P0))<0)
						goto free_all;
					J0=Jnew;
					break;
				}
				
				inner_it_cnt++;

				if(inner_it_cnt>ITERATION_LIMIT)
					break;
			}while(Jnew>=J0);

			//if converged by Pnew, break
			if(converged){
				break;
			}

			//not converged,but worth continuing
			if(Jnew<J0){
				//Pnew->P0
				if((error=matrix_cpy(Pnew,P0))<0){
					goto free_all;
				}
				//Jnew->J0
				J0=Jnew;
			}else
				break;

			outer_it_cnt++;
			inner_it_cnt=0;
		}while(outer_it_cnt<ITERATION_LIMIT);
	}//if

	#undef	D(i);

	puts("fitting done!");

	target->result.type=TYPE_2D_CIRCLE;
	target->result.u.circle_2dim_res.centre_x=P0->data[0][0];
	target->result.u.circle_2dim_res.centre_y=P0->data[1][0];
	target->result.u.circle_2dim_res.radius=P0->data[2][0];
	target->result.u.circle_2dim_res.eps=J0/count;
	error=0;

free_all:
	matrix_destroy(Pnew);
Pnew_err:
	matrix_destroy(result);
result_err:
	matrix_destroy(F0TDi);
F0TDi_err:
	matrix_destroy(Di);
Di_err:
	matrix_destroy(quot);
quot_err:
	matrix_destroy(F0T);
F0T_err:
	matrix_destroy(F0);
F0_err:
	matrix_destroy(P0);

	return error;
}

int fit_target(struct fitting_target *target){
	int ret=0;

	switch(target->type){
		case TYPE_2D_CIRCLE :
			ret=fit_2d_circle(target);
			break;
		default : break;
	}
	return ret;
}

int main(int argc,char **argv){
	int i;
	struct fitting_target target;
	char *fname;
	
	if(argc!=2){
		puts("USAGE:fit2dcircle filename");
		exit(1);
	}

	fname=argv[1];

	target.type=TYPE_2D_CIRCLE;
	
	if((get_data(fname,&target))<0)
		exit(1);

	
	if(fit_target(&target)){
		puts("failed to fit the circle!");
		put_data(&target);
		exit(1);
	}

	printf("x=%lf\ny=%lf\nr=%lf\neps=%lf\n",
		target.result.u.circle_2dim_res.centre_x,
		target.result.u.circle_2dim_res.centre_y,
		target.result.u.circle_2dim_res.radius,
		target.result.u.circle_2dim_res.eps);

	put_data(&target);

	return 0;
}