// Copyright 2022 James Pace
// All Rights Reserved.
//
// For a license to this software contact
// James Pace at jpace121@gmail.com.
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
#include <iostream>

#include "j7s-optimization/CostFunction.hpp"
#include "j7s-optimization/SimplexSolver.hpp"
#include "j7s-optimization/NelderMead.hpp"
#include "j7s-optimization/common.hpp"

void runSimpleSimplex(const j7s::CostFunction& cost, const std::vector<double>& init_simplex);
void runNelderMead(const j7s::CostFunction& cost, const std::vector<double>& init_simplex);

int main(int, char **)
{
    const j7s::CostFunction cost(2.0, 3.0, 4.0);
    const std::vector<double> init_simplex = {-10, 0, 10};

    runSimpleSimplex(cost, init_simplex);
    runNelderMead(cost, init_simplex);
    return 0;
}

void runSimpleSimplex(const j7s::CostFunction& cost, const std::vector<double>& init_simplex)
{
    j7s::SimplexSolver solver(cost, init_simplex);

    j7s::IterationState state = j7s::IterationState::OK;
    for (int cnt = 0; cnt < 1000; cnt++)
    {
        state = solver.update();
        if (state == j7s::IterationState::CONVERGED)
        {
            break;
        }
    }
    if (state == j7s::IterationState::CONVERGED)
    {
        const auto best = solver.bestCoord();
        std::cout << "SimpleSiplex Converged! Best Input: " << best.input << " Cost: " << best.cost << std::endl;
        std::cout << "Actual Best: " << cost.actualBest()
                  << " Cost: " << cost.eval(cost.actualBest()) << std::endl;
    }
    else
    {
        std::cout << "SimpleSimplex did not converge." << std::endl;
    }
}

void runNelderMead(const j7s::CostFunction& cost, const std::vector<double>& init_simplex)
{
    j7s::NelderMead solver(cost, init_simplex);

    j7s::IterationState state = j7s::IterationState::OK;
    for (int cnt = 0; cnt < 1000; cnt++)
    {
        state = solver.update();
        if (state == j7s::IterationState::CONVERGED)
        {
            break;
        }
    }
    if (state == j7s::IterationState::CONVERGED)
    {
        const auto best = solver.bestCoord();
        std::cout << "Nelder Mead Converged! Best Input: " << best.input << " Cost: " << best.cost << std::endl;
        std::cout << "Actual Best: " << cost.actualBest()
                  << " Cost: " << cost.eval(cost.actualBest()) << std::endl;
    }
    else
    {
        std::cout << "Nelder Mead did not converge." << std::endl;
    }
}
