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

#define N 4             // Matrix dimension
#define MAX_ITER 100    // Maximum number of sweeps
#define EPSILON 1e-6    // 収束判定条件 (Convergence threshold)

// Function prototypes
void jacobi_method(double A[N][N], double eigenvalues[N], double eigenvectors[N][N]);
void verify_results(double orig_A[N][N], double eigenvalues[N], double eigenvectors[N][N]);

int main() {
    // 1. Define the matrix from image_4.png
    double A[N][N] = {
        {5.0, 4.0, 1.0, 1.0},
        {4.0, 5.0, 1.0, 1.0},
        {1.0, 1.0, 4.0, 2.0},
        {1.0, 1.0, 2.0, 4.0}
    };

    // Keep a copy of the original matrix for validation later
    double orig_A[N][N];
    for (int i = 0; i < N; i++) {
        for (int j = 0; j < N; j++) {
            orig_A[i][j] = A[i][j];
        }
    }

    double eigenvalues[N];
    double eigenvectors[N][N];

    // 2. Run Jacobi Method (Prints detailed iteration step history)
    jacobi_method(A, eigenvalues, eigenvectors);

    // 3. Print Final Results
    printf("\n--- Final Results ---\n");
    for (int i = 0; i < N; i++) {
        printf("Eigenvalue %d (固有値): %9.6f\n", i + 1, eigenvalues[i]);
        printf("Eigenvector %d (固有ベクトル): [ ", i + 1);
        for (int j = 0; j < N; j++) {
            printf("%9.6f ", eigenvectors[j][i]); // Columns are eigenvectors
        }
        printf("]\n\n");
    }

    // 4. Verification Check (固有値・固有ベクトルの確認)
    verify_results(orig_A, eigenvalues, eigenvectors);

    return 0;
}

void jacobi_method(double A[N][N], double eigenvalues[N], double eigenvectors[N][N]) {
    // Initialize eigenvector matrix as identity matrix
    for (int i = 0; i < N; i++) {
        for (int j = 0; j < N; j++) {
            eigenvectors[i][j] = (i == j) ? 1.0 : 0.0;
        }
    }

    printf("--- Iteration Process (収束状況) ---\n");

    int step = 0;
    for (int iter = 1; iter <= MAX_ITER; iter++) {
        double max_off_diag = 0.0;
        int p = 0, q = 0;

        // Find the largest off-diagonal element |A[p][q]|
        for (int i = 0; i < N; i++) {
            for (int j = i + 1; j < N; j++) {
                if (fabs(A[i][j]) > max_off_diag) {
                    max_off_diag = fabs(A[i][j]);
                    p = i;
                    q = j;
                }
            }
        }

        // Check for convergence
        if (max_off_diag < EPSILON) {
            printf("\n[Convergence Achieved] All remaining off-diagonal elements are below the threshold.\n");
            break;
        }

        step++;
        // Print detailed iteration step showing exactly which elements are shrinking
        printf("Step %2d | Eliminating A[%d][%d] = %9.6f | Current Diagonals: [%.4f, %.4f, %.4f, %.4f]\n", 
               step, p, q, A[p][q], A[0][0], A[1][1], A[2][2], A[3][3]);

        // --- Pure Algebraic Calculation of cos(theta) and sin(theta) ---
        double app = A[p][p];
        double aqq = A[q][q];
        double apq = A[p][q];
        
        double c, s; 

        if (fabs(apq) < 1e-12) {
            c = 1.0;
            s = 0.0;
        } else {
            double phi = 0.5 * (app - aqq) / apq;
            double t;
            if (phi >= 0.0) {
                t = 1.0 / (phi + sqrt(phi * phi + 1.0));
            } else {
                t = -1.0 / (-phi + sqrt(phi * phi + 1.0));
            }
            c = 1.0 / sqrt(1.0 + t * t);
            s = t * c;
        }

        // Update elements of matrix A
        for (int i = 0; i < N; i++) {
            if (i != p && i != q) {
                double a_ip = A[i][p];
                double a_iq = A[i][q];
                A[i][p] = A[p][i] = c * a_ip + s * a_iq;
                A[i][q] = A[q][i] = -s * a_ip + c * a_iq;
            }
        }
        A[p][p] = c * c * app + 2.0 * s * c * apq + s * s * aqq;
        A[q][q] = s * s * app - 2.0 * s * c * apq + c * c * aqq;
        A[p][q] = A[q][p] = 0.0; 

        // Update accumulated eigenvector matrix P
        for (int i = 0; i < N; i++) {
            double v_ip = eigenvectors[i][p];
            double v_iq = eigenvectors[i][q];
            eigenvectors[i][p] = c * v_ip + s * v_iq;
            eigenvectors[i][q] = -s * v_ip + c * v_iq;
        }
    }

    // Extract eigenvalues from the diagonal elements
    for (int i = 0; i < N; i++) {
        eigenvalues[i] = A[i][i];
    }
}

// Verification function: Explicitly checks the defining equation: Ax - lambda * x = 0
void verify_results(double orig_A[N][N], double eigenvalues[N], double eigenvectors[N][N]) {
    printf("--- Verification Check (コード内での固有値・固有ベクトル関係性の確認) ---\n");
    printf("Equation verified: Ax - lambda * x = 0\n\n");
    
    for (int k = 0; k < N; k++) {
        double x[N];
        double Ax[N];
        double lambda_x[N];
        double error_vector[N];

        // Extract the k-th eigenvector from the matrix columns
        for (int i = 0; i < N; i++) {
            x[i] = eigenvectors[i][k];
        }

        // Calculate elements of vector Ax, vector lambda*x, and the resulting error vector
        for (int i = 0; i < N; i++) {
            Ax[i] = 0.0;
            for (int j = 0; j < N; j++) {
                Ax[i] += orig_A[i][j] * x[j];
            }
            lambda_x[i] = eigenvalues[k] * x[i];
            error_vector[i] = Ax[i] - lambda_x[i];
        }

        // Print the element breakdown showing the equation results
        printf("Pair %d Verification:\n", k + 1);
        printf("  Ax vector         = [ %9.6f  %9.6f  %9.6f  %9.6f ]\n", Ax[0], Ax[1], Ax[2], Ax[3]);
        printf("  lambda * x vector = [ %9.6f  %9.6f  %9.6f  %9.6f ]\n", lambda_x[0], lambda_x[1], lambda_x[2], lambda_x[3]);
        printf("  Ax - lambda * x   = [ %9.6f  %9.6f  %9.6f  %9.6f ]\n\n", error_vector[0], error_vector[1], error_vector[2], error_vector[3]);
    }
}