#include "time_heuristic.h"

#include "../global_state.h"
#include "../option_parser.h"
#include "../plugin.h"
#include "../lp/lp_solver.h"

#include "../task_utils/task_properties.h"
#include "../algorithms/dynamic_bitset.h"

#include <cstddef>
#include <limits>
#include <utility>
#include <stack>
#include <chrono>
#include <limits>
#include <iomanip>

using namespace std;

std::ostream& operator<<(std::ostream& os, const vector<int>& vec) {
    for (auto i: vec) {
        os << i << ",";
    }
    return os;
}
std::ostream& operator<<(std::ostream& os, const vector<unsigned int>& vec) {
    for (auto i: vec) {
        os << i << ",";
    }
    return os;
}
std::ostream& operator<<(std::ostream& os, const vector<vector<int>>& vec) {
    for ( const auto &row : vec )
    {
       for ( const auto &s : row ) {
            os << s << ' ';
       } 
       os << endl;
    }
    return os;
}
std::ostream& operator<<(std::ostream& os, const vector<vector<bool>>& vec) {
    for ( const auto &row : vec )
    {
       for ( const auto &s : row ) {
            os << s << ' ';
       } 
       os << endl;
    }
    return os;
} 

namespace time_heuristic {
    
TimeHeuristic::TimeHeuristic(const Options &opts)
    : Heuristic(opts), timeSteps(opts.get<int>("ts")), withRepetition(opts.get<bool>("wr")),
    iterativeApproach(opts.get<bool>("ia")), integerProgram(opts.get<bool>("ip")), debugOption(opts.get<bool>("do")), firstRun(true), firstOptimalSolution(true),
    goalsOnlyInLastTimeLayer(opts.get<bool>("gl")), restrictFlowInLastLayer(opts.get<bool>("rf")), transitionVariableOptimization(opts.get<bool>("tv")),
    debugHeuristicValues(opts.get<bool>("dv")),  limitIterative(opts.get<bool>("li")), debugConstraints(opts.get<bool>("dc")), debugFlow(opts.get<bool>("df")), solver(lp::LPSolverType::CPLEX) {
        
    task_properties::verify_no_axioms(task_proxy);
    task_properties::verify_no_conditional_effects(task_proxy);
    
    cout << "limit-iterative: " << limitIterative << endl;
    initial_timesteps_required = 0;

    if (opts.get<int>("st") < 0) {
        synchronizationSteps = timeSteps;
    } else {
        synchronizationSteps = opts.get<int>("st");
    }

    if(opts.get<int>("rd") == -1) {
        if (iterativeApproach) {
            removeDeadStates = true;
        } else {
            removeDeadStates = false;
        }
    } else {
        removeDeadStates = opts.get<int>("rd");
    }

    cout << "Remove dead states: " << removeDeadStates << "." << endl;
    cout << "Synchronization steps: " << synchronizationSteps << "." << endl;
    cout << "Time steps: " << timeSteps << "." << endl;
    
    // Verify that parameter combinations make sense.    
    if (synchronizationSteps > timeSteps) {
        cout << "Make sure, that there are more time steps than synchronization steps." << endl;
        exit(0);
    }
    
    if (restrictFlowInLastLayer && goalsOnlyInLastTimeLayer) {
        cout << "\033[1;35m" << "The following combination of parameter values is not permitted: " << "rf=true and gl=true." << "\033[0m" << endl;
        exit(0);
    }

    if (restrictFlowInLastLayer && !withRepetition) {
        cout << "\033[1;35m" << "The following combination of parameter values is not permitted: " << "rf=true and wr=false." << "\033[0m" << endl;
        exit(0);
    }

    if (!iterativeApproach && removeDeadStates) {
        cout << "\033[1;35m" << "The non-iterative approach doesn't support dead state elimination." << "\033[0m" << endl;
        exit(0);
    }
    
    cout << "Initializing time heuristic." << endl;
    cout << "Task has " << task_proxy.get_operators().size() << " operators and " << task_proxy.get_variables().size() << " variables." << endl;
    
    // Find lowest operator cost.
    minOperatorCost = std::numeric_limits<int>::max();
    for (auto o: task_proxy.get_operators()) {
        if (o.get_cost() < minOperatorCost) {
            minOperatorCost = o.get_cost();
        }
    }

    cout << "Lowest operator cost: " << minOperatorCost << endl;
    
    // The variables in and out are need in both cases, the iterative and the non-iterative approach.
    for (unsigned int varIndex = 0; varIndex < task_proxy.get_variables().size(); ++varIndex) {
        const VariableProxy& var = task_proxy.get_variables()[varIndex];
        
        // Init in and out; maybe unneccessary.
        for (int itValue = 0; itValue < var.get_domain_size(); ++itValue) {
            in[make_tuple(varIndex, itValue)] = vector<tuple<unsigned int, unsigned int, unsigned int>>();
            out[make_tuple(varIndex, itValue)] = vector<tuple<unsigned int, unsigned int, unsigned int>>();
        }

        // Evaluate in and out.
        for (unsigned int itOperator = 0; itOperator < task_proxy.get_operators().size(); ++itOperator) {
            const OperatorProxy& op = task_proxy.get_operators()[itOperator];
            int effectValue = -1;
            for (unsigned int itEffect = 0; itEffect < op.get_effects().size(); ++itEffect) {
                const EffectProxy& eff = op.get_effects()[itEffect];
                if (eff.get_fact().get_variable() == var) {
                    effectValue = eff.get_fact().get_value();
                }
            }
            int preconditionValue = -1;
            for (unsigned int itPrecon = 0; itPrecon < op.get_preconditions().size(); ++itPrecon) {
                const FactProxy& pre = op.get_preconditions()[itPrecon];
                if (pre.get_variable() == var) {
                    preconditionValue = pre.get_value();
                }
            }
            if (transitionVariableOptimization) {
                if (preconditionValue == -1) {
                    operatorHasVariableInPreconditions[make_tuple(itOperator, varIndex)] = false;
                } else {
                    operatorHasVariableInPreconditions[make_tuple(itOperator, varIndex)] = true;
                }
            }
            if (preconditionValue == -1) {
                int startValue = -1;
                int endValue = -1;
                for (int itValue = 0; itValue < var.get_domain_size(); ++itValue) {
                    startValue = itValue;
                    if (effectValue == -1) {
                        endValue = itValue;
                    } else {
                        endValue = effectValue;
                    }
                    
                    in[make_tuple(varIndex, endValue)].push_back(make_tuple(itOperator, startValue, endValue));
                    out[make_tuple(varIndex, startValue)].push_back(make_tuple(itOperator, startValue, endValue));
                }
            } else {
                int startValue = preconditionValue;
                int endValue = -1;
                if (effectValue == -1) {
                    endValue = preconditionValue;
                } else {
                    endValue = effectValue;
                }
                in[make_tuple(varIndex, endValue)].push_back(make_tuple(itOperator, startValue, endValue));
                out[make_tuple(varIndex, startValue)].push_back(make_tuple(itOperator, startValue, endValue));
            }
        }
    }
    
    // Precalculate skippable time condition equations if needed.
    if (transitionVariableOptimization) {
        for (unsigned int itOperator = 0; itOperator < task_proxy.get_operators().size(); ++itOperator) {
            bool synchronizedWithSharedVariable = false;
            if (operatorHasVariableInPreconditions[make_tuple(itOperator, 0)]) {
                synchronizedWithSharedVariable = true;
            }
            for (unsigned int itVar = 0; itVar < task_proxy.get_variables().size() - 1; ++itVar) {
                if (operatorHasVariableInPreconditions[make_tuple(itOperator, itVar + 1)] && synchronizedWithSharedVariable) {
//                if (operatorHasVariableInPreconditions[make_tuple(itOperator, itVar + 1)] && operatorHasVariableInPreconditions[make_tuple(itOperator, itVar)]) {
                    skippableSynchronizationEquations[make_tuple(itOperator, itVar)] = true;
                } else {
                    skippableSynchronizationEquations[make_tuple(itOperator, itVar)] = false;
                    if (operatorHasVariableInPreconditions[make_tuple(itOperator, itVar)] || operatorHasVariableInPreconditions[make_tuple(itOperator, itVar + 1)]) {
                        synchronizedWithSharedVariable = true;
                    }
                }
            }
        }
    }
    
    // In the non-iterative case LP can be created only once in the constructor.
    // During the computing heuristic part only the initial state constraints have to be changed.
    // We do not use forward- and backward-reachability, because we do not want to change the constraints,
    // when we change the initial state with every compute_heuristic call.
    if (!iterativeApproach) {
        
        // Setup transition LP variables and flow constraints.
        for (unsigned int varIndex = 0; varIndex < task_proxy.get_variables().size(); ++varIndex) {
            const VariableProxy& var = task_proxy.get_variables()[varIndex];
            
            // Hold goal variable indices to put them into an equation later on.
            goalVariableIndices.push_back(vector<unsigned int>());
            
            // Find goal value of the current variable.
            int goalValue = -1;
            for (unsigned int itGoal = 0; itGoal < task_proxy.get_goals().size(); ++itGoal) {
                if (task_proxy.get_goals()[itGoal].get_variable() == var) {
                    goalValue = task_proxy.get_goals()[itGoal].get_value();
                }
            }
            
            // Initialize transition LP variables.
            for (unsigned int time = 0; time < timeSteps + 1; ++time) {
                for (unsigned int itOp = 0; itOp < task_proxy.get_operators().size(); ++itOp) {
                    operatorTimeVariableIndices[make_tuple(varIndex, time, itOp)] = vector<unsigned int>();
                }
                if (time < timeSteps || withRepetition) {
                    for (int itValue = 0; itValue < var.get_domain_size(); ++itValue) {
                        vector<tuple<unsigned int, unsigned int, unsigned int>>& outgoings = out[make_tuple(varIndex, itValue)];
                        for (unsigned int itOut = 0; itOut < outgoings.size(); ++itOut) {
                            unsigned int newIndex = 0;
                            // Only put variables of first atomic projection into the objective function.
                            // Only create shared variable if single transition of current operator in current time step.
                            if (transitionVariableOptimization &&
                                operatorHasVariableInPreconditions[make_tuple(get<0>(outgoings[itOut]), varIndex)]) {
                                if (indicesForOptimization.find(make_tuple(get<0>(outgoings[itOut]), time)) == indicesForOptimization.end()) {
                                    lp::LPVariable newVar = lp::LPVariable(0, infty, varIndex == 0 ?
                                        task_proxy.get_operators()[get<0>(outgoings[itOut])].get_cost() : 0, integerProgram);
                                    variables.push_back(newVar);
                                    newIndex = variables.size() - 1;
                                    indicesForOptimization[make_tuple(get<0>(outgoings[itOut]), time)] = newIndex;
                                } else {
                                    newIndex = indicesForOptimization[make_tuple(get<0>(outgoings[itOut]), time)];
                                }
                            } else { // Always create new variable.
                                lp::LPVariable newVar = lp::LPVariable(0, infty, varIndex == 0 ?
                                    task_proxy.get_operators()[get<0>(outgoings[itOut])].get_cost() : 0, integerProgram);
                                variables.push_back(newVar);
                                newIndex = variables.size() - 1;
                            }
                            transitionVariableIndices[make_tuple(varIndex, itValue, get<0>(outgoings[itOut]), time)] = newIndex;
                            operatorTimeVariableIndices[make_tuple(varIndex, time, get<0>(outgoings[itOut]))].push_back(newIndex);
                        }
                    }
                }
            }
            
            // Set old initial value for further heuristic computations.
            oldInitialValues[varIndex] = task_proxy.get_initial_state().get_values()[varIndex];
            
            // Create constraints for every state in all time layers.
            for (unsigned int time = 0; time < timeSteps + 1; ++time) {
                for (int itValue = 0; itValue < var.get_domain_size(); ++itValue) {
                    // If initial state set bounds to 1 else to 0.
                    int upperAndLowerBound = (time == 0 && (task_proxy.get_initial_state().get_values()[varIndex] == itValue) ? 1 : 0);
                    lp::LPConstraint con(upperAndLowerBound, upperAndLowerBound);
                    // Add outgoing transitions to constraint of current state.
                    if (time < timeSteps || withRepetition) {
                        vector<tuple<unsigned int, unsigned int, unsigned int>>& outgoings = out[make_tuple(varIndex, itValue)];
                        for (unsigned int itOut = 0; itOut < outgoings.size(); ++itOut) {
                            // Do not add self loops, as they cancel out.
                            if (!(time == timeSteps && get<1>(outgoings[itOut]) == get<2>(outgoings[itOut]))) {
                                unsigned int newIndex = transitionVariableIndices[make_tuple(varIndex, get<1>(outgoings[itOut]), get<0>(outgoings[itOut]), time)];
                                con.insert(newIndex, 1);
                            }
                        }
                    }
                    // Add incoming transitions to constraint of current state.
                    // States in last layer do have two types of incoming transitions if repetition is enabled.
                    // The ones from the previous time layer and the ones from the last time layer.
                    if (time != 0 || (timeSteps == 0 && withRepetition)) {
                        vector<tuple<unsigned int, unsigned int, unsigned int>>& incomings = in[make_tuple(varIndex, itValue)];
                        for (unsigned int itIn = 0; itIn < incomings.size(); ++itIn) {
                            if (time != 0) {
                                unsigned int newIndex = transitionVariableIndices[make_tuple(varIndex, get<1>(incomings[itIn]), get<0>(incomings[itIn]), time - 1)];
                                con.insert(newIndex, -1);
                            }
                            // For states in the last time layer there might be more incoming transitions from the repetition.
                            if (time == timeSteps && withRepetition) {
                                // Do not add self loops, as they cancel out.
                                if (!(get<1>(incomings[itIn]) == get<2>(incomings[itIn]))) {
                                    unsigned int newIndex2 = transitionVariableIndices[make_tuple(varIndex, get<1>(incomings[itIn]), get<0>(incomings[itIn]), time)];
                                    con.insert(newIndex2, -1);
                                }
                            }
                        }
                    }
                    
                    // If goalsLast is true we will only have goal states in the last time layer.
                    if ((itValue == goalValue || goalValue == -1) && (goalsOnlyInLastTimeLayer ? time == timeSteps : true)) {
                        lp::LPVariable goalVar = lp::LPVariable(0, 1, 0, integerProgram);
                        variables.push_back(goalVar);
                        goalVariableIndices[varIndex].push_back(variables.size() - 1);
                        con.insert(variables.size() - 1, 1);
                    }
                    
                    constraints.push_back(con);
                    if (time == 0) {
                        firstLayerConstraintIndices[make_tuple(varIndex, itValue)] = constraints.size() - 1;
                    }
                    constraintIndices[make_tuple(varIndex, time, itValue)] = constraints.size() - 1;
                }
            }
            // Add goal variable constraint.
            lp::LPConstraint con(1, 1);
            for (auto goalVar : goalVariableIndices[varIndex]) {
                con.insert(goalVar, 1);
            }
            constraints.push_back(con);
        }
        
        // Add time synchronization constraints.
        for (unsigned int step = 0; step < synchronizationSteps + ((timeSteps == synchronizationSteps && withRepetition) ? 1 : 0); ++step) {
            for (unsigned int itOp = 0; itOp < task_proxy.get_operators().size(); ++itOp) {
                bool synchronizedWithSharedVariable = false;
                for(unsigned int varIndex = 0; varIndex < task_proxy.get_variables().size() - 1; ++varIndex) {
                    if (transitionVariableOptimization && operatorHasVariableInPreconditions[make_tuple(itOp, varIndex + 1)] &&
                        (synchronizedWithSharedVariable || operatorHasVariableInPreconditions[make_tuple(itOp, varIndex)])) {
                        continue;
                    }
                    
                    lp::LPConstraint con(0, 0);
                    vector<unsigned int>& varIndices = operatorTimeVariableIndices[make_tuple(varIndex, step, itOp)];
                    for (const auto& ind : varIndices) {
                        con.insert(ind, 1);
                    }
                    vector<unsigned int>& varIndices2 = operatorTimeVariableIndices[make_tuple(varIndex + 1, step, itOp)];
                    for (const auto& ind : varIndices2) {
                        con.insert(ind, -1);
                    }
                    constraints.push_back(con);
                    if (operatorHasVariableInPreconditions[make_tuple(itOp, varIndex + 1)]) {
                        synchronizedWithSharedVariable = true;
                    }
                }
            }
        }

        if (timeSteps != synchronizationSteps) {
            // Add summed operator flow constraints.
            for (unsigned int itOp = 0; itOp < task_proxy.get_operators().size(); ++itOp) {
                bool synchronizedWithSharedVariable = false;
                for(unsigned int varIndex = 0; varIndex < task_proxy.get_variables().size() - 1; ++varIndex) {
                    if (transitionVariableOptimization && operatorHasVariableInPreconditions[make_tuple(itOp, varIndex + 1)] &&
                        (synchronizedWithSharedVariable || operatorHasVariableInPreconditions[make_tuple(itOp, varIndex)])) {
                        continue;
                    }
                    lp::LPConstraint con(0, 0);
                    for (unsigned int step = synchronizationSteps; step < timeSteps + 1; ++step) {
                        vector<unsigned int>& varIndices = operatorTimeVariableIndices[make_tuple(varIndex, step, itOp)];
                        for (const auto& ind : varIndices) {
                            con.insert(ind, 1);
                        }
                        vector<unsigned int>& varIndices2 = operatorTimeVariableIndices[make_tuple(varIndex + 1, step, itOp)];
                        for (const auto& ind : varIndices2) {
                            con.insert(ind, -1);
                        }
                    }
                    constraints.push_back(con);
                    if (operatorHasVariableInPreconditions[make_tuple(itOp, varIndex + 1)]) {
                        synchronizedWithSharedVariable = true;
                    }
                }
            }
        }
        // Add flow restriction constraint if requested.
        if (restrictFlowInLastLayer && timeSteps != 0 && withRepetition) {
            lp::LPVariable boolVar(0, 1, 0, true);
            variables.push_back(boolVar);
            boolVarIndex = variables.size() - 1;
            // If the flow in the time step before the last is smaller than 1, the boolean is 0 (1),
            // which also means that the flow in the last time layer will be zero too (2).
            // Using the big M method:
            // Plans with 1e+4 operators are not expected.
            // If higher values are used, the boolean will be set to 1e-x as this seems to be close enough to zero
            // and the method does not work anymore.
            double bigM = 1e+4;
            lp::LPConstraint conOneBeforeLast(1 - bigM, infty);
            // Constraints only have to be forced to first atomic projection and will through
            // the time synchronization and summed operator flow conditions propagated.
            for (int itValue = 0; itValue < task_proxy.get_variables()[0].get_domain_size(); ++itValue) {
                vector<tuple<unsigned int, unsigned int, unsigned int>>& outgoings = out[make_tuple(0, itValue)];
                for (unsigned int itOut = 0; itOut < outgoings.size(); ++itOut) {
                    conOneBeforeLast.insert(transitionVariableIndices[make_tuple(0, itValue, get<0>(outgoings[itOut]), timeSteps - 1)], 1);
                }
            }
            conOneBeforeLast.insert(boolVarIndex, -1 * bigM);
            constraints.push_back(conOneBeforeLast);
            restrictionConstraintIndeces.push_back(constraints.size() - 1);
            
            lp::LPConstraint conLast(-infty, 0);
            for (int itValue = 0; itValue < task_proxy.get_variables()[0].get_domain_size(); ++itValue) {
                vector<tuple<unsigned int, unsigned int, unsigned int>>& outgoings = out[make_tuple(0, itValue)];
                for (unsigned int itOut = 0; itOut < outgoings.size(); ++itOut) {
                    conLast.insert(transitionVariableIndices[make_tuple(0, itValue, get<0>(outgoings[itOut]), timeSteps)], 1);
                }
            }
            conLast.insert(boolVarIndex, -1 * bigM);
            constraints.push_back(conLast);
            restrictionConstraintIndeces.push_back(constraints.size() - 1);
        }
    }
}

int TimeHeuristic::compute_heuristic(const GlobalState &global_state) {
    State state = convert_global_state(global_state);
    start = std::chrono::steady_clock::now();
    if (!iterativeApproach) {
        // Do debugging.
        // Debug constraints.
        if (debugConstraints) {
            for (auto i: constraints) {
                map<int, int> myMap;
                for (auto v: i.get_variables()) {
                    if (myMap.find(v) == myMap.end()) {
                        myMap[v] = 1;
                        cout << v << ",";
                    } else {
                        cout << "FAIL at: " << v << ",";
                        exit(0);
                    }
                }
                cout << endl;
            }
        }
        
        if (firstRun) {
            solver.load_problem(lp::LPObjectiveSense::MINIMIZE, variables, constraints);
            firstRun = false;
        } else {
            // Change constraints of old initial state and new initial state accordingly.
            for (unsigned int varIndex = 0; varIndex < task_proxy.get_variables().size(); ++varIndex) {
                solver.set_constraint_lower_bound(firstLayerConstraintIndices[make_tuple(varIndex, oldInitialValues[varIndex])], 0.0);
                solver.set_constraint_upper_bound(firstLayerConstraintIndices[make_tuple(varIndex, oldInitialValues[varIndex])], 0.0);
                solver.set_constraint_upper_bound(firstLayerConstraintIndices[make_tuple(varIndex, state.get_values()[varIndex])], 1.0);
                solver.set_constraint_lower_bound(firstLayerConstraintIndices[make_tuple(varIndex, state.get_values()[varIndex])], 1.0);
                oldInitialValues[varIndex] = state.get_values()[varIndex];
            }
        }
        end = std::chrono::steady_clock::now();
        lpInitTimes.push_back(std::chrono::duration_cast<std::chrono::microseconds> (end - start).count());
        start = std::chrono::steady_clock::now();
        solver.solve();
        end = std::chrono::steady_clock::now();
        lpSolvingTimes.push_back(std::chrono::duration_cast<std::chrono::microseconds> (end - start).count());
        if (solver.has_optimal_solution()) {
            if (debugFlow) {
                int oldPrecision = cout.precision();
                cout.precision(3);
                double e = 0.01;
                double obj = solver.get_objective_value();
                int res = ceil(obj - e);
                cout << "Result: " << res << endl;
                
                vector<double> solution = solver.extract_solution();
                for (unsigned int varIndex = 0; varIndex < task_proxy.get_variables().size(); ++varIndex) {
                    // if (varIndex == 1) {
                    //     cout << "We're actually here." << endl;

                    //     for (unsigned int i = 0; i < task_proxy.get_operators().size(); ++i) {
                    //         cout << task_proxy.get_operators()[i].get_cost() << endl;
                    //     }

                    //     exit(0);
                    // }
                    cout << endl;
                    cout << "\033[1;34m" << "NEW VAR(" << varIndex << ")" << ": \033[0m" << endl;
                    int goalValue = -1;
                    for (unsigned int itGoal = 0; itGoal < task_proxy.get_goals().size(); ++itGoal) {
                        if (task_proxy.get_goals()[itGoal].get_variable() == task_proxy.get_variables()[varIndex]) {
                            goalValue = task_proxy.get_goals()[itGoal].get_value(); 
                        }
                    }
                    
                    
                    // Iterative over all states and print outgoing transitions.
                    for (unsigned int time = 0; time < timeSteps + 1; ++time) {
                        if (time == timeSteps && !withRepetition) {
                            break;
                        }
                        cout << "\033[1;34m" << "T = " << time << ": \033[0m";
                        double summedFlow = 0;
                        for (int itValue = 0; itValue < task_proxy.get_variables()[varIndex].get_domain_size(); ++itValue) {
                            cout << endl << "\033[1;34m" << "NEW VALUE(" << itValue << "): " << "\033[0m";
                            vector<tuple<unsigned int, unsigned int, unsigned int>>& outgoings = out[make_tuple(varIndex, itValue)];
                            for (unsigned int itTransition = 0; itTransition < outgoings.size(); ++itTransition) {
                                string isGoal = ((itValue == goalValue || goalValue == -1) && time == timeSteps) ? "g" : "";
                                string isStart = ((itValue == task_proxy.get_initial_state().get_values()[varIndex]) && time == 0) ? "s" : "";
                                double flow = solution[transitionVariableIndices[make_tuple(varIndex, itValue, get<0>(outgoings[itTransition]), time)]];
                                summedFlow += flow;
                                cout << isStart;
                                if (flow > 0) {
                                    cout << "\033[1;31m" << flow << "\033[0m";
                                    cout << ":" << isGoal << task_proxy.get_operators()[get<0>(outgoings[itTransition])].get_name() << ":" << transitionVariableIndices[make_tuple(varIndex, itValue, get<0>(outgoings[itTransition]), time)] << " ";
                                } else {
                                }
                                
                            }
                            cout << endl << "Constraint:";
                            lp::LPConstraint con = constraints[constraintIndices[make_tuple(varIndex, time, itValue)]];
                            double subSummedFlow = 0;
                            for (unsigned int i = 0; i < con.get_variables().size(); ++i) {
                                if (solution[con.get_variables()[i]] != 0) {
                                    cout << "+" << con.get_coefficients()[i] << "*" << solution[con.get_variables()[i]];
                                }
                                subSummedFlow += (con.get_coefficients()[i]) * solution[con.get_variables()[i]];
                            }
                            cout << "(" << to_string(subSummedFlow) << (")") << "=" << (con.get_lower_bound() == con.get_upper_bound() ? to_string(con.get_lower_bound()) : "FAIL");
                            
                        }
                        cout << "\033[1;35m" << "SUMMED FLOW:" << summedFlow << "\033[0m" << endl;
                     }
                     cout << "Goal variables:";
                     for (auto i: goalVariableIndices[varIndex]) {
                         cout << " " << solution[i];
                     }
                     cout << endl;
                }

                if (restrictFlowInLastLayer) {
                    cout << "Restriction variables:";
                    lp::LPConstraint conBeforeLast = constraints[restrictionConstraintIndeces[0]];
                    lp::LPConstraint conLast = constraints[restrictionConstraintIndeces[1]];
                    vector<lp::LPConstraint> vect;
                    vect.push_back(conBeforeLast);
                    vect.push_back(conLast);
                    for (lp::LPConstraint con: vect) {
                        cout << endl;
                        for (unsigned int index: con.get_variables()) {
                            cout << " " << (boolVarIndex == index ? ">" : "") << solution[index];
                        }
                    }
                    cout << "Bool variable is actually considered to be 1: " << (solution[boolVarIndex] == 1) << endl;
                }
                cout << endl;

                cout.precision(oldPrecision);
                cout << "Objective value: " << res << endl;
                cout << "LP has solution: " << solver.has_optimal_solution();
                exit(0);
            }
            
            double epsilon = 0.01;
            double objective_value = solver.get_objective_value();
            int result = ceil(objective_value - epsilon);
            if (firstOptimalSolution) {
                initial_timesteps_required = timeSteps;
                printStatistics();
                firstOptimalSolution = false;
            }

            if (debugHeuristicValues) {
                cout << "Solution found, heuristic value is: " << result << " and flow cost is: " << solver.get_objective_value() << "." << endl; 
            }
            return result;
        } else {
            if (debugHeuristicValues) {
                cout << "No solution found." << endl; 
            }
            if (withRepetition) {
                if (goalsOnlyInLastTimeLayer) {
                    // State could be a dead end at this point. We would however have to do more work to find it out.
                    return 0;
                } else {
                    return DEAD_END;
                }
            } else {
                return timeSteps * minOperatorCost;
            }
            
        }
        
    } else {
        start = std::chrono::steady_clock::now();
        // Create LPs of time unrollings until solution is found.
        unsigned int time = 0;
        bool solutionFound = false;
        
        // Map var index, operator index and time to constraint index.
        vector<map<tuple<unsigned int, unsigned int>, unsigned int>> otConstraintIndices;
        otConstraintIndices.resize(task_proxy.get_variables().size() - 1);
        
        // Map var index, time and var value to constraint index.
        map<tuple<unsigned int, unsigned int, unsigned int>, unsigned int> constraintIndeces;
        
        // Map var index to goal constraint index.
        map<unsigned int, unsigned int> goalConstraintIndeces;
        
        for (unsigned int varIndex = 0; varIndex < task_proxy.get_variables().size(); ++varIndex) {
            constraints.push_back(lp::LPConstraint(1, 1));
            goalConstraintIndeces[varIndex] = constraints.size() - 1;
        }
        
        // Transition varibale optimization variables.
        // Maps and time step, var index and operator index to boolean.
        vector<vector<vector<bool>>> syncedWithSharedVariable;
        // Maps time step, var index and operator index to boolean.
        vector<vector<vector<bool>>> operatorDoesAppearInTimeStep; // Due to states being not reachable, this can be false.
        // Maps time step, var index and operator index to boolean.
        vector<vector<vector<bool>>> introduceZeroConstraint;
        
        // Note that backwardReachable gets initiated in reverse order.
        backwardReachable.resize(task_proxy.get_variables().size());
        forwardReachable.resize(task_proxy.get_variables().size());
        while (!solutionFound) {
            if (removeDeadStates) {
                for (unsigned int varIndex = 0; varIndex < task_proxy.get_variables().size(); ++varIndex) {
                    const VariableProxy& var = task_proxy.get_variables()[varIndex];
                    
                    // Find goal value of the current variable.
                    int goalValue = -1;
                    for (unsigned int itGoal = 0; itGoal < task_proxy.get_goals().size(); ++itGoal) {
                        if (task_proxy.get_goals()[itGoal].get_variable() == var) {
                            goalValue = task_proxy.get_goals()[itGoal].get_value();
                        }
                    }
                    
                    forwardReachable[varIndex].push_back(vector<bool>());
                    backwardReachable[varIndex].push_back(vector<bool>());

                    for (int itValue = 0; itValue < var.get_domain_size(); ++itValue) {
                        if (time == 0) {
                            if (itValue == state.get_values()[varIndex]) {
                                forwardReachable[varIndex][time].push_back(1);
                            } else {
                                forwardReachable[varIndex][time].push_back(0); // 0
                            }
                            if (goalValue == -1) {
                                backwardReachable[varIndex][time].push_back(1);
                            } else {
                                if (itValue == goalValue) {
                                    backwardReachable[varIndex][time].push_back(1);
                                } else {
                                    backwardReachable[varIndex][time].push_back(0); // 0 
                                }
                            }
                        } else {
                            for (unsigned int itValue = 0; itValue < (unsigned int) var.get_domain_size(); ++itValue) {
                                bool foundCorrespondingTransition = false;
                                for (int itValueLast = 0; itValueLast < var.get_domain_size() && !foundCorrespondingTransition; ++itValueLast) {
                                    if (forwardReachable[varIndex][time - 1][itValueLast] == 1) {
                                        vector<tuple<unsigned int, unsigned int, unsigned int>> outgoingtransitions = out[make_tuple(varIndex, itValueLast)];
                                        for (unsigned int itTransition = 0; itTransition < outgoingtransitions.size(); ++itTransition) {
                                            if (get<2>(outgoingtransitions[itTransition]) == itValue) {
                                                foundCorrespondingTransition = true;
                                                break;
                                            }
                                        }
                                    }
                                }
                                bool foundCorrespondingTransition2 = false;
                                for (int itValueLast = 0; itValueLast < var.get_domain_size() && !foundCorrespondingTransition2; ++itValueLast) {
                                    if (backwardReachable[varIndex][time - 1][itValueLast] == 1) {
                                        vector<tuple<unsigned int, unsigned int, unsigned int>> incomingtransitions = in[make_tuple(varIndex, itValueLast)];
                                        for (unsigned int itIn = 0; itIn < incomingtransitions.size(); ++itIn) {
                                            if (get<1>(incomingtransitions[itIn]) == itValue) {
                                                foundCorrespondingTransition2 = true;
                                                break;
                                            }
                                        }
                                    }
                                }
                                
                                forwardReachable[varIndex][time].push_back(foundCorrespondingTransition ? 1 : 0); // 0
                                if (!goalsOnlyInLastTimeLayer) {
                                    if (goalValue == -1 || (int) itValue == goalValue) {
                                        backwardReachable[varIndex][time].push_back(1);
                                    } else {
                                        backwardReachable[varIndex][time].push_back(foundCorrespondingTransition2 ? 1 : 0); // 0
                                    }
                                } else {
                                    backwardReachable[varIndex][time].push_back(foundCorrespondingTransition2 ? 1 : 0); // 0
                                }
                            }
                        }
                    }
                }
            } else {
                for (unsigned int varIndex = 0; varIndex < task_proxy.get_variables().size(); ++varIndex) {
                    forwardReachable[varIndex].push_back(vector<bool>(task_proxy.get_variables()[varIndex].get_domain_size(), true));
                    backwardReachable[varIndex].push_back(vector<bool>(task_proxy.get_variables()[varIndex].get_domain_size(), true));
                }
            }
            
            
            // Check for new states that are reachable and add state constraint plus add new transitions to already existing constraints.
            for (unsigned int varIndex = 0; varIndex < task_proxy.get_variables().size(); ++varIndex) {
                const VariableProxy& var = task_proxy.get_variables()[varIndex];
                
                int goalValue = -1;
                for (unsigned int itGoal = 0; itGoal < task_proxy.get_goals().size(); ++itGoal) {
                    if (task_proxy.get_goals()[itGoal].get_variable() == var) {
                        goalValue = task_proxy.get_goals()[itGoal].get_value();
                    }
                }
                
                for (unsigned int itTime = 0; itTime <= (unsigned int) time; ++itTime) {
                    for (unsigned int itValue = 0; itValue < (unsigned int) var.get_domain_size(); ++itValue) {
                        // If state is forward- and backward-reachable and is not yet considered (can also happen in lower time layers as backward reachability may change)
                        if (forwardReachable[varIndex][itTime][itValue] && backwardReachable[varIndex][time - itTime][itValue]
                        && (constraintIndeces.find(make_tuple(varIndex, itTime, itValue)) == constraintIndeces.end())) {
                            
                            // Bounds are 1 if state is the initial state and else is 0.
                            unsigned int lowerAndUpper = itTime == 0 && (int) itValue == state.get_values()[varIndex] ? 1 : 0;
                            lp::LPConstraint con(lowerAndUpper, lowerAndUpper);
                            
                            // First iteration covers outgoing transitions and second the incoming iterations.
                            const unsigned int OUTGOING = 0;
                            const unsigned int INCOMING = 1;
                            for (unsigned int transitionType = 0; transitionType < 2; ++transitionType) {
                                
                                unsigned int currentTime;
                                vector<tuple<unsigned int, unsigned int, unsigned int>> transitions;
                                
                                if (transitionType == OUTGOING) {
                                    transitions = out[make_tuple(varIndex, itValue)];
                                    if (itTime == time) {
                                        transitions.clear();
                                    }
                                    currentTime = itTime;
                                }
                                
                                if (transitionType == INCOMING) {
                                    transitions = in[make_tuple(varIndex, itValue)];
                                    if (itTime == 0) {
                                        transitions.clear();
                                    }
                                    currentTime = itTime - 1;
                                }
                                
                                for (unsigned int itTransition = 0; itTransition < transitions.size(); ++itTransition) {
                                    unsigned int valueOnOtherSide = (transitionType == OUTGOING ? get<2>(transitions[itTransition]) : get<1>(transitions[itTransition]));
                                    unsigned int timeOnOtherSide = itTime + (transitionType == OUTGOING ? 1 : -1);
                                    // Transition only exists if end state of transition is reachable.                                    
                                    if (constraintIndeces.find(make_tuple(varIndex, timeOnOtherSide, valueOnOtherSide)) != constraintIndeces.end()) {
                                        // At this place potentially do optimization and use shared variable if possible.
                                        unsigned int newIndex = 0;
                                        if (transitionVariableOptimization && operatorHasVariableInPreconditions[make_tuple(get<0>(transitions[itTransition]), varIndex)]) {
                                            if (indicesForOptimization.find(make_tuple(get<0>(transitions[itTransition]), currentTime)) == indicesForOptimization.end()) {
                                                variables.push_back(lp::LPVariable(0, infty, varIndex == 0 ? task_proxy.get_operators()[get<0>(transitions[itTransition])].get_cost() : 0, integerProgram));
                                                newIndex = variables.size() - 1;
                                                indicesForOptimization[make_tuple(get<0>(transitions[itTransition]), currentTime)] = newIndex;
                                            } else {
                                                newIndex = indicesForOptimization[make_tuple(get<0>(transitions[itTransition]), currentTime)];
                                            }
                                        } else {
                                            variables.push_back(lp::LPVariable(0, infty, varIndex == 0 ? task_proxy.get_operators()[get<0>(transitions[itTransition])].get_cost() : 0, integerProgram));
                                            newIndex = variables.size() - 1;
                                        }
                                        
                                        if (transitionVariableOptimization) {
                                            introduceZeroConstraint[currentTime][varIndex][get<0>(transitions[itTransition])] = false;
                                        }
                                        
                                        // Save index for debugging purposes.
                                        if (debugFlow) {
                                            transitionVariableIndices[make_tuple(varIndex, itValue, get<0>(transitions[itTransition]), time)] = newIndex;
                                        }
                                        
                                        // Add to both, the new constraint and the constraint of the end state of the current transition.
                                        con.insert(newIndex, transitionType == OUTGOING ? 1 : -1);
                                        
                                        constraints[constraintIndeces[make_tuple(varIndex, timeOnOtherSide, valueOnOtherSide)]].insert(newIndex, transitionType == OUTGOING ? -1 : 1);
                                        // Sync with both sides.
                                        const unsigned int RIGHT = 0;
                                        // const unsigned int LEFT = 1;
                                        for (unsigned int syncWith = 0; syncWith < 2; ++syncWith) {
                                            bool syncCondition = (syncWith == RIGHT ? varIndex < task_proxy.get_variables().size() - 1 : varIndex > 0);
                                            unsigned int conditionIndex = (syncWith == RIGHT ? varIndex : varIndex - 1);
                                            if (syncCondition) {
                                                if (otConstraintIndices[conditionIndex].find(make_tuple(get<0>(transitions[itTransition]), currentTime)) == otConstraintIndices[conditionIndex].end()) {
                                                    if (!transitionVariableOptimization || !syncedWithSharedVariable[currentTime][varIndex][get<0>(transitions[itTransition])]
                                                        || !syncedWithSharedVariable[currentTime][varIndex + (syncWith == RIGHT ? 1 : -1)][get<0>(transitions[itTransition])]) {
                                                        lp::LPConstraint conSynced(0, 0);
                                                        
                                                        conSynced.insert(newIndex, (syncWith == RIGHT ? 1 : -1));
                                                        
                                                        constraints.push_back(conSynced);
                                                        
                                                        otConstraintIndices[conditionIndex][make_tuple(get<0>(transitions[itTransition]), currentTime)] = constraints.size() - 1;
                                                        // If other ot-flow is shared, also mark this one as shared.
                                                        if (transitionVariableOptimization && syncedWithSharedVariable[currentTime][varIndex + (syncWith == RIGHT ? 1 : -1)][get<0>(transitions[itTransition])]) {
                                                            syncedWithSharedVariable[currentTime][varIndex][get<0>(transitions[itTransition])] = true;
                                                        }
                                                        // If current ot-flow is shared, also mark other one as shared.
                                                        if (transitionVariableOptimization && syncedWithSharedVariable[currentTime][varIndex][get<0>(transitions[itTransition])]) {
                                                            syncedWithSharedVariable[currentTime][varIndex + (syncWith == RIGHT ? 1 : -1)][get<0>(transitions[itTransition])] = true;
                                                        }
                                                    }
                                                } else {
                                                    lp::LPConstraint& conSynced = constraints[otConstraintIndices[conditionIndex][make_tuple(get<0>(transitions[itTransition]), currentTime)]];
                                                    conSynced.insert(newIndex, (syncWith == RIGHT ? 1 : -1));
                                                }
                                            }
                                        }
                                    }
                                }
                            }
                            
                            // If goal state add to goal variable constraint and to current constraint.
                            if (goalsOnlyInLastTimeLayer) {
                                if (itTime == time && ((int) itValue == goalValue || goalValue == -1)) {
                                    variables.push_back(lp::LPVariable(0, 1, 0, integerProgram));
                                    int newIndex = variables.size() - 1;
                                    con.insert(newIndex, 1);
                                    constraints[goalConstraintIndeces[varIndex]].insert(newIndex, 1);
                                }
                            } else {
                                if ((int) itValue == goalValue || goalValue == -1) {
                                    variables.push_back(lp::LPVariable(0, 1, 0, integerProgram));
                                    int newIndex = variables.size() - 1;
                                    con.insert(newIndex, 1);
                                    constraints[goalConstraintIndeces[varIndex]].insert(newIndex, 1);
                                }
                            }
                            constraints.push_back(con);
                            constraintIndeces[make_tuple(varIndex, itTime, itValue)] = constraints.size() - 1;
                        }
                    }
                }
            }
            
            // Due to states maybe being unreachable the synchronization chain may be broken.
            if (transitionVariableOptimization) {
                for (unsigned int itOp = 0; itOp < task_proxy.get_operators().size(); ++itOp) {
                    for (unsigned int itTime = 0; itTime < (unsigned int) time; ++itTime) {
                        bool allVarIndicesHaveProperTransition = true;
                        for (unsigned int varIndex = 0; varIndex < task_proxy.get_variables().size(); ++varIndex) {
                            if (introduceZeroConstraint[itTime][varIndex][itOp]) {
                                allVarIndicesHaveProperTransition = false;
                                break;
                            }
                        }
                        if (allVarIndicesHaveProperTransition) {
                            if (indicesForOptimization.find(make_tuple(itOp, itTime)) != indicesForOptimization.end()) {
                                variables[indicesForOptimization[make_tuple(itOp, itTime)]].upper_bound = infty;
                            }
                        } else {
                            if (indicesForOptimization.find(make_tuple(itOp, itTime)) != indicesForOptimization.end()) {
                                variables[indicesForOptimization[make_tuple(itOp, itTime)]].upper_bound = 0;
                            }
                        }
                    }
                }
            }
            
        //Debug forward- and backward-reachable tables.
        //    cout << "Forwards:" << endl;
        //    for (unsigned int itTime = 0; itTime < time + 1; ++itTime) {
        //        for (int itValue = 0; itValue < task_proxy.get_variables()[0].get_domain_size(); ++itValue) {
        //            cout << forwardReachable[0][itTime][itValue];
        //        }
        //        cout << endl;
        //    }
        //    cout << "Backwards:" << endl;
        //    for (unsigned int itTime = 0; itTime < time + 1; ++itTime) {
        //        for (int itValue = 0; itValue < task_proxy.get_variables()[0].get_domain_size(); ++itValue) {
        //            cout << backwardReachable[0][time - itTime][itValue];
        //        }
        //        cout << endl;
        //    }

        //    exit(0);
            
            //Debug constraints.
            
            
            // solver.clear_temporary_constraints();
            // vector<lp::LPConstraint> almostEmptyConstraintList;
            // almostEmptyConstraintList.push_back(lp::LPConstraint(0,0));
            // solver.load_problem(lp::LPObjectiveSense::MINIMIZE, variables, almostEmptyConstraintList);
            // solver.add_temporary_constraints(constraints);
            
            solver.load_problem(lp::LPObjectiveSense::MINIMIZE, variables, constraints);
            end = std::chrono::steady_clock::now();
            lpInitTimes.push_back(std::chrono::duration_cast<std::chrono::microseconds> (end - start).count());
            
            start = std::chrono::steady_clock::now();
            solver.solve();
            end = std::chrono::steady_clock::now();
            lpSolvingTimes.push_back(std::chrono::duration_cast<std::chrono::microseconds> (end - start).count());
            
//            if (debugOption) {
//                cout << "Found optimal solution: " << solver.has_optimal_solution() << endl;
                if (solver.has_optimal_solution()) {
                    if (debugConstraints) {
                        unsigned int counter = 0;
                        unsigned int cntsync = 0, cntstate = 0, cntgoal = 0;

                        for (auto i: constraints) {
                            map<int, int> myMap;
                            for (auto v: i.get_variables()) {
                                if (myMap.find(v) == myMap.end()) {
                                    myMap[v] = 1;
                                    cout << v << ",";
                                } else {
                                    cout << "FAIL at: " << v << ",";
                                    for (auto el : indicesForOptimization) {
                                        if (el.second == (unsigned int) v) {
                                            cout << "Is shared variable" << endl;
                                        }
                                    }
                                    for (unsigned int varIndex = 0; varIndex < task_proxy.get_variables().size() - 1; ++varIndex) {
                                        for (auto el : otConstraintIndices[varIndex]) {
                                            if (el.second == counter) {
                                                cout << "in sync";
                                            }
                                        }
                                    }
                                    for (auto el : constraintIndeces) {
                                        if (el.second == counter) {
                                            cout << "in state";
                                        }
                                    }
                                    exit(0);
                                }
                            }
                            for (unsigned int varIndex = 0; varIndex < task_proxy.get_variables().size() - 1; ++varIndex) {
                                for (auto el : otConstraintIndices[varIndex]) {
                                    if (el.second == counter) {
                                        cout << "sync";
                                        cntsync++;
                                    }
                                }
                            }
                            for (auto el : goalConstraintIndeces) {
                                if (el.second == counter) {
                                    cout << "goal";
                                    cntgoal++;
                                }
                            }
                            for (auto el : constraintIndeces) {
                                if (el.second == counter) {
                                    cout << "state";
                                    cntstate++;
                                }
                            }
                            cout << endl;
                            counter++;
                        }
                        cout << "sync: " << cntsync << " state: " << cntstate << " goal: " << cntgoal << endl;
                    }
                    if (debugFlow) {
                        int oldPrecision = cout.precision();
                        cout.precision(3);
                        double e = 0.01;
                        double obj = solver.get_objective_value();
                        int res = ceil(obj - e);
                        cout << "Result: " << res << endl;
                        
                        vector<double> solution = solver.extract_solution();
                        for (unsigned int varIndex = 0; varIndex < task_proxy.get_variables().size(); ++varIndex) {
                            if (varIndex == 1) {
            //                    cout << "We're actually here." << endl;
            //                    exit(0);
                            }
                            cout << endl;
                            cout << "\033[1;34m" << "NEW VAR(" << varIndex << ")" << ": \033[0m" << endl;
                            int goalValue = -1;
                            for (unsigned int itGoal = 0; itGoal < task_proxy.get_goals().size(); ++itGoal) {
                                if (task_proxy.get_goals()[itGoal].get_variable() == task_proxy.get_variables()[varIndex]) {
                                    goalValue = task_proxy.get_goals()[itGoal].get_value(); 
                                }
                            }
                            
                            
                            // Iterative over all states and print outgoing transitions.
                            for (unsigned int itTime = 0; itTime < time; ++itTime) {
                                cout << "\033[1;34m" << "T = " << itTime << ": \033[0m";
                                double summedFlow = 0;
                                for (int itValue = 0; itValue < task_proxy.get_variables()[varIndex].get_domain_size(); ++itValue) {
                                    // Ensure state exists.
                                    if (constraintIndeces.find(make_tuple(varIndex, itTime, itValue)) != constraintIndeces.end()) {
                                        cout << endl << "\033[1;34m" << "NEW VALUE(" << itValue << "): " << "\033[0m";
                                        vector<tuple<unsigned int, unsigned int, unsigned int>>& outgoings = out[make_tuple(varIndex, itValue)];
                                        for (unsigned int itTransition = 0; itTransition < outgoings.size(); ++itTransition) {
                                            // Ensure transition exists.
                                            if (transitionVariableIndices.find(make_tuple(varIndex, itValue, get<0>(outgoings[itTransition]), itTime)) != transitionVariableIndices.end()) {
                                                string isGoal = ((itValue == goalValue || goalValue == -1) && itTime == time) ? "g" : "";
                                                string isStart = ((itValue == task_proxy.get_initial_state().get_values()[varIndex]) && itTime == 0) ? "s" : "";
                                                double flow = solution[transitionVariableIndices[make_tuple(varIndex, itValue, get<0>(outgoings[itTransition]), itTime)]];
                                                summedFlow += flow;
                                                cout << isStart;
                                                if (flow > 0) {
                                                    cout << "\033[1;31m" << flow << "\033[0m";
                                                } else {
                                                    cout << flow;
                                                }
                                                cout << ":" << isGoal << get<0>(outgoings[itTransition]) << ":" << transitionVariableIndices[make_tuple(varIndex, itValue, get<0>(outgoings[itTransition]), itTime)] << " ";
                                            }
                                            
                                        }
                                        cout << endl << "Constraint:";
                                        lp::LPConstraint con = constraints[constraintIndeces[make_tuple(varIndex, itTime, itValue)]];
                                        double subSummedFlow = 0;
                                        for (unsigned int i = 0; i < con.get_variables().size(); ++i) {
                                            if (solution[con.get_variables()[i]] != 0) {
                                                cout << "+" << con.get_coefficients()[i] << "*" << solution[con.get_variables()[i]];
                                            }
                                            subSummedFlow += (con.get_coefficients()[i]) * solution[con.get_variables()[i]];
                                        }
                                        cout << "(" << to_string(subSummedFlow) << (")") << "=" << (con.get_lower_bound() == con.get_upper_bound() ? to_string(con.get_lower_bound()) : "FAIL");
                                    }
                                    
                                    
                                }
                                cout << "\033[1;35m" << "SUMMED FLOW:" << summedFlow << "\033[0m" << endl;
                             }
                             cout << "Goal variables:";
                             for (auto i: goalVariableIndices[varIndex]) {
                                 cout << " " << solution[i];
                             }
                             cout << endl;
                             if (restrictFlowInLastLayer) {
                                 cout << "Restriction variables:";
                                 lp::LPConstraint conBeforeLast = constraints[restrictionConstraintIndeces[0]];
                                 lp::LPConstraint conLast = constraints[restrictionConstraintIndeces[1]];
                                 vector<lp::LPConstraint> vect;
                                 vect.push_back(conBeforeLast);
                                 vect.push_back(conLast);
                                 for (lp::LPConstraint con: vect) {
                                     cout << endl;
                                     for (unsigned int index: con.get_variables()) {
                                         cout << " " << (boolVarIndex == index ? ">" : "") << solution[index];
                                     }
                                 }
                                 
                             }
                             cout << endl;
                        }
                        cout.precision(oldPrecision);
                        cout << "Objective value: " << res << endl;
                        cout << "LP has solution: " << solver.has_optimal_solution();
                        exit(0);
                    }
                }
//            }
            
            if (solver.has_optimal_solution()) {
                constraints.clear();
                variables.clear();
                forwardReachable.clear();
                backwardReachable.clear();
                indicesForOptimization.clear();
                double epsilon = 0.01;
                double objective_value = solver.get_objective_value();
                int result = ceil(objective_value - epsilon);
                if (firstOptimalSolution) {
                    initial_timesteps_required = time;
                    printStatistics();
                    firstOptimalSolution = false;
                }
                if (debugHeuristicValues) {
                cout << "Solution found, rounded up value is: " << result << " and flow cost is: " << solver.get_objective_value()
                     << " after using " << time << " time steps, the actual returned value is " << max(result, (int) (time * minOperatorCost)) << "." << endl; 
                }
                result = max(result, (int) (time * minOperatorCost));
                return result;
            }
            
            if (limitIterative && !firstOptimalSolution && time == initial_timesteps_required) {
                return minOperatorCost * initial_timesteps_required;
            }

            ++time;
            if (transitionVariableOptimization) {
                syncedWithSharedVariable.push_back(vector<vector<bool>>(task_proxy.get_variables().size(), vector<bool>(task_proxy.get_operators().size(), false)));
                for (unsigned int varIndex = 0; varIndex < task_proxy.get_variables().size(); ++varIndex) {
                    for (unsigned int itOp = 0; itOp < task_proxy.get_operators().size(); ++itOp) {
                        if (operatorHasVariableInPreconditions[make_tuple(itOp, varIndex)]) {
                            syncedWithSharedVariable[time - 1][varIndex][itOp] = true;
                        }
                    }
                }
                operatorDoesAppearInTimeStep.push_back(vector<vector<bool>>(task_proxy.get_variables().size(), vector<bool>(task_proxy.get_operators().size(), false)));
                introduceZeroConstraint.push_back(vector<vector<bool>>(task_proxy.get_variables().size(), vector<bool>(task_proxy.get_operators().size(), true)));
            }
        }
    }
    
    return 0;
}

// Prints how many percent of the overall time was spent initializing the LP.
void TimeHeuristic::printStatistics() {
    unsigned int sumInit = 0;
    for (auto& n : lpInitTimes) {
        sumInit += n;
    }
        
    unsigned int sumSolving = 0;
    for (auto& n : lpSolvingTimes) {
        sumSolving += n;
    }
    cout << "Initial time steps required: " << initial_timesteps_required << "." << endl;
    cout << "Time spent finding heuristic value for first state: " << (sumInit + sumSolving) << "." << endl;
    cout << "Percentage of the time spent with initialization of first LP with calculated optimal solution: " << ((double) sumInit) / (sumInit + sumSolving) * 100 << " %." << endl;
    solver.print_statistics();
    cout << "Peak memory in KB: " << utils::get_peak_memory_in_kb() << "." << endl;
}

static Heuristic *_parse(OptionParser &parser) {
    parser.document_synopsis("Time Heuristic",
                             "It is not finished by any means.");
    parser.document_language_support("action costs", "yes");
    parser.document_language_support("conditional effects", "no");
    parser.document_language_support("axioms", "no");
    parser.document_property("admissible", "only if non-iterative and repetition enabled");
    parser.document_property("consistent", "-");
    parser.document_property("safe", "yes");
    parser.document_property("preferred operators", "-");
    parser.add_option<bool>("ia", "iterative approach", "false", Bounds("false", "true"));
    parser.add_option<bool>("wr", "with repetition in last layer", "true", Bounds("false", "true"));
    parser.add_option<bool>("ip", "uses integer program", "false", Bounds("false", "true"));
    parser.add_option<bool>("do", "debug option, prints heuristic values", "false", Bounds("false", "true"));
    parser.add_option<bool>("df", "debug option, prints flow values", "false", Bounds("false", "true"));
    parser.add_option<bool>("dc", "debug option, prints constraints", "false", Bounds("false", "true"));
    parser.add_option<bool>("gl", "goals only in last layer", "false", Bounds("false", "true"));
    parser.add_option<bool>("rf", "restrict flow in last layer", "false", Bounds("false", "true"));
    parser.add_option<bool>("dv", "debug heuristic values", "false", Bounds("false", "true"));
    parser.add_option<bool>("tv", "transition - variable optimization", "true", Bounds("false", "true"));
    parser.add_option<bool>("li", "limit iterative", "false", Bounds("false", "true"));
    parser.add_option<int>("rd", "remove dead states - only works for ia-approach -1=default, 0=dont 1=do", "-1", Bounds("-1", "1"));
    parser.add_option<int>("ts", "number of time steps", "0", Bounds("0", "infinity"));
    parser.add_option<int>("st", "number of synchronization steps", "-1", Bounds("-1", "infinity"));

    Heuristic::add_options_to_parser(parser);
    Options opts = parser.parse();
    if (parser.dry_run())
        return 0;
    else
        return new TimeHeuristic(opts);
}

static Plugin<Heuristic> _plugin("time", _parse);


TimeHeuristic::~TimeHeuristic() {
}
}
// Performance ideas:
// - Prevent copying while pushing to vectors.
// - For synchronization constraints if optimization is used, maybe only include one variable instead a sum.
// - For optimization if variable has only one value it can be used too.
// - If state has only one incoming and one outgoing transition the variables can be merged and the state constraint omitted.
// - In iterative only check for changes in first and last time step.
// - If in synchronization constraints one side is zero and on the other side is just one variable just set the bounds of the variable instead of
//   making an additional constraint.
