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

#include "common.h"
#include "complex.h"
#include "matrix.h"

/**
 * matrix_t型を初期化します。
 * 
 * @param target 対象とする行列
 */
void matrix_init(matrix_t *target){
  target->values = NULL;
  target->rows = 0;
  target->columns = 0;
}

/**
 * 行列にメモリを割り当てます。
 * 
 * @param target 対象とする行列
 * @param rows 行数
 * @param columns 列数
 * @return エラーがおきた際には SUCCESS 以外の値が返ります
 */
error_code_t matrix_alloc(matrix_t *target, unsigned int rows, unsigned int columns){
  if(!(target->values = (double *)malloc(sizeof(double) * rows * columns))){
    return FAIL;
  }
  target->rows = rows;
  target->columns = columns;
  return SUCCESS;
}

/**
 * 行列の要素を0で初期化します。
 * 
 * @param target 対象とする行列
 * @return エラーがおきた際には SUCCESS 以外の値が返ります
 */
error_code_t matrix_zero(matrix_t *target){
  {
    // 埋めてね
    double *value_pointer = target->values + target->rows * target->columns;
    while(value_pointer > target->values){
      *(--value_pointer) = (double)0;
    }
  }
  return SUCCESS;
}

/**
 * 行列に割り当てたメモリを解放します。
 * 
 * @param target 行列
 * @return エラーがおきた際には SUCCESS 以外の値が返ります
 */
error_code_t matrix_free(matrix_t *target){
  if(target->values){
    free(target->values);
    target->values = NULL;
    target->rows = target->columns = 0;
  }
  return SUCCESS;
}

/**
 * 行列の成分を読み出し・書き込みする際に利用します。
 * (高速化の為にinline関数としています。)
 * 
 * @param target 行列
 * @param row 行番号(開始はC言語なので0〜、つまり1行目にアクセスする場合は0を指定)
 * @param column 列番号(行番号と同じく0〜) 
 * @return 要素へのポインタ
 */
inline double *matrix_value(matrix_t *target, unsigned int row, unsigned int column){
  return ((target->values) + (row * target->columns) + column);
}

/**
 * 行列を見やすい形で出力します。
 * 
 * @param target 行列
 * @return エラーがおきた際には SUCCESS 以外の値が返ります
 */
error_code_t matrix_print(matrix_t *target){
  {
    // 埋めてね
    unsigned int i, j;
    for(i = 0; i < target->rows; i++){
      for(j = 0; j < target->columns; j++){
        printf("(%i, %i) = %f\r\n", i, j, *matrix_value(target, i, j));
      }
    }
  }
  return SUCCESS;
}

/**
 * 行列が正方行列であるか調べます。
 * 
 * @return 正方行列である場合には TRUE、それ以外は FALSE
 */
BOOL matrix_is_square(matrix_t *target){
  {
    // 埋めてね
    if(target->rows != target->columns){
      return FALSE;
    }
  }
  return TRUE;
}

/**
 * 行列が対称行列であるか調べます。
 * 
 * @return 対称行列である場合には TRUE、それ以外は FALSE
 */
BOOL matrix_is_symmetric(matrix_t *target){
  if(!matrix_is_square(target)){return FALSE;}
  {
    // 埋めてね
    unsigned int i, j;
    for(i = 0; i < target->rows; i++){
      for(j = i + 1; j < target->columns; j++){
        if(*matrix_value(target, i, j) != *matrix_value(target, j, i)){
          return FALSE;
        }
      }
    }
  }
  return TRUE;
}

/**
 * 行列のコピーをします。
 * 
 * @param src コピー元
 * @param dist コピー先(matrix_allocでメモリが自動的に割り当てられます、使い終わったらmatrix_freeを忘れずに)
 * @return エラーがおきた際には SUCCESS 以外の値が返ります
 */
error_code_t matrix_copy(matrix_t *src, matrix_t *dist){
  if(matrix_alloc(dist, src->rows, src->columns)){return FAIL;}
  {
    // 埋めてね
    memcpy(dist->values, src->values, sizeof(double) * src->rows * src->columns);
  }
  return SUCCESS;
}

/**
 * 単位行列を生成します。
 * 
 * @param target 行列(matrix_allocでメモリが自動的に割り当てられます、使い終わったらmatrix_freeを忘れずに)
 * @param size 行数、すなわち、列数
 * @return エラーがおきた際には SUCCESS 以外の値が返ります
 */
error_code_t matrix_unit(matrix_t *target, unsigned int size){
  if(matrix_alloc(target, size, size)){return FAIL;}
  {
    // 埋めてね
    unsigned int i;
    matrix_zero(target);
    for(i = 0; i < size; i++){*matrix_value(target, i, i) = 1.;}
  }
  return SUCCESS;
}

/**
 * 行列の転置を行います。
 * 
 * @param target 行列(matrix_allocでメモリが自動的に割り当てられます、使い終わったらmatrix_freeを忘れずに)
 * @param result 結果の格納用
 * @return エラーがおきた際には SUCCESS 以外の値が返ります
 */
error_code_t matrix_transpose(matrix_t *target, matrix_t *result){
  if(matrix_alloc(result, target->columns, target->rows)){return FALSE;}
  {
    // 埋めてね
    unsigned int i, j;
    for(i = 0; i < target->rows; i++){
      for(j = 0; j < target->columns; j++){
        *matrix_value(result, j, i) = *matrix_value(target, i, j);
      }
    }
  }
  return SUCCESS;
}

/**
 * 行を入れ替えます。
 * 
 * @param target 行列
 * @param row1 行1
 * @param row2 行2
 * @return エラーがおきた際には SUCCESS 以外の値が返ります
 */
error_code_t matrix_swap_rows(matrix_t *target, unsigned int row1, unsigned int row2){
  if((row1 >= (target->rows)) || (row2 >= (target->rows))){return FAIL;}
  {
    double temp;
    unsigned int j;
    for(j = 0; j < target->columns; j++){
      temp = *matrix_value(target, row1, j);
      *matrix_value(target, row1, j) = *matrix_value(target, row2, j);
      *matrix_value(target, row2, j) = temp;
    }
  }
  return SUCCESS;
}

/**
 * 列を入れ替えます。
 * 
 * @param target 行列
 * @param column1 列1
 * @param column2 列2
 * @return エラーがおきた際には SUCCESS 以外の値が返ります
 */
error_code_t matrix_swap_columns(matrix_t *target, unsigned int column1, unsigned int column2){
  if((column1 >= (target->columns)) || (column2 >= (target->columns))){return FAIL;}
  {
    double temp;
    unsigned int i;
    for(i = 0; i < target->rows; i++){
      temp = *matrix_value(target, i, column1);
      *matrix_value(target, i, column1) = *matrix_value(target, i, column2);
      *matrix_value(target, i, column2) = temp;
    }
  }
  return SUCCESS;
}

/**
 * 逆行列を求めます。
 * 
 * @param target 行列(matrix_allocでメモリが自動的に割り当てられます、使い終わったらmatrix_freeを忘れずに)
 * @param result 結果の格納用
 * @return エラーがおきた際には SUCCESS 以外の値が返ります
 */
error_code_t matrix_inverse(matrix_t *target, matrix_t *result){
  if(!matrix_is_square(target)){return FAIL;}
  if(matrix_unit(result, target->rows)){return FAIL;}
  {
    // 埋めてね
    matrix_t left;
    matrix_copy(target, &left);
    unsigned int i, j, k;
    
    for(i = 0; i < target->rows; i++){
      if(*matrix_value(&left, i, i) == 0){ //(i, i)が存在するように並べ替え
        for(j = i+1; j <= (target->rows); j++){
          if(j == (target->rows)){return FAIL;}
          if(*matrix_value(&left, j, i) != 0){
            matrix_swap_rows(&left, j, i);
            matrix_swap_rows(result, j, i);
            break;
          }
        }
      }
      if(*matrix_value(&left, i, i) != 1){
        for(j = 0; j < (target->columns); j++){
          *matrix_value(result, i, j) /= *matrix_value(&left, i, i);
        }
        for(j = i+1; j < (target->columns); j++){
          *matrix_value(&left, i, j) /= *matrix_value(&left, i, i);
        }
        *matrix_value(&left, i, i) = 1;
      }
      for(k = 0; k < (target->rows); k++){
        if(k == i){continue;}
        if(*matrix_value(&left, k, i) != 0){
          for(j = 0; j < (target->columns); j++){
            *matrix_value(result, k, j) -= 
              (*matrix_value(result, i, j)) * (*matrix_value(&left, k, i));
          }
          for(j = i+1; j < (target->columns); j++){
            *matrix_value(&left, k, j) -= 
              (*matrix_value(&left, i, j)) * (*matrix_value(&left, k, i));
          }
          *matrix_value(&left, k, i) = 0;
        }
      }
    }
    
    matrix_free(&left);
  }
  return SUCCESS;
}

/**
 * 行列の和を求めます。 
 * 
 * @param target1 行列1
 * @param target2 行列2
 * @param result 結果の格納用(matrix_allocでメモリが自動的に割り当てられます、使い終わったらmatrix_freeを忘れずに)
 * @return エラーがおきた際には SUCCESS 以外の値が返ります
 */
error_code_t matrix_add(matrix_t *target1, matrix_t *target2, matrix_t *result){
  if((target1->rows != target2->rows) 
        || (target1->columns != target2->columns)){
    return FAIL;
  }
  if(matrix_alloc(result, target1->rows, target1->columns)){return FAIL;}
  {
    // 埋めてね
    unsigned int i, j;
    for(i = 0; i < result->rows; i++){
      for(j = 0; j < result->columns; j++){
        *matrix_value(result, i, j)
          = *matrix_value(target1, i, j)
            + (*matrix_value(target2, i, j));
      }
    }
  }
  return SUCCESS;
}

/**
 * 行列の差を求めます。
 * 
 * @param target1 行列1
 * @param target2 行列2
 * @param result 結果の格納用(matrix_allocでメモリが自動的に割り当てられます、使い終わったらmatrix_freeを忘れずに)
 * @return エラーがおきた際には SUCCESS 以外の値が返ります
 */
error_code_t matrix_sub(matrix_t *target1, matrix_t *target2, matrix_t *result){
  if((target1->rows != target2->rows) 
        || (target1->columns != target2->columns)){
    return FAIL;
  }
  if(matrix_alloc(result, target1->rows, target1->columns)){return FAIL;}
  {
    // 埋めてね
    unsigned int i, j;
    for(i = 0; i < result->rows; i++){
      for(j = 0; j < result->columns; j++){
        *matrix_value(result, i, j)
          = *matrix_value(target1, i, j)
            - (*matrix_value(target2, i, j));
      }
    }
  }
  return SUCCESS;
}

/**
 * 行列の積を求めます。
 * 
 * @param target1 行列1
 * @param target2 行列2
 * @param result 結果の格納用(matrix_allocでメモリが自動的に割り当てられます、使い終わったらmatrix_freeを忘れずに)
 * @return エラーがおきた際には SUCCESS 以外の値が返ります
 */
error_code_t matrix_multi(matrix_t *target1, matrix_t *target2, matrix_t *result){
  if(target1->columns != target2->rows){return FAIL;}
  if(matrix_alloc(result, target1->rows, target2->columns)){return FAIL;}
  {
    // 埋めてね
    unsigned int i, j, k;
    matrix_zero(result);
    for(i = 0; i < result->rows; i++){
      for(j = 0; j < result->columns; j++){
        for(k = 0; k < target1->columns; k++){
          *matrix_value(result, i, j)
            += *matrix_value(target1, i, k)
              * (*matrix_value(target2, k, j));
        }
      }
    }
  }
  return SUCCESS;
}

/**
 * 行列をスカラー倍します。
 * target *= scalar; なイメージ
 * 
 * @param target 行列(これがスカラー倍されます)
 * @param scalar かける値
 * @return エラーがおきた際には SUCCESS 以外の値が返ります
 */
error_code_t matrix_scalar_multi(matrix_t *target, double scalar){
  {
    // 埋めてね
    unsigned int i, j;
    for(i = 0; i < target->rows; i++){
      for(j = 0; j < target->rows; j++){
        *matrix_value(target, i, j) *= scalar;
      }
    }
  }
  return SUCCESS;
}

/**
 * 行列をスカラー割します。
 * target /= scalar; なイメージ
 * 
 * @param target 行列(これがスカラー割されます)
 * @param scalar 割る値
 * @return エラーがおきた際には SUCCESS 以外の値が返ります
 */
error_code_t matrix_scalar_div(matrix_t *target, double scalar){
  return matrix_scalar_multi(target, 1. / scalar);
}

/**
 * cmatrix_t(要素が複素数版)を初期化します。
 * 
 * @param target 対象とする行列
 */
void cmatrix_init(cmatrix_t *target){
  target->values = NULL;
  target->rows = 0;
  target->columns = 0;
}

/**
 * 行列(要素が複素数版)にメモリを割り当てます。
 * 
 * @param target 対象とする行列
 * @param rows 行数
 * @param columns 列数
 * @return エラーがおきた際には SUCCESS 以外の値が返ります
 */
error_code_t cmatrix_alloc(cmatrix_t *target, unsigned int rows, unsigned int columns){
  if(!(target->values = (complex_t *)malloc(sizeof(complex_t) * rows * columns))){
    return FAIL;
  }
  target->rows = rows;
  target->columns = columns;
  return SUCCESS;
}

/**
 * 行列の要素を0で初期化します。
 * 
 * @param target 対象とする行列
 * @return エラーがおきた際には SUCCESS 以外の値が返ります
 */
error_code_t cmatrix_zero(cmatrix_t *target){
  {
    // 埋めてね
    complex_t *value_pointer = target->values + (target->rows * target->columns);
    while(value_pointer > target->values){
      value_pointer--;
      value_pointer->real = 0;
      value_pointer->imaginary = 0;
    }
  }
  return SUCCESS;
}

/**
 * 行列(要素が複素数版)に割り当てたメモリを解放します。
 * 
 * @param target 行列
 * @return エラーがおきた際には SUCCESS 以外の値が返ります
 */
error_code_t cmatrix_free(cmatrix_t *target){
  if(target->values){
    free(target->values);
    target->values = NULL;
    target->rows = target->columns = 0;
  }
  return SUCCESS;
}

/**
 * 行列(要素が複素数版)の成分を読み出し・書き込みする際に利用します。
 * (高速化の為にinline関数としています。)
 * 
 * @param target 行列
 * @param row 行番号(開始はC言語なので0〜、つまり1行目にアクセスする場合は0を指定)
 * @param column 列番号(行番号と同じく0〜) 
 * @return 要素へのポインタ
 */
inline complex_t *cmatrix_value(cmatrix_t *target, unsigned int row, unsigned int column){
  return ((target->values) + (row * target->columns) + column);
}

/**
 * 行列を見やすい形で出力します。
 * 
 * @param target 行列
 * @return エラーがおきた際には SUCCESS 以外の値が返ります
 */
error_code_t cmatrix_print(cmatrix_t *target){
  {
    // 埋めてね
    unsigned int i, j;
    for(i = 0; i < target->rows; i++){
      for(j = 0; j < target->columns; j++){
        printf("(%i, %i) = ", i, j);
        complex_print(*cmatrix_value(target, i, j));
        printf("\r\n");
      }
    }
  }
  return SUCCESS;
}

/**
 * 2行2列小行列の固有値を求めます。
 * 
 * @param target 行列
 * @param row 小行列の左上の行番号
 * @param column 小行列の左上の列番号
 * @param eigen_values 固有値(NULLの場合は内部でmallocによって自動的にメモリが確保されます、freeを忘れずに)
 * @return エラーがおきた際には SUCCESS 以外の値が返ります
 */
error_code_t matrix_eigen22(matrix_t *target, unsigned int row, unsigned int column, complex_t *eigen_values){
  if(row + 1 >= target->rows || column + 1 >= target->columns){return FAIL;}
  if(!eigen_values){
    if(eigen_values = (complex_t *)malloc(sizeof(complex_t) * 2)){
      return FAIL;
    }
  }
  {
    double a = *matrix_value(target, row, column),
            b = *matrix_value(target, row, column + 1),
            c = *matrix_value(target, row + 1, column),
            d = *matrix_value(target, row + 1, column + 1);
    double root = pow((a - d), 2) + b * c * 4;
    if(root >= 0){
      root = sqrt(root);
      eigen_values[0].real = (a + d + root) / 2;
      eigen_values[0].imaginary = 0;
      eigen_values[1].real = (a + d - root) / 2;
      eigen_values[1].imaginary = 0;
    }else{
      root = sqrt(root * -1);
      eigen_values[0].real = (a + d) / 2;
      eigen_values[0].imaginary = root / 2;
      eigen_values[1].real = (a + d) / 2;
      eigen_values[1].imaginary = -root / 2;
    }
  }
  return SUCCESS;
}

/**
 * ピボットを指定して、加算します。
 * イメージとしては (targetの部分行列) += mergee;
 * 
 * @param target 行列
 * @param row 行インデックス
 * @param column 列インデックス
 * @param mergee 足す行列
 * @return エラーがおきた際には SUCCESS 以外の値が返ります
 */
error_code_t matrix_pivot_merge(
      matrix_t *target, 
      unsigned int row, unsigned int column, 
      matrix_t *mergee){
  unsigned int i, j;
  for(i = 0; i < mergee->rows; i++){
  	if(row + i < 0){continue;}
    else if(row + i >= target->rows){break;}
    for(j = 0; j < mergee->columns; j++){
    	if(column + j < 0){continue;}
      else if(column + j >= target->columns){break;}
      *matrix_value(target, row + i, column + j) += (*matrix_value(mergee, i, j));
    }
  }
  return SUCCESS;
}

/**
 * ハウスホルダー変換をしてヘッセンベルク行列を得ます。
 * 
 * @param target 対象となる行列
 * @param result 求めたヘッセンベルク行列を格納するためのポインタ
 * @param transform 変換に用いた行列の積を格納するポインタ
 * @return エラーがおきた際には SUCCESS 以外の値が返ります
 */
error_code_t matrix_hessenberg(
    matrix_t *target, 
    matrix_t *result, 
    matrix_t *transform){
  
  unsigned int i, j;

	if(!matrix_is_square(target)){return FAIL;}

	matrix_copy(target, result);
  matrix_unit(transform, target->rows);
	for(j = 0; j < target->columns - 2; j++){
    double t, s;
		t = 0;
		for(i = j + 1; i < target->rows; i++){
			t += pow(*matrix_value(result, i, j), 2);
		}
    s = sqrt(t);
    if(*matrix_value(result, j + 1, j) < 0){s *= -1;}
    
    {
      matrix_t omega, omega_trans, omega_multi_omega_trans;
      matrix_t p, p_result, p_result_p;
      matrix_t transform_p;

      matrix_alloc(&omega, target->rows - (j+1), 1);
		  for(i = 0; i < omega.rows; i++){
			  *matrix_value(&omega, i, 0) = *matrix_value(result, j+i+1, j);	
		  }
		  *matrix_value(&omega, 0, 0) += s;
      
      matrix_transpose(&omega, &omega_trans);
      matrix_multi(&omega, &omega_trans, &omega_multi_omega_trans);

      matrix_unit(&p, target->rows);
      matrix_scalar_div(
          &omega_multi_omega_trans, 
          -(t + (*matrix_value(result, j + 1, j)) * s)
        );
      matrix_pivot_merge(
          &p, 
          j+1, 
          j+1,
          &omega_multi_omega_trans
        );
      
      matrix_multi(&p, result, &p_result);
      matrix_multi(&p_result, &p, &p_result_p);

      matrix_free(result);
      matrix_copy(&p_result_p, result);

		  matrix_multi(transform, &p, &transform_p);
      matrix_free(transform);
      matrix_copy(&transform_p, transform);
      
      matrix_free(&omega);
      matrix_free(&omega_trans);
      matrix_free(&omega_multi_omega_trans);
      matrix_free(&p);
      matrix_free(&p_result);
      matrix_free(&p_result_p);
      matrix_free(&transform_p);
    }
	}
	
	//ゼロ処理
  {
	  BOOL sym = matrix_is_symmetric(target);
	  for(j = 0; j < target->columns - 2; j++){
		  for(i = j + 2; i < target->rows; i++){
			  *matrix_value(result, i, j) = 0;
			  if(sym){*matrix_value(result, j, i) = 0;}
		  }	
	  }
  }
	
	return SUCCESS;	
}

/**
 * 固有値、固有ベクトルを求めます。
 *
 * @param target 行列
 * @param eigen_values 結果の格納用(固有値)
 * @param eigne_vectors 結果の格納用(固有ベクトル)
 * @return エラーがおきた際には SUCCESS 以外の値が返ります
 */
error_code_t matrix_eigen(
    matrix_t *target, 
    complex_t *eigen_values[], 
    cmatrix_t *eigen_vectors[]){
  
  if(!matrix_is_square(target)){return FAIL;}
  
  //ダブルQR法
  /* <手順>
   * ハウスホルダー法を適用して、上ヘッセンベルク行列に置換後、
   * ダブルQR法を適用。
   * 結果、固有値が得られるので、固有ベクトルを計算。
   */
   
  // 結果格納用メモリの確保
  {
    *eigen_values = (complex_t *)malloc(sizeof(complex_t) * target->rows);
    *eigen_vectors = (cmatrix_t *)malloc(sizeof(cmatrix_t) * target->rows);
  }
  
  // 固有値の計算
  {
    // 変数の宣言
    double mu_sum, mu_multi;
    complex_t p1, p2;
    int m;
    BOOL first;
    matrix_t transform, a, a_backup;
    
    // 初期化
    {
      mu_sum = 0; mu_multi = 0;
      complex_zero(&p1);
      complex_zero(&p2);
      m = target->rows;
      first = TRUE;
      
      matrix_hessenberg(target, &a, &transform);
      matrix_copy(&a, &a_backup);
    }
  
    while(TRUE){
    
      //m = 1 or m = 2
      if(m == 1){
        eigen_values[0]->real = *matrix_value(&a, 0, 0);
        eigen_values[0]->imaginary = 0;
        break;
      }else if(m == 2){
        matrix_eigen22(&a, 0, 0, eigen_values[0]);
        break;
      }
      
      //μ、μ*の更新(4.143)
      {
        complex_t p_new[2];
        matrix_eigen22(&a, m-2, m-2, p_new);
        if(first ? (first = FALSE) : TRUE){
          if(complex_abs(complex_sub(p_new[0], p1)) > complex_abs(p_new[0]) / 2){
            if(complex_abs(complex_sub(p_new[1], p2)) > complex_abs(p_new[1]) / 2){
              mu_sum = complex_add(p1, p2).real;
              mu_multi = complex_multi(p1, p2).real;
            }else{
              mu_sum = p_new[1].real * 2;
              mu_multi = pow(p_new[1].real, 2);
            }
          }else{
            if(complex_abs(complex_sub(p_new[1], p2)) > complex_abs(p_new[1]) / 2){
              mu_sum = p_new[0].real * 2;
              mu_multi = pow(p_new[0].real, 2);
            }else{
              mu_sum = complex_add(p_new[0], p_new[1]).real;
              mu_multi = complex_multi(p_new[0], p_new[1]).real;
            }
          }
        }
        complex_copy(&p_new[0], &p1), complex_copy(&p_new[1], &p2);
      }
    
      //ハウスホルダー変換を繰り返す
      {
        double b1, b2, b3, r;
        int i;
        for(i = 0; i < m - 1; i++){
          if(i == 0){
            b1 = pow(*matrix_value(&a, 0, 0), 2) 
              - mu_sum * (*matrix_value(&a, 0, 0)) 
              + mu_multi 
              + *matrix_value(&a, 0, 1) * *matrix_value(&a, 1, 0);
            b2 = *matrix_value(&a, 1, 0) 
              * ((*matrix_value(&a, 0, 0)) + (*matrix_value(&a, 1, 1)) - mu_sum);
            b3 = *matrix_value(&a, 2, 1) * (*matrix_value(&a, 1, 0));
          }else{
            b1 = *matrix_value(&a, i, i - 1);
            b2 = *matrix_value(&a, i + 1, i - 1);
            b3 = (i == m - 2 ? 0 : *matrix_value(&a, i + 2, i - 1));
          }
          /*printf("m: %d", m);
          printf(" b1: %f", b1);
          printf(" b2: %f", b2);
          printf(" b3: %f\n", b3);*/
      
          r = sqrt(pow(b1, 2) + pow(b2, 2) + pow(b3, 2));
        
          {
            matrix_t omega, omega_trans;
            matrix_t omega_multi_omega_trans, omega_trans_multi_omega;
            matrix_t p, pa, pap;

            matrix_alloc(&omega, 3, 1);
            {
              *matrix_value(&omega, 0, 0) = b1 + r * (b1 >= 0 ? 1 : -1);
              *matrix_value(&omega, 1, 0) = b2;
              *matrix_value(&omega, 2, 0) = b3;
            }
            matrix_transpose(&omega, &omega_trans);

            matrix_unit(&p, target->rows);
            matrix_multi(&omega, &omega_trans, &omega_multi_omega_trans);
            matrix_multi(&omega_trans, &omega, &omega_trans_multi_omega);
            matrix_scalar_multi(
                &omega_multi_omega_trans, 
                -2.0 / *matrix_value(&omega_trans_multi_omega, 0, 0));
            matrix_pivot_merge(&p, i, i, &omega_multi_omega_trans);

            matrix_multi(&p, &a, &pa);
            matrix_multi(&pa, &p, &pap);

            matrix_free(&a);
            matrix_copy(&pap, &a);

            matrix_free(&omega);
            matrix_free(&omega_trans);
            matrix_free(&omega_multi_omega_trans);
            matrix_free(&omega_trans_multi_omega);
            matrix_free(&p);
            matrix_free(&pa);
            matrix_free(&pap);
          }
        }
      }
      
      //収束判定
      {
        double epsilon = EIGEN_VALUE_THRESHOLD
          * (1. 
              + fabs(
                  fabs(*matrix_value(&a, m-2, m-2)) < fabs(*matrix_value(&a, m-1, m-1)) 
                    ? *matrix_value(&a, m-2, m-2) 
                    : *matrix_value(&a, m-1, m-1)
              ));
        //printf("eps: %0.20f\n", epsilon);
        if(fabs(*matrix_value(&a, m-1, m-2)) < epsilon){
          m--;
          (*eigen_values)[m].real = *matrix_value(&a, m, m);
          (*eigen_values)[m].imaginary = 0;
        }else if(fabs(*matrix_value(&a, m-2, m-3)) < epsilon){
          m -= 2;
          matrix_eigen22(&a, m, m, &((*eigen_values)[m]));
        }
      }
    }

    //固有ベクトルの計算
    {
      unsigned int i, j, k;
      cmatrix_t x;

      matrix_free(&a);
      matrix_copy(&a_backup, &a);
      cmatrix_alloc(&x, target->rows, target->rows); //固有ベクトル
      cmatrix_zero(&x);
      
      for(j = 0; j < target->rows; j++){
        int n = target->rows;
        for(i = 0; i < j; i++){
          if(complex_is_equal((*eigen_values)[j], (*eigen_values)[i])){--n;}
        }
        cmatrix_value(&x, --n, j)->real = 1;
        cmatrix_value(&x, n, j)->imaginary = 0;
        while(n-- > 0){
          *cmatrix_value(&x, n, j) = complex_multi(
                *cmatrix_value(&x, n+1, j), 
                complex_scalar_sub((*eigen_values)[j], *matrix_value(&a, n+1, n+1))
              );
          for(i = n+2; i < target->rows; i++){
            cmatrix_value(&x, n, j)->real
              -= cmatrix_value(&x, i, j)->real * (*matrix_value(&a, n+1, i));
            cmatrix_value(&x, n, j)->imaginary
              -= cmatrix_value(&x, i, j)->imaginary * (*matrix_value(&a, n+1, i));
          }
          *cmatrix_value(&x, n, j) 
            = complex_scalar_div(
                  *cmatrix_value(&x, n, j), 
                  *matrix_value(&a, n+1, n)
                );
        }
      }

      //結果の格納
      for(j = 0; j < x.columns; j++){
        cmatrix_alloc(&((*eigen_vectors)[j]), target->rows, 1);
        cmatrix_zero(&((*eigen_vectors)[j]));
        for(i = 0; i < x.rows; i++){
          for(k = 0; k < transform.columns; k++){
            cmatrix_value(&((*eigen_vectors)[j]), i, 0)->real
              += cmatrix_value(&x, k, j)->real
                  * (*matrix_value(&transform, i, k));
            cmatrix_value(&((*eigen_vectors)[j]), i, 0)->imaginary
              += cmatrix_value(&x, k, j)->imaginary
                  * (*matrix_value(&transform, i, k));
          }
        }
        
        //正規化
        {
          double norm = 0;
          for(i = 0; i < target->rows; i++){
            norm += complex_abs2(*cmatrix_value(&((*eigen_vectors)[j]), i, 0));
          }
          norm = sqrt(norm);
          for(i = 0; i < target->rows; i++){
            cmatrix_value(&((*eigen_vectors)[j]), i, 0)->real /= norm;
            cmatrix_value(&((*eigen_vectors)[j]), i, 0)->imaginary /= norm;
          }
        }
      }

      cmatrix_free(&x);
    }
    
    // メモリの解放
    {
      matrix_free(&transform);
      matrix_free(&a);
      matrix_free(&a_backup);
    }
  }
  
  return SUCCESS;
}