/*
Optimizer.cpp
Written by Matthew Fisher

Generic non-linear optimizer.  Uses conjugate gradient descent (or other inferior/sub-component algorithms like steepest descent.)
For a full description of the Optimizer class, se Optimizer.h.
*/

const double SmallDelta = 1e-4;         //small delta used to calculate the derivative
const double LineSearchEpsilon = 1e-4;  //starting epsilon for the line search
const int LineSearchIterations = 20;    //maximum number of line search iterations

void Optimizer::ComputeGradient()
{
    double BaseError = Error();         //store the starting error
    double VStart,NewError;

    for(UINT i=0;i<Variables.Length();i++)
    {
        VStart = *(Variables[i].Target);        //store the starting weight
        *(Variables[i].Target) += SmallDelta;   //displace a small amount
        NewError = Error();                     //get the new error
        (*Variables[i].Target) = VStart;        //restore the old weight
        Variables[i].Partial = (NewError - BaseError) / SmallDelta; //approximate the partial derivative
    }
}

void Optimizer::AdaptiveLearning(double &LearningRate, double Alpha, double Beta)
{
    double BaseError = Error();     //store the old error
    ComputeGradient();
    for(UINT i=0;i<Variables.Length();i++)
    {
        *(Variables[i].Target) -= Variables[i].Partial * LearningRate;  //gradient descent
    }
    double CurError = Error();
    if(CurError < BaseError)        //if the step was good,
    {
        LearningRate *= Alpha;      //increase the learning rate
    }
    else
    {
        for(UINT i=0;i<Variables.Length();i++)
        {
            *(Variables[i].Target) += Variables[i].Partial * LearningRate;  //otherwise reset,
        }
        LearningRate *= Beta;       //and decresase the learning rate
    }
}

void Optimizer::MomentumGradientDescent(double LearningRate, double MomentumRate)
{
    ComputeGradient();
    for(UINT i=0;i<Variables.Length();i++)
    {
        *(Variables[i].Target) += -Variables[i].Partial * LearningRate + MomentumRate * Variables[i].OldPartial;    //proceed according to the previous weight change and the gradient,
        Variables[i].OldPartial = -Variables[i].Partial * LearningRate + MomentumRate * Variables[i].OldPartial;    //and store this weight change as the previous weight change
    }
}

void Optimizer::GradientDescent(double LearningRate)
{
    ComputeGradient();
    for(UINT i=0;i<Variables.Length();i++)
    {
        *(Variables[i].Target) -= Variables[i].Partial * LearningRate;  //descent along the gradient
    }
}

void Optimizer::SteepestDescent()
{
    ComputeGradient();
    for(UINT i=0;i<Variables.Length();i++)
    {
        Variables[i].LineSearchDirection = -Variables[i].Partial;       //set up the line search direction as the gradient
    }
    LineSearch();   //search in this direction
}

void Optimizer::LineSearch()
{
    bool Exit = false;
    double PrevError = Error(), CurError,   //errors
        DistPrevW, DistCurW, DistNextW, DistM1, DistM2, //five points for line search
        ErrorM, ErrorM1, ErrorM2;   //current error at the three middle points

    for(UINT i=0;i<Variables.Length();i++)
    {
        Variables[i].LineSearchStart = *(Variables[i].Target);  //store where we started
    }

    UINT LSIters = 0;
    while(!Exit)
    {
        DistPrevW = LineSearchEpsilon * pow(2,LSIters-2);
        DistCurW = LineSearchEpsilon * pow(2,LSIters-1);
        DistNextW = LineSearchEpsilon * pow(2,LSIters); //calculate the three distances
        for(UINT i=0;i<Variables.Length();i++)
        {
            *(Variables[i].Target) = Variables[i].LineSearchStart + DistNextW * Variables[i].LineSearchDirection;   //go the appropriate distance along this direction,
        }
        CurError = Error(); //get the error
        if(CurError > PrevError) Exit = true;   //if we gained error, exit the search
        else PrevError = CurError;              //otherwise keep going
        LSIters++;
        if(LSIters > 25)                        //if we're stuck in an infinite loop,
        {
            cout << "Iterations failed." << endl;
            for(UINT i=0;i<Variables.Length();i++)
            {
                *(Variables[i].Target) = Variables[i].LineSearchStart;  //reset
            }
            return;                                                     //and abort
        }
    }

    for(UINT Iterations=0;Iterations<LineSearchIterations;Iterations++)     //loop a bunch of times
    {
        DistM1 = (DistPrevW + DistCurW) * 0.5f;
        DistM2 = (DistCurW + DistNextW) * 0.5f; //get the midpoints

        for(i=0;i<Variables.Length();i++)
            *(Variables[i].Target) = Variables[i].LineSearchStart + DistM1 * Variables[i].LineSearchDirection;
        ErrorM1 = Error();                      //get the error at the left midpoint

        for(i=0;i<Variables.Length();i++)
            *(Variables[i].Target) = Variables[i].LineSearchStart + DistM2 * Variables[i].LineSearchDirection;
        ErrorM2 = Error();                      //get the error at the right midpoint

        for(i=0;i<Variables.Length();i++)
            *(Variables[i].Target) = Variables[i].LineSearchStart + DistCurW * Variables[i].LineSearchDirection;
        ErrorM = Error();                       //get the error at the central point

        if(ErrorM1 < ErrorM2 && ErrorM1 < ErrorM) { //depending on which is smaller,
            DistPrevW = DistPrevW;
            DistCurW = DistM1;
            DistNextW = DistCurW;               //load left three points
        } else if(ErrorM < ErrorM2) {
            DistPrevW = DistM1;
            DistCurW = DistCurW;
            DistNextW = DistM2;                 //load middle three points
        } else {
            DistPrevW = DistCurW;
            DistCurW = DistM2;
            DistNextW = DistNextW;              //load right three points
        }
    }
    for(i=0;i<Variables.Length();i++)           //move into the midpoint
        *(Variables[i].Target) = Variables[i].LineSearchStart + DistCurW * Variables[i].LineSearchDirection;
}

void Optimizer::ConjugateReset(int _RepeatRate)
{
    for(UINT i=0;i<Variables.Length();i++)
    {
        Variables[i].OldPartial = 0.0;          //reset beta
    }
    RepeatRate = _RepeatRate;                   //load the repeat rate
    RepeatsLeft = RepeatRate;
}

void Optimizer::ConjugateGradient()
{
    double Beta, Numerator = 0.0, Denominator = 0.0;

    ComputeGradient();

    for(UINT i=0;i<Variables.Length();i++)
    {
        Numerator += Variables[i].Partial * (Variables[i].Partial - Variables[i].OldPartial);
        Denominator += Variables[i].OldPartial * Variables[i].OldPartial;                       //load the numerator and denominator of beta
    }
    if(Denominator == 0.0) Beta = 0.0;
    else Beta = Numerator / Denominator;    //load beta

    RepeatsLeft--;
    if(RepeatsLeft == 0)
    {
        RepeatsLeft = RepeatRate;           //reset counter has come
        Beta = 0.0;
    }

    for(i=0;i<Variables.Length();i++)
    {
        Variables[i].LineSearchDirection = -Variables[i].Partial + Beta * Variables[i].LineSearchDirection; //load the line search direction based upon conj. gradient
    }
    LineSearch();       //search

    for(UINT i=0;i<Variables.Length();i++)
    {
        Variables[i].OldPartial = Variables[i].Partial; //save the current partials
    }
}