#include "eager_tunnel_search.h"

#include "globals.h"
#include "heuristic.h"
#include "option_parser.h"
#include "successor_generator.h"
#include "g_evaluator.h"
#include "sum_evaluator.h"
#include "plugin.h"

#include <cassert>
#include <cstdlib>
#include <set>
using namespace std;

EagerTunnelSearch::EagerTunnelSearch(
    const Options &opts)
    : EagerSearch(opts),
      reopen_closed_nodes(opts.get<bool>("reopen_closed")),
      do_pathmax(opts.get<bool>("pathmax")),
      use_multi_path_dependence(opts.get<bool>("mpd")),
      open_list(opts.get<OpenList<StateID> *>("open")) {
    if (opts.contains("f_eval")) {
        f_evaluator = opts.get<ScalarEvaluator *>("f_eval");
    } else {
        f_evaluator = 0;
    }
    if (opts.contains("preferred")) {
        preferred_operator_heuristics =
            opts.get_list<Heuristic *>("preferred");
    }
}

void EagerTunnelSearch::initialize() {
    //TODO children classes should output which kind of search
    cout << "Conducting best first search with Tunnel"
         << (reopen_closed_nodes ? " with" : " without")
         << " reopening closed nodes, (real) bound = " << bound
         << endl;
    if (do_pathmax)
        cout << "Using pathmax correction" << endl;
    if (use_multi_path_dependence)
        cout << "Using multi-path dependence (LM-A*)" << endl;
    assert(open_list != NULL);

    set<Heuristic *> hset;
    open_list->get_involved_heuristics(hset);

    for (set<Heuristic *>::iterator it = hset.begin(); it != hset.end(); it++) {
        estimate_heuristics.push_back(*it);
        search_progress.add_heuristic(*it);
    }

    // add heuristics that are used for preferred operators (in case they are
    // not also used in the open list)
    hset.insert(preferred_operator_heuristics.begin(),
                preferred_operator_heuristics.end());

    // add heuristics that are used in the f_evaluator. They are usually also
    // used in the open list and hence already be included, but we want to be
    // sure.
    if (f_evaluator) {
        f_evaluator->get_involved_heuristics(hset);
    }

    for (set<Heuristic *>::iterator it = hset.begin(); it != hset.end(); it++) {
        heuristics.push_back(*it);
    }

    assert(!heuristics.empty());

    const State &initial_state = g_initial_state();
    for (size_t i = 0; i < heuristics.size(); i++)
        heuristics[i]->evaluate(initial_state);
    open_list->evaluate(0, false);
    search_progress.inc_evaluated_states();
    search_progress.inc_evaluations(heuristics.size());

    if (open_list->is_dead_end()) {
        cout << "Initial state is a dead end." << endl;
    } else {
        search_progress.get_initial_h_values();
        if (f_evaluator) {
            f_evaluator->evaluate(0, false);
            search_progress.report_f_value(f_evaluator->get_value());
        }
        search_progress.check_h_progress(0);
        SearchNode node = search_space.get_node(initial_state);
        node.open_initial(heuristics[0]->get_value());

        open_list->insert(initial_state.get_id());
    }
     create_tunnel_map();
}

SearchStatus EagerTunnelSearch::step() {
    pair<SearchNode, bool> n = fetch_next_node();
    if (!n.second) {
        return FAILED;
    }
    SearchNode node = n.first;

    State s = node.get_state();

    if (check_goal_and_set_plan(s))
        return SOLVED;

    vector<const Operator *> applicable_ops;
    set<const Operator *> preferred_ops;

    g_successor_generator->generate_applicable_ops(s, applicable_ops);


    //changed in eager_tunnel_search

    //check in tunnel_map, if last_op allows Tunnel
    const Operator *last_op = node.get_creating_operator();
    
    if (last_op){
        for(int i=0;i<tunnelMap.size();i++){
            if(last_op==tunnelMap[i].first){
                if(!tunnelMap[i].second.empty()){
                    applicable_ops=tunnelMap[i].second;
                    break;
                }

            }
        }
    }




    // This evaluates the expanded state (again) to get preferred ops
    for (int i = 0; i < preferred_operator_heuristics.size(); i++) {
        Heuristic *h = preferred_operator_heuristics[i];
        h->evaluate(s);
        if (!h->is_dead_end()) {
            // In an alternation search with unreliable heuristics, it is
            // possible that this heuristic considers the state a dead end.
            vector<const Operator *> preferred;
            h->get_preferred_operators(preferred);
            preferred_ops.insert(preferred.begin(), preferred.end());
        }
    }
    search_progress.inc_evaluations(preferred_operator_heuristics.size());


    for (int i = 0; i < applicable_ops.size(); i++) {
        const Operator *op = applicable_ops[i];

        if ((node.get_real_g() + op->get_cost()) >= bound)
            continue;

        State succ_state = g_state_registry->get_successor_state(s, *op);
        search_progress.inc_generated();
        bool is_preferred = (preferred_ops.find(op) != preferred_ops.end());

        SearchNode succ_node = search_space.get_node(succ_state);

        // Previously encountered dead end. Don't re-evaluate.
        if (succ_node.is_dead_end())
            continue;

        // update new path
        if (use_multi_path_dependence || succ_node.is_new()) {
            bool h_is_dirty = false;
            for (size_t i = 0; i < heuristics.size(); ++i) {
                /*
                  Note that we can't break out of the loop when
                  h_is_dirty is set to true or use short-circuit
                  evaluation here. We must call reach_state for each
                  heuristic for its side effects.
                */
                if (heuristics[i]->reach_state(s, *op, succ_state))
                    h_is_dirty = true;
            }
            if (h_is_dirty && use_multi_path_dependence)
                succ_node.set_h_dirty();
        }

        if (succ_node.is_new()) {
            // We have not seen this state before.
            // Evaluate and create a new node.
            for (size_t i = 0; i < heuristics.size(); i++)
                heuristics[i]->evaluate(succ_state);
            succ_node.clear_h_dirty();
            search_progress.inc_evaluated_states();
            search_progress.inc_evaluations(heuristics.size());

            // Note that we cannot use succ_node.get_g() here as the
            // node is not yet open. Furthermore, we cannot open it
            // before having checked that we're not in a dead end. The
            // division of responsibilities is a bit tricky here -- we
            // may want to refactor this later.
            open_list->evaluate(node.get_g() + get_adjusted_cost(*op), is_preferred);
            bool dead_end = open_list->is_dead_end();
            if (dead_end) {
                succ_node.mark_as_dead_end();
                search_progress.inc_dead_ends();
                continue;
            }

            //TODO:CR - add an ID to each state, and then we can use a vector to save per-state information
            int succ_h = heuristics[0]->get_heuristic();
            if (do_pathmax) {
                if ((node.get_h() - get_adjusted_cost(*op)) > succ_h) {
                    //cout << "Pathmax correction: " << succ_h << " -> " << node.get_h() - get_adjusted_cost(*op) << endl;
                    succ_h = node.get_h() - get_adjusted_cost(*op);
                    heuristics[0]->set_evaluator_value(succ_h);
                    open_list->evaluate(node.get_g() + get_adjusted_cost(*op), is_preferred);
                    search_progress.inc_pathmax_corrections();
                }
            }
            succ_node.open(succ_h, node, op);

            open_list->insert(succ_state.get_id());
            if (search_progress.check_h_progress(succ_node.get_g())) {
                reward_progress();
            }

        } else if (succ_node.get_g() > node.get_g() + get_adjusted_cost(*op)) {
            // We found a new cheapest path to an open or closed state.
            if (reopen_closed_nodes) {
                //TODO:CR - test if we should add a reevaluate flag and if it helps
                // if we reopen closed nodes, do that
                if (succ_node.is_closed()) {
                    /* TODO: Verify that the heuristic is inconsistent.
                     * Otherwise, this is a bug. This is a serious
                     * assertion because it can show that a heuristic that
                     * was thought to be consistent isn't. Therefore, it
                     * should be present also in release builds, so don't
                     * use a plain assert. */
                    //TODO:CR - add a consistent flag to heuristics, and add an assert here based on it
                    search_progress.inc_reopened();
                }
                succ_node.reopen(node, op);
                heuristics[0]->set_evaluator_value(succ_node.get_h());
                // TODO: this appears fishy to me. Why is here only heuristic[0]
                // involved? Is this still feasible in the current version?
                open_list->evaluate(succ_node.get_g(), is_preferred);

                open_list->insert(succ_state.get_id());
            } else {
                // if we do not reopen closed nodes, we just update the parent pointers
                // Note that this could cause an incompatibility between
                // the g-value and the actual path that is traced back
                succ_node.update_parent(node, op);
            }
        }
    }

    return IN_PROGRESS;
}


void EagerTunnelSearch::create_tunnel_map(){


    int TunnelLength=0;
    /*
    //---------------------------Debug---------------------------------
    cout<<"createTunnelMap "<<g_operators.size()<<endl;
    for(int i=0;i<g_goal.size();i++){
        cout<<"win condition"<<i<<" : "<< g_goal[i].first<<" , "<<g_goal[i].second<<endl;
    }
    for(int i=0;i<g_operators.size();i++){
        Operator *op =&g_operators[i];
        vector<Effect> eff=op->get_effects();
        vector<Condition> pre=op->get_preconditions();

        cout<<"Operator "<<i<<" : "<<op->get_name()<<endl;
        for(int j=0;j<pre.size();j++){
            int var=pre[j].var;
            int val=pre[j].val;
            cout<<"Condition"<<j<<": "<<var <<" , "<<val<<endl;
        }
        for(int j=0;j<eff.size();j++){
            int var=eff[j].var;
            int val=eff[j].val;
            cout<<"Effect"<<j<<": "<<var <<" , "<<val<<endl;
        }
        //----------------------------------end Debug--------------------------------

    }*/
    for(int i=0;i<g_operators.size();i++){


        bool allowsTunnel=false;
        Operator *op = &g_operators[i];

        vector<Effect> eff=op->get_effects();
        vector<Condition> pre=op->get_preconditions();


        //check if op writes a goal variable and if op sets the value of the goal variable to a goal value
        bool tunnel_goal_condition = false;

        for(int j=0;j<eff.size();j++){
            
            int var = eff[j].var;
            int val = eff[j].val;

            for (int k=0; k < g_goal.size(); k++) {

                if (g_goal[k].first == var && g_goal[k].second != val) {
                    tunnel_goal_condition = true;
                    break;
                }
            }
        }

        if (tunnel_goal_condition) {


            //compare op with all other operations and check if op allows a tunnel
            for(int j=0;j<g_operators.size();j++){

                if(j!=i){
                    Operator *op2 = &g_operators[j];
                    vector<Condition> pre2= op2->get_preconditions();
                    vector<Effect> eff2= op2->get_effects();


                    //checks if the condition of op2 is in effect of op
                    if(cond_is_in_effect(pre2,eff)){

                        //checks if op2 has some pre-post condition (v,p,p') such that (v,p) element eff
                        if(has_pre_post(pre2,eff2,eff)){

                            //checks if preconditions of op2 are in s_min
                            if(inSmin(pre2,pre,eff)){

                                //checks if op2 only effects variables effected by op
                                if(has_same_effects(eff2,eff)){
                                    allowsTunnel=true;

                                }else{
                                    allowsTunnel=false;
                                    break;
                                }
                            }else{
                                allowsTunnel=false;
                                break;
                            }

                        }else{
                            allowsTunnel=false;
                            break;
                        }

                    }

                }


            }
        }
	    
	

        //create Tunnelmap
        std::vector<const Operator*> tunnel;
        if(allowsTunnel){

            //if op allows a tunnel then find all operators that belong in the tunnel
            for(int j=0;j<g_operators.size();j++){
                if(i!=j){
                    Operator *op2 = &g_operators[j];
                    vector<Condition> pre2= op2->get_preconditions();
                    vector<Effect> eff2= op2->get_effects();
                    if(has_pre_post(pre2,eff2,eff) || has_prevail(pre2,eff2,eff) ){
                        //cout<<"Operator "<<j<<",[ "<<op2->get_name()<<" ] in Tunnel of Operator " <<i<<endl;
                        tunnel.push_back(op2);
                    }

                }


            }
            std::pair<const Operator*, std::vector<const Operator*> > map_entry(op,tunnel);
            tunnelMap.push_back(map_entry);
            TunnelLength=TunnelLength+tunnel.size();

        }

    }
    cout<<"Number of Tunnels: "<<tunnelMap.size()<<endl;
    if(tunnelMap.size()>0){

        float average=TunnelLength/tunnelMap.size();
        cout<<"Average Length of Tunnels: "<<average<<endl;
    }else{
        cout<<"Average Length of Tunnels: 0"<<endl;
    }
}


//checks if the condition of op2 is in effect of op
bool EagerTunnelSearch::cond_is_in_effect(std::vector<Condition> pre2, std::vector<Effect> eff){

    for(int i=0;i<pre2.size();i++){
        for(int j=0;j<eff.size();j++){

            if(pre2[i].var==eff[j].var && pre2[i].val==eff[j].val){
                return true;
            }
        }
    }
    return false;

}

//checks if op2 has some pre-post condition (v,p,p') such that (v,p) element eff
bool EagerTunnelSearch::has_pre_post(std::vector<Condition> pre2,std::vector<Effect> eff2,std::vector<Effect> eff){

    for(int i=0;i<pre2.size();i++){
       for(int j=0;j<eff2.size();j++){

           if(pre2[i].var==eff2[j].var){
               for(int k=0;k<eff.size();k++){
                    if(pre2[i].var==eff[k].var && pre2[i].val==eff[k].val){
                        return true;
                    }
                }
           }
        }
    }
    return false;

}


//checks if preconditions of op2 are in s_min
bool EagerTunnelSearch::inSmin(std::vector<Condition> cond, std::vector<Condition> last_cond, std::vector<Effect> last_eff){

    vector<pair<int,int> > facts_to_check; // prevail + effect facts of op (=last_cond+last_eff)

    for (int i = 0; i < last_cond.size(); i++) {

        int var = last_cond[i].var;
        int val = last_cond[i].val;

        bool found = false;
        for (int j = 0; j < last_eff.size(); j++) {

            if (last_eff[j].var == var) {
                found = true;
                break;
            }

        }

        if (!found) {
            facts_to_check.push_back(make_pair(var,val));
        }
    }
    
    for (int i = 0; i < last_eff.size(); i++) {
        facts_to_check.push_back(make_pair(last_eff[i].var, last_eff[i].val));
    }


    for(int i=0;i<cond.size();i++){
        //bool inSmin=false;

        int var = cond[i].var;
        int val = cond[i].val;

        bool condition_satisfied = false;

        for (int j = 0; j < facts_to_check.size(); j++) {
            if (facts_to_check[j].first == var && facts_to_check[j].second == val) {
                condition_satisfied = true;
                break;
            }
        }

        if (!condition_satisfied) {
            return false;
        }

    }

    return true;

}

//checks if op2 only effects variables effected by op
bool EagerTunnelSearch::has_same_effects(std::vector<Effect> eff2,std::vector<Effect> eff){

    for(int i=0;i<eff2.size();i++){
        bool has_same_effect=false;
        for(int j=0;j<eff.size();j++){
            if(eff2[i].var==eff[i].var){
                has_same_effect=true;
            }
        }
        if(!has_same_effect){
            return false;
        }
    }
    return true;

}

//checks if op2 (pre2,eff2) has a prevail (v,p) in (v,p) element eff
bool EagerTunnelSearch::has_prevail(std::vector<Condition> pre2,std::vector<Effect> eff2,std::vector<Effect> eff){

    vector<Condition> prevail;


    //get prevail
    for(int i=0;i<pre2.size();i++){
        for(int j=0;j<eff2.size();j++){

            if(pre2[i].var!=eff2[j].var){
                prevail.push_back(pre2[i]);
            }
        }
    }
    for(int i=0;i<prevail.size();i++){
        for(int j=0;j<eff.size();j++){

            if(prevail[i].var==eff[j].var && prevail[i].val==eff[j].val){
                return true;
            }
        }
    }
    return false;

}




void EagerTunnelSearch::dump_search_space() {
    search_space.dump();
}

static SearchEngine *_parse_astar_tunnel(OptionParser &parser) {
    parser.document_synopsis(
        "A* search (eager)",
        "A* is a special case of eager best first search that uses g+h "
        "as f-function. "
        "We break ties using the evaluator. Closed nodes are re-opened.");
    parser.document_note(
        "mpd option",
        "This option is currently only present for the A* algorithm and not "
        "for the more general eager search, "
        "because the current implementation of multi-path depedence "
        "does not support general open lists.");
    parser.document_note("Equivalent statements using general eager search",
        "\n```\n--search astar(evaluator)\n```\n"
        "is equivalent to\n"
        "```\n--heuristic h=evaluator\n"
        "--search eager(tiebreaking([sum([g(), h]), h], unsafe_pruning=false),\n"
        "               reopen_closed=true, pathmax=false, progress_evaluator=sum([g(), h]))\n"
        "```\n", true);
    parser.add_option<ScalarEvaluator *>("eval", "evaluator for h-value");
    parser.add_option<bool>("pathmax",
                            "use pathmax correction", "false");
    parser.add_option<bool>("mpd",
                            "use multi-path dependence (LM-A*)", "false");
    SearchEngine::add_options_to_parser(parser);
    Options opts = parser.parse();

    EagerTunnelSearch *engine = 0;
    if (!parser.dry_run()) {
        GEvaluator *g = new GEvaluator();
        vector<ScalarEvaluator *> sum_evals;
        sum_evals.push_back(g);
        ScalarEvaluator *eval = opts.get<ScalarEvaluator *>("eval");
        sum_evals.push_back(eval);
        ScalarEvaluator *f_eval = new SumEvaluator(sum_evals);

        // use eval for tiebreaking
        std::vector<ScalarEvaluator *> evals;
        evals.push_back(f_eval);
        evals.push_back(eval);
        OpenList<StateID> *open = \
            new TieBreakingOpenList<StateID>(evals, false, false);

        opts.set("open", open);
        opts.set("f_eval", f_eval);
        opts.set("reopen_closed", true);
        engine = new EagerTunnelSearch(opts);
    }

    return engine;
}

static Plugin<SearchEngine> _plugin_astar("astar_tunnel", _parse_astar_tunnel);
