#include <iostream>
#include <cstdio>
#include "MAT.h"
#include "VEC.h"
#include <vector>
#include <cmath>


using namespace std;

double fn1(double x) {
    return pow(2, x) + pow(2, pow(2, 2*x)) - 3;
}

double fn2(double x) {
    return pow(2, x) + 3;
}

double fn3(double x) {
    return sin(2*x) + sin(4 * x*x) + sin(8*x*x*x) - 1;
}

double fn4(double x) {
    return pow(2, x) + pow(sin(x), 2)*pow( cos(x),2) - 1;
}

double fn5(double x) {
    return 1.0 / x + 1.0 / x / x + 1.0 /x/x/x + 1.0 /x/x/x/x - 1;
}


double fn6(double x) {
    return x + pow(x, 1.0/2) + pow(x, 1.0/3)  + pow(x, 1.0/4) - 10;
}

void p2_1() {
    int max_iter = 10000;
    double tol = 1e-10;
    VEC X = zeros(2);
    MAT A = mat_zeros(2);
    VEC X_prev = zeros(2);
    MAT J(2);
    VEC b(2);
    double error = tol + 1 ;
    for (int i=0; i<max_iter && error >= tol; i++) {
        J[0][0] = 2*X_prev[0] + 3;
        J[0][1] = -1;
        J[1][0] = -1;
        J[1][1] = 6*X_prev[1] + 1;
        b[0] = X_prev[0] * X_prev[0] + 3 * X_prev[0] - X_prev[1] - 3.81;
        b[1] = 3*X_prev[1]*X_prev[1] + X_prev[1] - X_prev[0] - 1.07;
        X = LU_fact_and_solve(J, J*X_prev - b);
        error = infnorm(X_prev - X);
        cout << "err " << error << endl;
        X_prev = X;
    }
    cout << "ans:\n";
    X.show();
}

void p2_2() {
    int max_iter = 10000;
    double tol = 1e-10;
    VEC X = zeros(2);
    MAT A = mat_zeros(2);
    VEC X_prev = zeros(2);
    MAT J(2);
    VEC b(2);
    double error = tol + 1 ;
    for (int i=0; i<max_iter && error >= tol; i++) {
        J[0][0] = 2*X_prev[0] + X_prev[1] + 1;
        J[0][1] = X_prev[0] + 2 * X_prev[1];
        J[1][0] = 1;
        J[1][1] = 2*X_prev[1] + 1;
        b[0] = X_prev[0] * X_prev[0] + X_prev[0] * X_prev[1]\
            + X_prev[1] * X_prev[1] + X_prev[0]-4.16;
        b[1] = X_prev[1]*X_prev[1] + X_prev[0] + X_prev[1]-2.56;
        X = LU_fact_and_solve(J, J*X_prev - b);
        error = infnorm(X_prev - X);
        cout << "err " << error << endl;
        X_prev = X;
    }
    cout << "ans:\n";
    X.show();
}


void p2_3() {
    int max_iter = 10000;
    double tol = 1e-10;
    VEC X = zeros(2);
    MAT A = mat_zeros(2);
    VEC X_prev = zeros(2);
    MAT J(2);
    VEC b(2);
    double error = tol + 1 ;
    for (int i=0; i<max_iter && error >= tol; i++) {
        J[0][0] = 1.0 / 2 / sqrt(X_prev[0]+1);
        J[0][1] = -1;
        J[1][0] = -1;
        J[1][1] = 1.0 / 2 / sqrt(X_prev[1] +2);
        b[0] = sqrt(X_prev[0] +1 ) - X_prev[1] - 0.60384;
        b[1] = sqrt(X_prev[1]+2) -X_prev[0] - 0.943168;
        X = LU_fact_and_solve(J, J*X_prev - b);
        error = infnorm(X_prev - X);
        cout << "err " << error << endl;
        X_prev = X;
    }
    cout << "ans:\n";
    X.show();
}


void p2_4() {
    int max_iter = 10000;
    double tol = 1e-10;
    VEC X = zeros(2);
    MAT A = mat_zeros(2);
    VEC X_prev = zeros(2);
    MAT J(2);
    VEC b(2);
    double error = tol + 1 ;
    for (int i=0; i<max_iter && error >= tol; i++) {
        J[0][0] = 3*X_prev[0]*X_prev[0] + 1;
        J[0][1] = -1;
        J[1][0] = -1;
        J[1][1] = 3*X_prev[1]*X_prev[1] + 2;
        b[0] = pow(X_prev[0], 3) +X_prev[0] - X_prev[1] + 1.375;
        b[1] = pow(X_prev[1], 3) + 2 * X_prev[1] - X_prev[0] - 11.5;
        X = LU_fact_and_solve(J, J*X_prev - b);
        error = infnorm(X_prev - X);
        cout << "err " << error << endl;
        X_prev = X;
    }
    cout << "ans:\n";
    X.show();
}


void p2_5() {
    int max_iter = 10000;
    double tol = 1e-10;
    VEC X = zeros(2);
    MAT A = mat_zeros(2);
    VEC X_prev = zeros(2);
    X_prev[0] = X_prev[1] = 2;
    MAT J(2);
    VEC b(2);
    double error = tol + 1 ;
    for (int i=0; i<max_iter && error >= tol; i++) {
        J[0][0] = cos(X_prev[0]);
        J[0][1] = -1;
        J[1][0] = -1;
        J[1][1] = -sin(X_prev[1]);
        b[0] = sin(X_prev[0]) - X_prev[1] + 0.208793;
        b[1] = cos(X_prev[1]) - X_prev[0] + 0.646404;
        X = LU_fact_and_solve(J, J*X_prev - b);
        error = infnorm(X_prev - X);
        cout << "err " << error << endl;
        X_prev = X;
    }
    cout << "ans:\n";
    X.show();
}


void p4_1() {
    const double h= 0.1;
    double y1 = 1;
    double y2 = 0;
    double y1_prev = y1;
    double y2_prev = y2;
    printf("t, y1, y2\n");
    for (double t= 0; t<=3.1; t+=h) {
        printf("%g %g %g\n", t, y1 ,y2);
        y1 = y2_prev * h + y1_prev;
        y2 = h * (-0.1*y2_prev -y1_prev) + y2_prev;
        y1_prev = y1;
        y2_prev = y2;
    }
}


void p4_2() {
    const double h= 0.1;
    double y1 = 1;
    double y2 = 0;
    printf("t, y1, y2\n");
    VEC Y(2);
    MAT A(2);
    VEC Y_n(2);
    Y[0] = Y_n[0] = 1;
    Y[1] = Y_n[1] = 0;
    for (double t= 0; t<=3.1; t+=h) {
        printf("%g %g %g\n", t, Y[0], Y[1]);
        A[0][0] = 1.0 / h;
        A[0][1] = -1.0 / 2;
        A[1][0] = 1.0 / 2;
        A[1][1] = 1.0/h + 0.1 / 2;
        Y[0] = Y[0] / h + 1.0 / 2 * Y[1];
        Y[1] = Y[1] / h - 0.1 / 2 * Y[1] - Y[0]/2;
        Y_n = LU_fact_and_solve(A, Y);
        Y = Y_n;
    }
}
int main() {
    //cout << bisection(1e-10, 5, 1e-12, 5000, fn6) << endl;
    p4_2();

    return 0;
}