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

#define N 4         // Matrix size
#define EPS 1e-6    // Convergence threshold
#define MAX_ITER 100

void print_matrix(double mat[N][N]) {
    for (int i = 0; i < N; i++) {
        for (int j = 0; j < N; j++) {
            printf("%7.4f ", mat[i][j]);
        }
        printf("\n");
    }
}

int main() {
    // Original matrix A
    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 backup of the original matrix A for verification (Ax = lambda * x)
    double A_orig[N][N];
    for (int i = 0; i < N; i++)
        for (int j = 0; j < N; j++)
            A_orig[i][j] = A[i][j];

    // Initialize P as an Identity Matrix. Columns of P will become our eigenvectors.
    double P[N][N] = {0};
    for (int i = 0; i < N; i++) P[i][i] = 1.0;

    int iter = 0;
    printf("=== Jacobi Method Convergence Process ===\n\n");

    while (iter < MAX_ITER) {
        // 1. Find the maximum off-diagonal element A[p][q]
        int p = 0, q = 1;
        double max_val = fabs(A[0][1]);
        for (int i = 0; i < N; i++) {
            for (int j = i + 1; j < N; j++) {
                if (fabs(A[i][j]) > max_val) {
                    max_val = fabs(A[i][j]);
                    p = i;
                    q = j;
                }
            }
        }

        // Print track of convergence
        printf("Iteration %d: Max off-diagonal = %f (at A[%d][%d])\n", iter, max_val, p, q);

        if (max_val < EPS) {
            printf("\n--> Successfully converged after %d iterations.\n\n", iter);
            break;
        }

        // 2. Calculate the rotation angle theta (using acos(-1.0) for Pi)
        double phi, cos_t, sin_t;
        if (fabs(A[p][p] - A[q][q]) < 1e-12) {
            phi = acos(-1.0) / 4.0; 
        } else {
            phi = 0.5 * atan2(2.0 * A[p][q], A[p][p] - A[q][q]);
        }
        cos_t = cos(phi);
        sin_t = sin(phi);

        // 3. Update Matrix A (P_k^T * A * P_k)
        double Ap_old = A[p][p];
        double Aq_old = A[q][q];
        
        A[p][p] = Ap_old * cos_t * cos_t + Aq_old * sin_t * sin_t + 2.0 * A[p][q] * sin_t * cos_t;
        A[q][q] = Ap_old * sin_t * sin_t + Aq_old * cos_t * cos_t - 2.0 * A[p][q] * sin_t * cos_t;
        A[p][q] = A[q][p] = 0.0; 

        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] = a_ip * cos_t + a_iq * sin_t;
                A[i][q] = A[q][i] = -a_ip * sin_t + a_iq * cos_t;
            }
        }

        // 4. Update Eigenvector Matrix P (P = P * P_k)
        // We modify the columns p and q of matrix P
        for (int i = 0; i < N; i++) {
            double p_ip = P[i][p];
            double p_iq = P[i][q];
            P[i][p] = p_ip * cos_t + p_iq * sin_t;
            P[i][q] = -p_ip * sin_t + p_iq * cos_t;
        }

        iter++;
    }

    // === Output Final Eigenvalues & Eigenvectors ===
    printf("=== Final Results ===\n");
    for (int j = 0; j < N; j++) {
        double lambda = A[j][j]; // Diagonal elements are eigenvalues
        printf("Eigenvalue %d: %7.4f\n", j + 1, lambda);
        printf("Eigenvector %d (Column vector):\n", j + 1);
        for (int i = 0; i < N; i++) {
            printf("  [%7.4f]\n", P[i][j]); // P[i][j] extracts the j-th column
        }
        printf("\n");
    }

    // === Verification (Ax = lambda * x) ===
    printf("=== Code-Level Verification (Ax - lambda * x) ===\n");
    printf("If the values below are essentially 0, Ax = lambda * x holds true perfectly.\n\n");

    for (int j = 0; j < N; j++) {
        double lambda = A[j][j];
        printf("Verifying Eigenvalue %d (%7.4f):\n", j + 1, lambda);
        
        for (int i = 0; i < N; i++) {
            // Compute i-th row of (A_orig * j-th column of P)
            double Ax_i = 0.0;
            for (int k = 0; k < N; k++) {
                Ax_i += A_orig[i][k] * P[k][j];
            }
            // Compute i-th row of (lambda * j-th column of P)
            double lambda_x_i = lambda * P[i][j];
            
            // Difference should approach 0
            printf("  Row %d difference: %10.3e\n", i + 1, Ax_i - lambda_x_i);
        }
        printf("\n");
    }

    return 0;
}