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

#define EPS pow(10.0, -8.0)
#define KMAX 100
#define N 3

void newton2(double x, double y, double z);
double *dvector(int i, int j);
void free_dvector(double *a, int i);
double **dmatrix(int nr1, int nr2, int nl1, int nl2);
void free_dmatrix(double **a, int nr1, int nr2, int nl1, int nl2);
double f(double x, double y, double z);
double g(double x, double y, double z);
double h(double x, double y, double z);
double f_x(double x, double y, double z);
double f_y(double x, double y, double z);
double f_z(double x, double y, double z);
double g_x(double x, double y, double z);
double g_y(double x, double y, double z);
double g_z(double x, double y, double z);
double h_x(double x, double y, double z);
double h_y(double x, double y, double z);
double h_z(double x, double y, double z);
double *gauss(double **a, double *b);
double vector_norm1(double *a, int m, int n);

int main(void)
{
  double x, y, z;
  
  printf("初期値 x0, y0, z0 を入力してください : ");
  scanf("%lf %lf %lf", &x, &y, &z);

  newton2(x, y, z);
  
  return 0;
}

void newton2(double x, double y, double z)
{
  int i, k = 0;
  double *xk, *d, **J;
  
  d = dvector(1, N);
  xk = dvector(1, N);
  J = dmatrix(1, N, 1, N);

  xk[1] = x; xk[2] = y; xk[3] = z;
  
  do {
    /* 右辺ベクトル */
    d[1] = -f(xk[1], xk[2], xk[3]);
    d[2] = -g(xk[1], xk[2], xk[3]);
    d[3] = -h(xk[1], xk[2], xk[3]);
    
    /* ヤコビ行列 */
    J[1][1] = f_x(xk[1], xk[2], xk[3]);
    J[1][2] = f_y(xk[1], xk[2], xk[3]);
    J[1][3] = f_z(xk[1], xk[2], xk[3]);
    J[2][1] = g_x(xk[1], xk[2], xk[3]);
    J[2][2] = g_y(xk[1], xk[2], xk[3]);
    J[2][3] = g_z(xk[1], xk[2], xk[3]);
    J[3][1] = h_x(xk[1], xk[2], xk[3]);
    J[3][2] = h_y(xk[1], xk[2], xk[3]);
    J[3][3] = h_z(xk[1], xk[2], xk[3]);
    
    /* ガウス法 */
    d = gauss(J, d);
    
    for (i = 1; i <= N; i++)
      xk[i] += d[i];
    k++;
  } while (vector_norm1(d, 1, N) > EPS && k < KMAX);
  
  if (k == KMAX)
    puts("解が見つかりませんでした。");
  else
    printf("解は x = %f, y = %f, z = %f\n", xk[1], xk[2], xk[3]);
  
  free_dmatrix(J, 1, N, 1, N);
  free_dvector(d, 1);
  free_dvector(xk, 1);
}

double *dvector(int i, int j)
{
  double *a;
  
  if ((a = (double *)malloc(((j - i + 1) * sizeof(double)))) == NULL) {
    puts("メモリが確保できません (from dvector)");
    exit(1);
  }
  
  return a - i; /* この動作はi > 0 で未定義な気がするが */
}

void free_dvector(double *a, int i)
{
  free((void *)(a + i));
}

double **dmatrix(int nr1, int nr2, int nl1, int nl2)
{
  int i, nrow, ncol;
  double **a;
  
  nrow = nr2 - nr1 + 1;
  ncol = nl2 - nl1 + 1;
  
  if ((a = (double **)malloc(nrow * sizeof(double *))) == NULL) {
    puts("メモリが確保できません (from dmatrix 行)");
    exit(1);
  }
  
  a -= nr1;
  
  for (i = nr1; i <= nr2; i++)
    if ((a[i] = (double *)malloc(ncol * sizeof(double))) == NULL) {
      puts("メモリが確保できません (from dmatrix 列)");
      exit(1);
    }
  for (i = nr1; i <= nr2; i++)
    a[i] -= nl1;
  
  return a;
}

void free_dmatrix(double **a, int nr1, int nr2, int nl1, int nl2)
{
  int i;
  
  for (i = nr1; i <= nr2; i++)
    free((void *)(a[i] + nl1));
  free((void *)(a + nr1));
}

/* -y＾2 * z＾2 - y＾2 + 24*y*z - z＾2 -13 = 0 */
double f(double x, double y, double z)
{
  return -y * y * z * z - y * y + 24.0 * y * z - z * z - 13.0;
}

/* -x＾2 * z＾2 - x＾2 + 24*x*z - z＾2 -13 = 0 */
double g(double x, double y, double z)
{
  return -x * x * z * z - x * x + 24.0 * x * z - z * z - 13.0;
}

/* -x＾2 * y＾2 - x＾2 + 24*x*y - y＾2 -13 = 0 */
double h(double x, double y, double z)
{
  return -x * x * y * y - x * x + 24.0 * x * y - y * y - 13.0;
}

/* ここから偏微分 */
double f_x(double x, double y, double z)
{
  return 0.0;
}

double f_y(double x, double y, double z)
{
  return -2.0 * y * z * z - 2.0 * y + 24.0 * z;
}

double f_z(double x, double y, double z)
{
  return -2.0 * z * y * y + 24.0 * y - 2.0 * z;
}

double g_x(double x, double y, double z)
{
  return -2.0 * x * z * z - 2.0 * x + 24.0 * z;
}

double g_y(double x, double y, double z)
{
  return 0.0;
}

double g_z(double x, double y, double z)
{
  return -2.0 * z * x * x + 24.0 * x - 2.0 * z;
}

double h_x(double x, double y, double z)
{
  return -2.0 * x * y * y - 2.0 * x + 24.0 * y;
}

double h_y(double x, double y, double z)
{
  return -2.0 * y * x * x + 24.0 * x - 2.0 * y;
}

double h_z(double x, double y, double z)
{
  return 0.0;
}

/* ガウスの消去法(部分ピボット選択) */
double *gauss(double **a, double *b)
{
  int i, j, k, ip;
  double alpha, tmp;
  double amax, eps = pow(2.0, -50.0);
  
  for (k = 1; k <= N - 1; k++) {
    amax = fabs(a[k][k]); ip = k;
    for (i = k + 1; i <= N; i++)
      if (fabs(a[i][k]) > amax) {
        amax = fabs(a[i][k]);
        ip = i;
      }
  
    if (amax < eps)
      puts("行列は正則ではありません。");
    
    if (ip != k) {
      for (j = k; j <= N; j++)
        tmp = a[k][j], a[k][j] = a[ip][j], a[ip][j] = tmp;
      tmp = b[k], b[k] = b[ip], b[ip] = tmp;
    }
    
    for (i = k + 1; i <= N; i++) {
      alpha = -a[i][k] / a[k][k];
      for (j = k + 1; j <= N; j++)
        a[i][j] += alpha * a[k][j];
      b[i] += alpha * b[k];
    }
  }
    
  b[N] /= a[N][N];
    
  for (k = N - 1; k >= 1; k--) {
    tmp = b[k];
    for (j = k + 1; j <= N; j++)
      tmp -= a[k][j] * b[j];
    b[k] = tmp / a[k][k];
  }
    
  return b;
}

/* 1-ノルム */
double vector_norm1(double *a, int m, int n)
{
  int i;
  double norm = 0.0;
  
  for (i = m; i <= n; i++)
    norm += fabs(a[i]);
  
  return norm;
}