//
// Created by marvin on 20.11.18.
//

#include "fmm.h"
#include <typeinfo>
#include <algorithm>

#include "fmm_sub.h"
#include "../open_list_factory.h"
#include "../option_parser.h"
#include "../tasks/root_task.h"
#include "../task_utils/task_properties.h"

#include "../evaluators/g_evaluator.h"
#include "../evaluators/sum_evaluator.h"
#include "../evaluators/max_evaluator.h"
#include "../evaluators/weighted_double_evaluator.h"
#include "../open_lists/tiebreaking_open_list.h"

using namespace std;

namespace fmm {

//----------------------- Bidirectional Search -----------------------
    FmmSearch::FmmSearch(const options::Options &opts)
            : SearchEngine(opts),
              p(compute_fraction_p(true, opts.get<double>("p"))),
              p_inverted(compute_fraction_p(false, opts.get<double>("p"))),
              backward_task(make_shared<backward_tasks::BackwardTask>(task)),
              engine_forward(make_shared<fmm::FmmSubSearch>(
                      get_ops(true, opts), true, tasks::g_root_task)),
              engine_backward(make_shared<fmm::FmmSubSearch>(
                      get_ops(false, opts), false, backward_task)),
              reopen_closed_nodes(opts.get<bool>("reopen_closed")),
              print_g_groups(opts.get<bool>("print_g_groups")){
        task_properties::verify_no_axioms(task_proxy);
        task_properties::verify_no_conditional_effects(task_proxy);
        cout << "Initializing fMM search..." << endl;
    }

    void FmmSearch::initialize() {
        cout << "Conducting fractional MM search"
             << "with bound = " << bound << "."
             << endl;

        cout << "- - Initialize Search Forward:" << endl;
        engine_forward->initialize_public();
        cout << "- Initialize Search Backward:" << endl;
        engine_backward->initialize_public();

        //First step to expand all artificial goals.
        engine_backward->step(engine_forward, backward_task);
    }

    SearchStatus FmmSearch::step() {
        pair<double,bool> pr_min_F = engine_forward->get_lowest_open_value();
        pair<double,bool> pr_min_B = engine_backward->get_lowest_open_value();

        double c = min(pr_min_F.first,pr_min_B.first);

        // Empty open list
        if (c == INFTY){
            if (engine_forward->found_solution()){
                set_plan(engine_backward->get_plan());
                return SOLVED;
            } else {
                cout << "Completely explored state space -- no solution!" << endl;
                return FAILED;
            }
        }

        // Found solution is optimal
        int u = engine_forward->current_plan_cost;
        u = (u==-1)?fmm::INFTY:u;

        // u <= max (C, fminF, fminB, gminF + gminB + epsilon)
        int epsilon = task_properties::get_min_operator_cost(task_proxy);
        int fw_min_f = engine_forward->min_f.begin()->first;
        int bw_min_f = engine_backward->min_f.begin()->first;
        int fw_min_g = engine_forward->min_g.begin()->first;
        int bw_min_g = engine_backward->min_g.begin()->first;
        double temp1 = max(fw_min_f,bw_min_f);
        double path = fw_min_g + bw_min_g + epsilon;
        double temp2 = max(c, path);// here -1 to fix
        if (u <= max(temp1,temp2)){
            set_plan(engine_backward->get_plan());
            ((p == 1)?engine_forward:engine_backward)->gather_and_print_g_groups();
            return SOLVED;
        }


        bool expandForward = (c == pr_min_F.first);
        if (pr_min_B == pr_min_F) {
            expandForward = pr_min_F.second;
        }

        // Expanding next state
        if (expandForward){
            engine_forward->step(engine_backward, backward_task);
            engine_forward->update_f_value_statistics(c);
        } else {
            engine_backward->step(engine_forward, backward_task);
            engine_backward->update_f_value_statistics(c);
        }

        return IN_PROGRESS;
    }

    options::Options FmmSearch::get_ops(bool forward_flag, const options::Options &opts) const {
        using Ev = Evaluator;

        using GEval = g_evaluator::GEvaluator;
        using SumEval = sum_evaluator::SumEvaluator;
        using MaxEval = max_evaluator::MaxEvaluator;
        using WeightEval = weighted_double_evaluator::WeightedDoubleEvaluator;

        // Evaluators
        shared_ptr<Ev> g = make_shared<GEval>();
        shared_ptr<Ev> h;
        double fraction_p;
        if (forward_flag){
            h = opts.get<shared_ptr<Ev>>("eval");
            fraction_p = p;
        } else {
            h = opts.get<shared_ptr<Ev>>("b_eval");
            fraction_p = p_inverted;
        }
        //Scaling to get more precision
        shared_ptr<Ev> g_scaled = make_shared<WeightEval>(g, 10000);
        shared_ptr<Ev> h_scaled = make_shared<WeightEval>(h, 10000);

        shared_ptr<Ev> g_fraction_scaled = make_shared<WeightEval>(g_scaled, fraction_p);

        shared_ptr<Ev> f = make_shared<SumEval>(vector<shared_ptr<Ev>>({g, h}));
        shared_ptr<Ev> f_scaled = make_shared<SumEval>(vector<shared_ptr<Ev>>({g_scaled, h_scaled}));
        Options max_opts;
        max_opts.set("evals", vector<shared_ptr<Ev>>({f_scaled, g_fraction_scaled}));
        shared_ptr<Ev> pr = make_shared<MaxEval>(max_opts);
        vector<shared_ptr<Ev>> open_evals = {pr}; // no tie-breaking?

        // Open List
        Options wait_opts;
        wait_opts.set("evals", open_evals);
        wait_opts.set("pref_only", false);
        wait_opts.set("unsafe_pruning", false);
        shared_ptr<OpenListFactory> open =
                make_shared<tiebreaking_open_list::TieBreakingOpenListFactory>(wait_opts);

        // Engine Options
        Options new_opts = opts;
        new_opts.set("open", open);
        new_opts.set("f_eval", f); // only f as heuristic
        new_opts.set("g_eval", g);
        new_opts.set("fraction_p", fraction_p);
        new_opts.set("reopen_closed", true);
        vector<shared_ptr<Ev>> preferred_list;
        new_opts.set("preferred", preferred_list);

        return new_opts;
    }

    double FmmSearch::compute_fraction_p(bool forward_flag, double fraction_p) const{
        if (forward_flag){
            if (fraction_p == 0) {
                fraction_p = fmm::INFTY;
            } else {
                fraction_p = 1.0/fraction_p;
            }
        } else {
            if (fraction_p == 1) {
                fraction_p = fmm::INFTY;
            } else {
                fraction_p = 1.0/(1-fraction_p);
            }
        }
        return fraction_p;
    }

    void FmmSearch::print_statistics() const {
        #define f engine_forward->get_statistics()
        #define b engine_backward->get_statistics()
        #define f2 engine_forward->get_expanded_statistics()
        #define b2 engine_backward->get_expanded_statistics()

        cout << "- - Forward Statistics:" << endl;

        engine_forward->printDirectionalStatistic(backward_task);
        cout << "F_Expansions before Meeting: " << f2.get_expansions_before_first_meeting() << " state(s)." << endl;

        cout << "- - Backward Statistics:" << endl;

        engine_backward->printDirectionalStatistic(backward_task);
        cout << "B_Pruned: " << b2.get_pruned() << " states(s)." << endl;
        cout << "B_Initial_Goals: " << b2.get_real_goal_states() << " state(s)." << endl;
        cout << "B_Expansions before Meeting: " << b2.get_expansions_before_first_meeting() << " state(s)." << endl;

        cout << "- - Bidirectional Statistics:" << endl;

        cout << "Expanded " << b.get_expanded() + f.get_expanded() << " state(s)." << endl;
        cout << "Reopened " << b.get_reopened() + f.get_reopened() << " state(s)." << endl;
        cout << "Evaluated " << b.get_evaluated_states() + f.get_evaluated_states() << " state(s)." << endl;
        cout << "Evaluations: " << b.get_evaluations() + f.get_evaluations() << endl;
        cout << "Generated " << b.get_generated() + f.get_generated()<< " state(s)." << endl;
        cout << "Branching Ratio: " << ((b.get_generated()==0)?0:(double)(f.get_generated())/b.get_generated()) << "." << endl;
        cout << "Frontier Meetings: " << f2.get_frontier_meetings() + b2.get_frontier_meetings() << " time(s)." << endl;

        cout << "Meeting Point - f_g: " << engine_forward->current_plan_g
             << ", f_h: " << engine_forward->current_plan_h
             << ", b_g: " << engine_backward->current_plan_g
             << ", b_h: " << engine_backward->current_plan_h << endl;

        cout << "- - Search Statistics:" << endl;
    }

}