// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2007-2008 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_UNIT_SYSTEMS_METRIC_NAUTICAL_MILE_HPP_INCLUDED
#define BOOST_UNIT_SYSTEMS_METRIC_NAUTICAL_MILE_HPP_INCLUDED

#include <boost/units/scaled_base_unit.hpp>
#include <boost/units/static_rational.hpp>
#include <boost/units/scale.hpp>
#include <boost/units/base_units/si/meter.hpp>

namespace boost {
namespace units {
namespace metric {

typedef scaled_base_unit<boost::units::si::meter_base_unit, scale<1852, static_rational<1> > > nautical_mile_base_unit;

}

template<>
struct base_unit_info<metric::nautical_mile_base_unit> {
    static BOOST_CONSTEXPR const char* name()   { return("nautical mile"); }
    static BOOST_CONSTEXPR const char* symbol() { return("nmi"); }
};

}
}

#endif // BOOST_UNIT_SYSTEMS_METRIC_NAUTICAL_MILE_HPP_INCLUDED


//==================================================================================================
/**
  Copyright 2016 Numscale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_SLIDE_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_SLIDE_HPP_INCLUDED

#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/detail/overload.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD ( slide_
                          , (typename T, typename Offset)
                          , bs::sse2_
                          , bs::pack_< bd::fundamental_<T>, bs::sse_ >
                          , bd::constant_<bd::integer_<Offset>>
                          )
  {
    using bits_t  = typename T::template rebind<std::uint8_t>::template resize<16>;
    using bcnt    = std::integral_constant<std::size_t,16u/T::static_size>;

    // slide with positive offset
    static BOOST_FORCEINLINE T side(T const& a0, std::true_type const&) BOOST_NOEXCEPT
    {
      using imm = std::integral_constant<std::size_t,Offset::value*bcnt::value>;
      return bitwise_cast<T>(_mm_srli_si128(bitwise_cast<bits_t>(a0),imm::value));
    }

    // slide with negative offset
    static BOOST_FORCEINLINE T side( T const& a0, std::false_type const& ) BOOST_NOEXCEPT
    {
      using imm = std::integral_constant<std::size_t,(-Offset::value)*bcnt::value>;
      return bitwise_cast<T>(_mm_slli_si128(bitwise_cast<bits_t>(a0),imm::value));
    }

    BOOST_FORCEINLINE T operator()(T const& a0, Offset const&) const BOOST_NOEXCEPT
    {
      return side(a0, brigand::bool_<(Offset::value >= 0)>{});
    }
  };

  BOOST_DISPATCH_OVERLOAD ( slide_
                          , (typename T, typename Offset)
                          , bs::sse2_
                          , bs::pack_< bd::fundamental_<T>, bs::sse_ >
                          , bs::pack_< bd::fundamental_<T>, bs::sse_ >
                          , bd::constant_<bd::integer_<Offset>>
                          )
  {
    using bits_t  = typename T::template rebind<std::uint8_t>::template resize<16>;

    BOOST_FORCEINLINE T operator()(T const& a0, T const& a1, Offset const&) const BOOST_NOEXCEPT
    {
      // Compute relative offsets for shifted loads pair
      using bcnt  = std::integral_constant<std::size_t,(16u/T::static_size)>;
      using ls    = std::integral_constant<std::size_t,bcnt::value*Offset::value>;
      using rs    = std::integral_constant<std::size_t,bcnt::value*(T::static_size-Offset::value)>;

      // Shift everything in place
      return bitwise_cast<T>( _mm_or_si128( _mm_srli_si128(bitwise_cast<bits_t>(a0),ls::value)
                                          , _mm_slli_si128(bitwise_cast<bits_t>(a1),rs::value)
                                          )
                            );
    }
  };
} } }

#endif


#include <signal.h>
#include <stdio.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/tcp.h>
#include <netinet/in.h>
#include <netdb.h>
#include <sys/time.h>
#include <iostream>
#include <future>
#include <string>
#include <array>
#include <vector>
#include <stdexcept>
#include <random>
#include <chrono>
#include <thread>
#include <limits>
#include <boost/program_options.hpp>
#include <boost/timer/timer.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/moment.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <parallel/algorithm>
#ifdef _OPENMP
#include <omp.h>
#endif

using std::cout;
using std::cerr;
using std::endl;
using std::string;
using std::array;
using std::vector;
using std::future;
using std::async;
using std::launch;
using std::runtime_error;
using std::numeric_limits;
using std::default_random_engine;
using boost::timer::auto_cpu_timer;

typedef boost::numeric::ublas::matrix<int> Matrix;
typedef boost::numeric::ublas::zero_matrix<int> ZeroMatrix;

namespace ba = boost::accumulators;
typedef ba::accumulator_set<double, ba::stats<ba::tag::mean, ba::tag::min, ba::tag::max, ba::tag::count, ba::tag::variance, ba::tag::moment<2>>> Acc;

struct __attribute__ ((__packed__))  NetReq {
    int64_t        reqID;
    int32_t        train;        // [0, 5000)
    int16_t        start;        // [0, 10)
    int16_t        stop;
};

struct __attribute__ ((__packed__))  NetResp {
    int64_t        reqID;
    int32_t        respID;
    int32_t        seat;
};

// return resp_ts - req_ts in seconds
double time_diff (struct timeval const &resp_ts, struct timeval const &req_ts) {
    double ss = resp_ts.tv_sec - req_ts.tv_sec;
    ss += (resp_ts.tv_usec - req_ts.tv_usec) / 1000000.0;
    return ss;
}

struct NetStat {
    struct timeval req_ts;
    struct timeval resp_ts;
    NetReq const *req;
    NetResp const *resp;
    int sid;    // global seat id, == (train * tspace.seats + resp.seat) * segments + segment

    NetStat (): req(nullptr), resp(nullptr), sid(-1) {
    }

    // in seconds
    double latency () const {
        return time_diff(resp_ts, req_ts);
    }
};

struct TicketSpace {
    int trains;
    int segments;
    int seats;
    int max_length;

    void check_size () {
        int64_t v = 1;
        v *= trains;
        v *= segments;
        v *= max_length;
        BOOST_VERIFY(v <= numeric_limits<int>::max());
    }

    // generate one random request
    template <typename RANDOM_ENGINE>
    void sample (RANDOM_ENGINE &e, NetReq *req) const {
        req->train = e() % trains;
        req->start = e() % segments;
        int ml = segments - req->start;
        // extreme case, last segment:  req->start = segments - 1
        // ml = 1
        if (ml > max_length) {
            ml = max_length;
        }
        int length = (e() % ml) + 1;
        req->stop = req->start + length;
    }

    // SID uniquely identify (train, seat, segment) triplet
    int make_sid (int train, int seat, int segment) const {
        return (train * seats + seat) * segments + segment;
    }

    int total_sids () const {
        return trains * seats * segments;
    }
};

enum {  // performance counters
    CNT_SEND = 0,
    CNT_RECV,
    CNT_SUCC,
    CNT_SEND_BYTE,
    CNT_RECV_BYTE,
    CNT_NUM
};

typedef array<size_t, CNT_NUM> Counters;

void send_all (int sockfd, void const *buf, size_t len) {
    while (len > 0) {
        ssize_t r = send(sockfd, buf, len, 0);
        if (r < 0 || r > len) throw runtime_error("send error");
        buf += r;
        len -= r;
    }
}

void recv_all (int sockfd, void *buf, size_t len) {
    while (len > 0) {
        ssize_t r = recv(sockfd, buf, len, 0);
        if (r < 0 || r > len) throw runtime_error("recv error");
        buf += r;
        len -= r;
    }
}

class Client: public vector<NetStat> {

    TicketSpace ts;
    int sockfd;
    bool volatile done;  // stop flag, reader/writer will
                // stop when done becomes true
    size_t batch;
    size_t queue;
    size_t sleep;
    Counters counters;
    size_t volatile *pcounters;

    future<void> rfuture;
    future<void> wfuture;

    vector<NetReq> reqs;
    vector<NetResp> resps;

    void reader () {
        for (auto &resp: resps) {
            if (done) break;
            struct timeval tv;
            recv_all(sockfd, reinterpret_cast<char *>(&resp), sizeof(resp));
            pcounters[CNT_RECV_BYTE] += sizeof(resp);
            gettimeofday(&tv, NULL);
            if (resp.reqID < 0 || resp.reqID >= size()) {
                throw runtime_error("bad server response");
            }
            auto &st = at(resp.reqID);
            st.resp = &resp;
            st.resp_ts = tv;
            ++pcounters[CNT_RECV];
            if (resp.seat >= 0) {
                ++pcounters[CNT_SUCC];
                st.sid = ts.make_sid(st.req->train, resp.seat, st.req->start);
            }
        }
    }

    void writer () {
        for (unsigned i = 0; i < reqs.size(); i += batch) {
            while ((pcounters[CNT_SEND] + batch - pcounters[CNT_RECV] >= queue) && (!done)) {
                usleep(sleep);
            }
            // i is batch start
            if (done) break;
            struct timeval ts;
            gettimeofday(&ts, NULL);
            unsigned n = reqs.size() - i;
            if (n > batch) n = batch;
            for (unsigned j = i; j < i + n; ++j) {
                auto const &req = reqs[j];
                auto &st = at(j);
                st.req_ts = ts;
                st.req = &req;
            }
            send_all(sockfd, reinterpret_cast<char const *>(&reqs[i]), sizeof(NetReq) * n);
            pcounters[CNT_SEND] += batch;
            pcounters[CNT_SEND_BYTE] += sizeof(NetReq) * n;
        }
        done = true;
    }

public:
    Client ()
        : sockfd(-1), done(false), batch(1), queue(100), sleep(100), pcounters(&counters[0])
    {
        counters.fill(0);
    }

    ~Client () {
        BOOST_VERIFY(sockfd < 0);
    }

    void setBatch (size_t b) {
        batch = b;
    }

    void setQueue (size_t q) {
        queue = q;
    }

    void setSleep (size_t s) {
        sleep = s;
    }

    // pre-generate random queries
    template <typename RANDOM_ENGINE>
    void generate (TicketSpace ts, size_t n, RANDOM_ENGINE &e) {
        reqs.resize(n);
        resps.resize(n);
        resize(n, NetStat());
        for (unsigned i = 0; i < reqs.size(); ++i) {
            auto &req = reqs[i];
            req.reqID = i;
            ts.sample(e, &req);
        }
    }

    void start (string const &server, unsigned short port) {
        struct sockaddr_in serv_addr;
        struct hostent *ent;

        BOOST_VERIFY(batch < queue);

        bzero((char *) &serv_addr, sizeof(serv_addr));

        ent = gethostbyname(server.c_str());
        if (ent == NULL) {
            throw runtime_error("ERROR, no such host");
        }
        serv_addr.sin_family = AF_INET;
        bcopy((char *)ent->h_addr,
             (char *)&serv_addr.sin_addr.s_addr,
             ent->h_length);
        serv_addr.sin_port = htons(port);

        sockfd = socket(AF_INET, SOCK_STREAM, 0);
        if (sockfd < 0)
            throw runtime_error("ERROR opening socket");
        {
            int flags = 1;
            int r = setsockopt(sockfd, IPPROTO_TCP, TCP_NODELAY, reinterpret_cast<char *>(&flags), sizeof(flags));
            if (r != 0) throw runtime_error("ERROR setsockopt");
        }
        if (::connect(sockfd, (struct sockaddr *)&serv_addr, sizeof(serv_addr)) < 0)
            throw runtime_error("ERROR connecting");
        rfuture = async(launch::async, &Client::reader, this);
        wfuture = async(launch::async, &Client::writer, this);
    }

    void stop () { done = true;
    }

    void join () {
        rfuture.get();
        wfuture.get();
        close(sockfd);
        sockfd = -1;
    }

    void getCounters (Counters *c) const {
        std::copy(pcounters, pcounters + CNT_NUM, &(*c)[0]);
    }
};

void count_all (vector<Client> const &clients, Counters *p) {
    Counters total;
    total.fill(0);
    for (auto const &c: clients) {
        Counters cnts;
        c.getCounters(&cnts);
        for (unsigned i = 0; i < total.size(); ++i) {
            total[i] += cnts[i];
        }
    }
    *p = total;
}

// print performance counters every cycle seconds
// stop when *stop becomes true
void monitor_throughput (vector<Client> const *clients, float cycle, bool *stop) {
    Counters old;
    count_all(*clients, &old);
    std::chrono::milliseconds delta(int(cycle * 1000));
    auto next = std::chrono::system_clock::now() + delta;
    cout << "Reporting throughput every " << cycle << " seconds..." << endl;
    cout << "Mt/s ==  million transactions per second" << endl;
    cout << "MB/s ==  1024x1024 bytes per second" << endl;
    cout.setf(std::ios::fixed);
    cout.precision(5);
    double MB = 1024*1024;
    while (!*stop) {
        std::this_thread::sleep_until(next);
        next += delta;
        Counters cnts;
        count_all(*clients, &cnts);
        cout << "Mt/s "
             << "send: " << (cnts[CNT_SEND] - old[CNT_SEND]) / cycle / 1000000.0
             << " receive: " << (cnts[CNT_RECV] - old[CNT_RECV]) / cycle / 1000000.0
             << " succeed: " << (cnts[CNT_SUCC] - old[CNT_SUCC]) / cycle / 1000000.0;
        cout << "    MB/s "
             << "send: " << (cnts[CNT_SEND_BYTE] - old[CNT_SEND_BYTE]) / MB
             << " receive: " << (cnts[CNT_RECV_BYTE] - old[CNT_RECV_BYTE]) / MB
             << endl;
        old = cnts;
    }
}

// MUST BE SINGLETON
// handle signals during the life cycle of this object
// when signal received, stop clients
class Signal {
    static vector<Client> *clients;
    static void stop_all (int) {
        if (clients) {
            for (auto &client: *clients) {
                client.stop();
            }
        }
    }
public:
    Signal (vector<Client> *clients_) {
        BOOST_VERIFY(clients == nullptr);
        clients = clients_;
        signal(SIGTERM, &Signal::stop_all);
        signal(SIGINT, &Signal::stop_all);
    }

    ~Signal () {
        clients = nullptr;
        signal(SIGTERM, SIG_DFL);
        signal(SIGINT, SIG_DFL);
    }
};

vector<Client> *Signal::clients = nullptr;

int main (int argc, char *argv[]) {

    namespace po = boost::program_options;
    TicketSpace tspace;
    string server;
    unsigned short port;
    size_t N, C, B, Q, S;
    float cycle;

    po::options_description desc("Allowed options");
    desc.add_options()
    ("help,h", "produce help message.")
    ("server,s", po::value(&server)->default_value("localhost"), "")
    ("port,p", po::value(&port)->default_value(12306), "")
    ("trains", po::value(&tspace.trains)->default_value(5000), "")
    ("segments", po::value(&tspace.segments)->default_value(10), "")
    ("seats", po::value(&tspace.seats)->default_value(3000), "")
    ("max-length", po::value(&tspace.max_length)->default_value(5), "")
    (",N", po::value(&N)->default_value(10000000), "queries per client")
    (",C", po::value(&C)->default_value(4), "number parallel clients")
    (",B", po::value(&B)->default_value(20), "request batch size")
    (",Q", po::value(&Q)->default_value(1000), "maximal outstanding request per client")
    (",S", po::value(&S)->default_value(100), "if queue is fall, sleep this # us")
    ("cycle", po::value(&cycle)->default_value(1), "print counters every cycle seconds")
    ;

    po::positional_options_description p;

    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).
                     options(desc).positional(p).run(), vm);
    po::notify(vm);

    if (vm.count("help")) {
        cout << desc;
        return 0;
    }

    tspace.check_size();

    vector<Client> clients(C);
    {
        auto_cpu_timer timer(cout);
        cout << "Generating queries..." << endl;
        // this is not efficient when # clients < # available OMP threads
#pragma omp parallel
        {
            int seed = 0;
#ifdef _OPENMP
            seed = omp_get_thread_num();
#endif
            default_random_engine engine(seed);
#pragma omp for
            for (unsigned i = 0; i < clients.size(); ++i) {
                clients[i].generate(tspace, N, engine);
            }
        }
    }

    {
        cout << "Starting clients, interrupt with Ctrl+C..." << endl;
        Signal signal(&clients);
        auto_cpu_timer timer(cout);
        for (auto &client: clients) {
            client.setBatch(B);
            client.setQueue(Q);
            client.start(server, port);
        }
        bool stop_monitor = false;
        future<void> ft = async(launch::async, monitor_throughput, &clients, cycle, &stop_monitor);
        // wait for clients
        for (auto &client: clients) {
            client.join();
        }
        if (clients.empty()) {
            cout << "No clients running, sleep 10s for testing..." << endl;
            sleep(10); // if no clients, sleep 00s for testing.
        }
        stop_monitor = true;
        asm volatile("": : :"memory");
        // wait for throughput monitor
        ft.get();
    }

    // do statistics
    Counters cnts;
    count_all(clients, &cnts);
    cout << "send: " << cnts[CNT_SEND] << endl;
    cout << "receive: " << cnts[CNT_RECV] << endl;
    cout << "succeed: " << cnts[CNT_SUCC] << endl;
    // the array of tickets assigned to each seat
    vector<NetStat const *> v;
    size_t asked = 0;
    size_t succ = 0;
    size_t sold = 0;
    Acc acc;
    for (auto const &client: clients) {
        for (NetStat const &st: client) {
            if (!st.req) continue;
            if (!st.resp) continue;
            if (st.resp->respID < 0) {
                cerr << "respID overflow." << endl;
            }
            acc(st.latency()); // in seconds
            ++asked;
            if (st.sid >= 0) {
                ++succ;
                sold += st.req->stop - st.req->start;
            }
            v.push_back(&st);
        }
    }
    cout << "latency.mean: " << ba::mean(acc) * 1000 << " ms" << endl;
    cout << "latency.std: " << sqrt(ba::variance(acc)) * 1000 << " ms" << endl;
    cout << "latency.min: " << ba::min(acc) * 1000  << " ms" << endl;
    cout << "latency.max: " << ba::max(acc) * 1000 << " ms" << endl;
    cout << "latency.count: " << ba::count(acc) << endl;
    cout << "fill.rate: " <<  1.0 * sold / tspace.total_sids() << "    (rate of inventory sold)" << endl;
    cout << "fulfill.rate: " <<  1.0 * succ / asked << "     (rate of reqs satisfied)" << endl;
    __gnu_parallel::sort(v.begin(), v.end(),
                [](NetStat const *p1, NetStat const *p2) {
                    return p1->resp->respID < p2->resp->respID;
                });
    {
        double max_ood = 0;
        for (unsigned i = 1; i < v.size(); ++i) {
            auto prev = v[i-1]->resp->respID;
            auto cur = v[i]->resp->respID;
            if (prev >= cur) {
                cerr << "respID: " << prev << ", " << cur << endl;
            }
            double ood = time_diff(v[i-1]->resp_ts, v[i]->resp_ts);
            if (ood > max_ood) {
                max_ood = ood;
            }
        }
        cerr << "ooo.time.max: " << max_ood * 1000 << " ms    (big respIDs arrive before small ones)" << endl;
    }
    // detect conflicts 1
    size_t conflict1 = 0;
    // conflict1: later successful request containing earlier failed request
    {
        vector<Matrix> failed(tspace.trains, ZeroMatrix(tspace.segments, tspace.segments));
        // failure matrix
        // elemnet (a, b) = number of times a request [a, b] as failed.
        for (NetStat const *p: v) {
            // this test is train specific
            BOOST_VERIFY(p->req->train < tspace.trains);
            auto &matrix = failed[p->req->train];
            int a = p->req->start;
            int b = p->req->stop-1; // the stop segment is not included as part of the request
                                    // it is always true that stop >= start + 1
                                    // so it's always that b >= a
            BOOST_VERIFY(a >= 0 && a < tspace.segments);
            BOOST_VERIFY(b >= a && b < tspace.segments);
            // the segment matrix is a upper-right triangle matrix
            if (p->sid < 0) {
                matrix(a, b) += 1;
            }
            else {
                // request for [a, b] succeeded,
                // ( it's always true that b >= a )
                // all previous request [A, B] contained in [a, b] must not fail.
                // equivalently, all matrix entries (A, B) with a <= A <= B <= b must not fail
                for (int A = a; A <= b; ++A) {
                    for (int B = A; B <= b; ++B) {
                        conflict1 += matrix(A, B);
                    }
                }
            }
        }
    }
    cout << "conflict1: " << conflict1 << "    (successful reservation containing failed requests)" << endl;

    // remove all failed transactions
    {
        unsigned i = 0;
        unsigned sz = v.size();
        while (i < sz) {
            if (v[i]->sid < 0) {
                v[i] = v[sz-1]; // replace v[i] with last element
                --sz;
            }
            else {
                ++i;
            }
        }
        v.resize(sz);
    }

    // detect conflicts 2
    __gnu_parallel::sort(v.begin(), v.end(),
                [](NetStat const *p1, NetStat const *p2) {
                    return p1->sid < p2->sid;
                });
    // verify no overlap
    size_t conflict2 = 0;
    for (unsigned i = 1; i < v.size(); ++i) {
        if (v[i-1]->req->train != v[i]->req->train) continue;
        if (v[i-1]->resp->seat != v[i]->resp->seat) continue;
        // same seat on same train, detect conflict
        if (v[i-1]->req->stop >= v[i]->req->start) {
            /*
            cout << "conflict: "
                << v[i-1]->resp->reqID << '.'
                << v[i-1]->resp->respID
                << " and "
                << v[i]->resp->reqID << '.'
                << v[i]->resp->respID
                << endl;
            */
            ++conflict2;
        }
    }
    cout << "conflict2: " << conflict2 << "    (seats are sold twice)" << endl;
    return 0;
}


/********************************************************
  Stanford Driving Software
  Copyright (c) 2011 Stanford University
  All rights reserved.

  Redistribution and use in source and binary forms, with
  or without modification, are permitted provided that the
  following conditions are met:

* Redistributions of source code must retain the above
  copyright notice, this list of conditions and the
  following disclaimer.
* Redistributions in binary form must reproduce the above
  copyright notice, this list of conditions and the
  following disclaimer in the documentation and/or other
  materials provided with the distribution.
* The names of the contributors may not be used to endorse
  or promote products derived from this software
  without specific prior written permission.

  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
  CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
  WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
  PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
  INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
  OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
  DAMAGE.
 ********************************************************/

#include <boost/format.hpp>

#include <aw_CGAL.h>
#include <vlrException.h>
#include <perception/PerceptionObstacles.h>
//#include <clothoid.h>
//#include <bezCtxPS.h>

#include <aw_ChsmPlanner.hpp>
#include <aw_StPause.hpp>
#include <aw_StActive.hpp>
#include <aw_LaneChangeManager.hpp>
#include <obstaclePrediction.h>
//#include <aw_SituationInterpretation.hpp>

namespace drc = driving_common;

namespace vlr {

#undef TRACE
#define TRACE(str) std::cout << "[ChsmPlanner] "<< str << std::endl

template<class T, void*(T::*mem_fn)(void*)>
void* threadCBWrapper(void* ptr) {
  return (static_cast<T*>(ptr)->*mem_fn)(NULL);
  }

using namespace std;
using namespace CGAL_Geometry;
using namespace driving_common;

#define EPS 0.01

/*---------------------------------------------------------------------------
 * ChsmPlanner::Parameters
 *---------------------------------------------------------------------------*/
ChsmPlanner::Parameters::Parameters() {

  center_line_back_sample_length = -15;      // in what distance are curvepoints sampled behind the car
  center_line_sample_dist  = 7;    // distance between (re)sampled waypoints
  center_line_post_smoothing_sample_dist = 1;      // distance between sampled point after clothoid fitting
  center_line_min_lookahead_dist = 20; // we look at least 20m ahead, regardless of speed
  smoothing_range = 8;                // smooth over 8 points
  // kturn params
  kturn_delta = 4.7;                  // kturn distance offset
  kturn_radius = 6.0;                 // kturn radius
  kturn_switch_speed = 0.1;           // switches to next phase if speed below this threshold
  kturn_switch_distance = 0.5;        // switches to next phase if position difference below this threshold

  // message buffer
  message_buffer_size = 15;           // size of the internal message buffer

  safety_stop_infront_vehicle_distance = 1.5*SAFETY_DISTANCE; // 1.5 = compensate for measurement failures

  enable_recovery = false;
  enable_blockade_detection = false;

  // speed related params
  max_speed_global                      = dgc::dgc_mph2ms(25.0); // maximal velocity used by the planner in m/s
  curvature_slowdown_factor             = 1.5;              // slowdown factor for curvatures in tentacle mode
  surface_quality_slowdown_factor       = .5;               // slowdown factor for rough surface areas
  max_speed_drive                       = dgc::dgc_mph2ms(65);
  max_speed_kturn                       = dgc::dgc_mph2ms(6);
  max_speed_intersection                = dgc::dgc_mph2ms(35);
  max_speed_merge_intersection          = dgc::dgc_mph2ms(35);
  max_speed_intersection_approach       = dgc::dgc_mph2ms(55);
  max_speed_traffic_light_approach      = dgc::dgc_mph2ms(45);
  max_speed_crosswalk_approach_empty    = dgc::dgc_mph2ms(25);
  max_speed_crosswalk_approach_occupied = dgc::dgc_mph2ms(15);
  max_speed_goal                        = dgc::dgc_mph2ms(5);
  max_speed_pass_obstacle               = dgc::dgc_mph2ms(25);
  max_speed_pass_switch_lane            = dgc::dgc_mph2ms(5); // ?!?
  max_speed_enter_zone                  = dgc::dgc_mph2ms(5);
  max_speed_lane_merge_false            = dgc::dgc_mph2ms(5); // ?!?
}

ChsmPlanner::Parameters::~Parameters() {

}

/*---------------------------------------------------------------------------
 * ChsmPlanner
 *---------------------------------------------------------------------------*/
ChsmPlanner::ChsmPlanner(std::string rndf_filename, std::string mdf_filename,
                        double static_obstacle_map_width, double static_obstacle_map_height,
                        double static_obstacle_map_resolution)
                        : pose_queue_(pose_queue_size_), robot_yaw_rate_(0),
                          vehicle_manager(NULL), topology_(NULL), route_sampler(NULL),
                          publish_traffic_lights_(false),
                          current_index_on_mission_(0),
                          transitionCounter(0),
                          inPause(true),
                          traj_eval_(NULL),
                          last_start_idx_(0),
                          min_lookahead_s_(0),
                          lane_change_data(NULL),
                          stop_before_replanning(true),
                          forced_start_edge(NULL),
                          has_pause_lanechange(false),
                          road_boundaries_enabled( false ),
                          replan_reason( ChsmPlanner::UNKNOWN ),
                          static_obstacle_map_raw_(NULL),
                          static_obstacle_map_tmp_(NULL),
                          static_obstacle_map_(NULL),
                          emergency_map_clear_(false),
                          emergency_stop_initiated_(false),
                          quit_chsm_planner_(false),
                          new_static_obstacle_map_ready_(false) {

  pthread_attr_init(&def_thread_attr_);

  pthread_mutex_init(&pose_mutex_, NULL);
  pthread_mutex_init(&trajectory_mutex_, NULL);
  pthread_mutex_init(&dyn_obstacles_mutex_, NULL);
  pthread_mutex_init(&static_obstacle_map_mutex_, NULL);
  pthread_mutex_init(&static_obstacle_points_mutex_, NULL);
  pthread_mutex_init(&mission_mutex_, NULL);
  pthread_mutex_init(&topology_mutex_, NULL);
  pthread_mutex_init(&center_line_mutex_, NULL);
  pthread_mutex_init(&intersection_predictor_mutex_, NULL);
  pthread_mutex_init(&traffic_light_states_mutex_, NULL);
  pthread_mutex_init(&traffic_light_poses_mutex_, NULL);
  pthread_mutex_init(&pedestrian_prediction_mutex_, NULL);

  pthread_mutex_init(&static_obstacle_map_cycle_mutex_, NULL);
  pthread_cond_init(&static_obstacle_map_cycle_cv_, NULL);

  // create helper
  topology_ = new Topology(rndf_filename.c_str(), mdf_filename.c_str(), "Topo");
  vehicle_manager = new VehicleManager(*topology_);
  topology_->vehicle_manager = vehicle_manager;
  route_sampler = new RouteSampler(topology_);
  blockade_manager = new BlockadeManager(topology_);
  topology_->blockade_manager = blockade_manager;
  traj_eval_ = new TrajectoryEvaluator(static_obstacle_map_mutex_, dyn_obstacles_mutex_);

  velocity_desired_ = 0;
  velocity_following_ = 0;
  follow_distance_ = 0;
  stop_distance_ = 0;

  turn_signal_.signal = driving_common::TurnSignal::NONE;

  updateStaticObstacleMapSize(static_obstacle_map_width, static_obstacle_map_height, static_obstacle_map_resolution);

  pthread_create(&obstacle_map_update_thread_id_, &def_thread_attr_,
      threadCBWrapper<ChsmPlanner, &ChsmPlanner::updateStaticObstacleMapThread>, this);

  bIntersection = false;
  bOffroad = false;

    // TODO: remove...
  replan_distance_map_.insert(make_pair(0, 2));
  replan_distance_map_.insert(make_pair(.5, 2.5));
  replan_distance_map_.insert(make_pair(6, 3));
  replan_distance_map_.insert(make_pair(8, 4));
  replan_distance_map_.insert(make_pair(15, 5));
  replan_distance_map_.insert(make_pair(20, 6));
}

ChsmPlanner::~ChsmPlanner()
{
  delete topology_;
  delete vehicle_manager;
  delete route_sampler;
  delete blockade_manager;
  quit_chsm_planner_ = true;

  pthread_mutex_lock(&static_obstacle_map_cycle_mutex_);
  pthread_cond_signal(&static_obstacle_map_cycle_cv_);
  pthread_mutex_unlock(&static_obstacle_map_cycle_mutex_);

  pthread_join(obstacle_map_update_thread_id_, NULL);
  if(static_obstacle_map_raw_) {delete[] static_obstacle_map_raw_;}
  if(static_obstacle_map_tmp_) {delete[] static_obstacle_map_tmp_;}
  if(static_obstacle_map_) {delete[] static_obstacle_map_;}

  pthread_attr_destroy(&def_thread_attr_);

  pthread_mutex_destroy(&pose_mutex_);
  pthread_mutex_destroy(&trajectory_mutex_);
  pthread_mutex_destroy(&dyn_obstacles_mutex_);
  pthread_mutex_destroy(&static_obstacle_map_mutex_);
  pthread_mutex_destroy(&static_obstacle_points_mutex_);
  pthread_mutex_destroy(&mission_mutex_);
  pthread_mutex_destroy(&topology_mutex_);
  pthread_mutex_destroy(&center_line_mutex_);
  pthread_mutex_destroy(&traffic_light_states_mutex_);
  pthread_mutex_destroy(&traffic_light_poses_mutex_);
  pthread_mutex_destroy(&pedestrian_prediction_mutex_);

  pthread_mutex_destroy(&static_obstacle_map_cycle_mutex_);
  pthread_cond_destroy(&static_obstacle_map_cycle_cv_);

}

void ChsmPlanner::activate() {
  process_event(EvActivate());
}

void ChsmPlanner::pause() {
  process_event(EvPause());
}

void ChsmPlanner::start() {

  if(pose_queue_.empty()) {
    throw VLRException("Could not start chsm planner since current position is unknown (pose queue is empty).");
  }

  drc::GlobalPose pose = pose_queue_.pose(drc::Time::current());
  cout << "[CHSM] plan the mission" << endl;
  addMessage("plan the mission");
  pthread_mutex_lock(&mission_mutex_);
  topology_->planMission(pose.utmX(), pose.utmY(), pose.yaw());

  createSmoothedMissionLine();

  last_start_idx_ = 0;

  static bool dump_mission_lines = true;

  if(dump_mission_lines) {
    std::string ml_name = "mission_line.txt";
    std::string ml_raw_name = "mission_line_raw.txt";
    std::string ml_rndf_name = "mission_line_rndf.txt";
    try {
      dumpMissionLines(ml_name, ml_raw_name, ml_rndf_name);
    }
    catch(vlr::Ex<>& e) {
      std::cout << "Could not write center line data for current mission: " << e.what() << std::endl;
    }
  }

  pthread_mutex_unlock(&mission_mutex_);

  pthread_mutex_lock(&topology_mutex_);
  LaneChangeManager::annotateLaneChanges(topology_);
  pthread_mutex_unlock(&topology_mutex_);

  // initiate state machine
  cout << "[CHSM] initiate state machine" << endl;
  addMessage("initiate state machine");
  initiate();
}

void ChsmPlanner::process()
{
  transitionCounter = 0;
  process_event(EvProcess());
  process_event(EvAfterProcess());
}

void ChsmPlanner::stop() {
  //  process_event(EvStop());
}

uint32_t ChsmPlanner::extendAtMissionStart(std::vector<CurvePoint>& mission) {
  CurvePoint cp;
  double extended_length = - 2 * params().center_line_back_sample_length;
  std::vector<CurvePoint> pre_mission_points;
  while(extended_length > 0) {
    extended_length -= params().center_line_post_smoothing_sample_dist;
    cp.s = mission[0].s - extended_length; // negative s since it's before mission
    cp.kappa = 0;// smoothed_mission_points_[0].kappa;
    cp.theta = mission[0].theta;
    cp.kappa_prime = 0;
    cp.x = mission[0].x - extended_length*cos(cp.theta);
    cp.y = mission[0].y - extended_length*sin(cp.theta);
    pre_mission_points.push_back(cp);
  }
  mission.insert(mission.begin(), pre_mission_points.begin(), pre_mission_points.end());

return pre_mission_points.size();
}

uint32_t ChsmPlanner::extendAtMissionEnd(std::vector<CurvePoint>& mission, double front_sample_dist) {
  CurvePoint cp;
  CurvePoint& cp_last = mission[mission.size()-1];
  double extended_length=0;
  uint32_t num_post_sample_points=0;
  while(extended_length < front_sample_dist) {
    extended_length += params().center_line_post_smoothing_sample_dist;
    cp.s = cp_last.s + extended_length; // negative s since it's before mission
    cp.kappa = 0;// cp_last.kappa;
    cp.theta = cp_last.theta;
    cp.kappa_prime = 0;
    cp.x = cp_last.x + extended_length*cos(cp.theta);
    cp.y = cp_last.y + extended_length*sin(cp.theta);
    mission.push_back(cp);
    num_post_sample_points++;
  }

  return num_post_sample_points;
}

void ChsmPlanner::createSmoothedMissionLine() {
  std::vector<bool> mission_points_to_ignore;
  route_sampler->sampleMission(mission_points_, mission_points_to_ignore);

  smoother.sampleLinearEquidist(mission_points_, mission_points_to_ignore, params().center_line_sample_dist, sampled_mission_points_);

  if(sampled_mission_points_.size() < 2) {
    throw VLRException("Mission must contain at least 2 waypoints; resampling failed.");
  }

    // replacement for clothoid due to licence issues :-(
  mission_bezier_points_.clear();
  for(int32_t i=0; i<(int32_t)sampled_mission_points_.size()-1; i++) {
    CurvePoint& p0 = sampled_mission_points_[i];
    CurvePoint& p1 = sampled_mission_points_[i+1];
    CurvePoint tp1, tp2;
    if(i==0) {
      CurvePoint& p2 = sampled_mission_points_[i+2];
      double len0 = hypot(p1.x-p0.x, p1.y-p0.y);
      double len1 = hypot(p2.x-p1.x, p2.y-p1.y);
      double scale1 = len0/len1;
      tp2.x = p1.x - 1./3. * (p2.x-p1.x)*scale1;
      tp2.y = p1.y - 1./3. * (p2.y-p1.y)*scale1;
      double lenm1 = hypot(tp2.x-p0.x, tp2.y-p0.y);
      double scale0 = len0/lenm1;
      tp1.x = p0.x + 1./3. * (tp2.x-p0.x)*scale0;
      tp1.y = p0.y + 1./3. * (tp2.y-p0.y)*scale0;
    }
    else if((size_t)i==sampled_mission_points_.size()-2) {
      CurvePoint& pm1 = sampled_mission_points_[i-1];
      double lenm1 = hypot(p0.x-pm1.x, p0.y-pm1.y);
      double len0 = hypot(p1.x-p0.x, p1.y-p0.y);
      double scale0 = len0/lenm1;
      tp1.x = p0.x + 1./3. * (p0.x-pm1.x)*scale0;
      tp1.y = p0.y + 1./3. * (p0.y-pm1.y)*scale0;
      double len1 = hypot(p1.x-tp1.x, p1.y-tp1.y);
      double scale1 = len0/len1;
      tp2.x = p1.x - 1./3. * (p1.x-tp1.x)*scale1;
      tp2.y = p1.y - 1./3. * (p1.y-tp1.y)*scale1;
    }
    else {
      CurvePoint& pm1 = sampled_mission_points_[i-1];
      CurvePoint& p2 = sampled_mission_points_[i+2];
      double lenm1 = hypot(p0.x-pm1.x, p0.y-pm1.y);
      double len0 = hypot(p1.x-p0.x, p1.y-p0.y);
      double len1 = hypot(p2.x-p1.x, p2.y-p1.y);
      double scale0 = len0/lenm1;
      double scale1 = len0/len1;
      tp1.x = p0.x + 1./3. * (p0.x-pm1.x)*scale0;
      tp1.y = p0.y + 1./3. * (p0.y-pm1.y)*scale0;
      tp2.x = p1.x - 1./3. * (p2.x-p1.x)*scale1;
      tp2.y = p1.y - 1./3. * (p2.y-p1.y)*scale1;
    }
    mission_bezier_points_.push_back(p0);
    mission_bezier_points_.push_back(tp1);
    mission_bezier_points_.push_back(tp2);
  }

//  smoother.cubicBezierFromClothoid(sampled_mission_points_, mission_bezier_points_);

  mission_bezier_points_.push_back(sampled_mission_points_[sampled_mission_points_.size()-1]);
  std::vector<CurvePoint> temp_smoothed_mission_points;
  std::vector<CurvePoint> temp_smoothed_mission_points2;
  smoother.sampleCubicBezierEquidist(mission_bezier_points_, 1, temp_smoothed_mission_points);

  extendAtMissionStart(temp_smoothed_mission_points);
  double post_sample_dist = dgc::dgc_mph2ms(75)*traj_eval_->params().checked_horizon;  // assumed worst case
  extendAtMissionEnd(temp_smoothed_mission_points, post_sample_dist);

  smoother.clothoideSpline(temp_smoothed_mission_points, temp_smoothed_mission_points[0].theta, temp_smoothed_mission_points[0].kappa, temp_smoothed_mission_points[0].s, params().smoothing_range, smoothed_mission_points_);

  mission_points_set_.clear();
  std::vector<CurvePoint>::const_iterator cit = temp_smoothed_mission_points.begin();
  double x0=mission_points_[0].x;
  double y0=mission_points_[0].y;
  uint32_t mp_idx=0;
  for(uint32_t i=0; i<temp_smoothed_mission_points.size(); i++) {
     if(temp_smoothed_mission_points[i].x == x0 && temp_smoothed_mission_points[i].y == y0) {
       mission_points_[mp_idx].s=(double)i;   // TODO: index is stored as s, find different structure to store mission data..
       mission_points_set_.insert(&mission_points_[mp_idx]);
       mp_idx++;
       x0=mission_points_[mp_idx].x;
       y0=mission_points_[mp_idx].y;
     }
   }

createVelocityProfile();
}

void ChsmPlanner::createVelocityProfile() {
  // step 1: annotate each points with its maximum velocity
  Route::RouteEdgeList::const_iterator it = topology_->route.route.begin(), it_end = topology_->route.route.end();
  Route::RouteEdgeList::const_iterator it_last = --topology_->route.route.end();
  uint32_t last_idx=0;
  for(; it != it_end; it++ ) {
//    const RoutePlanner::AnnotatedRouteEdge::AnnotationList& annotation_list = (*it)->annotations();

    if(it != it_last) {
      Route::RouteEdgeList::const_iterator it2=it;
      it2++;

      uint32_t idx = 0, idx2 = 0;
      try {
        idx = missionIndex((*it)->edge()->fromVertex()->x(), (*it)->edge()->fromVertex()->y(), last_idx);
      }
      catch(vlr::Ex<>& e) {
        std::cout << e.what() << std::endl;
      }
      try {
        idx2 = missionIndex((*it2)->edge()->fromVertex()->x(), (*it2)->edge()->fromVertex()->y(), idx);
      }
      catch(vlr::Ex<>& e) {
        std::cout << e.what() << std::endl;
      }
      last_idx=idx;

        // TODO: Do proper preflight/run to determine state (machine) velocities
      double max_state_speed = params().max_speed_global;
//      if(annotation_list.empty()) {continue;}
//      RoutePlanner::AnnotatedRouteEdge::AnnotationList::const_iterator ait = annotation_list.begin(), ait_end = annotation_list.end();
//      std::stringstream s;
//      for(;ait != ait_end; ait++) {
//        if((*ait)->maneuver() == UC_MANEUVER_TRAVEL) {continue;}
//      }

      double min_edge_speed=(*it)->edge()->minSpeed();
      double max_edge_speed=(*it)->edge()->maxSpeed();
      double max_speed = std::max(min_edge_speed, std::min(max_state_speed, std::min(params().max_speed_global, max_edge_speed)));
      if(smoothed_mission_points_[idx].kappa >=EPS) {
        max_speed = std::min(max_speed, sqrt(params().curvature_slowdown_factor/smoothed_mission_points_[idx].kappa));
      }
//      if((*it)->edge()->fromVertex()->isStopVertex()) {max_speed=1;}
      while(idx<idx2) {
        smoothed_mission_points_[idx++].v=max_speed;
      }
    }
    else {
      uint32_t idx = 0;
      try {
        idx = missionIndex((*it)->edge()->fromVertex()->x(), (*it)->edge()->fromVertex()->y(), last_idx);
      }
      catch(vlr::Ex<>& e) {
        std::cout << e.what() << std::endl;
      }
      while(idx<smoothed_mission_points_.size()) {
        smoothed_mission_points_[idx++].v = 0;
      }
    }
  }
  double max_accel = 3;
  double max_decel = -2;

  // 2. Step forward through the mission and make velocities achievable with given accel
  std::vector<CurvePoint>::iterator mit = smoothed_mission_points_.begin(), mit_end = smoothed_mission_points_.end();
  for(; mit != mit_end; mit++ ) {
    std::vector<CurvePoint>::iterator mit2 = mit;
    mit2++;
    while(mit2 != mit_end) {
      if(checkAndReplaceVelocityForward(*mit, *mit2, max_accel)) {break;}
      mit2++;
    }
  }

  // 3. Step backwards through the mission and make velocities achievable with given decel
  std::vector<CurvePoint>::reverse_iterator rit = smoothed_mission_points_.rbegin(), rit_end = smoothed_mission_points_.rend();
  for(; rit != rit_end; rit++ ) {
    std::vector<CurvePoint>::reverse_iterator rit2 = rit;
    rit2++;
    while(rit2 != rit_end) {
      if(checkAndReplaceVelocityBackward(*rit, *rit2, max_decel)) {break;}
      rit2++;
    }
  }

    // 4. add time indices
  mission_time_map_.clear();
  mit = smoothed_mission_points_.begin(), mit_end = smoothed_mission_points_.end();
  double last_s, last_v, last_t;
  if(mit != mit_end) {
    CurvePoint& cp = *mit;
    cp.t = 0;
    last_s=cp.s;
    last_v=cp.v;
    last_t=cp.t;
    mit++;
  }
  for(uint32_t i=0; mit != mit_end; mit++, i++) {
    CurvePoint& cp = *mit;
    double ds=cp.s-last_s;
    if(ds==0) {
      cp.t=last_t;
      continue;
    }

    double dv=cp.v-last_v;
    double a=(cp.v*cp.v-last_v*last_v)/(2*ds);
    double dt;
    if (a != 0) {
      if (dv >= 0) {
        dt = -last_v / a + sqrt((last_v / a) * (last_v / a) + 2 * ds / a);
      }
      else {
         dt = -last_v / a - sqrt((last_v / a) * (last_v / a) + 2 * ds / a);
      }
    }
    else {
      if(last_v !=0) {
        dt = ds/last_v;
      } else {
      dt=std::numeric_limits<double>::infinity();
      }
    }
    cp.t = last_t + dt;
    last_t = cp.t;
    last_v = cp.v;
    last_s = cp.s;
    mission_time_map_.insert(std::make_pair(cp.t, &cp));
  }

//  {
//  printf("with t:\n");
//  std::vector<CurvePoint>::iterator tmit = smoothed_mission_points_.begin(), tmit_end = smoothed_mission_points_.end();
//  FILE* f = fopen("vt.txt", "w");
//  for(uint32_t i=0; tmit != tmit_end; tmit++, i++ ) {
//    fprintf(f, "%u. %f %f %f\n", i, tmit->s, dgc::dgc_ms2mph(tmit->v), tmit->t);
//  }
//  fclose(f);
//  }
}

bool ChsmPlanner::checkAndReplaceVelocityBackward(CurvePoint& p_ref, CurvePoint& p, double max_decel) {

  if (p.v > p_ref.v) {
    double t = -p_ref.v/max_decel-sqrt((p_ref.v/max_decel)*(p_ref.v/max_decel)+2*(p.s-p_ref.s)/max_decel);
    double v_feasible = p_ref.v + max_decel * t;
    if (p.v > v_feasible) {
      p.v = v_feasible;
      return false;
    }
  }
  return true;
}

bool ChsmPlanner::checkAndReplaceVelocityForward(CurvePoint& p_ref, CurvePoint& p, double max_accel) {
  if (p.v > p_ref.v) {
    double t = -p_ref.v/max_accel+sqrt((p_ref.v/max_accel)*(p_ref.v/max_accel)+2*(p.s-p_ref.s)/max_accel);
    double v_feasible = p_ref.v + max_accel * t;
    if (v_feasible < p.v) {
      p.v = v_feasible;
      return false;
    }
  }
  return true;
}

void ChsmPlanner::updateRobot(double timestamp, double smooth_x, double smooth_y, double offset_x, double offset_y, double yaw, double speed, double ax, double ay, double az, double yaw_rate)
{
  pthread_mutex_lock(&pose_mutex_);
  robot_yaw_rate_ = yaw_rate;
  double a = ax * cos(-yaw) - ay*sin(-yaw); // TODO: check this...+-theta?!?
  double a_lat = ax * sin(-yaw) + ay*cos(-yaw);// TODO: check this...+-theta?!?

  pose_queue_.push(GlobalPose(smooth_x, smooth_y, offset_x, offset_y, yaw, 0, 0, speed, 0, 0, a, a_lat, az), timestamp);

  pthread_mutex_unlock(&pose_mutex_);

  if(!topology_->isMissionPlanned()) {return;}

  // update ego vehicle
  pthread_mutex_lock(&topology_mutex_);
  topology_->updateEgoVehicle(smooth_x+offset_x, smooth_y+offset_y, yaw, speed);
  pthread_mutex_unlock(&topology_mutex_);

  // set offroad flag
  bOffroad = (*topology_->current_edge_it_on_complete_mission_graph)->edge()->isOffroadEdge();
}

void ChsmPlanner::updateTrafficLightStates(const driving_common::TrafficLightStates& tf_states) {
  pthread_mutex_lock(&traffic_light_states_mutex_);
  std::vector<driving_common::TrafficLightState>::const_iterator tlsit=tf_states.light_state.begin(), tlsit_end = tf_states.light_state.end();

  for (;tlsit != tlsit_end; tlsit++) {

    // TODO: remove old traffic lights...
//    printf("Got traffic light update for tl id %s (state: %c)\n", (*tlsit).name.c_str(), (*tlsit).state);
    std::map<std::string, driving_common::TrafficLightState>::iterator exists_tlsit = traffic_light_states_.find((*tlsit).name);
    if (exists_tlsit != traffic_light_states_.end()) {
      (*exists_tlsit).second = *tlsit;
    }
    else {
      traffic_light_states_.insert(std::make_pair((*tlsit).name, (*tlsit)));
    }
  }
  pthread_mutex_unlock(&traffic_light_states_mutex_);
}

   // find minimum lane width along center line piece used
   // for trajectory planning in the next time step
double ChsmPlanner::minLookaheadLaneWidth(Route::RouteEdgeList::iterator edge_it, double front_sample_dist) {
  double d = 0, lane_width = std::numeric_limits<double>::infinity();
  while(edge_it != topology_->route.route.end() && d < front_sample_dist) {
    lane_width = std::min(lane_width, (*edge_it)->edge()->width());
    d += (*edge_it)->edge()->length();
    edge_it++;
  }

  if(lane_width == std::numeric_limits<double>::infinity()) {
    lane_width = rndf::Lane::default_width;  // TODO: make independent of rndf lib...
  }

  return lane_width;
}

uint32_t ChsmPlanner::missionIndex(double x, double y, uint32_t last_idx) {
  CurvePoint cp;
  cp.x=x; cp.y=y;
  return missionIndex(cp, last_idx);
}

uint32_t ChsmPlanner::missionIndex(CurvePoint& cp, uint32_t last_idx) {
  std::pair<std::multiset<CurvePoint*, CurvePointComp>::const_iterator,
            std::multiset<CurvePoint*, CurvePointComp>::const_iterator> range = mission_points_set_.equal_range(&cp);

  if(range.first == range.second) {
    std::stringstream s;
    s << "Mission invalid: requested point (" << cp.x << ", " << cp.y << ") is not contained in mission.";
    throw VLRException(s.str());
  }

  std::multiset<CurvePoint*, CurvePointComp>::const_iterator cpit=range.first;
  for(; cpit != range.second; cpit++) {
    uint32_t idx = uint32_t((*cpit)->s); // TODO: find a proper way to handle index
    if(idx>=last_idx) {return idx;}
  }

  std::stringstream s;
  s << "Mission invalid: requested point (" << cp.x << ", " << cp.y << ") is not contained in mission before last index (" << last_idx << ")";
  throw VLRException(s.str());
}

void ChsmPlanner::generateCurvePoints(double front_sample_dist, double maxSpeed) {

 //printf("stop distance: %f\n", curvepoints.stop_distance);
 //double dt = Time::current();
 Route::RouteEdgeList::iterator edge_it = topology_->current_edge_it;
 double dist_from_start = topology_->ego_vehicle.distFromStart();

   // find current position in smoothed mission data set
 CurvePoint ocp;
 ocp.x = (*edge_it)->edge()->fromVertex()->x();
 ocp.y = (*edge_it)->edge()->fromVertex()->y();

 uint32_t min_idx=0;
 try {
   min_idx = missionIndex(ocp, last_start_idx_);
 }
 catch(vlr::Ex<>& e) {
   std::cout << "Error: requesting point on edge " << (*edge_it)->edge()->name() << ": " << e.what() << std::endl;
   throw e;
 }

 double s0 = smoothed_mission_points_[min_idx].s + dist_from_start;

    // Hysteresis for lookahead range to avoid velocity oscillations
  if(min_lookahead_s_ - s0 > front_sample_dist) {
    front_sample_dist = min_lookahead_s_ - s0;
  }
  else {
    min_lookahead_s_ = s0 + front_sample_dist;
  }

    // used later on for velocity calculation, this is discretized in original wp steps!
  current_index_on_mission_ = min_idx;

    // find closest (regarding to s) smoothly sampled index
  while(smoothed_mission_points_[current_index_on_mission_].s < s0 && current_index_on_mission_<smoothed_mission_points_.size()) {
      current_index_on_mission_++;
  }
      // find index for first back-sampled point
  while(s0-smoothed_mission_points_[min_idx].s < -params().center_line_back_sample_length) {
    if(min_idx==0) {break;}
    min_idx--;
  }

  last_start_idx_ = min_idx;

  pthread_mutex_lock(&center_line_mutex_);
  center_line_.clear();

//    // dependent on angle between car and centerline at mission start,
//    // it's not always possible to project current position onto centerline => extend backwards
//  if(min_idx == 0 && s0-smoothed_mission_points_[0].s < -params().center_line_back_sample_length) {
//    extendCenterLineAtMissionStart(s0);
//   }

  uint32_t idx = min_idx;
  while(idx<smoothed_mission_points_.size() && smoothed_mission_points_[idx].s-s0 < front_sample_dist) {
    center_line_.push_back(smoothed_mission_points_[idx]);
    idx++;
  }

//  if(idx == smoothed_mission_points_.size()) {
//    extendCenterLineAtMissionEnd(s0, front_sample_dist);
//  }

    // for the trajectory generation we need a lookahead range taking the max speed into account, for everything
    // else, the lookahead range is based on the last desired velocity (replace with last actual?!?)
  double current_front_sample_dist = std::max(params().center_line_min_lookahead_dist, traj_eval_->params().checked_horizon * velocity_desired_);
  // calculate desired velocity based on speed limits and curvature
  velocity_desired_ = calculateVelocity(maxSpeed, current_front_sample_dist);
  double accel_desired = 0;
/*  if (current_index_on_mission_ < smoothed_mission_points_.size() - 1) {
    double mission_t_current = smoothed_mission_points_[current_index_on_mission_].t;
    uint32_t lookahead_index_ = current_index_on_mission_ + 1;
    while (lookahead_index_ < smoothed_mission_points_.size()
            && smoothed_mission_points_[lookahead_index_].t - mission_t_current  < traj_eval_->params().checked_horizon) {
//      printf("mission_t_current: %f, lookahead t: smoothed_mission_points_[current_index_on_mission_].t: %f, dt: %f\n",
//          mission_t_current, smoothed_mission_points_[current_index_on_mission_].t, smoothed_mission_points_[current_index_on_mission_].t- mission_t_current);
      lookahead_index_++;
    }
    if (lookahead_index_ == smoothed_mission_points_.size()) {
      lookahead_index_--;
    }
    velocity_desired_ = std::min(maxSpeed, smoothed_mission_points_[lookahead_index_].v);
  }
  else {
    velocity_desired_ = 0;
  }
*/
  // -------
  double stop_s;
  if (stop_distance_ == std::numeric_limits<double>::infinity()) {
    stop_s = std::numeric_limits<double>::infinity();
  }
  else {
    uint32_t stop_idx = missionIndex(stop_point_, last_start_idx_);
    stop_s = smoothed_mission_points_[stop_idx].s - FRONT_BUMPER_DELTA;
  }

  drc::GlobalPose pose = pose_queue_.pose(drc::Time::current());

  double lane_width = minLookaheadLaneWidth(edge_it, current_front_sample_dist)-2.0; // since we want to stay inside lane
//  lane_width*=3.5; //for circledemo

  double follow_s = s0 + follow_distance_ - 5.0;  // TODO: for at 1m safety distance
  double accel_following = 0;
  double current_s = smoothed_mission_points_[current_index_on_mission_].s;
  //  printf("init time: %f\n", drc::Time::current() - dt);
//  printf("velocity_desired: %f\n", velocity_desired_);

  try {
    pthread_mutex_lock(&trajectory_mutex_);
    TrajectoryEvaluator::parameters te_params = traj_eval_->params();
    te_params.lane_keeping.d_offset_min = -0.5*lane_width;
    te_params.lane_keeping.d_offset_max =  0.5*lane_width;
    if(pose.v() < 10) { // TODO: make velocity dependent..more than this parking lot hack...
      te_params.velocity_keeping.v_offset_max     = 1.0;
      te_params.velocity_keeping.v_offset_min     = -4.0;
      te_params.velocity_keeping.v_res            = .5;
    }
    else {
      te_params.velocity_keeping.v_offset_max     = 3.0;
      te_params.velocity_keeping.v_offset_min     = -8.0;
      te_params.velocity_keeping.v_res            = 1;
    }
    te_params.reinit_dist_thresh = replanDistance(velocity_desired_);
    traj_eval_->setParams(te_params);

    traj_eval_->makeTrajectory(center_line_, vehicle_manager->vehicle_map, velocity_desired_,
                               accel_desired, current_s, stop_s, follow_s, velocity_following_, accel_following, pose,
                               robot_yaw_rate_, trajectory_points_);
  }
  catch(vlr::Ex<>& e) {
    printf("Error in trajectory generation: %s\n", e.what().c_str());
    pthread_mutex_unlock(&trajectory_mutex_);
    pthread_mutex_unlock(&center_line_mutex_);

   // TODO: integrate vehicle interface
//    vehiclecmd.beeper_on = 1;
//   vehiclecmd.hazard_lights_on = 1;

//    vehicle_manager->vehicle_map.clear(); // TODO: this will invalidate edge iterators :-(
    emergency_map_clear_ = true;
    pthread_mutex_lock(&static_obstacle_map_mutex_);
    memset(static_obstacle_map_raw_, 0, static_obstacle_map_width_*static_obstacle_map_height_*sizeof(uint8_t));
    memset(static_obstacle_map_, 0, static_obstacle_map_width_*static_obstacle_map_height_*sizeof(uint8_t));
    pthread_mutex_unlock(&static_obstacle_map_mutex_);

    generateStopTrajectory();
    emergency_stop_initiated_=true;
  }
  catch(...) {
    printf("Error in trajectory generation.\n");
  }

  pthread_mutex_unlock(&trajectory_mutex_);
  pthread_mutex_unlock(&center_line_mutex_);
//  printf("generation time: %f\n", drc::Time::current() - dt);
}

void ChsmPlanner::generateCurvePoints(double stop_distance, double follow_distance, double maxSpeed) {
  double front_sample_length = std::max(params().center_line_min_lookahead_dist, traj_eval_->params().checked_horizon * maxSpeed);
//  double front_sample_length = std::max(params.center_line_min_lookahead_dist, traj_eval_->params().checked_horizon * velocity_desired_);

  if (follow_distance < stop_distance && follow_distance < TRIGGER_DIST_FOLLOWING) { // following mode

    follow_distance_ = follow_distance;
    stop_distance_ = std::numeric_limits<double>::infinity();

    Vehicle* veh = topology_->get_next_vehicle();
    if (veh) {
        // TODO: matched/matched_from incosistent but pose is not used right now...
      dgc::dgc_pose_t pose;
      pose.x = veh->xMatched();
      pose.y = veh->yMatched();
      pose.z = 0.0;
      pose.yaw = veh->yawMatchedFrom();
      pose.pitch = 0.0;
      pose.roll = 0.0;
      velocity_following_ = calculateFollowingSpeed(veh->speed(), pose, veh->angleToMatchedEdge());
    }
    else {
      velocity_following_ = 0;
    }
    generateCurvePoints(front_sample_length, maxSpeed);
  }
  else if (stop_distance < TRIGGER_DIST_STOPLINE) { // stop mode
   follow_distance_  = std::numeric_limits<double>::infinity();
    stop_distance_ = stop_distance;
    generateCurvePoints(front_sample_length, maxSpeed);
  }
  else { // drive mode
    follow_distance_  = std::numeric_limits<double>::infinity();
    stop_distance_ = std::numeric_limits<double>::infinity();
    generateCurvePoints(front_sample_length, maxSpeed);
  }
}

void ChsmPlanner::generateCurvePoints(double maxSpeed) {
  double stop_distance,follow_distance;
  double goal_dist = topology_->distToMissionEnd();
  double kturn_dist = topology_->distToNextKTurn();

  if(goal_dist < kturn_dist) {
    stop_distance = goal_dist;
    stop_point_.x = mission_points_[mission_points_.size()-1].x;
    stop_point_.y = mission_points_[mission_points_.size()-1].y;
    if(stop_distance < 0) {maxSpeed = 0;} // TODO: Otherwise car might start again :-(
  }
  else {
    stop_distance = kturn_dist;
//    printf("TODO: set k turn start point.\n");
//    stop_point_.x = mission_points_[mission_points_.size()-1].x;
//    stop_point_.y = mission_points_[mission_points_.size()-1].y;
  }

  double mv_veh_dist, st_veh_dist;
  getVehicleDistances(st_veh_dist, mv_veh_dist);
  follow_distance = min(mv_veh_dist, st_veh_dist);

  generateCurvePoints(stop_distance, follow_distance, maxSpeed);
}

void ChsmPlanner::generateStopTrajectory() {
  velocity_desired_= 0;
  velocity_following_ = 0;
  follow_distance_ = std::numeric_limits<double>::infinity();
  stop_distance_ = std::numeric_limits<double>::infinity();
  generateCurvePoints(params().center_line_min_lookahead_dist, 0.0);
}

void ChsmPlanner::getVehicleDistances(double& standing, double& moving) {
  standing = topology_->distToNextStandingVehicle() - params().safety_stop_infront_vehicle_distance;
  moving = topology_->distToNextMovingVehicle();
}

double ChsmPlanner::calculateCurvatureVelocity(double max_speed, double curvature_slowdown_factor, double sample_length_front) {

  if(current_index_on_mission_>=smoothed_mission_points_.size()) {
    std::cout << "Error: position index not in mission.\n";
    return 0.0;
  }

  double maxAbsCurvature=0;//, meanAbsCurvature=0;
  double s0 = smoothed_mission_points_[current_index_on_mission_].s;

   // early acceleration: ignore curvature right in front of car
  s0 += max_speed * 0.5;
  uint32_t num_points=0, idx=current_index_on_mission_;
  while(smoothed_mission_points_[idx].s-s0 < sample_length_front) {
    idx++; num_points++;
    if(idx==smoothed_mission_points_.size()) {break;}

    maxAbsCurvature = std::max(maxAbsCurvature, std::abs(smoothed_mission_points_[idx].kappa));
//    meanAbsCurvature += std::abs(smoothed_mission_points_[idx].kappa);
  }

//  if(num_points>0) {
//    meanAbsCurvature/=((double)num_points);
//  }

  //  return max_speed/(1.0+curvature_slowdown_factor*sqrt(maxAbsCurvature));
//  printf("s: %f - macu: %f -> max vel: %f, or: %f or %f or %f\n", s0, maxAbsCurvature,
//      dgc::dgc_ms2mph(std::min(max_speed, sqrt(curvature_slowdown_factor/maxAbsCurvature))),
//      dgc::dgc_ms2mph(max_speed/(1.0+curvature_slowdown_factor*sqrt(maxAbsCurvature))),
//      dgc::dgc_ms2mph(max_speed/(1.0+curvature_slowdown_factor*maxAbsCurvature*maxAbsCurvature)),
//      dgc::dgc_ms2mph(std::min(max_speed, curvature_slowdown_factor/pow(maxAbsCurvature,1.5))));

  if(maxAbsCurvature < EPS) {
    return max_speed;
  }
  else {
//      return std::min(max_speed, curvature_slowdown_factor/pow(maxAbsCurvature,1.5));
//    return max_speed/(1.0+curvature_slowdown_factor*maxAbsCurvature*maxAbsCurvature);
//    return max_speed/(1.0+curvature_slowdown_factor*sqrt(maxAbsCurvature));
      return std::min(max_speed, sqrt(curvature_slowdown_factor/maxAbsCurvature));
  }
}

double ChsmPlanner::calculateRoughSurfaceVelocity(double max_speed) {
//  int i;
//  double mean=0, var=0;
//
//  for(i=0; i<ACCEL_HISTORY_SIZE; i++) {mean+=robot_az[i];}
//  mean/=((double)ACCEL_HISTORY_SIZE);
//
//  for(i=0; i<ACCEL_HISTORY_SIZE; i++) {var+=(robot_az[i]-mean)*(robot_az[i]-mean);}
//  var/=((double)ACCEL_HISTORY_SIZE); // no bias and simpler :-)

  double var = 0;
  //cout << " var " << var << endl;

  return max_speed/(1.0+params().surface_quality_slowdown_factor*var);
}

double ChsmPlanner::calculateVelocity(double max_state_speed, double sample_length_front) {
  double min_edge_speed=(*topology_->current_edge_it)->edge()->minSpeed();
  double max_edge_speed=(*topology_->current_edge_it)->edge()->maxSpeed();

  double max_speed = std::max(min_edge_speed, std::min(max_state_speed, std::min(params().max_speed_global, max_edge_speed)));

  double max_curvature_speed = calculateCurvatureVelocity(max_speed, params().curvature_slowdown_factor, sample_length_front);
  double max_roughness_speed = calculateRoughSurfaceVelocity(max_curvature_speed);

  double speed = max_roughness_speed; // std::max(min_edge_speed, std::min(max_state_speed, std::min(max_edge_speed, std::min(max_speed, std::min(max_roughness_speed, max_curvature_speed)))));
//  std::cout << "max speed:\nstate: " << dgc::dgc_ms2mph(max_state_speed) << " edge: " << dgc::dgc_ms2mph(max_edge_speed) << " global: " << dgc::dgc_ms2mph(max_speed) << " curvature: " << dgc::dgc_ms2mph(max_curvature_speed) << " roughness: " << dgc::dgc_ms2mph(max_roughness_speed) << " -> " << dgc::dgc_ms2mph(speed) << endl;
  return speed;
}

int ChsmPlanner::addMessage(const string& message) {
  last_message_timestamp_ = drc::Time::current();
  message_buffer_.push_back(message);
  while(message_buffer_.size()>params().message_buffer_size)
  message_buffer_.pop_front();
  return message_buffer_.size();
}

double ChsmPlanner::distance(const GlobalPose& pose) {
  GlobalPose current_pose = currentPose();
  return hypot(current_pose.utmX() - pose.utmX(), current_pose.utmY() - pose.utmY());
}

const std::string& ChsmPlanner::name() const
{
  return name_;
}

GlobalPose ChsmPlanner::pose(double timestamp) {
  pthread_mutex_lock(&pose_mutex_);
  try {
    GlobalPose pose = pose_queue_.pose(timestamp);
    pthread_mutex_unlock(&pose_mutex_);
    return pose;
  }
  catch(vlr::Ex<>& e) {
    pthread_mutex_unlock(&pose_mutex_);
    throw e;
  }
}

void ChsmPlanner::latestPose(GlobalPose& latest_pose, double& latest_timestamp) {
  pthread_mutex_lock(&pose_mutex_);
  try {
    pose_queue_.latestPose(latest_pose, latest_timestamp);
    pthread_mutex_unlock(&pose_mutex_);
  }
  catch(vlr::Ex<>& e) {
    pthread_mutex_unlock(&pose_mutex_);
    throw e;
  }
}

void ChsmPlanner::currentLocalizeOffsets(double& offset_x, double& offset_y) {
  pthread_mutex_lock(&pose_mutex_);
  pose_queue_.offsets(drc::Time::current(), offset_x, offset_y);
  pthread_mutex_unlock(&pose_mutex_);
}

// if the car we want to follow comes towards us the speed will be set to zero
double ChsmPlanner::calculateFollowingSpeed(const double nextVehicleSpeed, const dgc::dgc_pose_t& /*nextVehiclePose*/, const double angleToEdge)
{

  //double angle_diff = deltaAngle(nextVehiclePose.yaw, robot_pose.yaw);
  bool is_opposite_angle = angleToEdge > M_PI_2/*  && distance(nextVehiclePose) < 30*/;
  return is_opposite_angle?-nextVehicleSpeed:nextVehicleSpeed;
}

void perpBisector(double x1, double y1, double x2, double y2, double& AM, double& BM, double& CM) {
  double A = y2-y1;
  double B = x1-x2;

  double xm1 = 0.5*(x1+x2);
  double ym1 = 0.5*(y1+y2);

  double D = -B*xm1 + A*ym1;

  AM = -B;
  BM = A;
  CM = D;
}

void ChsmPlanner::estimateKappa(CurvePoint& p1, CurvePoint& p2, CurvePoint& p3, double& kappa) {

    // Ax+By=C
  double A1, B1, C1, A2, B2, C2;
  perpBisector(p1.x, p1.y, p2.x, p2.y, A1, B1, C1);
  perpBisector(p2.x, p2.y, p3.x, p3.y, A2, B2, C2);

  double det = A1*B2 - A2*B1;
  if(det == 0) {
     kappa=0;
     return;
  }

  double cx = (B2 * C1 - B1 * C2) / det;
  double cy = (A1 * C2 - A2 * C1) / det;

  kappa = 1.0/hypot(p1.x-cx, p1.y-cy);
}

void ChsmPlanner::dumpMissionLines(std::string& ml_name, std::string& ml_raw_name, std::string& ml_rndf_name) {

  FILE* dbgf = NULL;
  char buf[1000];

  static const std::string err_str_open = "Cannot open file ";
  static const std::string err_str_write = "Cannot write to file ";

  if(!(dbgf = fopen(ml_name.c_str(), "w"))) {
    throw VLRException(err_str_open + ml_name);
  }

  for (std::vector<CurvePoint>::const_iterator it = smoothed_mission_points_.begin(); it
      != smoothed_mission_points_.end(); it++) {
    sprintf(buf, "%lf %lf %lf %lf %lf %lf\n", (*it).s, (*it).x, (*it).y, (*it).theta, (*it).kappa, (*it).kappa_prime);
    size_t len = fwrite(buf, 1, strlen(buf), dbgf);
    if(len != strlen(buf)) {
      throw VLRException(err_str_write + ml_name);
    }
  }
  fclose(dbgf);
  dbgf=NULL;

  if(!(dbgf = fopen(ml_rndf_name.c_str(), "w"))) {
    throw VLRException(err_str_open + ml_rndf_name);
  }

  for (std::vector<CurvePoint>::const_iterator it = mission_points_.begin(); it != mission_points_.end(); it++) {
    sprintf(buf, "%lf %lf %lf %lf %lf %lf\n", (*it).s, (*it).x, (*it).y, (*it).theta, (*it).kappa, (*it).kappa_prime);
    size_t len = fwrite(buf, 1, strlen(buf), dbgf);
    if(len != strlen(buf)) {
      throw VLRException(err_str_write + ml_rndf_name);
    }
  }
  fclose(dbgf);
  dbgf=NULL;

  if(!(dbgf = fopen(ml_raw_name.c_str(), "w"))) {
    throw VLRException(err_str_open + ml_raw_name);
  }

  for (std::vector<CurvePoint>::const_iterator it = sampled_mission_points_.begin(); it
      != sampled_mission_points_.end(); it++) {
    sprintf(buf, "%lf %lf %lf %lf %lf %lf\n", (*it).s, (*it).x, (*it).y, (*it).theta, (*it).kappa, (*it).kappa_prime);
    size_t len = fwrite(buf, 1, strlen(buf), dbgf);
    if(len != strlen(buf)) {
      throw VLRException(err_str_write + ml_raw_name);
    }
  }
  fclose(dbgf);
}

double ChsmPlanner::replanDistance(double velocity) {
std::map<double, double>::const_iterator rpit = replan_distance_map_.lower_bound(velocity);
if(rpit != replan_distance_map_.end()) {
  return rpit->second;
}

return (*(--replan_distance_map_.end())).second;
}

const uint32_t ChsmPlanner::pose_queue_size_ = 2000; // buffer 10s @ current applanix rate (or equivalent of localizer rate)

} // namespace vlr


#include <ndt_calibration/ndt_calib.h>
#include <ndt_calibration/ndt_calib_rviz.h>

#include <ndt_map/ndt_map.h>
#include <ndt_map/ndt_cell.h>
#include <ndt_map/pointcloud_utils.h>

#include <tf_conversions/tf_eigen.h>
#include <eigen_conversions/eigen_msg.h>
#include <cstdio>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <fstream>
#include <stdio.h>
#include <stdlib.h>
#include <dirent.h>
#include <algorithm>

#include <ros/ros.h>

#include <sensor_msgs/PointCloud2.h>

#include <boost/program_options.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>

namespace po = boost::program_options;
using namespace std;

int main(int argc, char **argv){
    std::string gt_file;
    std::string est_sensorpose_file;
    std::string base_name_pcd;
    std::string bag_file;
    std::string pose_frame;
    std::string world_frame;
    Eigen::Vector3d transl;
    Eigen::Vector3d euler;
    int score_type, objective_type;
    double max_translation;
    double min_rotation;
    double sensor_time_offset;
    double resolution;
    int index_offset;
    po::options_description desc("Allowed options");
    desc.add_options()
	("help", "produce help message")
	("visualize", "visualize the output")
	("gt_file", po::value<std::string>(&gt_file), "vehicle pose files in world frame")
	("est_sensorpose_file", po::value<std::string>(&est_sensorpose_file)->default_value(std::string("")), "estimated sensor poses in world frame")
	("bag_file", po::value<std::string>(&bag_file), "bag file containing the poses to use for performing sensor time offset calibration")
	("base_name_pcd", po::value<string>(&base_name_pcd)->default_value(std::string("")), "prefix for the .pcd files")
      ("pose_frame", po::value<string>(&pose_frame)->default_value("/state_base_link"), "frame containing the vehicle pose") // EKF
        ("world_frame", po::value<string>(&world_frame)->default_value("/world"), "default world frame used")  // World
	("x", po::value<double>(&transl[0])->default_value(0.), "translation vector x")
	("y", po::value<double>(&transl[1])->default_value(0.), "translation vector y")
	("z", po::value<double>(&transl[2])->default_value(0.), "translation vector z")
	("ex", po::value<double>(&euler[0])->default_value(0.), "euler angle vector x")
	("ey", po::value<double>(&euler[1])->default_value(0.), "euler angle vector y")
	("ez", po::value<double>(&euler[2])->default_value(0.), "euler angle vector z")
	("no_calib", "no calibration is performed")
	("score_type", po::value<int>(&score_type)->default_value(3), "score type used in the optimization, 0 - ICP, 1 - sensorpose difference, 2 - relative sensorpose difference, 3 - NDT")
	("objective_type", po::value<int>(&objective_type)->default_value(0), "objective type used in the optimization, 0 - full 6D, 1 - only time offset")

	("max_translation", po::value<double>(&max_translation)->default_value(3.), "max allowed translation between two scan pair")
	("min_rotation", po::value<double>(&min_rotation)->default_value(0.2), "min required rotation between two scan pair")
	("sensor_time_offset", po::value<double>(&sensor_time_offset)->default_value(0.), "initial time offset for the sensor")
        ("cx", "calibrate x axis")
        ("cy", "calibrate y axis")
        ("cz", "calibrate z axis")
        ("cex", "calibrate roll")
        ("cey", "calibrate pitch")
        ("cez", "calibrate yaw")
        ("ct", "calibrate time offset")
	("resolution", po::value<double>(&resolution)->default_value(2.), "NDT map resolution")
      ("index_offset", po::value<int>(&index_offset)->default_value(0), "if there is an index offset bettwen the cloudXXX.pcd and the rows in the pose files")
    ("no_visualize", "if the output should not be visualized (visualization markers to RViz)")
        ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (!vm.count("gt_file"))
    {
	cout << "Missing gt_file.\n";
	cout << desc << "\n";
	return 1;
    }
    if (vm.count("help"))
    {
	cout << desc << "\n";
	return 1;
    }
    bool visualize = !vm.count("no_visualize");
    bool calib = !vm.count("no_calib");
    bool cx = vm.count("cx");
    bool cy = vm.count("cy");
    bool cz = vm.count("cz");
    bool cex = vm.count("cex");
    bool cey = vm.count("cey");
    bool cez = vm.count("cez");
    bool ct = vm.count("ct");
    Eigen::Affine3d Ts;
    //Ts.setIdentity();
    Ts = getAsAffine3d(transl, euler);

    NDTCalibScanPairs pairs;
    loadNDTCalibScanPairs(gt_file, est_sensorpose_file, base_name_pcd, pairs, max_translation, min_rotation, index_offset);
    cout << "Ts : " << affine3dToString(Ts) << std::endl;

    if (pairs.empty()) {
      std::cout << "failed in  loading / no scan pairs detected..." << std::endl;
    }
    std::cout << "got : " << pairs.size() << " scan pairs" << std::endl;

    std::cout << "Computing NDTMap... " << std::endl;
    pairs.computeNDTMap(resolution);
    std::cout << "done." << std::endl;

    std::cout << "Setting up pose interpolation" << std::endl;
    PoseInterpolationNavMsgsOdo pose_interp(bag_file, std::string("/tf"), world_frame, ros::Duration(3600));
    pose_interp.readBagFile();
    std::cout << "done." << std::endl;

    NDTCalibOptimize::ObjectiveType objective(cx, cy, cz, cex, cey, cez, ct);

    NDTCalibOptimize opt(pairs, score_type, objective, pose_interp, pose_frame);
    std::cout << "Optimize time : " << objective.optimizeTime() << std::endl;

    opt.interpPairPoses(sensor_time_offset);

    std::cout << " initial sensor pose est : " << affine3dToString(Ts) << " dt : " << sensor_time_offset << " error : " << opt.getScore6d(Ts) << std::endl;
    if (calib) {
      std::cout << "starting calibration, using score type : " << score_type << std::endl;
      opt.calibrate(Ts, sensor_time_offset);
      opt.interpPairPoses(sensor_time_offset);
      std::cout << "done. " << std::endl;
      std::cout << " new sensor pose est : " << affine3dToString(Ts) << " dt : " << sensor_time_offset << " error : " << opt.getScore6d(Ts) << std::endl;
    }

    // ROS based visualization below...
    if (!visualize) {
      exit(0);
    }

    std::cout << "Starting visualization..." << std::endl;

    ros::init(argc, argv, "ndt_calib");
    srand(time(NULL));

    ros::NodeHandle nh;

    visualization_msgs::MarkerArray m_array = ndt_visualisation::getMarkerArrayFromNDTCalibScanPairs(pairs);
    visualization_msgs::MarkerArray m_array2 = ndt_visualisation::getMarkerArrayRelFromNDTCalibScanPair(pairs[0], Ts);

    ros::Publisher marker_array_pub = nh.advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 10);

    // Get a global point cloud for all poses with calibration and data in sensor coords.
    pcl::PointCloud<pcl::PointXYZ> global_cloud;
    pairs.getGlobalPointCloud(Ts, global_cloud);
    global_cloud.header.frame_id = "/world";

    ros::Publisher pointcloud_pub = nh.advertise<pcl::PointCloud<pcl::PointXYZ> > ("global_points", 1);
    ros::Rate r(1);

    int i = 0;
    while (ros::ok()) {

      pointcloud_pub.publish(global_cloud);

      // Publish the marker...
      marker_array_pub.publish(m_array);
      marker_array_pub.publish(m_array2);

      ros::spinOnce();
      r.sleep();
      m_array2 = ndt_visualisation::getMarkerArrayRelFromNDTCalibScanPair(pairs[i++], Ts);
      i = i % pairs.size();
    }

}


//
//  electron_optimisation.cpp
//  indigo-bondorder
//
//  Created by Welsh, Ivan on 12/09/17.
//  Copyright © 2017 Allison Group. All rights reserved.
//

#include <algorithm>
#include <climits>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>

#include <boost/algorithm/string.hpp>

#include "algorithm/electron_optimisation.hpp"
#include "classes/atom.hpp"
#include "classes/electron_graph.hpp"
#include "classes/molecular_graph.hpp"
#include "algorithm/formalbonds/astar.hpp"
#include "algorithm/formalbonds/fpt.hpp"
#include "algorithm/formalbonds/local_optimisation.hpp"
#include "classes/periodictable.hpp"
#include "utils/filereader.hpp"
#include "utils/options.hpp"

namespace indigo_bondorder {
  enum class SortOrder {
    F_ONE,
    CL_ONE,
    BR_ONE,
    O_ONE,
    S_ONE,
    O_TWO,
    S_TWO,
    C_TWO_N_ONE,
    C_TWO_C_TWO,
    C_THREE_C_THREE,
    N_ONE,
    P_ONE,
    N_TWO,
    P_TWO,
    N_THREE,
    P_THREE,
    C_THREE_O_ONE,
    O_ONE_S_FOUR,
    C_THREE_S_ONE,
    O_ONE_P_FOUR,
    C_THREE_N_TWO,
    N_TWO_N_TWO,
    N_THREE_O_ONE,
    UNDEFINED
  };
}

using namespace indigo_bondorder;
typedef Options::AssignElectrons opt_;

ElectronOpt::ElectronOpt()
: electronsToAdd_(0), molGraph_(std::make_shared<MolecularGraph>()),
elnGraph_(std::make_shared<ElectronGraph>()) { }

ElectronOpt::ElectronOpt(std::shared_ptr<MolecularGraph> G)
: electronsToAdd_(0), molGraph_(G),
elnGraph_(std::make_shared<ElectronGraph>()) { }

void ElectronOpt::SetMolecularGraph(std::shared_ptr<MolecularGraph> G) {
  molGraph_ = G;
  elnGraph_.reset(new ElectronGraph(*G));
}

size_t ElectronOpt::Run() {
  possibleLocations_.clear();
  if (scores_.empty()) LoadScores();

  switch (opt_::ALGORITHM) {
    case opt_::Algorithm::LOCAL_OPTIMISATION:
      algo_.reset(new algorithm::LocalOptimisation(this));
      break;
    case opt_::Algorithm::ASTAR:
      algo_.reset(new algorithm::AStarOptimisation(this));
      break;
    case opt_::Algorithm::FPT:
      algo_.reset(new algorithm::FPTOptimisation(this));
      break;

    default:
      throw std::runtime_error("Unsupported optimisation algorithm");
      break;
  }
  SetMolecularGraph(molGraph_);
  DetermineElectronsToAdd();
  DeterminePotentialElectronLocations();
  if (opt_::ALGORITHM == opt_::Algorithm::ASTAR) SortPotentialLocations();
  algo_->PopulateMVP2EV();
  algo_->Run();
  size_t res = algo_->GetResultCount();
  finalScore_ = algo_->GetResultEnergy();
  return res;
}

bool ElectronOpt::ApplyElectronAssigment(Uint i) {
  return algo_->ApplyElectronAssignment(i);
}

void ElectronOpt::DetermineElectronsToAdd() {
  electronsToAdd_ = -molGraph_->GetTotalCharge();
  MolVertIterPair vs = molGraph_->GetVertices();
  for (MolVertexIter v = vs.first; v != vs.second; ++v)
    electronsToAdd_ += molGraph_->GetProperties(*v)->atom->GetElement()->GetValenceElectronCount();
  // All bonds should have order of at least 1
  // Preplace code
  ElnVertIterPair elnvs = elnGraph_->GetVertices();
  for (; elnvs.first != elnvs.second; ++elnvs.first)
    electronsToAdd_ -= elnGraph_->GetProperties(*elnvs.first)->pre_placed;
  //electronsToAdd_ -= 2 * molGraph_->NumEdges();
  // End Preplace code

  if (opt_::AUTO_USE_ELECTRON_PAIRS && electronsToAdd_ % 2)
    opt_::USE_ELECTRON_PAIRS = false;
  else if (opt_::AUTO_USE_ELECTRON_PAIRS)
    opt_::USE_ELECTRON_PAIRS = true;

  if (opt_::USE_ELECTRON_PAIRS && electronsToAdd_ % 2)
    throw std::runtime_error("Unable to handle odd number of electrons when using electron pairs.");
  else if (opt_::USE_ELECTRON_PAIRS)
    electronsToAdd_ /= 2;
}

void ElectronOpt::DeterminePotentialElectronLocations() {
  MolVertIterPair vs = molGraph_->GetVertices();
  for (MolVertexIter v = vs.first; v != vs.second; ++v) {
    int8_t octet;
    if (molGraph_->Degree(*v) > 2)
      octet = molGraph_->GetProperties(*v)->atom->GetElement()->GetHypervalentOctet();
    else
      octet = molGraph_->GetProperties(*v)->atom->GetElement()->GetOctet();

    int8_t bondedElectrons = 2 * molGraph_->Degree(*v);
    int8_t missingElectrons = octet - bondedElectrons;

    MolVertPair id = std::make_pair(*v, *v);
    // Preplace code
    ElnVertex ev = elnGraph_->GetVertex(id);
    missingElectrons -= elnGraph_->GetProperties(ev)->pre_placed;
    // End Preplace code

    while (missingElectrons > 0) {
      possibleLocations_.push_back(id);
      if (opt_::USE_ELECTRON_PAIRS)
        missingElectrons -= 2;
      else
        missingElectrons -= 1;
    }
  }

  MolEdgeIterPair es = molGraph_->GetEdges();
  for (MolEdgeIter e = es.first; e != es.second; ++e) {
    MolVertex u = molGraph_->GetSource(*e);
    MolVertex v = molGraph_->GetTarget(*e);
    if (u > v) {
      MolVertex tmp = u;
      u = v;
      v = tmp;
    }
    int8_t u_oct, v_oct, u_bond, v_bond, u_miss, v_miss;
    if (molGraph_->Degree(u) > 2)
      u_oct = molGraph_->GetProperties(u)->atom->GetElement()->GetHypervalentOctet();
    else
      u_oct = molGraph_->GetProperties(u)->atom->GetElement()->GetOctet();
    if (molGraph_->Degree(v) > 2)
      v_oct = molGraph_->GetProperties(v)->atom->GetElement()->GetHypervalentOctet();
    else
      v_oct = molGraph_->GetProperties(v)->atom->GetElement()->GetOctet();
    u_bond = 2 * molGraph_->Degree(u);
    v_bond = 2 * molGraph_->Degree(v);
    u_miss = u_oct - u_bond;
    v_miss = v_oct - v_bond;
    uint8_t order = 1;
    MolVertPair id = std::make_pair(u, v);
    // TODO: Check energy tables for available bond orders
    while (u_miss > 0 && v_miss > 0 && order <= opt_::MAXIMUM_BOND_ORDER) {
      possibleLocations_.push_back(id);
      if (!opt_::USE_ELECTRON_PAIRS)
        possibleLocations_.push_back(id);
      u_miss -= 2;
      v_miss -= 2;
      order++;
    }
  }
}

void ElectronOpt::LoadScores() {
  PeriodicTable_p pt = PeriodicTable::GetInstance();
  if (Options::DATA_DIRECTORY.back() != '/') {
    Options::DATA_DIRECTORY.append("/");
  }
  String atmFile = Options::DATA_DIRECTORY + opt_::ATOM_ENERGY_FILE;
  utils::FileReader at(atmFile);
  String bndFile = Options::DATA_DIRECTORY + opt_::BOND_ENERGY_FILE;
  utils::FileReader bn(bndFile);
  std::vector<std::string> at_items, bn_items;
  at.GetAllItems(at_items);
  bn.GetAllItems(bn_items);

  int64_t global_min = LLONG_MAX;
  std::map<String, int64_t> la_mins;
  std::map<std::pair<String, String>, int64_t> lb_mins;

  // Convert atom energies to ints
#define NUM_DECIMAL_PLACE 5
  for (size_t i = 2; i < at_items.size(); i += 3) {
    std::string lead, tail;
    size_t sep_pos = at_items[i].find_first_of('.');
    if (sep_pos == std::string::npos)
      throw std::invalid_argument(at_items[i] + std::string(" is not a decimal value."));
    lead = at_items[i].substr(0, sep_pos);
    tail = at_items[i].substr(sep_pos + 1, NUM_DECIMAL_PLACE);
    while (tail.size() < NUM_DECIMAL_PLACE) tail.append("0");
    int64_t score;
    try {
      score = std::stoll(lead + tail);
    } catch (const std::invalid_argument& e) {
      throw std::invalid_argument(at_items[i] + std::string(" is not a decimal value."));
    }
    if (la_mins.find(at_items[i-2]) == la_mins.end()) {
      la_mins.emplace(at_items[i-2], score);
    } else if (la_mins.at(at_items[i-2]) > score) {
      la_mins.at(at_items[i-2]) = score;
    }
    if (score < global_min) global_min = score;
    at_items[i] = lead + tail;
  }

  // Convert bond energies to ints
  for (size_t i = 3; i < bn_items.size(); i += 4) {
    std::string lead, tail;
    size_t sep_pos = bn_items[i].find_first_of('.');
    if (sep_pos == std::string::npos)
      throw std::invalid_argument(bn_items[i] + std::string(" is not a decimal value."));
    lead = bn_items[i].substr(0, sep_pos);
    tail = bn_items[i].substr(sep_pos + 1, NUM_DECIMAL_PLACE);
    while (tail.size() < NUM_DECIMAL_PLACE) tail.append("0");
    int64_t score;
    try {
      score = std::stoll(lead + tail);
    } catch (const std::invalid_argument& e) {
      throw std::invalid_argument(bn_items[i] + std::string(" is not a decimal value."));
    }
    if (score < global_min) global_min = score;

    String atomA = bn_items[i-3];
    String atomB = bn_items[i-2];
    size_t pos_sign_a, pos_sign_b;
    pos_sign_a = atomA.find_first_of('+');
    pos_sign_b = atomB.find_first_of('+');

    if (pos_sign_a != std::string::npos) {
      atomA = atomA.substr(0, pos_sign_a);
    } else {
      pos_sign_a = atomA.find_first_of('-');
      if (pos_sign_a != std::string::npos) atomA = atomA.substr(0, pos_sign_a);
    }

    if (pos_sign_b != std::string::npos) {
      atomB = atomB.substr(0, pos_sign_b);
    } else {
      pos_sign_b = atomB.find_first_of('-');
      if (pos_sign_b != std::string::npos) atomB = atomB.substr(0, pos_sign_b);
    }

    std::pair<String, String> b1 = std::make_pair(atomA, atomB);
    std::pair<String, String> b2 = std::make_pair(atomB, atomA);
    if (lb_mins.find(b1) == lb_mins.end()) {
      lb_mins.emplace(b1, score);
    } else if (lb_mins.at(b1) > score) {
      lb_mins.at(b1) = score;
    }
    if (lb_mins.find(b2) == lb_mins.end()) {
      lb_mins.emplace(b2, score);
    } else if (lb_mins.at(b2) > score) {
      lb_mins.at(b2) = score;
    }
    bn_items[i] = lead + tail;
  }

  // Load atom energies
  for (size_t i = 0; i < at_items.size(); i += 3) {
    uint8_t Z = pt->GetElement(at_items[i])->GetAtomicNumber();
    if (Z == 0) throw std::invalid_argument(at_items[i] + std::string(" is not a valid atomic symbol."));
    int fc;
    try {
      fc = std::stoi(at_items[i + 1]);
    } catch (const std::invalid_argument& e) {
      throw std::invalid_argument(at_items[i+1] + std::string(" is not an integer value."));
    }
    uint32_t k = Z + (std::abs(fc) << 8);
    if (fc < 0) k += (1 << 15);
    Score val = (Score)(std::stoll(at_items[i + 2]) - la_mins.at(at_items[i]));
    scores_.emplace(k, val);
  }

  // Load bond energies
  for (size_t i = 0; i < bn_items.size(); i += 4) {
    char a_sign, b_sign;
    size_t pos_sign_a, pos_sign_b;
    pos_sign_a = bn_items[i].find_first_of('+');
    pos_sign_b = bn_items[i + 1].find_first_of('+');

    if (pos_sign_a != std::string::npos) {
      a_sign = '+';
      bn_items[i] = bn_items[i].substr(0, pos_sign_a);
    } else {
      pos_sign_a = bn_items[i].find_first_of('-');
      if (pos_sign_a != std::string::npos) {
        a_sign = '-';
        bn_items[i] = bn_items[i].substr(0, pos_sign_a);
      } else a_sign = '0';
    }

    if (pos_sign_b != std::string::npos) {
      b_sign = '+';
      bn_items[i+1] = bn_items[i+1].substr(0, pos_sign_b);
    } else {
      pos_sign_b = bn_items[i+1].find_first_of('-');
      if (pos_sign_b != std::string::npos) {
        b_sign = '-';
        bn_items[i+1] = bn_items[i+1].substr(0, pos_sign_b);
      } else b_sign = '0';
    }

    uint8_t Za = pt->GetElement(bn_items[i])->GetAtomicNumber();
    uint8_t Zb = pt->GetElement(bn_items[i+1])->GetAtomicNumber();
    if (Za == 0) throw std::invalid_argument(at_items[i] + std::string(" is not a valid atomic symbol."));
    if (Zb == 0) throw std::invalid_argument(at_items[i+1] + std::string(" is not a valid atomic symbol."));

    int order;
    try {
      order = std::stoi(bn_items[i + 2]);
    } catch (const std::invalid_argument& e) {
      throw std::invalid_argument(bn_items[i+2] + std::string(" is not an integer value."));
    }
    if (order < 1) throw std::invalid_argument(bn_items[i+2] + std::string(" is an invalid bond order."));

    uint32_t k1 = Za + (Zb << 8) + ((order * 2) << 20);
    uint32_t k2 = Zb + (Za << 8) + ((order * 2) << 20);

    if (a_sign == '+') {
      k1 += (1 << 16);
      k2 += (1 << 18);
    } else if (a_sign == '-') {
      k1 += (2 << 16);
      k2 += (2 << 18);
    }

    if (b_sign == '+') {
      k2 += (1 << 16);
      k1 += (1 << 18);
    } else if (b_sign == '-') {
      k2 += (2 << 16);
      k1 += (2 << 18);
    }
    std::pair<String, String> bondType = std::make_pair(bn_items[i], bn_items[i+1]);
    Score val = (Score)(std::stoll(bn_items[i + 3]) - lb_mins.at(bondType));
//    Score val = (Score)(std::stoll(bn_items[i + 3]) - global_min);
    scores_.emplace(k1, val);
    if (k1 != k2) scores_.emplace(k2, val);
  }
}

void ElectronOpt::SortPotentialLocations() {
  std::vector<ElnVertProp*> sortedUniques;
  sortedUniques.reserve(possibleLocations_.size());
  for (MolVertPair& vp : possibleLocations_) {
    ElnVertProp* p = elnGraph_->GetProperties(elnGraph_->GetVertex(vp));
    if (vp.first == vp.second) {
      MolVertProp* prop = molGraph_->GetProperties(vp.first);
      switch (prop->atom->GetElement()->GetAtomicNumber()) {
        case 7:  // Nitrogen
          switch (molGraph_->Degree(vp.first)) {
            case 1: p->sort_score = SortOrder::N_ONE; break;
            case 2: p->sort_score = SortOrder::N_TWO; break;
            case 3: p->sort_score = SortOrder::N_THREE; break;
            default: p->sort_score = SortOrder::UNDEFINED; break;
          }
          break;
        case 8:  // Oxygen
          switch (molGraph_->Degree(vp.first)) {
            case 1: p->sort_score = SortOrder::O_ONE; break;
            case 2: p->sort_score = SortOrder::O_TWO; break;
            default: p->sort_score = SortOrder::UNDEFINED; break;
          }
          break;
        case 9:  // Fluorine
          switch (molGraph_->Degree(vp.first)) {
            case 1: p->sort_score = SortOrder::F_ONE; break;
            default: p->sort_score = SortOrder::UNDEFINED; break;
          }
          break;
        case 15:  // Phosphorus
          switch (molGraph_->Degree(vp.first)) {
            case 1: p->sort_score = SortOrder::P_ONE; break;
            case 2: p->sort_score = SortOrder::P_TWO; break;
            case 3: p->sort_score = SortOrder::P_THREE; break;
            default: p->sort_score = SortOrder::UNDEFINED; break;
          }
          break;
        case 16:  // Sulfur
          switch (molGraph_->Degree(vp.first)) {
            case 1: p->sort_score = SortOrder::S_ONE; break;
            case 2: p->sort_score = SortOrder::S_ONE; break;
            default: p->sort_score = SortOrder::UNDEFINED; break;
          }
          break;
        case 17:  // Chlorine
          switch (molGraph_->Degree(vp.first)) {
            case 1: p->sort_score = SortOrder::CL_ONE; break;
            default: p->sort_score = SortOrder::UNDEFINED; break;
          }
          break;
        case 35:
          switch (molGraph_->Degree(vp.first)) {
            case 1: p->sort_score = SortOrder::BR_ONE; break;
            default: p->sort_score = SortOrder::UNDEFINED; break;
          }
          break;
        default: p->sort_score = SortOrder::UNDEFINED; break;
      }
    } else {
      MolVertProp* propa = molGraph_->GetProperties(vp.first);
      MolVertProp* propb = molGraph_->GetProperties(vp.second);
      switch (propa->atom->GetElement()->GetAtomicNumber()) {
        case 6:
          switch (propb->atom->GetElement()->GetAtomicNumber()) {
            case 6:
              if (molGraph_->Degree(vp.first) == 2
                  && molGraph_->Degree(vp.second) == 2)
                p->sort_score = SortOrder::C_TWO_C_TWO;
              else if (molGraph_->Degree(vp.first) == 3
                    && molGraph_->Degree(vp.second) == 3)
                  p->sort_score = SortOrder::C_THREE_C_THREE;
              else p->sort_score = SortOrder::UNDEFINED;
              break;
            case 7:
              if (molGraph_->Degree(vp.first) == 2
                  && molGraph_->Degree(vp.second) == 1)
                p->sort_score = SortOrder::C_TWO_N_ONE;
              else if (molGraph_->Degree(vp.first) == 3
                       && molGraph_->Degree(vp.second) == 2)
                p->sort_score = SortOrder::C_THREE_N_TWO;
              else p->sort_score = SortOrder::UNDEFINED;
              break;
            case 8:
              if (molGraph_->Degree(vp.first) == 3
                  && molGraph_->Degree(vp.second) == 1)
                p->sort_score = SortOrder::C_THREE_O_ONE;
              else p->sort_score = SortOrder::UNDEFINED;
              break;
            case 16:
              if (molGraph_->Degree(vp.first) == 3
                  && molGraph_->Degree(vp.second) == 1)
                p->sort_score = SortOrder::C_THREE_S_ONE;
              else p->sort_score = SortOrder::UNDEFINED;
              break;
            default: p->sort_score = SortOrder::UNDEFINED; break;
          }
          break;
        case 7:
          switch (propb->atom->GetElement()->GetAtomicNumber()) {
            case 6:
              if (molGraph_->Degree(vp.first) == 1
                  && molGraph_->Degree(vp.second) == 2)
                p->sort_score = SortOrder::C_TWO_N_ONE;
              else if (molGraph_->Degree(vp.first) == 2
                       && molGraph_->Degree(vp.second) == 3)
                p->sort_score = SortOrder::C_THREE_N_TWO;
              else p->sort_score = SortOrder::UNDEFINED;
              break;
            case 7:
              if (molGraph_->Degree(vp.first) == 2
                  && molGraph_->Degree(vp.second) == 2)
                p->sort_score = SortOrder::N_TWO_N_TWO;
              else p->sort_score = SortOrder::UNDEFINED;
              break;
            case 8:
              if (molGraph_->Degree(vp.first) == 3
                  && molGraph_->Degree(vp.second) == 1)
                p->sort_score = SortOrder::N_THREE_O_ONE;
              else p->sort_score = SortOrder::UNDEFINED;
              break;
            default: p->sort_score = SortOrder::UNDEFINED; break;
          }
          break;
        case 8:
          switch (propb->atom->GetElement()->GetAtomicNumber()) {
            case 6:
              if (molGraph_->Degree(vp.first) == 1
                  && molGraph_->Degree(vp.second) == 3)
                p->sort_score = SortOrder::C_THREE_O_ONE;
              else p->sort_score = SortOrder::UNDEFINED;
              break;
            case 7:
              if (molGraph_->Degree(vp.first) == 1
                  && molGraph_->Degree(vp.second) == 3)
                p->sort_score = SortOrder::N_THREE_O_ONE;
              else p->sort_score = SortOrder::UNDEFINED;
              break;
            case 15:
              if (molGraph_->Degree(vp.first) == 1
                  && molGraph_->Degree(vp.second) == 4)
                p->sort_score = SortOrder::O_ONE_P_FOUR;
              else p->sort_score = SortOrder::UNDEFINED;
              break;
            case 16:
              if (molGraph_->Degree(vp.first) == 1
                  && molGraph_->Degree(vp.second) == 4)
                p->sort_score = SortOrder::O_ONE_S_FOUR;
              else p->sort_score = SortOrder::UNDEFINED;
              break;
            default: p->sort_score = SortOrder::UNDEFINED; break;
          }
          break;
        case 15:
          switch (propb->atom->GetElement()->GetAtomicNumber()) {
            case 8:
              if (molGraph_->Degree(vp.first) == 4
                  && molGraph_->Degree(vp.second) == 1)
                p->sort_score = SortOrder::O_ONE_P_FOUR;
              else p->sort_score = SortOrder::UNDEFINED;
              break;
            default: p->sort_score = SortOrder::UNDEFINED; break;
          }
          break;
        case 16:
          switch (propb->atom->GetElement()->GetAtomicNumber()) {
            case 6:
              if (molGraph_->Degree(vp.first) == 1
                  && molGraph_->Degree(vp.second) == 3)
                p->sort_score = SortOrder::C_THREE_S_ONE;
              else p->sort_score = SortOrder::UNDEFINED;
              break;
            case 8:
              if (molGraph_->Degree(vp.first) == 4
                  && molGraph_->Degree(vp.second) == 1)
                p->sort_score = SortOrder::O_ONE_S_FOUR;
              else p->sort_score = SortOrder::UNDEFINED;
              break;
            default: p->sort_score = SortOrder::UNDEFINED; break;
          }
          break;
        default: p->sort_score = SortOrder::UNDEFINED; break;
      }
    }

    sortedUniques.push_back(p);
  }
  std::stable_sort(sortedUniques.begin(), sortedUniques.end(),
                   [](const ElnVertProp* lhs, const ElnVertProp* rhs) {
                     if (opt_::ALGORITHM == opt_::Algorithm::ASTAR)
                       return lhs->sort_score < rhs->sort_score;
                     else return lhs->sort_score > rhs->sort_score;
                   });

  for (unsigned int i = 0; i < sortedUniques.size(); ++i)
    possibleLocations_[i] = sortedUniques[i]->id;
}