% We use gradient descent to ascend g:R3-->R

% Params
dStepSize = 1;
initialGuess = [0, 0, 0];

% Define g
f1 = @(vX) 3*vX(1) - cos(vX(2)*vX(3))-(1/2);
f2 = @(vX) (vX(1)^2) - (81*(vX(2)+0.1)^2) + sin(vX(3))+1.06;
f3 = @(vX) exp(-vX(1)*vX(2)) + 20*vX(3) + ((10*pi-3)/3);

g = @(vX) f1(vX)^2 + f2(vX)^2 + f3(vX)^2;

grad_g = @(vX) [ 2*f1(vX)*3 + 2*f2(vX)*2*vX(1) + 2*f3(vX)*(-vX(1))*exp(-vX(1)*vX(2)), ...
                    2*f1(vX)*vX(3)*sin(vX(2)*vX(3)) - 2*f2(vX)*(2*81*(vX(2)+0.1)) + 2*f3(vX)*((-vX(1))*exp(-vX(1)*vX(2))), ...
                    2*f1(vX)*vX(2)*sin(vX(2)*vX(3)) + 2*f2(vX)*cos(vX(3))+2*f3(vX)*20 ];

% The magnitude of grad_g at vX
dMagnitude = @(vX) norm(grad_g(vX));

% A unit vector in the direction the slope points at vX
vecDirection = @(vX) grad_g(vX)./dMagnitude(vX);


% Now start looping.

% vecP0 represents our current best guess.
vecP0 = initialGuess;
vecP1 = initialGuess;
for k = 1:3
    % Update our best guess
    vecP0 = vecP1;

    % Now from our guess, we're going to make 3 new guesses, corresponding to different step sizes.
    alpha1 = 0;
    alpha2 = dStepSize/2;
    alpha3 = dStepSize;

    % Form guess position vectors
    vecA1Guess = vecP0 - alpha1*vecDirection(vecP0);
    vecA2Guess = vecP0 - alpha2*vecDirection(vecP0);
    vecA3Guess = vecP0 - alpha3*vecDirection(vecP0);

    % Evaluate the function at each guess
    dGuessA1Height = g(vecA1Guess);
    dGuessA2Height = g(vecA2Guess);
    dGuessA3Height = g(vecA3Guess);

    h1 = (dGuessA2Height-dGuessA1Height)/(alpha2-alpha1);
    h2 = (dGuessA3Height-dGuessA2Height)/(alpha3-alpha2);
    h3 = (dGuessA3Height-dGuessA1Height)/(alpha3-alpha1);

    % We will use these guesses to make a new, 'descended guess'
    vecDescendedGuess = [0,0,0];
    while dGuessA3Height >= dGuessA1Height
        % Take a smaller step
        alpha3 = alpha3/2;
        alpha2 = alpha3/2;

        % Form guess position vectors
        vecA1Guess = vecP0 - alpha1*vecDirection(vX);
        vecA2Guess = vecP0 - alpha2*vecDirection(vX);
        vecA3Guess = vecP0 - alpha3*vecDirection(vX);

        % Evaluate the function at each guess
        dGuessA1Height = g(vecA1Guess);
        dGuessA2Height = g(vecA2Guess);
        dGuessA3Height = g(vecA3Guess);

        h1 = (dGuessA2Height-dGuessA1Height)/(alpha2-alpha1);
        h2 = (dGuessA3Height-dGuessA2Height)/(alpha3-alpha2);
        h3 = (dGuessA3Height-dGuessA1Height)/(alpha3-alpha1);

        vecA = [dGuessA1Height; dGuessA3Height; dGuessA3Height];
        vecB = [vecA1Guess; vecA2Guess; vecA3Guess]'
        [~, idx] = max(A)
        vecDescendedGuess = B(idx);
    end

    alphaK = -(h1-alpha2*h3)/(2*h3);

    % Form a new best guess
    vecP1 = vecP0 - alphaK*vecDirection(vecDescendedGuess)

end