#include <iostream>
#include <math.h>
using namespace std;

#include "ArrayVector.h"
#include "Optimizer.h"
#include "Optimizer.cpp"

/*
This will demonstrate the use of the Optimizer class to find a 4th degree 
polynomial fit to a set of 10 input (x, y) pairs.
*/

//the variables we are minimizing
double a, b, c, d;

//the X-Y pairs
double XVector[10] =
{-5.0, -4.0, -3.5, -2.5, -1.0, 0.0, 1.0, 2.0, 3.0, 4.0};

double YVector[10] =
{0.0, 1.2, 2.3, 4.1, 5.6, 5.5, 4.2, 3.1, 2.0, 1.0};

//the function we are trying to minimize
double ErrorFunction()
{
    int i;
    double ErrorTotal = 0.0, CurError, CurY;
    for(i=0;i<10;i++)
    {
        CurY = a + XVector[i] * (b + XVector[i] * (c + XVector[i] * d));    //get the approximate X value
        CurError = CurY - YVector[i];                                       //get the current difference
        ErrorTotal += CurError*CurError;                                    //sum up the squared difference as the error
    }
    return ErrorTotal;
}

void LoadInitialGuess()
{
    a = b = c = d = 1e-5;
}

void OutputState()
{
    cout << "Error -> " << ErrorFunction() << endl;
    cout << "a -> " << a << endl;
    cout << "b -> " << b << endl;
    cout << "c -> " << c << endl;
    cout << "d -> " << d << endl;
    int i;
    cout << "X,\tY,\t,Guessed Y\n";
    for(i=0;i<10;i++)
    {
        cout << XVector[i] << '\t' <<
                YVector[i] << '\t' <<   //output the X-Y pair
                a + XVector[i] * (b + XVector[i] * (c + XVector[i] * d)) << endl;   //output the polynomial approximation of Y, given X
    }
}

int main()
{
    Optimizer O;        //the optimizer itself

    O.Variables.Allocate(4);        //make room for 4 variables
    O.Variables[0].Target = &a;
    O.Variables[1].Target = &b;
    O.Variables[2].Target = &c;
    O.Variables[3].Target = &d;     //load pointers to each variable
    O.ConjugateReset(10);           //reset the conjugate gradient algorithm.  If you don't know what the repeat count
                                    //means for conjugate gradient, use a value between 10 and 100.  It doesn't affect much.
    O.Error = ErrorFunction;        //load the error function intro the optimizer

    LoadInitialGuess();             //load a (bad) initial guess
    OutputState();                  //output the current state of the variables

    int i;
    for(i=0;i<100;i++)              //iterate 100 times
    {
        cout << "Conjugate gradient iteration, Error -> " << ErrorFunction() << endl;   //output the error
        O.ConjugateGradient();                                                          //optimize (1 iteration)
    }

    OutputState();                  //output the final state of the variables
}