#include "Header.h"
#include <Eigen/Dense>
using namespace Eigen;

Solver::Solver(double _A, double _B, double _C, double _D, int _N, int _l, int _u) {
    A = _A, B = _B, C = _C, D = _D, N = _N;
    upper = _u, lower = _l;
	L = (double)(upper - lower) / N;
}

void Solver::Execute_Linear(double val1, double val2) {

    // Локальная матрица жёсткости
    MatrixXd local = MatrixXd::Zero(2, 2);
    
    // Локальный вектор нагрузки
    VectorXd local_load(2);

    local(0, 0) = -A / L - B / 2. + C * L / 3.;
    local(0, 1) = A / L + B / 2. + C * L / 6.;
    local(1, 0) = A / L - B / 2. + C * L / 6.;
    local(1, 1) = -A / L + B / 2. + C * L / 3.;

    local_load(0) = -D * L / 2.;
    local_load(1) = -D * L / 2.;

    // Глобальная матрица жёсткости
    MatrixXd ansamb = MatrixXd::Zero(N + 1, N + 1);
    
    // Глобальный вектор нагрузок
    VectorXd global_load = VectorXd::Zero(N + 1);

    // Ансамблирование
    for (int elem = 0; elem < N; ++elem) {
        int node_i = elem;
        int node_j = elem + 1;

        ansamb(node_i, node_i) += local(0, 0);
        ansamb(node_i, node_j) += local(0, 1);
        ansamb(node_j, node_i) += local(1, 0);
        ansamb(node_j, node_j) += local(1, 1);

        global_load(node_i) += local_load(0);
        global_load(node_j) += local_load(1);
    }

#if DEBUG
    std::cout << std::endl << "Before:" << std::endl;
    std::cout << "Ansamb matrix:\n" << ansamb << std::endl;
    std::cout << "Ansamb load vector:\n" << global_load << std::endl;
#endif

    // Граничные условия
    double u_right = val2;
    ansamb.row(0).setZero();
    ansamb.row(N).setZero();

    ansamb(0, 0) = L + 1;
    ansamb(0, 1) = -1;
    global_load(0) = 0;

    ansamb(N, N) = 1;
    global_load(N) = u_right;

#if DEBUG
    std::cout << "\nAfter:" << std::endl;
    std::cout << "Modified matrix:\n" << ansamb << std::endl;
    std::cout << "Modified load vector:\n" << global_load << std::endl;
#endif

    // Решение системы 
    VectorXd solution = ansamb.fullPivLu().solve(global_load);
    std::cout << "\nSolution:" << std::endl;
    std::cout << solution << std::endl;

    std::ofstream file("matrix_linear_" + std::to_string(N) + ".txt");
    for (int i = 0; i < solution.size(); i++) {
        file << solution(i) << ' ';
    }
    file << std::endl;
}

void Solver::Execute_Cubic(double val1, double val2) {

    int mat_dim = 1 + N * 3;

    // Локальная матрица жёсткости
    MatrixXd local = MatrixXd::Zero(4, 4);

    // Локальный вектор нагрузки
    VectorXd local_load(4);

    // Формирование локальной матрицы жёсткости
    local(0, 0) = -A * 37.0 / 10.0 / L + B * (-1.0) / 2.0;
    local(0, 1) = -A * (-189.0) / 40.0 / L + B * 57.0 / 80.0;
    local(0, 2) = -A * 27.0 / 20.0 / L + B * (-3.0) / 10.0;
    local(0, 3) = -A * (-13.0) / 40.0 / L + B * 7.0 / 80.0;

    local(1, 0) = -A * (-189.0) / 40.0 / L + B * (-57.0) / 80.0;
    local(1, 1) = -A * 54.0 / 5.0 / L;
    local(1, 2) = -A * (-297.0) / 40.0 / L + B * 81.0 / 80.0;
    local(1, 3) = -A * 27.0 / 20.0 / L + B * (-3.0) / 10.0;

    local(2, 0) = -A * 27.0 / 20.0 / L + B * 3.0 / 10.0;
    local(2, 1) = -A * (-297.0) / 40.0 / L + B * (-81.0) / 80.0;
    local(2, 2) = -A * 54.0 / 5.0 / L;
    local(2, 3) = -A * (-189.0) / 40.0 / L + B * 57.0 / 80.0;

    local(3, 0) = -A * (-13.0) / 40.0 / L + B * (-7.0) / 80.0;
    local(3, 1) = -A * 27.0 / 20.0 / L + B * 3.0 / 10.0;
    local(3, 2) = -A * (-189.0) / 40.0 / L + B * (-57.0) / 80.0;
    local(3, 3) = -A * 37.0 / 10.0 / L + B * 1.0 / 2.0;

    // Формирование локального вектора нагрузки
    local_load(0) = -D * L / 8.0;
    local_load(1) = -D * 3.0 * L / 8.0;
    local_load(2) = -D * 3.0 * L / 8.0;
    local_load(3) = -D * L / 8.0;

    // Глобальная матрица жёсткости
    MatrixXd ansamb = MatrixXd::Zero(mat_dim, mat_dim);

    // Глобальный вектор нагрузок
    VectorXd global_load = VectorXd::Zero(mat_dim);

    // Ансамблирование
    for (int elem = 0; elem < N; ++elem) {
        int node_i = elem * 3;

        for (int i = 0; i < 4; i++) {
            for (int j = 0; j < 4; j++) {
                ansamb(node_i + i, node_i + j) += local(i, j);
            }
            global_load(node_i + i) += local_load(i);
        }
    }

#if DEBUG
    std::cout << std::endl << "Before:" << std::endl;
    std::cout << "Ansamb matrix:\n" << ansamb << std::endl;
    std::cout << "Ansamb load vector:\n" << global_load << std::endl;
#endif

    // Граничные условия
    double u_right = val2;

    ansamb.row(0).setZero();
    ansamb(0, 0) = L / 3.0 + 1;
    ansamb(0, 1) = -1;
    global_load(0) = 0;

    ansamb.row(mat_dim - 1).setZero();
    ansamb(mat_dim - 1, mat_dim - 1) = 1;
    global_load(mat_dim - 1) = u_right;

#if DEBUG
    std::cout << "\nAfter:" << std::endl;
    std::cout << "Modified matrix:\n" << ansamb << std::endl;
    std::cout << "Modified load vector:\n" << global_load << std::endl;
#endif

    // Решение системы
    VectorXd solution = ansamb.fullPivLu().solve(global_load);
    std::cout << "\nSolution:" << std::endl;
    std::cout << solution << std::endl;

    std::ofstream file("matrix_cubic_" + std::to_string(N) + ".txt");
    for (int i = 0; i < solution.size(); i++) {
        file << solution(i) << ' ';
    }
    file << std::endl;
}