// Copyright (C) 2007  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.
#ifndef DLIB_SOLVE_QP3_USING_SMo_H__
#define DLIB_SOLVE_QP3_USING_SMo_H__

#include "optimization_solve_qp3_using_smo_abstract.h"
#include <cmath>
#include <limits>
#include <sstream>
#include "../matrix.h"
#include "../algs.h"


namespace dlib 
{

// ----------------------------------------------------------------------------------------

    class invalid_qp3_error : public dlib::error 
    { 

    public: 
        invalid_qp3_error(
            const std::string& msg, 
            double B_,
            double Cp_,
            double Cn_
        ) : 
            dlib::error(msg), 
            B(B_),
            Cp(Cp_),
            Cn(Cn_)
        {};

        const double B;
        const double Cp;
        const double Cn;
    };

// ----------------------------------------------------------------------------------------

    template <
        typename matrix_type
        >
    class solve_qp3_using_smo
    {
    public:
        typedef typename matrix_type::mem_manager_type mem_manager_type;
        typedef typename matrix_type::type scalar_type;
        typedef typename matrix_type::layout_type layout_type;
        typedef matrix<scalar_type,0,0,mem_manager_type,layout_type> general_matrix;
        typedef matrix<scalar_type,0,1,mem_manager_type,layout_type> column_matrix;


        template <
            typename EXP1,
            typename EXP2,
            typename EXP3,
            long NR
            >
        unsigned long operator() ( 
            const matrix_exp<EXP1>& Q,
            const matrix_exp<EXP2>& p,
            const matrix_exp<EXP3>& y,
            const scalar_type B,
            const scalar_type Cp,
            const scalar_type Cn,
            matrix<scalar_type,NR,1,mem_manager_type, layout_type>& alpha,
            scalar_type eps
        ) 
        {
            DLIB_ASSERT(Q.nr() == Q.nc() && y.size() == Q.nr() && p.size() == y.size() && 
                        y.size() > 0 && is_col_vector(y) && is_col_vector(p) &&
                        sum((y == +1) + (y == -1)) == y.size() &&
                        Cp > 0 && Cn > 0 &&
                        eps > 0,
                "\t void solve_qp3_using_smo::operator()"
                << "\n\t invalid arguments were given to this function"
                << "\n\t Q.nr():                     " << Q.nr() 
                << "\n\t Q.nc():                     " << Q.nc() 
                << "\n\t is_col_vector(p):           " << is_col_vector(p) 
                << "\n\t p.size():                   " << p.size() 
                << "\n\t is_col_vector(y):           " << is_col_vector(y) 
                << "\n\t y.size():                   " << y.size() 
                << "\n\t sum((y == +1) + (y == -1)): " << sum((y == +1) + (y == -1)) 
                << "\n\t Cp:                         " << Cp
                << "\n\t Cn:                         " << Cn
                << "\n\t eps:                        " << eps 
                );



            set_initial_alpha(y, B, Cp, Cn, alpha);


            const scalar_type tau = 1e-12;

            typedef typename colm_exp<EXP1>::type col_type;

            // initialize df.  Compute df = Q*alpha + p
            df = p;
            for (long r = 0; r < df.nr(); ++r)
            {
                if (alpha(r) != 0)
                {
                    df += alpha(r)*matrix_cast<scalar_type>(colm(Q,r));
                }
            }

            unsigned long count = 0;
            // now perform the actual optimization of alpha
            long i=0, j=0;
            while (find_working_group(y,alpha,Q,df,Cp,Cn,tau,eps,i,j))
            {
                ++count;
                const scalar_type old_alpha_i = alpha(i);
                const scalar_type old_alpha_j = alpha(j);

                optimize_working_pair(alpha,Q,y,df,tau,i,j, Cp, Cn );

                // update the df vector now that we have modified alpha(i) and alpha(j)
                scalar_type delta_alpha_i = alpha(i) - old_alpha_i;
                scalar_type delta_alpha_j = alpha(j) - old_alpha_j;

                col_type Q_i = colm(Q,i);
                col_type Q_j = colm(Q,j);
                for(long k = 0; k < df.nr(); ++k)
                    df(k) += Q_i(k)*delta_alpha_i + Q_j(k)*delta_alpha_j;
            }

            return count;
        }

        const column_matrix& get_gradient (
        ) const { return df; }

    private:

    // -------------------------------------------------------------------------------------

        template <
            typename scalar_type,
            typename scalar_vector_type,
            typename scalar_vector_type2
            >
        inline void set_initial_alpha (
            const scalar_vector_type& y,
            const scalar_type B,
            const scalar_type Cp,
            const scalar_type Cn,
            scalar_vector_type2& alpha
        ) const
        {
            alpha.set_size(y.size());

            set_all_elements(alpha,0);

            // It's easy in the B == 0 case
            if (B == 0)
                return;

            const scalar_type C = (B > 0)?  Cp : Cn;

            scalar_type temp = std::abs(B)/C;
            long num = (long)std::floor(temp);
            long num_total = (long)std::ceil(temp);

            bool has_slack = false;
            long count = 0;
            for (long i = 0; i < alpha.nr(); ++i)
            {
                if (y(i) == 1)
                {
                    if (count < num)
                    {
                        ++count;
                        alpha(i) = C;
                    }
                    else 
                    {
                        has_slack = true;
                        if (num_total > num)
                        {
                            ++count;
                            alpha(i) = C*(temp - std::floor(temp));
                        }
                        break;
                    }
                }
            }

            if (count != num_total || has_slack == false)
            {
                std::ostringstream sout;
                sout << "Invalid QP3 constraint parameters of B: " << B << ", Cp: " << Cp << ", Cn: "<< Cn;
                throw invalid_qp3_error(sout.str(),B,Cp,Cn);
            }
        }

    // ------------------------------------------------------------------------------------

        template <
            typename scalar_vector_type,
            typename scalar_type,
            typename EXP,
            typename U, typename V
            >
        inline bool find_working_group (
            const V& y,
            const U& alpha,
            const matrix_exp<EXP>& Q,
            const scalar_vector_type& df,
            const scalar_type Cp,
            const scalar_type Cn,
            const scalar_type tau,
            const scalar_type eps,
            long& i_out,
            long& j_out
        ) const
        {
            using namespace std;

            long ip = 0;
            long jp = 0;


            typedef typename colm_exp<EXP>::type col_type;
            typedef typename diag_exp<EXP>::type diag_type;

            scalar_type ip_val = -numeric_limits<scalar_type>::infinity();
            scalar_type jp_val = numeric_limits<scalar_type>::infinity();

            // loop over the alphas and find the maximum ip and in indices.
            for (long i = 0; i < alpha.nr(); ++i)
            {
                if (y(i) == 1)
                {
                    if (alpha(i) < Cp)
                    {
                        if (-df(i) > ip_val)
                        {
                            ip_val = -df(i);
                            ip = i;
                        }
                    }
                }
                else
                {
                    if (alpha(i) > 0.0)
                    {
                        if (df(i) > ip_val)
                        {
                            ip_val = df(i);
                            ip = i;
                        }
                    }
                }
            }

            scalar_type Mp = -numeric_limits<scalar_type>::infinity();

            // Pick out the column and diagonal of Q we need below.  Doing
            // it this way is faster if Q is actually a symmetric_matrix_cache
            // object.
            col_type Q_ip = colm(Q,ip);
            diag_type Q_diag = diag(Q);



            // now we need to find the minimum jp indices
            for (long j = 0; j < alpha.nr(); ++j)
            {
                if (y(j) == 1)
                {
                    if (alpha(j) > 0.0)
                    {
                        scalar_type b = ip_val + df(j);
                        if (df(j) > Mp)
                            Mp = df(j);

                        if (b > 0)
                        {
                            scalar_type a = Q_ip(ip) + Q_diag(j) - 2*y(ip)*Q_ip(j); 
                            if (a <= 0)
                                a = tau;
                            scalar_type temp = -b*b/a;
                            if (temp < jp_val)
                            {
                                jp_val = temp;
                                jp = j;
                            }
                        }
                    }
                }
                else
                {
                    if (alpha(j) < Cn)
                    {
                        scalar_type b = ip_val - df(j);
                        if (-df(j) > Mp)
                            Mp = -df(j);

                        if (b > 0)
                        {
                            scalar_type a = Q_ip(ip) + Q_diag(j) + 2*y(ip)*Q_ip(j); 
                            if (a <= 0)
                                a = tau;
                            scalar_type temp = -b*b/a;
                            if (temp < jp_val)
                            {
                                jp_val = temp;
                                jp = j;
                            }
                        }
                    }
                }
            }

            // if we are at the optimal point then return false so the caller knows
            // to stop optimizing
            if (Mp + ip_val < eps)
                return false;


            i_out = ip;
            j_out = jp;

            return true;
        }

    // ------------------------------------------------------------------------------------

        template <
            typename EXP,
            typename EXP2,
            typename T, typename U
            >
        inline void optimize_working_pair (
            T& alpha,
            const matrix_exp<EXP>& Q,
            const matrix_exp<EXP2>& y,
            const U& df,
            const scalar_type& tau,
            const long i,
            const long j,
            const scalar_type& Cp,
            const scalar_type& Cn
        ) const
        {
            const scalar_type Ci = (y(i) > 0 )? Cp : Cn;
            const scalar_type Cj = (y(j) > 0 )? Cp : Cn;

            if (y(i) != y(j))
            {
                scalar_type quad_coef = Q(i,i)+Q(j,j)+2*Q(j,i);
                if (quad_coef <= 0)
                    quad_coef = tau;
                scalar_type delta = (-df(i)-df(j))/quad_coef;
                scalar_type diff = alpha(i) - alpha(j);
                alpha(i) += delta;
                alpha(j) += delta;

                if (diff > 0)
                {
                    if (alpha(j) < 0)
                    {
                        alpha(j) = 0;
                        alpha(i) = diff;
                    }
                }
                else
                {
                    if (alpha(i) < 0)
                    {
                        alpha(i) = 0;
                        alpha(j) = -diff;
                    }
                }

                if (diff > Ci - Cj)
                {
                    if (alpha(i) > Ci)
                    {
                        alpha(i) = Ci;
                        alpha(j) = Ci - diff;
                    }
                }
                else
                {
                    if (alpha(j) > Cj)
                    {
                        alpha(j) = Cj;
                        alpha(i) = Cj + diff;
                    }
                }
            }
            else
            {
                scalar_type quad_coef = Q(i,i)+Q(j,j)-2*Q(j,i);
                if (quad_coef <= 0)
                    quad_coef = tau;
                scalar_type delta = (df(i)-df(j))/quad_coef;
                scalar_type sum = alpha(i) + alpha(j);
                alpha(i) -= delta;
                alpha(j) += delta;

                if(sum > Ci)
                {
                    if(alpha(i) > Ci)
                    {
                        alpha(i) = Ci;
                        alpha(j) = sum - Ci;
                    }
                }
                else
                {
                    if(alpha(j) < 0)
                    {
                        alpha(j) = 0;
                        alpha(i) = sum;
                    }
                }

                if(sum > Cj)
                {
                    if(alpha(j) > Cj)
                    {
                        alpha(j) = Cj;
                        alpha(i) = sum - Cj;
                    }
                }
                else
                {
                    if(alpha(i) < 0)
                    {
                        alpha(i) = 0;
                        alpha(j) = sum;
                    }
                }

            }
        }

    // ------------------------------------------------------------------------------------

        column_matrix df; // gradient of f(alpha)
    };

// ----------------------------------------------------------------------------------------

}

#endif // DLIB_SOLVE_QP3_USING_SMo_H__


