#include <stdio.h>
#include <math.h>

#define ITER_MAX 1000

typedef double(* pf3)(double, double, double);

double eval_f(pf3 f, double x, double y, double z);
void   eval_df3(pf3 *f, double x, double y, double z, double df[][3]);
double det33(double a[][3]);
double newton(pf3 *f, double *_x, double *_y, double *_z);

// double f(double x, double y, double z){
// 	return x*x+y*y+z*z-1;
// }

// double fx(double x, double y, double z){
// 	double dx = 1e-6;
// 	return (f(x+dx,y,z) - f(x-dx,y,z))/(2*dx);
// }

// double fy(double x, double y, double z){
// 	double dy = 1e-6;
// 	return (f(x,y+dy,z) - f(x,y-dy,z))/(2*dy);
// }

// double fz(double x, double y, double z){
// 	double dz = 1e-6;
// 	return (f(x,y,z+dz) - f(x,y,z-dz))/(2*dz);
// }

#define SOLUTION_X 1
#define SOLUTION_Y 2
#define SOLUTION_Z 3

double f1(double x, double y, double z){return x+y*x+z;}
double f2(double x, double y, double z){return x-y*z+2*z;}
double f3(double x, double y, double z){return 5*x+y+x*z;}

double h1(double x, double y, double z){
	return f1(x,y,z)-f1(SOLUTION_X,SOLUTION_Y,SOLUTION_Z);
}
double h2(double x, double y, double z){
	return f2(x,y,z)-f2(SOLUTION_X,SOLUTION_Y,SOLUTION_Z);
}
double h3(double x, double y, double z){
	return f3(x,y,z)-f3(SOLUTION_X,SOLUTION_Y,SOLUTION_Z);
}

int main(int argc, char const *argv[])
{
	pf3 fs[3];
	fs[0] = h1;
	fs[1] = h2;
	fs[2] = h3;
	double x, y, z, r;
	r = newton(fs, &x, &y, &z);
	printf("x = %f, y = %f, z = %f, error = %f\n\n", x, y, z, r);
	printf("fs[0](%f,%f,%f)=%f\n\n",x,y,z,fs[0](x,y,z));
	printf("fs[1](%f,%f,%f)=%f\n\n",x,y,z,fs[1](x,y,z));
	printf("fs[2](%f,%f,%f)=%f\n\n",x,y,z,fs[2](x,y,z));
	return 0;
}

double eval_f(pf3 f, double x, double y, double z){
	return f(x,y,z);
}

void eval_df3(pf3 *f, double x, double y, double z, double df[][3]){
	double dx, dy, dz; 
	dx = dy = dz = 1e-12;
	df[0][0] = (f[0](x+dx,y,z) - f[0](x-dx,y,z)) / (2*dx);
	df[0][1] = (f[0](x,y+dy,z) - f[0](x,y-dy,z)) / (2*dy);
	df[0][2] = (f[0](x,y,z+dz) - f[0](x,y,z-dz)) / (2*dz);

	df[1][0] = (f[1](x+dx,y,z) - f[1](x-dx,y,z)) / (2*dx);
	df[1][1] = (f[1](x,y+dy,z) - f[1](x,y-dy,z)) / (2*dy);
	df[1][2] = (f[1](x,y,z+dz) - f[1](x,y,z-dz)) / (2*dz);

	df[2][0] = (f[2](x+dx,y,z) - f[2](x-dx,y,z)) / (2*dx);
	df[2][1] = (f[2](x,y+dy,z) - f[2](x,y-dy,z)) / (2*dy);
	df[2][2] = (f[2](x,y,z+dz) - f[2](x,y,z-dz)) / (2*dz);
}

double det33(double a[][3]){
	return a[0][0]*a[1][1]*a[2][2]+
		   a[0][1]*a[1][2]*a[2][0]+
		   a[0][2]*a[1][0]*a[2][1]-
		   a[0][2]*a[1][1]*a[2][0]-
		   a[0][1]*a[1][0]*a[2][2]-
		   a[0][0]*a[1][2]*a[2][1];
}

double newton(pf3 *f, double *_x, double *_y, double *_z){
	double x, y, z, xn, yn, zn;
	xn = yn = zn = 1.0;

	double df[3][3];
	double df_inv[3][3];

	int iter = 0;
	double f0, f1, f2;
	double eps = 1e-12;
	double r = 1.0;

	while(iter<ITER_MAX && r>eps)
	{
		x = xn;
		y = yn;
		z = zn;
		eval_df3(f, x, y, z, df);
		double det = det33(df);
		
		//	#	*	*
		//	*	11	12
		//	*	21	22
		df_inv[0][0] = (df[1][1]*df[2][2] - df[2][1]*df[1][2])/det;
		//	*	01	02
		//	#	*	*
		//	*	21	22
		df_inv[0][1] = -(df[0][1]*df[2][2] - df[2][1]*df[0][2])/det;
		//	*	01	02
		//	*	11	12
		//	#	*	*
		df_inv[0][2] = (df[0][1]*df[1][2] - df[1][1]*df[0][2])/det;
		//	*	#	*
		//	10	*	12
		//	20	*	22
		df_inv[1][0] = -(df[1][0]*df[2][2] - df[2][0]*df[1][2])/det;
		//	00	*	02
		//	*	#	*
		//	20	*	22
		df_inv[1][1] = (df[0][0]*df[2][2] - df[2][0]*df[0][2])/det;
		//	00	*	02
		//	10	*	12
		//	*	#	*
		df_inv[1][2] = -(df[0][0]*df[1][2] - df[1][0]*df[0][2])/det;
		//	*	*	#
		//	10	11	*
		//	20	21	*
		df_inv[2][0] = (df[1][0]*df[2][1] - df[2][0]*df[1][1])/det;
		//	00	01	*
		//	*	*	#
		//	20	21	*
		df_inv[2][1] = -(df[0][0]*df[2][1] - df[2][0]*df[0][1])/det;
		//	00	01	*
		//	10	11	*
		//	*	*	#
		df_inv[2][2] = (df[0][0]*df[1][1] - df[1][0]*df[0][1])/det;

		f0 = f[0](x,y,z);
		f1 = f[1](x,y,z);
		f2 = f[2](x,y,z);

		xn = x - df_inv[0][0]*f0 - df_inv[0][1]*f1 - df_inv[0][2]*f2;
		yn = y - df_inv[1][0]*f0 - df_inv[1][1]*f1 - df_inv[1][2]*f2;
		zn = z - df_inv[2][0]*f0 - df_inv[2][1]*f1 - df_inv[2][2]*f2;

		f0 = f[0](xn,yn,zn);
		f1 = f[1](xn,yn,zn);
		f2 = f[2](xn,yn,zn);
		r = fabs(f0)+fabs(f1)+fabs(f2);
		printf("number of iterations = %d, error = %f\n\n", iter, r);
		iter++;
	}

	*_x = xn;
	*_y = yn;
	*_z = zn;

	return r;
}