#ifndef QUADRATICMINIMALIZATOR_H
#define	QUADRATICMINIMALIZATOR_H

#include <cstdlib>
#include <itpp/itbase.h>
#include <iostream>


using namespace itpp;
using namespace std;

class QuadraticMinimalizator {
protected:
    mat A;
    mat B;
    mat Q;
    mat R;
    double maxdif;

    bool test() {
        if ( !isDiagonal(Q) ) {
            stringstream e;
            e << "Matrix Q is not diagonal" << endl;
            e << Q << endl;
            it_warning(e.str());
        }
        if ( !isDiagonal(R) ) {
            stringstream e;
            e << "Matrix R is not diagonal" << endl;
            e << R << endl;
            it_warning(e.str());
        }
        if ( !isPositiveDefinite(Q) ) {
            stringstream e;
            e << "Matrix Q is not positive definite" << endl;
            e << Q << endl;
            it_warning(e.str());
        }
        if ( !isPositiveDefinite(R) ) {
            stringstream e;
            e << "Matrix R is not positive definite" << endl;
            e << R << endl;
            it_warning(e.str());
        }
    }

public:
    QuadraticMinimalizator( const mat &A, const mat &B, const mat &Q, const mat &R ) {
        maxdif = 0.00000001;
        this->A = A;
        this->B = B;
        this->Q = Q;
        this->R = R;
        test();
    }


    /**
     * x(t+1) = Ax(t) + Bu(t) + C
     * @param horizont
     * @param C
     * @return L: u(t) = L [ x(t) 1 ]
     */
    mat L( const int horizont, const mat & C ) {
        it_warning("Nijak se tam C neprojevuje!!!");
        int xdim = A.cols();
        int udim = B.cols();
        mat sqQ = sqrt(Q);
        mat sqR = sqrt(R);
        mat M0, M;
        mat qrQ;
        mat Lall, Lc1, Lc2, Lc3, Lx1, Lx2, Lu, L;
        M0 = concat_vertical(
                concat_vertical(
                    concat_horizontal( sqQ * B,   concat_horizontal( sqQ * A,             zeros(xdim, udim) ) ),
                    concat_horizontal( sqR,       concat_horizontal( zeros( udim, xdim ), zeros(udim, udim) ) )
                ),
                concat_horizontal( zeros( xdim, xdim + udim ) , sqQ * C)
            );

        M = M0;
        for (int h = horizont; h >= 0; h --) {
            qr(M, Lall);

            Lu  = Lall(0, udim-1, 0,         udim-1     );
            Lx1 = Lall(0, udim-1, udim,      udim+xdim-1);
            Lc1 = Lall(0, udim-1, udim+xdim, udim+xdim  );
            Lx2 = Lall(udim, udim+xdim-1,  udim, udim+xdim-1);
            Lc2 = Lall(udim, udim+xdim-1,  udim+xdim, udim+xdim);
            Lc3 = Lall(udim+xdim, udim+xdim+xdim-1,  udim+xdim, udim+xdim);
            M = concat_vertical(
                    M0,
                    concat_vertical(
                        concat_horizontal( Lx2 * B, concat_horizontal( Lx2 * A, Lc2 ) ),
                        concat_horizontal( zeros(xdim, xdim + udim), Lc3 )
                    )
                );
        }
        L = concat_horizontal( - inv(Lu) * Lx1, - inv(Lu) * Lc1);
        return L;
    }
    /**
     * x(t+1) = Ax(t) + Bu(t)
     * @param horizont
     * @param C
     * @return L: u(t) = Lx(t)
     */
    mat L( const int horizont ) {
        int xdim = A.cols();
        int udim = B.cols();
        mat sqQ = sqrt(Q);
        mat sqR = sqrt(R);
        mat M0, M;
        mat qrQ;
        mat Lall, Ls, Lx, Lu, L;
        M0 = concat_vertical(
                concat_horizontal( sqQ * B,     sqQ * A ),
                concat_horizontal( sqR,         zeros( udim, xdim ) )
            );
        M = M0;
        for (int h = horizont; h >= 0; h --) {
            qr(M, Lall);
            Lu = Lall(0,    udim-1,       0,    udim-1);
            Lx = Lall(udim, udim+xdim-1,  udim, udim+xdim-1);
            Ls = Lall(0,    udim-1,       udim, udim+xdim-1);
            M = concat_vertical(
                    M0,
                    concat_horizontal(Lx * B, Lx * A )
                );
        }
        L = - inv(Lu) * Ls;
        return L;
    }

    double maxDif( const mat & M1, const mat & M2 ) {
        return max(max(abs(M1 - M2), 1));
    }

    bool isSymmetric( const mat & M) {
        return isSymmetric(M, maxdif);
    }

    bool isSymmetric( const mat & M, double maxdif ) {
        mat D = abs(M - M.transpose());
        double m = max(max(D, 1));
        if ( m < maxdif )
            return true;
        else
            return false;
    }

    bool isDiagonal(  const mat M ) {
        mat D = abs(M);
        for ( int i = 0; i < M.rows(); i ++ ) {
            for ( int j = 0; j < M.cols(); j ++ ) {
                if ( i != j && D(i,j) > maxdif ) {
                    return false;
                }
            }
        }
        return true;
    }

    bool isPositiveDefinite( const mat M ) {
        int dim = M.rows();
        if ( dim != M.cols() ) {
            stringstream err;
            err << "Matrix " << M << " is not square";
            it_warning(err.str());
        } else {
            for ( int i = dim-1; i >= 0; i-- ) {
                // subdeterminanty
                if ( det(M(0,i,0,i)) <= 0 ) {
                    return false;
                }
            }
            return true;
        }
        return false;
    }




};

#endif	/* QUADRATICMINIMALIZATOR_H */