//
// Created by marvin on 14.08.2018.
//
#include <typeinfo>
#include <algorithm>

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

#include "nbs_search.h"

#include "../evaluators/g_evaluator.h"
#include "../evaluators/sum_evaluator.h"
#include "../evaluators/weighted_evaluator.h"
#include "../open_lists/tiebreaking_open_list.h"

using namespace std;

namespace nbs_search {

//----------------------- Bidirectional Search -----------------------
    NBSSearch::NBSSearch(const options::Options &opts)
            : SearchEngine(opts),
              backward_task(make_shared<backward_tasks::BackwardTask>(task)),
              engine_forward(make_shared<nbs_search::DirectionalSearch>(
                      get_ops(true, opts), true, tasks::g_root_task)),
              engine_backward(make_shared<nbs_search::DirectionalSearch>(
                      get_ops(false, opts), false, backward_task)),
              reopen_closed_nodes(opts.get<bool>("reopen_closed")) {
        task_properties::verify_no_axioms(task_proxy);
        task_properties::verify_no_conditional_effects(task_proxy);
        cout << "Initializing NBS search..." << endl;
    }

    void NBSSearch::initialize() {
        cout << "Conducting bidirectional search"
             << "with bound = " << bound << "."
             << endl;

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

    SearchStatus NBSSearch::step() {
        SearchStatus stoppingStatus = prepare_best();
        if (stoppingStatus != IN_PROGRESS){
            return stoppingStatus;
        }

        //Backward
        engine_backward->step(engine_forward, backward_task);
        stoppingStatus = check_solved_condition();
        if (stoppingStatus != IN_PROGRESS){
            return stoppingStatus;
        }

        //Forward
        engine_forward->step(engine_backward, backward_task);
        stoppingStatus = check_solved_condition();
        if (stoppingStatus != IN_PROGRESS){
            return stoppingStatus;
        }

        return IN_PROGRESS;
    }

    SearchStatus NBSSearch::prepare_best(){
        // Check if either list is empty, and no solution found
        SearchStatus s = check_failed_condition();
        if (s != IN_PROGRESS){
            return s;
        }

        // Waiting to ready
        engine_forward->flush_update(clb);
        engine_backward->flush_update(clb);

        int f_ready, b_ready;
        int f_waiting, b_waiting;

        while (true){
            // c(F) + c(B) <= clb : expand next states
            f_ready = engine_forward->get_lowest_ready_value();
            b_ready = engine_backward->get_lowest_ready_value();
            #ifdef BACKWARD_DEBUG
            cout << "f_ready: " << f_ready << ", b_ready: " << b_ready << ", clb: " << clb << endl;
            #endif
            if (f_ready <= clb && b_ready <= clb && f_ready + b_ready  <= clb){
                return check_solved_condition();
            }
            f_waiting = engine_forward->get_lowest_waiting_value();
            b_waiting = engine_backward->get_lowest_waiting_value();
            #ifdef BACKWARD_DEBUG
            cout << "f_waiting: " << f_waiting << ", b_waiting: " << b_waiting << endl;
            #endif
            if (f_waiting <= clb || b_waiting <= clb){
                if (f_waiting < b_waiting){ // find best
                    engine_forward->flush_state(false, -1);
                } else if (f_waiting > b_waiting){
                    engine_backward->flush_state(false, -1);
                } else if (f_waiting != INFTY) {
                    engine_forward->flush_state(false, -1);
                    engine_backward->flush_state(false, -1);
                }
            } else {
                int clbTemp = min(min(f_waiting, b_waiting), (f_ready == INFTY || b_ready == INFTY)?INFTY:f_ready + b_ready);
                assert(clbTemp > clb);
                clb = clbTemp;
                engine_forward->update_f_value_statistics(clb);
                engine_backward->update_f_value_statistics(clb);
            }
        }
    }

    SearchStatus NBSSearch::check_solved_condition(){
        if (engine_forward->found_solution() &&
            engine_forward->current_plan_cost <= clb){
            set_plan(engine_backward->get_plan());
            return SOLVED;
        } else {
            return IN_PROGRESS;
        }
    }

    SearchStatus NBSSearch::check_failed_condition(){
        if (! (engine_forward->ready_open_list->empty() && engine_forward->waiting_open_list->empty())){
            if (! (engine_backward->ready_open_list->empty() && engine_backward->waiting_open_list->empty())){
                return IN_PROGRESS;
            }
        }
        if (found_solution()){
            set_plan(engine_backward->get_plan());
            return SOLVED;
        } else {
            cout << "Completely explored state space -- no solution!" << endl;
            return FAILED;
        }
    }

    options::Options NBSSearch::get_ops(bool forward_flag, const options::Options &opts) const {
        using GEval = g_evaluator::GEvaluator;
        using SumEval = sum_evaluator::SumEvaluator;

        // Evaluators
        shared_ptr<Evaluator> g = make_shared<GEval>();
        shared_ptr<Evaluator> h;
        if (!forward_flag){
            h = opts.get<shared_ptr<Evaluator>>("b_eval");
        } else {
            h = opts.get<shared_ptr<Evaluator>>("eval");
        }

        shared_ptr<Evaluator> f = make_shared<SumEval>(vector<shared_ptr<Evaluator>>({g, h}));
        vector<shared_ptr<Evaluator>> wait_evals = {f, g}; // cost tie-breaking
        vector<shared_ptr<Evaluator>> ready_evals = {g}; // no tie-breaking?

        // Waiting_List
        Options wait_opts;
        wait_opts.set("evals", wait_evals);
        wait_opts.set("pref_only", false);
        wait_opts.set("unsafe_pruning", false);
        shared_ptr<OpenListFactory> waiting =
                make_shared<tiebreaking_open_list::TieBreakingOpenListFactory>(wait_opts);

        // Ready List
        Options ready_opts;
        ready_opts.set("evals", ready_evals);
        ready_opts.set("pref_only", false);
        ready_opts.set("unsafe_pruning", false);
        shared_ptr<OpenListFactory> ready =
                make_shared<tiebreaking_open_list::TieBreakingOpenListFactory>(ready_opts);

        // Engine Options
        Options new_opts = opts;
        new_opts.set("waiting_open", waiting);
        new_opts.set("ready_open", ready);
        new_opts.set("f_eval", f); // only f as heuristic
        new_opts.set("g_eval", g);
        new_opts.set("reopen_closed", true);
        vector<shared_ptr<Evaluator>> preferred_list;
        new_opts.set("preferred", preferred_list);

        return new_opts;
    }


    void NBSSearch::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_Dead_Goals: " << b2.get_dead_goal_states() << " 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: " << (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;
    }
}