#include "optimization.h"
#include <thread>
#include <iostream>
#include <sstream>

using namespace optimization;

//namespace optimization {

//best x setter
OptimizerABC&
OptimizerABC::xopt(const vector<data_t>& x) {
	if (_xs.n_rows() < 1) {
		_xs = Matrix<data_t>(x, 1, x.size());
		_xopt = x;
		return *this;
	}
	if (x.size() != _xs.n_cols()) {
		throw new std::invalid_argument("xopt has wrong shape");
	}
	vector<data_t> data(_xs.data());
	data.insert(data.end(), x.begin(), x.end());
    _xs = Matrix<double>(data, _xs.n_rows() + 1, x.size());
    _xopt = x;
    return *this;
}

void OptimizerABC::_find_opt() {
    _fopt = *std::min_element(_fs.begin(), _fs.end());
    vector<size_t> ind = common::arg_ksmallest(_fs, 1);
    _xopt = _xs(ind[0]);
}

//best function value setter
OptimizerABC& OptimizerABC::fopt(score_t fval) {
    _n_evals.push_back(_total_evals);
    _fs.push_back(fval);
    _fopt = fval;
    return *this;
}

score_t OptimizerABC::func(const vector<data_t>& x) {
    _total_evals++;
    score_t res = _fitness->func(x);
    return res;
}

void OptimizerABC::run(FitnessABC* fitness, const InteropNegotiator& negotiator) {
    _total_evals = 0;
	_xs = Matrix<data_t>(0,0);
    _n_evals.clear();
    _fs.clear();
    _fitness = fitness;
    _run(negotiator);
    _find_opt();
}


CoordinateDescentSettings::CoordinateDescentSettings(size_t n_dimensions)
    :x_init(n_dimensions), x_grid(n_dimensions) {
    for(size_t i = 0; i < n_dimensions; i++) {
        x_init[i] = _DEFAULT_CD_INIT_VAL_;
        x_grid[i] = vector<data_t>({ _DEFAULT_CD_GRID_ });
    }
}

CoordinateDescent::CoordinateDescent(const CoordinateDescentSettings& settings)
    :_settings(settings), _current_x(_settings.x_init) {
    if(_settings.x_grid.size() != _settings.x_init.size())
        throw std::invalid_argument("x_grid has wrong dimensionality");

}

void CoordinateDescent::_run(const InteropNegotiator& negotiator) {
    if(negotiator.message)
        negotiator.message(negotiator.control_frame_handle, "running coordinate descent...");

    xopt(_current_x);
    fopt(func(xopt())); //cmp current optimum
    size_t n = _settings.x_grid.size();
    //cmp total function evaluations (to report progress)
    size_t needed_func_evals = 0;
    for(size_t i = 0; i < n; i++) {
        needed_func_evals += _settings.n_iter * _settings.x_grid[i].size();
    }
    //find local minimum
    for(size_t it = 0; it < _settings.n_iter; it++) {
        std::vector<size_t> axis_order;
        for(size_t i = 0; i < n; i++)
            axis_order.push_back(i);
        if(_settings.random_order)
            std::random_shuffle(axis_order.begin(), axis_order.end());
        for(size_t idx = 0; idx < n; idx++) {
            size_t axis = axis_order[idx];
            //line search across current coordinate
            _current_x = this->xopt();
            for(size_t i = 0; i < _settings.x_grid[axis].size(); i++) {
                _current_x[axis] = _settings.x_grid[axis][i];
                score_t fval = func(_current_x);
                if(fval < fopt()) {
                    fopt(fval);
                    xopt(_current_x);
                    std::stringstream ss;
                    ss << "f: " << fopt() << " , x: ";
                    for(size_t i = 0; i < xopt().size(); i++)
                        ss << xopt()[i] << ", ";
                    if(negotiator.message)
                        negotiator.message(negotiator.control_frame_handle, ss.str().c_str());
                }
                if(negotiator.report_progress)
                    negotiator.report_progress(negotiator.control_frame_handle,
                    total_evals() / static_cast<float>(needed_func_evals));
                if(cancellation_pending) {
                    cancellation_pending = false;
                    return;
                }
            }
        }
    }
}


//} //namespace optimization