#include <spot/tl/parse.hh>
#include <spot/tl/print.hh>
#include <spot/twaalgos/translate.hh>
#include <spot/twa/bddprint.hh>
#include <spot/misc/minato.hh>
#include <spot/twa/formula2bdd.hh>

#include <ompl/control/planners/ltl/Automaton.h>
#include <typeinfo>

#include <boost/lexical_cast.hpp>


namespace oc = ompl::control;


static oc::AutomatonPtr ltl2a(unsigned numprops, std::string input){
    spot::formula f = spot::parse_formula(input);
    spot::translator trans;
    // We want deterministic output (dfa)
    spot::postprocessor::output_pref pref = spot::postprocessor::Deterministic;

    // Apply all optimizations - will be slowest
    spot::postprocessor::optimization_level level = spot::postprocessor::High;

    trans.set_pref(pref);
    trans.set_type(spot::postprocessor::BA);
    spot::twa_graph_ptr au = trans.run(f);

    oc::AutomatonPtr ret = oc::AutomatonPtr(new oc::Automaton(numprops));

    const auto& dict = au->get_dict();
    unsigned n = au->num_states();
    for (unsigned s=0; s<n; ++s){
        ret->addState(false);
    }
    for (unsigned s = 0; s < n; ++s)
    {
      // The out(s) method returns a fake container that can be
      // iterated over as if the contents was the edges going
      // out of s.  Each of these edge is a quadruplet
      // (src,dst,cond,acc).  Note that because this returns
      // a reference, the edge can also be modified.
      for (auto& t: au->out(s))
      {
        if (t.acc) ret->setAccepting(s, true);

        // Parse the formula
        std::vector<spot::formula> v;
        spot::minato_isop isop(t.cond);
        bdd clause = isop.next();
        if (clause == bddfalse){
            ompl::control::World edgeLabel(ret->numProps());
            ret->addTransition(s, edgeLabel, t.dst);
        } else {
            while (clause != bddfalse){
                ompl::control::World edgeLabel(ret->numProps());
                while (clause != bddtrue){
                    int var = bdd_var(clause);
                    const spot::bdd_dict::bdd_info& i = dict->bdd_map[var];
                    assert(i.type == spot::bdd_dict::var);
                    unsigned index = boost::lexical_cast<unsigned>(i.f.ap_name().substr(1));
                    bdd high = bdd_high(clause);
                    assert(index<numprops);
                    if (high == bddfalse){
                        edgeLabel[index] = false;
                        clause = bdd_low(clause);
                    } else {
                        assert(bdd_low(clause) == bddfalse);
                        edgeLabel[index] = true;
                        clause = high;
                    }
                }
                ret->addTransition(s, edgeLabel, t.dst);

                clause = isop.next();
            }
        }
      }
    }

    ret->setStartState(au->get_init_state_number());
    return ret;
}