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

#define SIZE 4        // 行列のサイズ
#define MAX_ITER 1000 // 最大反復回数
#define TOL 0.0000001      // 収束判定の許容誤差


// QR分解を行うためのヘルパー関数（グラムシュミット直交化を使用）
void qr_decompose(double A[SIZE][SIZE], double Q[SIZE][SIZE], double R[SIZE][SIZE]) {
    int i, j, k;
    double norm;

    // 初期化
    for (i = 0; i < SIZE; i++) {
        for (j = 0; j < SIZE; j++) {
            Q[i][j] = 0.0;
            R[i][j] = 0.0;
        }
    }

    // グラムシュミット直交化
    for (k = 0; k < SIZE; k++) {
        for (i = 0; i < SIZE; i++) {
            Q[i][k] = A[i][k];
        }
        for (j = 0; j < k; j++) {
            R[j][k] = 0.0;
            for (i = 0; i < SIZE; i++) {
                R[j][k] += Q[i][j] * A[i][k];
            }
            for (i = 0; i < SIZE; i++) {
                Q[i][k] -= R[j][k] * Q[i][j];
            }
        }
        norm = 0.0;
        for (i = 0; i < SIZE; i++) {
            norm += Q[i][k] * Q[i][k];
        }
        R[k][k] = sqrt(norm);
        for (i = 0; i < SIZE; i++) {
            Q[i][k] /= R[k][k];
        }
    }
}

// シフトなしQR法で固有値を計算
void qr_algorithm(double A[SIZE][SIZE], double eigenvalues[SIZE]) {
    double Q[SIZE][SIZE], R[SIZE][SIZE], A_next[SIZE][SIZE];
    int i, j, k, l;

    for (k = 0; k < MAX_ITER; k++) {
        qr_decompose(A, Q, R);

        // 新しい行列 A = R * Q を計算
        for (i = 0; i < SIZE; i++) {
            for (j = 0; j < SIZE; j++) {
                A_next[i][j] = 0.0;
                for (l = 0; l < SIZE; l++) {
                    A_next[i][j] += R[i][l] * Q[l][j];
                }
            }
        }

        // 収束判定（非対角成分が小さいか確認）
        double diff_norm = 0.0;
for (i = 0; i < SIZE; i++) {
    for (j = 0; j < SIZE; j++) {
        if (i != j) {
            diff_norm += A_next[i][j] * A_next[i][j];
        }
    }
}
if (sqrt(diff_norm) < TOL) break;

        // 更新
        for (i = 0; i < SIZE; i++) {
            for (j = 0; j < SIZE; j++) {
                A[i][j] = A_next[i][j];
            }
        }
    }

    // 固有値を対角成分として取得
    for (i = 0; i < SIZE; i++) {
        eigenvalues[i] = A[i][i];
    }
}

   void solve_linear_system(double A[SIZE][SIZE], double b[SIZE], double x[SIZE]) {
    double augmented[SIZE][SIZE + 1];
    int i, j, k;

    // 拡大係数行列の作成
    for (i = 0; i < SIZE; i++) {
        for (j = 0; j < SIZE; j++) {
            augmented[i][j] = A[i][j];
        }
        augmented[i][SIZE] = b[i]; // 最右列に b を追加
    }

    // 前進消去（部分ピボット選択付き）
    for (k = 0; k < SIZE; k++) {
        // ピボット選択
        int max_row = k;
        for (i = k + 1; i < SIZE; i++) {
            if (fabs(augmented[i][k]) > fabs(augmented[max_row][k])) {
                max_row = i;
            }
        }

        // 行交換（必要な場合）
        if (max_row != k) {
            for (j = 0; j <= SIZE; j++) {
                double temp = augmented[k][j];
                augmented[k][j] = augmented[max_row][j];
                augmented[max_row][j] = temp;
            }
        }

        // ピボット列を基準に前進消去
        for (i = k + 1; i < SIZE; i++) {
            double factor = augmented[i][k] / augmented[k][k];
            for (j = k; j <= SIZE; j++) {
                augmented[i][j] -= factor * augmented[k][j];
            }
        }
    }

    // 後退代入
    for (i = SIZE - 1; i >= 0; i--) {
        x[i] = augmented[i][SIZE]; // 拡大係数行列の最後の列が定数項
        for (j = i + 1; j < SIZE; j++) {
            x[i] -= augmented[i][j] * x[j];
        }
        x[i] /= augmented[i][i];
    }
}


void inverse_iteration(double A[SIZE][SIZE], double mu, double eigenvector[SIZE]) {
    double I[SIZE][SIZE], shifted_A[SIZE][SIZE], y[SIZE], x[SIZE];
    int i, j, k;

    // 単位行列 I の作成
    for (i = 0; i < SIZE; i++) {
        for (j = 0; j < SIZE; j++) {
            I[i][j] = (i == j) ? 1.0 : 0.0;
        }
    }

    // 初期ベクトルをランダムに設定
  
    for (i = 0; i < SIZE; i++) {
          srand((unsigned int)time(NULL)); 
        x[i] = (double)rand() / RAND_MAX; // 0.0～1.0の範囲で初期化
    }

    // (A - mu * I) の計算
    for (i = 0; i < SIZE; i++) {
        for (j = 0; j < SIZE; j++) {
            shifted_A[i][j] = A[i][j] - mu * I[i][j];
        }
    }

    // 反復計算
    for (k = 0; k < MAX_ITER; k++) {
        // (A - mu * I)y = x を解く
        solve_linear_system(shifted_A, x, y);

        // 正規化
        double norm = 0.0;
        for (i = 0; i < SIZE; i++) {
            norm += y[i] * y[i];
        }
        norm = sqrt(norm);
        for (i = 0; i < SIZE; i++) {
            y[i] /= norm; // 正規化
        }

        // 収束判定
        double diff = 0.0;
        for (i = 0; i < SIZE; i++) {
            diff += fabs(y[i] - x[i]);
        }
        if (diff < TOL) {
            break;
        }

        // x を更新
        for (i = 0; i < SIZE; i++) {
            x[i] = y[i];
        }
    }

    // 固有ベクトルを返す
    for (i = 0; i < SIZE; i++) {
        eigenvector[i] = y[i];
    }
}

// メイン関数
int main() {
    double A[SIZE][SIZE] = {
        {16, -1, 1, 2},
        {2, 12, 1, -1},
        {1, 3, 24, 2},
        {4, -2, 1, 20}
    };
    double eigenvalues[SIZE], eigenvector[SIZE];

    printf("初期行列:\n");
    for (int i = 0; i < SIZE; i++) {
        for (int j = 0; j < SIZE; j++) {
            printf("%8.4f ", A[i][j]);
        }
        printf("\n");
    }

    // シフトなしQR法で固有値を計算
    qr_algorithm(A, eigenvalues);

    printf("\n固有値:\n");
    for (int i = 0; i < SIZE; i++) {
        printf("lambda[%d] = %8.4f\n", i, eigenvalues[i]);
    }

   
    return 0;
}