// Peti labos iz APR - implementacija numerickih
//  integracija: Runge-Kutt i trapezni postupak

#include <iostream>
#include <fstream>
#include <cstdlib>
#include "Matrix.h"
#include "engine.h" // ovo je include fajl za MATLABov engine

// malo prototipova
void radi(Matrix& matricaA, Matrix& matricaB, Matrix& matricaX);
void inicijalizacijaTrapez(const Matrix& matricaA, const Matrix&
                           matricaB, double T, Matrix& R, Matrix& S);
void ispis(const Matrix& matricaX, int T);
void izracunajRungeKutt(const Matrix& m1, const Matrix& m2, const Matrix&
                        m3, const Matrix& m4, double T, Matrix& matricaX);
void izracunajTrapez(Matrix& matricaXk, const Matrix& R, const Matrix& S);
void napuni(Matrix& m1, Matrix& m2, Matrix& m3, Matrix& m4, const Matrix&
            matricaA, const Matrix& matricaB, const Matrix& matricaX, int T);
void posalji_u_MATLAB(double *trapez1, double *trapez2, double
                      *rungeKutt1, double *rungeKutt2, double *vrijeme, int brojac);
void unosPodataka(double *donjaGranica, double *gornjaGranica, double *T,
                  int *iteracija);

// zapocnimo algoritme
void radi(Matrix& matricaA, Matrix& matricaB, Matrix& matricaX)
{
    double T;
    Matrix m1, m2, m3, m4, matricaXk, R, S;

    int iteracija;
    double gornjaGranica, donjaGranica;

    unosPodataka(&donjaGranica, &gornjaGranica, &T, &iteracija);

    napuni(m1, m2, m3, m4, matricaA, matricaB, matricaX, T);

    inicijalizacijaTrapez(matricaA, matricaB, T, R, S);

    // matricaXk je za trapez a matricaX za Runge-Kutt
    matricaXk = matricaX;

    double suma = donjaGranica;
    double *trapez1, *trapez2, *rungeKutt1, *rungeKutt2, *vrijeme;
    int brojac = (gornjaGranica - donjaGranica) / T;

    // sluze za vracanje vrijednosti u MATLAB za crtanje
    trapez1  = (double*)malloc(sizeof(double)*brojac);
    trapez2  = (double*)malloc(sizeof(double)*brojac);
    rungeKutt1 = (double*)malloc(sizeof(double)*brojac);
    rungeKutt2 = (double*)malloc(sizeof(double)*brojac);
    vrijeme  = (double*)malloc(sizeof(double)*brojac);

    // vrtimo iteracije za oba postupka dok se ne postigne gornja granica
    for(int brojIteracije = 1; suma < gornjaGranica; ++brojIteracije)
    {
        izracunajRungeKutt(m1, m2, m3, m4, T, matricaX);
        izracunajTrapez(matricaXk, R, S);

        if(iteracija != 0 && brojIteracije % iteracija == 0)
        {
            std::cout << "Runge-Kutt:\n" << std::endl;
            ispis(matricaX, brojIteracije );
            std::cout << '\n' << std::endl;
            std::cout << "Trapez:\n" << std::endl;
            ispis(matricaXk, brojIteracije);
        }

        fflush(stdin);

        suma += T;
        vrijeme[brojIteracije-1]  = suma;

        if(brojac == brojIteracije - 1)
            break;

        trapez1[brojIteracije -1]  = matricaXk.getData(0,0);
        trapez2[brojIteracije -1]  = matricaXk.getData(1,0);
        rungeKutt1[brojIteracije  -1] = matricaX.getData(0,0) ;
        rungeKutt2[ brojIteracije -1] = matricaX.getData(1,0) ;
    }

    posalji_u_MATLAB(trapez1, trapez2, rungeKutt1, rungeKutt2, vrijeme, brojac);

}

// Funkcija izracunava matrice R i S
// X(k+1) = R*Xk + S
// za racunanje inverza koristi se MATLABova funkcija.
void inicijalizacijaTrapez(const Matrix& matricaA, const Matrix& matricaB, const double T, Matrix& R, Matrix& S)
{
    // dio za inicijalizaciju MATLABa
    Engine *ep; // pointer na MATLAB-ov engine
    mxArray *ulaz, *izlaz; // deklariramo MATLAB varijable
    double *p;

    // pokretanje MATLAB engine-a, uz standardne provjere ispravnosti
    if (!(ep = engOpen("\0")))
    {
        fprintf(stderr, "\nCan't start MATLAB engine\n");
        getchar();
    }

    ulaz = mxCreateDoubleMatrix(matricaA.getBrojRedova(), matricaA.getBrojStupaca(), mxREAL); // definiramo double matricu 3x3
    p = mxGetPr(ulaz); // dobavljamo pointer na (realne) vrijednosti matrice

    Matrix temp = matricaA;
    Matrix I(matricaA.getBrojRedova(), matricaA.getBrojStupaca());

    temp *= -T/2;

    // kreiramo jedinicnu matricu I
    for(int i = 0; i < temp.getBrojRedova(); ++i)
        for(int j = 0; j < temp.getBrojStupaca(); ++j)
            if(i == j)
                I.putData(i, j, 1);
            else
                I.putData(i, j, 0);

    temp += I;

    int k = 0;

    // saljemo MATLABu matricu za koju reba inverz izracunati
    for(i = 0; i < temp.getBrojRedova(); i++) // i punimo istu po redu
        for(int j = 0; j < temp.getBrojStupaca(); j++)
            *(p + k++) = temp.getData(i, j);

    engPutVariable(ep, "X", ulaz);  // saljemo je u MATLAB engine
    engEvalString(ep, "X"); // MATLAB sprema matrice PO STUPCIMA, A NE PO RETCIMA KAO U C-U!
    engEvalString(ep, "X = inv(X)"); // saljemo string na izracunavanje
    //  engEvalString(ep, "X = X'"); // saljemo string na izracunavanje

    izlaz = engGetVariable(ep, "X"); // dohvacamo varijablu iz MATLAB okoline
    p = mxGetPr(izlaz);
    k = 0;

    //vracamo u temp nazad inverz temp-a
    for(i = 0; i < temp.getBrojRedova(); i++)
        for(int j = 0; j < temp.getBrojStupaca(); j++)
            temp.putData(i, j, *(p + k++));

    // sad je u matrici temp (I - A*T/2)^-1

    S  = temp;
    S *= T;
    S *= matricaB;

    R   = temp;
    temp  = matricaA;
    temp *= T/2;
    temp += I;
    R  *= temp;

    // zatvaramo vezu s MATLABom i cistimo memoriju
    engClose(ep); // gasimo engine
    mxDestroyArray(ulaz);
    mxDestroyArray(izlaz);
}

// ispisuje matricu X na std output
void ispis(const Matrix& matricaX, const int brojIteracije)
{
    std::cout << "Iteracija = " << brojIteracije << "\t" << std::endl;
    std::cout << "Matrica X = " << std::endl;
    std::cout << matricaX;

    fflush(stdin);
    getchar();
}

// izracunava vrijednost matrice Xk+1
void izracunajRungeKutt(const Matrix& m1, const Matrix& m2, const Matrix& m3, const Matrix& m4, const double T, Matrix& matricaX)
{
    Matrix temp = m1;

    temp += m2;
    temp += m2;
    temp += m3;
    temp += m3;
    temp += m4;

    temp *= T/6;
    matricaX += temp;
}

// izracunava vrijednost matrice Xk za trapeznu metodu.
void izracunajTrapez(Matrix& matricaXk, const Matrix& R, const Matrix& S)
{
    Matrix temp = R;

    temp *= matricaXk;
    temp += S;

    matricaXk = temp;
}

// izracunava vrijednost matrica m1, m2, m3, m4..
void napuni(Matrix& m1, Matrix& m2, Matrix& m3, Matrix& m4, const Matrix& matricaA, const Matrix& matricaB, const Matrix& matricaX, const int T)
{
    Matrix temp;

    //m1 = Ax + B
    m1  = matricaA;
    m1 *= matricaX;
    m1 += matricaB;

    //m2 = A(x + T/2 * m1) + B
    temp  = m1;
    temp *= T/2;
    temp += matricaX;
    m2  = matricaA;
    m2 *= temp;
    m2 += matricaB;

    //m3 = A(x + T/2 * m2) + B
    temp  = m2;
    temp *= T/2;
    temp += matricaX;
    m3  = matricaA;
    m3 *= temp;
    m3 += matricaB;

    //m2 = A(x + T * m3) + B
    temp  = m3;
    temp *= T;
    temp += matricaX;
    m4  = matricaA;
    m4 *= temp;
    m4 += matricaB;
}

// salje rezultate dobijene za trapeznu metodu i Runge-Kutt metotdu u
// MATLAB; salje i polje s vremenskim trenucima u kojima su izracunate
// vrijednosti
void posalji_u_MATLAB(double *trapez1, double *trapez2, double *rungeKutt1, double *rungeKutt2, double *vrijeme, int brojac)
{
    Engine *ep2; // pointer na MATLAB-ov engine
    mxArray *ulaz2; // deklariramo MATLAB varijable
    double *p2;
    int i;

    // pokretanje MATLAB engine-a, uz standardne provjere ispravnosti
    if (!(ep2 = engOpen("\0")))
    {
        fprintf(stderr, "\nCan't start MATLAB engine\n");
        fflush(stdin);
        getchar();
    }

    ulaz2 = mxCreateDoubleMatrix(1, brojac, mxREAL); // definiramo double matricu

    p2 = mxGetPr(ulaz2); // dobavljamo pointer na (realne) vrijednosti matrice

    for(i = 0; i < brojac; i++) // i punimo istu po redu
        *(p2 + i) = trapez1[i];

    engPutVariable(ep2, "Trapez1", ulaz2);  // saljemo je u MATLAB engine

    mxDestroyArray(ulaz2);

    ulaz2 = mxCreateDoubleMatrix(1, brojac, mxREAL); // definiramo double matricu
    p2 = mxGetPr(ulaz2); // dobavljamo pointer na (realne) vrijednosti matrice

    for(i = 0; i < brojac; i++) // i punimo istu po redu
        *(p2 + i) = trapez2[i];

    engPutVariable(ep2, "Trapez2", ulaz2);  // saljemo je u MATLAB engine

    mxDestroyArray(ulaz2);

    ulaz2 = mxCreateDoubleMatrix(1, brojac, mxREAL); // definiramo double matricu
    p2 = mxGetPr(ulaz2); // dobavljamo pointer na (realne) vrijednosti matrice

    for(i = 0; i < brojac; i++) // i punimo istu po redu
        *(p2 + i) = rungeKutt1[i];

    engPutVariable(ep2, "rungeKutt1", ulaz2);  // saljemo je u MATLAB engine

    mxDestroyArray(ulaz2);

    ulaz2 = mxCreateDoubleMatrix(1, brojac, mxREAL); // definiramo double matricu
    p2 = mxGetPr(ulaz2); // dobavljamo pointer na (realne) vrijednosti matrice

    for(i = 0; i < brojac; i++) // i punimo istu po redu
        *(p2 + i) = rungeKutt2[i];

    engPutVariable(ep2, "rungeKutt2", ulaz2);  // saljemo je u MATLAB engine

    mxDestroyArray(ulaz2);

    ulaz2 = mxCreateDoubleMatrix(1, brojac, mxREAL); // definiramo double matricu
    p2 = mxGetPr(ulaz2); // dobavljamo pointer na (realne) vrijednosti matrice

    for(i = 0; i < brojac; i++) // i punimo istu po redu
        *(p2 + i) = vrijeme[i];

    engPutVariable(ep2, "vrijeme", ulaz2);  // saljemo je u MATLAB engine

    mxDestroyArray(ulaz2);

    engEvalString(ep2, "plot(vrijeme,Trapez1,vrijeme,Trapez2,vrijeme,rungeKutt1,vrijeme,rungeKutt2)");
    fflush(stdin);
    getchar();
    engClose(ep2);
}

// sa std. inputa ucitava interval vremenski te korak integracije; pita se
// svakih koliko iteracija da ispisuje (nula znaci da se ne zeli ispis
// iteracija)
void unosPodataka(double *donjaGranica, double *gornjaGranica, double *T, int *iteracija)
{
    std::cout << "\n Unesite donju granicu : ";
    std::cin >> *donjaGranica;

    std::cout << "Unesite gornju granicu : ";
    std::cin >> *gornjaGranica;

    std::cout << "Unesite korak integracije T = ";
    std::cin >> *T;
    fflush(stdin);

    std::cout << "Svakih koliko iteracija zelite ispis? ";
    std::cin >> *iteracija;

    if(*gornjaGranica < *donjaGranica)
    {
        std::cout << "Donja granica je veca od gornje!!" << std::endl;
        fflush(stdin);
        getchar();
        exit(1);
    }

    if(*gornjaGranica - *donjaGranica <= *T)
    {
        std::cout << "Prevelik je korak integracije" << std::endl;
        fflush(stdin);
        getchar();
        exit(1);
    }
}

int main()
{
    Matrix matrixA, matrixB, matrixX;

    std::ifstream("matricaA.txt") >> matrixA;
    std::ifstream("matricaB.txt") >> matrixB;
    std::ifstream("matricaX.txt") >> matrixX;

    if(!matrixA.allocated() || !matrixB.allocated() || !matrixX.allocated())
    {
        std::cout << "Neka od matrica nije dobro ucitana!" << std::endl;
        fflush(stdin);
        getchar();
        exit(1);
    }

    radi(matrixA, matrixB, matrixX);

    return 0;
}