//
//  Copyright (c) 2009-2011 Artyom Beilis (Tonkikh)
//
//  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)
//
#define BOOST_LOCALE_SOURCE
#include "all_generator.hpp"
#include "cdata.hpp"
#include "time_zone.hpp"
#include "uconv.hpp"

#include <math.h>
#include <unicode/calendar.h>
#include <unicode/gregocal.h>
#include <unicode/utypes.h>

#include <boost/locale/date_time.hpp>
#include <boost/locale/date_time_facet.hpp>
#include <boost/locale/formatting.hpp>
#include <boost/locale/hold_ptr.hpp>
#include <boost/thread.hpp>
#include <iostream>
#include <memory>

namespace boost
{
namespace locale
{
namespace impl_icu
{

static void check_and_throw_dt(UErrorCode& e)
{
    if (U_FAILURE(e))
    {
        throw date_time_error(u_errorName(e));
    }
}
using period::marks::period_mark;

static UCalendarDateFields to_icu(period::marks::period_mark f)
{
    using namespace period::marks;

    switch (f)
    {
        case era:
            return UCAL_ERA;
        case year:
            return UCAL_YEAR;
        case extended_year:
            return UCAL_EXTENDED_YEAR;
        case month:
            return UCAL_MONTH;
        case day:
            return UCAL_DATE;
        case day_of_year:
            return UCAL_DAY_OF_YEAR;
        case day_of_week:
            return UCAL_DAY_OF_WEEK;
        case day_of_week_in_month:
            return UCAL_DAY_OF_WEEK_IN_MONTH;
        case day_of_week_local:
            return UCAL_DOW_LOCAL;
        case hour:
            return UCAL_HOUR_OF_DAY;
        case hour_12:
            return UCAL_HOUR;
        case am_pm:
            return UCAL_AM_PM;
        case minute:
            return UCAL_MINUTE;
        case second:
            return UCAL_SECOND;
        case week_of_year:
            return UCAL_WEEK_OF_YEAR;
        case week_of_month:
            return UCAL_WEEK_OF_MONTH;
        default:
            throw std::invalid_argument("Invalid date_time period type");
    }
}

class calendar_impl : public abstract_calendar
{
  public:
    calendar_impl(cdata const& dat)
    {
        UErrorCode err = U_ZERO_ERROR;
        calendar_.reset(icu::Calendar::createInstance(dat.locale, err));
        check_and_throw_dt(err);
#if U_ICU_VERSION_MAJOR_NUM * 100 + U_ICU_VERSION_MINOR_NUM < 402
        // workaround old/invalid data, it should be 4 in general
        calendar_->setMinimalDaysInFirstWeek(4);
#endif
        encoding_ = dat.encoding;
    }
    calendar_impl(calendar_impl const& other)
    {
        calendar_.reset(other.calendar_->clone());
        encoding_ = other.encoding_;
    }

    calendar_impl* clone() const
    {
        return new calendar_impl(*this);
    }

    void set_value(period::marks::period_mark p, int value)
    {
        calendar_->set(to_icu(p), int32_t(value));
    }

    int get_value(period::marks::period_mark p, value_type type) const
    {
        UErrorCode err = U_ZERO_ERROR;
        int v = 0;
        if (p == period::marks::first_day_of_week)
        {
            guard l(lock_);
            v = calendar_->getFirstDayOfWeek(err);
        }
        else
        {
            UCalendarDateFields uper = to_icu(p);
            guard l(lock_);
            switch (type)
            {
                case absolute_minimum:
                    v = calendar_->getMinimum(uper);
                    break;
                case actual_minimum:
                    v = calendar_->getActualMinimum(uper, err);
                    break;
                case greatest_minimum:
                    v = calendar_->getGreatestMinimum(uper);
                    break;
                case current:
                    v = calendar_->get(uper, err);
                    break;
                case least_maximum:
                    v = calendar_->getLeastMaximum(uper);
                    break;
                case actual_maximum:
                    v = calendar_->getActualMaximum(uper, err);
                    break;
                case absolute_maximum:
                    v = calendar_->getMaximum(uper);
                    break;
            }
        }
        check_and_throw_dt(err);
        return v;
    }

    virtual void set_time(posix_time const& p)
    {
        double utime = p.seconds * 1000.0 + p.nanoseconds / 1000000.0;
        UErrorCode code = U_ZERO_ERROR;
        calendar_->setTime(utime, code);
        check_and_throw_dt(code);
    }
    virtual void normalize()
    {
        // Can't call complete() explicitly (protected)
        // calling get wich calls complete
        UErrorCode code = U_ZERO_ERROR;
        calendar_->get(UCAL_YEAR, code);
        check_and_throw_dt(code);
    }
    virtual posix_time get_time() const
    {

        UErrorCode code = U_ZERO_ERROR;
        double rtime = 0;
        {
            guard l(lock_);
            rtime = calendar_->getTime(code);
        }
        check_and_throw_dt(code);
        rtime /= 1000.0;
        double secs = floor(rtime);
        posix_time res;
        res.seconds = static_cast<int64_t>(secs);
        res.nanoseconds = static_cast<uint32_t>((rtime - secs) / 1e9);
        if (res.nanoseconds > 999999999)
            res.nanoseconds = 999999999;
        return res;
    }
    virtual void set_option(calendar_option_type opt, int /*v*/)
    {
        switch (opt)
        {
            case is_gregorian:
                throw date_time_error(
                    "is_gregorian is not settable options for calendar");
            case is_dst:
                throw date_time_error(
                    "is_dst is not settable options for calendar");
            default:;
        }
    }
    virtual int get_option(calendar_option_type opt) const
    {
        switch (opt)
        {
            case is_gregorian:
                return dynamic_cast<icu::GregorianCalendar const*>(
                           calendar_.get()) != 0;
            case is_dst:
            {
                guard l(lock_);
                UErrorCode err = U_ZERO_ERROR;
                bool res = (calendar_->inDaylightTime(err) != 0);
                check_and_throw_dt(err);
                return res;
            }
            default:
                return 0;
        }
    }
    virtual void adjust_value(period::marks::period_mark p, update_type u,
                              int difference)
    {
        UErrorCode err = U_ZERO_ERROR;
        switch (u)
        {
            case move:
                calendar_->add(to_icu(p), difference, err);
                break;
            case roll:
                calendar_->roll(to_icu(p), difference, err);
                break;
        }
        check_and_throw_dt(err);
    }
    virtual int difference(abstract_calendar const* other_ptr,
                           period::marks::period_mark p) const
    {
        UErrorCode err = U_ZERO_ERROR;
        double other_time = 0;
        //
        // fieldDifference has side effect of moving calendar (WTF?)
        // So we clone it for performing this operation
        //
        hold_ptr<icu::Calendar> self(calendar_->clone());

        calendar_impl const* other_cal =
            dynamic_cast<calendar_impl const*>(other_ptr);
        if (other_cal)
        {
            guard l(other_cal->lock_);
            other_time = other_cal->calendar_->getTime(err);
            check_and_throw_dt(err);
        }
        else
        {
            posix_time p = other_ptr->get_time();
            other_time = p.seconds * 1000.0 + p.nanoseconds / 1000000.0;
        }

        int diff = self->fieldDifference(other_time, to_icu(p), err);

        check_and_throw_dt(err);
        return diff;
    }
    virtual void set_timezone(std::string const& tz)
    {
        calendar_->adoptTimeZone(get_time_zone(tz));
    }
    virtual std::string get_timezone() const
    {
        icu::UnicodeString tz;
        calendar_->getTimeZone().getID(tz);
        icu_std_converter<char> cvt(encoding_);
        return cvt.std(tz);
    }
    virtual bool same(abstract_calendar const* other) const
    {
        calendar_impl const* oc = dynamic_cast<calendar_impl const*>(other);
        if (!oc)
            return false;
        return calendar_->isEquivalentTo(*oc->calendar_) != 0;
    }

  private:
    typedef boost::unique_lock<boost::mutex> guard;
    mutable boost::mutex lock_;
    std::string encoding_;
    hold_ptr<icu::Calendar> calendar_;
};

class icu_calendar_facet : public calendar_facet
{
  public:
    icu_calendar_facet(cdata const& d, size_t refs = 0) :
        calendar_facet(refs), data_(d)
    {
    }
    virtual abstract_calendar* create_calendar() const
    {
        return new calendar_impl(data_);
    }

  private:
    cdata data_;
};

std::locale create_calendar(std::locale const& in, cdata const& d)
{
    return std::locale(in, new icu_calendar_facet(d));
}

} // namespace impl_icu
} // namespace locale
} // namespace boost

// vim: tabstop=4 expandtab shiftwidth=4 softtabstop=4


#pragma once

#include <Eigen/Core>
#include <memory>

#include "../motionmodule.hh"
#include "../../Smoother/LinearSmoother/linearsmoother.hh"
#include "../../WalkAttitude/walkattitude.hh"

namespace bold
{
  class Balance;
  class WalkEngine;
  template<typename> class Setting;

  enum class WalkStatus : uchar
  {
    Stopped = 0,
    Starting = 1,
    Walking = 2,
    Stabilising = 3
  };

  std::string getWalkStatusName(WalkStatus status);

  class WalkModule : public MotionModule
  {
  public:
    WalkModule(std::shared_ptr<MotionTaskScheduler> scheduler);
    ~WalkModule() override = default;

    void step(std::shared_ptr<JointSelection> const& selectedJoints) override;

    void applyHead(HeadSection* head) override;
    void applyArms(ArmSection* arms) override;
    void applyLegs(LegSection* legs) override;

    /** Slow walking to a stop and stabilise.
     */
    void stop();

    /** Stop walking abruptly. Likely unstable.
     *
     * Call stop instead to have walking stop smoothly.
     */
    void stopImmediately();

    bool isRunning() const { return !d_immediateStopRequested && d_status != WalkStatus::Stopped; }

    WalkStatus getStatus() const { return d_status; }

    /**
     * Set the direction of motion, where positive X is in the forwards
     * direction, and positive Y is to the left. The length of the vector
     * determines the velocity of motion (unspecified units).
     *
     * Note that the same value will result in different speeds in the X and Y
     * axes.
     */
    void setMoveDir(double x, double y);

    void setMoveDir(Eigen::Vector2d const& dir)
    {
      setMoveDir(dir.x(), dir.y());
    }

    /**
     * Set the rate of turning, where positive values turn right (clockwise)
     * and negative values turn left (counter-clockwise) (unspecified units).
     */
    void setTurnAngle(double turnSpeed);

  private:
    WalkModule(const WalkModule&) = delete;
    WalkModule& operator=(const WalkModule&) = delete;

    void start();

    std::shared_ptr<WalkEngine> d_walkEngine;
    std::shared_ptr<Balance> d_balance;

    Setting<int>* d_stabilisationTimeMillis;
    int d_stabilisationCycleCount;
    int d_stabilisationCyclesRemaining;

    LinearSmoother d_xAmpSmoother;
    LinearSmoother d_yAmpSmoother;
    LinearSmoother d_turnAmpSmoother;
    LinearSmoother d_hipPitchSmoother;
    WalkAttitude d_attitude;
    Setting<bool>* d_isParalysed;

    bool d_turnAngleSet;
    bool d_moveDirSet;
    bool d_immediateStopRequested;
    WalkStatus d_status;
  };
}


#ifndef BOOST_GEOMETRY_PROJECTIONS_STERE_HPP
#define BOOST_GEOMETRY_PROJECTIONS_STERE_HPP

// Boost.Geometry - extensions-gis-projections (based on PROJ4)
// This file is automatically generated. DO NOT EDIT.

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

// Use, modification and distribution is subject to 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)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 4.9.1

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#include <boost/config.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/math/special_functions/hypot.hpp>

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>
#include <boost/geometry/srs/projections/impl/pj_tsfn.hpp>

namespace boost { namespace geometry
{

namespace srs { namespace par4
{
    struct stere {};
    struct ups {};

}} //namespace srs::par4

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace stere
    {
            static const double EPS10 = 1.e-10;
            static const double TOL = 1.e-8;
            static const int NITER = 8;
            static const double CONV = 1.e-10;
            static const int S_POLE = 0;
            static const int N_POLE = 1;
            static const int OBLIQ = 2;
            static const int EQUIT = 3;

            template <typename T>
            struct par_stere
            {
                T   phits;
                T   sinX1;
                T   cosX1;
                T   akm1;
                int mode;
            };

            template <typename T>
            inline T ssfn_(T const& phit, T sinphi, T const& eccen)
            {
                sinphi *= eccen;
                return (tan (.5 * (geometry::math::half_pi<T>() + phit)) *
                   pow((1. - sinphi) / (1. + sinphi), .5 * eccen));
            }

            // template class, using CRTP to implement forward/inverse
            template <typename CalculationType, typename Parameters>
            struct base_stere_ellipsoid : public base_t_fi<base_stere_ellipsoid<CalculationType, Parameters>,
                     CalculationType, Parameters>
            {

                typedef CalculationType geographic_type;
                typedef CalculationType cartesian_type;

                par_stere<CalculationType> m_proj_parm;

                inline base_stere_ellipsoid(const Parameters& par)
                    : base_t_fi<base_stere_ellipsoid<CalculationType, Parameters>,
                     CalculationType, Parameters>(*this, par) {}

                // FORWARD(e_forward)  ellipsoid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    static const CalculationType HALFPI = detail::HALFPI<CalculationType>();

                    CalculationType coslam, sinlam, sinX=0.0, cosX=0.0, X, A, sinphi;

                    coslam = cos(lp_lon);
                    sinlam = sin(lp_lon);
                    sinphi = sin(lp_lat);
                    if (this->m_proj_parm.mode == OBLIQ || this->m_proj_parm.mode == EQUIT) {
                        sinX = sin(X = 2. * atan(ssfn_(lp_lat, sinphi, this->m_par.e)) - HALFPI);
                        cosX = cos(X);
                    }
                    switch (this->m_proj_parm.mode) {
                    case OBLIQ:
                        A = this->m_proj_parm.akm1 / (this->m_proj_parm.cosX1 * (1. + this->m_proj_parm.sinX1 * sinX +
                           this->m_proj_parm.cosX1 * cosX * coslam));
                        xy_y = A * (this->m_proj_parm.cosX1 * sinX - this->m_proj_parm.sinX1 * cosX * coslam);
                        goto xmul;
                    case EQUIT:
                        A = this->m_proj_parm.akm1 / (1. + cosX * coslam);
                        xy_y = A * sinX;
                xmul:
                        xy_x = A * cosX;
                        break;
                    case S_POLE:
                        lp_lat = -lp_lat;
                        coslam = - coslam;
                        sinphi = -sinphi;
                        BOOST_FALLTHROUGH;
                    case N_POLE:
                        xy_x = this->m_proj_parm.akm1 * pj_tsfn(lp_lat, sinphi, this->m_par.e);
                        xy_y = - xy_x * coslam;
                        break;
                    }
                    xy_x = xy_x * sinlam;
                }

                // INVERSE(e_inverse)  ellipsoid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    static const CalculationType HALFPI = detail::HALFPI<CalculationType>();

                    CalculationType cosphi, sinphi, tp=0.0, phi_l=0.0, rho, halfe=0.0, halfpi=0.0;
                    int i;

                    rho = boost::math::hypot(xy_x, xy_y);
                    switch (this->m_proj_parm.mode) {
                    case OBLIQ:
                    case EQUIT:
                        cosphi = cos( tp = 2. * atan2(rho * this->m_proj_parm.cosX1 , this->m_proj_parm.akm1) );
                        sinphi = sin(tp);
                                if( rho == 0.0 )
                            phi_l = asin(cosphi * this->m_proj_parm.sinX1);
                                else
                            phi_l = asin(cosphi * this->m_proj_parm.sinX1 + (xy_y * sinphi * this->m_proj_parm.cosX1 / rho));

                        tp = tan(.5 * (HALFPI + phi_l));
                        xy_x *= sinphi;
                        xy_y = rho * this->m_proj_parm.cosX1 * cosphi - xy_y * this->m_proj_parm.sinX1* sinphi;
                        halfpi = HALFPI;
                        halfe = .5 * this->m_par.e;
                        break;
                    case N_POLE:
                        xy_y = -xy_y;
                        BOOST_FALLTHROUGH;
                    case S_POLE:
                        phi_l = HALFPI - 2. * atan(tp = - rho / this->m_proj_parm.akm1);
                        halfpi = -HALFPI;
                        halfe = -.5 * this->m_par.e;
                        break;
                    }
                    for (i = NITER; i--; phi_l = lp_lat) {
                        sinphi = this->m_par.e * sin(phi_l);
                        lp_lat = 2. * atan(tp * pow((1.+sinphi)/(1.-sinphi), halfe)) - halfpi;
                        if (fabs(phi_l - lp_lat) < CONV) {
                            if (this->m_proj_parm.mode == S_POLE)
                                lp_lat = -lp_lat;
                            lp_lon = (xy_x == 0. && xy_y == 0.) ? 0. : atan2(xy_x, xy_y);
                            return;
                        }
                    }
                    BOOST_THROW_EXCEPTION( projection_exception(-20) );
                }

                static inline std::string get_name()
                {
                    return "stere_ellipsoid";
                }

            };

            // template class, using CRTP to implement forward/inverse
            template <typename CalculationType, typename Parameters>
            struct base_stere_spheroid : public base_t_fi<base_stere_spheroid<CalculationType, Parameters>,
                     CalculationType, Parameters>
            {

                typedef CalculationType geographic_type;
                typedef CalculationType cartesian_type;

                par_stere<CalculationType> m_proj_parm;

                inline base_stere_spheroid(const Parameters& par)
                    : base_t_fi<base_stere_spheroid<CalculationType, Parameters>,
                     CalculationType, Parameters>(*this, par) {}

                // FORWARD(s_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    static const CalculationType FORTPI = detail::FORTPI<CalculationType>();
                    static const CalculationType HALFPI = detail::HALFPI<CalculationType>();

                    CalculationType  sinphi, cosphi, coslam, sinlam;

                    sinphi = sin(lp_lat);
                    cosphi = cos(lp_lat);
                    coslam = cos(lp_lon);
                    sinlam = sin(lp_lon);
                    switch (this->m_proj_parm.mode) {
                    case EQUIT:
                        xy_y = 1. + cosphi * coslam;
                        goto oblcon;
                    case OBLIQ:
                        xy_y = 1. + this->m_proj_parm.sinX1 * sinphi + this->m_proj_parm.cosX1 * cosphi * coslam;
                oblcon:
                        if (xy_y <= EPS10)
                            BOOST_THROW_EXCEPTION( projection_exception(-20) );
                        xy_x = (xy_y = this->m_proj_parm.akm1 / xy_y) * cosphi * sinlam;
                        xy_y *= (this->m_proj_parm.mode == EQUIT) ? sinphi :
                           this->m_proj_parm.cosX1 * sinphi - this->m_proj_parm.sinX1 * cosphi * coslam;
                        break;
                    case N_POLE:
                        coslam = - coslam;
                        lp_lat = - lp_lat;
                        BOOST_FALLTHROUGH;
                    case S_POLE:
                        if (fabs(lp_lat - HALFPI) < TOL)
                            BOOST_THROW_EXCEPTION( projection_exception(-20) );
                        xy_x = sinlam * ( xy_y = this->m_proj_parm.akm1 * tan(FORTPI + .5 * lp_lat) );
                        xy_y *= coslam;
                        break;
                    }
                }

                // INVERSE(s_inverse)  spheroid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    CalculationType  c, rh, sinc, cosc;

                    sinc = sin(c = 2. * atan((rh = boost::math::hypot(xy_x, xy_y)) / this->m_proj_parm.akm1));
                    cosc = cos(c);
                    lp_lon = 0.;
                    switch (this->m_proj_parm.mode) {
                    case EQUIT:
                        if (fabs(rh) <= EPS10)
                            lp_lat = 0.;
                        else
                            lp_lat = asin(xy_y * sinc / rh);
                        if (cosc != 0. || xy_x != 0.)
                            lp_lon = atan2(xy_x * sinc, cosc * rh);
                        break;
                    case OBLIQ:
                        if (fabs(rh) <= EPS10)
                            lp_lat = this->m_par.phi0;
                        else
                            lp_lat = asin(cosc * this->m_proj_parm.sinX1 + xy_y * sinc * this->m_proj_parm.cosX1 / rh);
                        if ((c = cosc - this->m_proj_parm.sinX1 * sin(lp_lat)) != 0. || xy_x != 0.)
                            lp_lon = atan2(xy_x * sinc * this->m_proj_parm.cosX1, c * rh);
                        break;
                    case N_POLE:
                        xy_y = -xy_y;
                        BOOST_FALLTHROUGH;
                    case S_POLE:
                        if (fabs(rh) <= EPS10)
                            lp_lat = this->m_par.phi0;
                        else
                            lp_lat = asin(this->m_proj_parm.mode == S_POLE ? - cosc : cosc);
                        lp_lon = (xy_x == 0. && xy_y == 0.) ? 0. : atan2(xy_x, xy_y);
                        break;
                    }
                }

                static inline std::string get_name()
                {
                    return "stere_spheroid";
                }

            };

            template <typename Parameters, typename T>
            inline void setup(Parameters& par, par_stere<T>& proj_parm)  /* general initialization */
            {
                static const T FORTPI = detail::FORTPI<T>();
                static const T HALFPI = detail::HALFPI<T>();

                T t;

                if (fabs((t = fabs(par.phi0)) - HALFPI) < EPS10)
                    proj_parm.mode = par.phi0 < 0. ? S_POLE : N_POLE;
                else
                    proj_parm.mode = t > EPS10 ? OBLIQ : EQUIT;
                proj_parm.phits = fabs(proj_parm.phits);
                if (par.es != 0.0) {
                    T X;

                    switch (proj_parm.mode) {
                    case N_POLE:
                    case S_POLE:
                        if (fabs(proj_parm.phits - HALFPI) < EPS10)
                            proj_parm.akm1 = 2. * par.k0 /
                               sqrt(pow(1+par.e,1+par.e)*pow(1-par.e,1-par.e));
                        else {
                            proj_parm.akm1 = cos(proj_parm.phits) /
                               pj_tsfn(proj_parm.phits, t = sin(proj_parm.phits), par.e);
                            t *= par.e;
                            proj_parm.akm1 /= sqrt(1. - t * t);
                        }
                        break;
                    case EQUIT:
                        //proj_parm.akm1 = 2. * par.k0;
                        //break;
                    case OBLIQ:
                        t = sin(par.phi0);
                        X = 2. * atan(ssfn_(par.phi0, t, par.e)) - HALFPI;
                        t *= par.e;
                        proj_parm.akm1 = 2. * par.k0 * cos(par.phi0) / sqrt(1. - t * t);
                        proj_parm.sinX1 = sin(X);
                        proj_parm.cosX1 = cos(X);
                        break;
                    }
                } else {
                    switch (proj_parm.mode) {
                    case OBLIQ:
                        proj_parm.sinX1 = sin(par.phi0);
                        proj_parm.cosX1 = cos(par.phi0);
                        BOOST_FALLTHROUGH;
                    case EQUIT:
                        proj_parm.akm1 = 2. * par.k0;
                        break;
                    case S_POLE:
                    case N_POLE:
                        proj_parm.akm1 = fabs(proj_parm.phits - HALFPI) >= EPS10 ?
                           cos(proj_parm.phits) / tan(FORTPI - .5 * proj_parm.phits) :
                           2. * par.k0 ;
                        break;
                    }
                }
            }

            // Stereographic
            template <typename Parameters, typename T>
            inline void setup_stere(Parameters& par, par_stere<T>& proj_parm)
            {
                static const T HALFPI = detail::HALFPI<T>();

                proj_parm.phits = pj_param(par.params, "tlat_ts").i ?
                        pj_param(par.params, "rlat_ts").f : HALFPI;
                setup(par, proj_parm);
            }

            // Universal Polar Stereographic
            template <typename Parameters, typename T>
            inline void setup_ups(Parameters& par, par_stere<T>& proj_parm)
            {
                static const T HALFPI = detail::HALFPI<T>();

                /* International Ellipsoid */
                par.phi0 = pj_param(par.params, "bsouth").i ? -HALFPI: HALFPI;
                if (!par.es)
                    BOOST_THROW_EXCEPTION( projection_exception(-34) );
                par.k0 = .994;
                par.x0 = 2000000.;
                par.y0 = 2000000.;
                proj_parm.phits = HALFPI;
                par.lam0 = 0.;
                setup(par, proj_parm);
            }

    }} // namespace detail::stere
    #endif // doxygen

    /*!
        \brief Stereographic projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal
         - Spheroid
         - Ellipsoid
        \par Projection parameters
         - lat_ts: Latitude of true scale (degrees)
        \par Example
        \image html ex_stere.gif
    */
    template <typename CalculationType, typename Parameters>
    struct stere_ellipsoid : public detail::stere::base_stere_ellipsoid<CalculationType, Parameters>
    {
        inline stere_ellipsoid(const Parameters& par) : detail::stere::base_stere_ellipsoid<CalculationType, Parameters>(par)
        {
            detail::stere::setup_stere(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Stereographic projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal
         - Spheroid
         - Ellipsoid
        \par Projection parameters
         - lat_ts: Latitude of true scale (degrees)
        \par Example
        \image html ex_stere.gif
    */
    template <typename CalculationType, typename Parameters>
    struct stere_spheroid : public detail::stere::base_stere_spheroid<CalculationType, Parameters>
    {
        inline stere_spheroid(const Parameters& par) : detail::stere::base_stere_spheroid<CalculationType, Parameters>(par)
        {
            detail::stere::setup_stere(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Universal Polar Stereographic projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal
         - Spheroid
         - Ellipsoid
        \par Projection parameters
         - south: Denotes southern hemisphere UTM zone (boolean)
        \par Example
        \image html ex_ups.gif
    */
    template <typename CalculationType, typename Parameters>
    struct ups_ellipsoid : public detail::stere::base_stere_ellipsoid<CalculationType, Parameters>
    {
        inline ups_ellipsoid(const Parameters& par) : detail::stere::base_stere_ellipsoid<CalculationType, Parameters>(par)
        {
            detail::stere::setup_ups(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Universal Polar Stereographic projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal
         - Spheroid
         - Ellipsoid
        \par Projection parameters
         - south: Denotes southern hemisphere UTM zone (boolean)
        \par Example
        \image html ex_ups.gif
    */
    template <typename CalculationType, typename Parameters>
    struct ups_spheroid : public detail::stere::base_stere_spheroid<CalculationType, Parameters>
    {
        inline ups_spheroid(const Parameters& par) : detail::stere::base_stere_spheroid<CalculationType, Parameters>(par)
        {
            detail::stere::setup_ups(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::stere, stere_spheroid, stere_ellipsoid)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::ups, ups_spheroid, ups_ellipsoid)

        // Factory entry(s)
        template <typename CalculationType, typename Parameters>
        class stere_entry : public detail::factory_entry<CalculationType, Parameters>
        {
            public :
                virtual base_v<CalculationType, Parameters>* create_new(const Parameters& par) const
                {
                    if (par.es)
                        return new base_v_fi<stere_ellipsoid<CalculationType, Parameters>, CalculationType, Parameters>(par);
                    else
                        return new base_v_fi<stere_spheroid<CalculationType, Parameters>, CalculationType, Parameters>(par);
                }
        };

        template <typename CalculationType, typename Parameters>
        class ups_entry : public detail::factory_entry<CalculationType, Parameters>
        {
            public :
                virtual base_v<CalculationType, Parameters>* create_new(const Parameters& par) const
                {
                    if (par.es)
                        return new base_v_fi<ups_ellipsoid<CalculationType, Parameters>, CalculationType, Parameters>(par);
                    else
                        return new base_v_fi<ups_spheroid<CalculationType, Parameters>, CalculationType, Parameters>(par);
                }
        };

        template <typename CalculationType, typename Parameters>
        inline void stere_init(detail::base_factory<CalculationType, Parameters>& factory)
        {
            factory.add_to_factory("stere", new stere_entry<CalculationType, Parameters>);
            factory.add_to_factory("ups", new ups_entry<CalculationType, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_STERE_HPP


/*
 * Copyright (C) 2020 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/

#ifndef RMF_FLEET_ADAPTER__AGV__WAYPOINT_HPP
#define RMF_FLEET_ADAPTER__AGV__WAYPOINT_HPP

#include <rmf_traffic/Time.hpp>

#include <rmf_utils/impl_ptr.hpp>

#include <Eigen/Geometry>

namespace rmf_fleet_adapter {
namespace agv {

//==============================================================================
class Waypoint
{
public:

  /// Constructor
  ///
  /// \param[in] map_name
  ///   The name of the reference map that the position is on.
  ///
  /// \param[in] position
  ///   A position along the robot's path where it will (or can) have zero
  ///   instantaneous velocity. This will usually be a point where the robot
  ///   needs to turn, or a point that comes before an intersection, so the
  ///   robot can come to a stop and allow other vehicles to pass. This is a 2D
  ///   heterogeneous vector. The first two elements refer to translational
  ///   (x,y) position while the third element is a yaw value in radians.
  ///
  /// \param[in] mandatory_delay
  ///   A delay that the robot is expected to experience once it arrives at
  ///   this waypoint. Usually this will be caused by the robots needing to
  ///   wait for doors to open or lifts to arrive.
  ///
  /// \param[in] yield
  ///   Whether or not the robot can wait at this point. If yield is false, then
  ///   the planner will assume that it cannot ask the robot to wait here. If
  ///   yield is true, then the planner may request that the robot wait here to
  ///   avoid conflicts with other traffic participants.
  Waypoint(
    std::string map_name,
    Eigen::Vector3d position,
    rmf_traffic::Duration mandatory_delay = std::chrono::nanoseconds(0),
    bool yield = true);

  /// Get the map of this Waypoint.
  const std::string& map_name() const;

  /// Set the map of this Waypoint.
  Waypoint& map_name(std::string new_map_name);

  /// Get the position of this Waypoint.
  const Eigen::Vector3d& position() const;

  /// Set the position of this Waypoint.
  Waypoint& position(Eigen::Vector3d new_position);

  /// Get the mandatory delay associated with this Waypoint.
  rmf_traffic::Duration mandatory_delay() const;

  /// Set the mandatory delay associated with this Waypoint.
  Waypoint& mandatory_delay(rmf_traffic::Duration duration);

  /// Get whether the robot can yield here.
  bool yield() const;

  /// Set whether the robot can yield here.
  Waypoint& yield(bool on);

  class Implementation;
private:
  rmf_utils::impl_ptr<Implementation> _pimpl;
};

} // namespace agv
} // namespace rmf_fleet_adapter

#endif // RMF_FLEET_ADAPTER__AGV__WAYPOINT_HPP


#include "UnlabeledDisagreementLoss.h"
#include <iostream>
#include <boost/lexical_cast.hpp>
#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET
#include "LearnIt/Eigen/Sparse"
#include "ActiveLearning/DataView.h"
#include "ActiveLearning/ActiveLearningData.h"
#include "ActiveLearning/SmoothedL1.h"

using Eigen::VectorXd;
using Eigen::SparseVector;

UnlabeledDisagreementLoss::UnlabeledDisagreementLoss(InferenceDataView_ptr view1,
		InferenceDataView_ptr view2, ActiveLearningData_ptr alData,
		double whenAnnotatedLossWeight, double whenUnannotatedLossWeight,
		Loss lossFunc)
: _data1(view1), _data2(view2), _alData(alData),
_whenAnnotatedLossWeight(whenAnnotatedLossWeight),
_whenUnannotatedLossWeight(whenUnannotatedLossWeight),
_lossFunc(lossFunc) {}

double UnlabeledDisagreementLoss::operator()(VectorXd& gradient,
		size_t thread_idx, size_t n_threads) const {
	double loss = 0.0;
	double dbg = 0.0;
	for (unsigned int i=thread_idx; i<_data1->nInstances(); i+=n_threads) {
		if (!_alData->instanceAnnotation(i)) {
				// 1= slot
				// 2= sent
			SparseVector<double>::InnerIterator it1(_data1->features(i));
			SparseVector<double>::InnerIterator it2(_data2->features(i));

			// skip instances where one FV is empty
			if (it1 && it2) {
				// generally, we want to penalize disagreement more when
				// we have an annotation to make us more sure of one of
				// the models
				double lossWeight = _alData->instanceHasAnnotatedFeature(i)
					? _whenAnnotatedLossWeight : _whenUnannotatedLossWeight;

				// we dont take the prior into account when calculating disagreement,
				// since otherwise the default value for an instance we know nothing
				// about is rather extreme
				//double pos1 = _data1.predictionNoPrior(i);
				double pos1 = _data1->prediction(i);
				double neg1 = 1.0 - pos1;
				//double pos2= _data2.predictionNoPrior(i);
				double pos2= _data2->prediction(i);
				double neg2 = 1.0 - pos2;

				if (_lossFunc == SQUARED) {
					double diff = pos1 - pos2;
					loss -= lossWeight * (diff)*(diff);
					double both_mult = lossWeight * 2.0 * diff;
					double mult1 = both_mult*pos1*neg1;
					double mult2 = both_mult*pos2*neg2;
					for (; it1; ++it1) {
						gradient(it1.index()) -= mult1*it1.value();
						/*if (_debug_gradient) {
						_local_gradient(it1.index()) -= mult1*it1.value();
						}*/
					}
					for (; it2; ++it2) {
						gradient(it2.index()) += mult2*it2.value();
						/*if (_debug_gradient) {
						_local_gradient(it2.index()) += mult2*it2.value();
						}*/
					}
				} else if (_lossFunc == KL) {
					//_loss -= _lossWeight * (pos1*log(pos1/pos2) + neg1*log(neg1/neg2));
					loss -= lossWeight * (pos1*(log(pos1)-log(pos2))
						+ neg1*(log(neg1)-log(neg2)));
					//double factor1 = -_lossWeight * pos1*neg1*(log((pos1*neg2)/(neg1 * pos2)));
					double factor1 = -lossWeight * pos1*neg1*
						(log(pos1*neg2) -log(neg1 * pos2));
					double factor2 = -lossWeight * (neg1*pos2-pos1*neg2);

					for (; it1; ++it1) {
						gradient(it1.index()) += factor1 * it1.value();
					}
					for (; it2; ++it2) {
						gradient(it2.index()) += factor2 * it2.value();
					}
				} else if (_lossFunc == L1) {
					loss += SmoothedL1::SmoothedL1ProbProb(pos1, pos2,
						_data1->features(i), _data2->features(i),
						gradient, lossWeight);
				}
			}
		}
	}
	_debug_loss = loss;
	return loss;
}

std::wstring UnlabeledDisagreementLoss::status() const {
	return L"diss_loss = " + boost::lexical_cast<std::wstring>(_debug_loss);
}

void UnlabeledDisagreementLoss::snapshot() {
}

double UnlabeledDisagreementLoss::forInstance(int inst,
								Eigen::SparseVector<double>& gradient) const
{
	double ret = 0.0;

	if (!_alData->instanceAnnotation(inst)) {
		// 1= slot
		// 2= sent
		SparseVector<double>::InnerIterator it1(_data1->features(inst));
		SparseVector<double>::InnerIterator it2(_data2->features(inst));

		// skip instances where one FV is empty
		if (it1 && it2) {
			double lossWeight = _alData->instanceHasAnnotatedFeature(inst)
				? _whenAnnotatedLossWeight : _whenUnannotatedLossWeight;

			// we dont take the prior into account when calculating disagreement,
			// since otherwise the default value for an instance we know nothing
			// about is rather extreme
			//double pos1 = _data1.predictionNoPrior(i);
			double pos1 = _data1->prediction(inst);
			double neg1 = 1.0 - pos1;
			//double pos2= _data2.predictionNoPrior(i);
			double pos2= _data2->prediction(inst);
			double neg2 = 1.0 - pos2;

			double diff = pos1 - pos2;
			ret -= lossWeight * (diff)*(diff);
			double both_mult = lossWeight * 2.0 * diff;
			double mult1 = both_mult*pos1*neg1;
			double mult2 = both_mult*pos2*neg2;
			for (; it1; ++it1) {
				gradient.coeffRef(it1.index()) -= mult1*it1.value();

				/*if (_debug_gradient) {
				_local_gradient(it1.index()) -= mult1*it1.value();
				}*/
			}
			for (; it2; ++it2) {
				gradient.coeffRef(it2.index()) += mult2*it2.value();

				/*if (_debug_gradient) {
				_local_gradient(it2.index()) += mult2*it2.value();
				}*/
			}
		}
	}
	return ret;
}


#include "extractor/tarjan_scc.hpp"
#include "util/coordinate_calculation.hpp"
#include "util/dynamic_graph.hpp"
#include "util/exception.hpp"
#include "util/fingerprint.hpp"
#include "util/graph_loader.hpp"
#include "util/make_unique.hpp"
#include "util/simple_logger.hpp"
#include "util/static_graph.hpp"
#include "util/typedefs.hpp"

#include <boost/filesystem.hpp>

#if defined(__APPLE__) || defined(_WIN32)
#include <gdal.h>
#include <ogrsf_frmts.h>
#else
#include <gdal/gdal.h>
#include <gdal/ogrsf_frmts.h>
#endif

#include "osrm/coordinate.hpp"

#include <fstream>
#include <memory>
#include <string>
#include <vector>

namespace osrm
{
namespace tools
{

struct TarjanEdgeData
{
    TarjanEdgeData() : distance(INVALID_EDGE_WEIGHT), name_id(INVALID_NAMEID) {}
    TarjanEdgeData(unsigned distance, unsigned name_id) : distance(distance), name_id(name_id) {}
    unsigned distance;
    unsigned name_id;
};

using TarjanGraph = util::StaticGraph<TarjanEdgeData>;
using TarjanEdge = TarjanGraph::InputEdge;

void deleteFileIfExists(const std::string &file_name)
{
    if (boost::filesystem::exists(file_name))
    {
        boost::filesystem::remove(file_name);
    }
}

std::size_t loadGraph(const char *path,
                      std::vector<extractor::QueryNode> &coordinate_list,
                      std::vector<TarjanEdge> &graph_edge_list)
{
    std::ifstream input_stream(path, std::ifstream::in | std::ifstream::binary);
    if (!input_stream.is_open())
    {
        throw util::exception("Cannot open osrm file");
    }

    // load graph data
    std::vector<extractor::NodeBasedEdge> edge_list;
    std::vector<NodeID> traffic_light_node_list;
    std::vector<NodeID> barrier_node_list;

    auto number_of_nodes = util::loadNodesFromFile(
        input_stream, barrier_node_list, traffic_light_node_list, coordinate_list);

    util::loadEdgesFromFile(input_stream, edge_list);

    traffic_light_node_list.clear();
    traffic_light_node_list.shrink_to_fit();

    // Building an node-based graph
    for (const auto &input_edge : edge_list)
    {
        if (input_edge.source == input_edge.target)
        {
            continue;
        }

        if (input_edge.forward)
        {
            graph_edge_list.emplace_back(input_edge.source,
                                         input_edge.target,
                                         (std::max)(input_edge.weight, 1),
                                         input_edge.name_id);
        }
        if (input_edge.backward)
        {
            graph_edge_list.emplace_back(input_edge.target,
                                         input_edge.source,
                                         (std::max)(input_edge.weight, 1),
                                         input_edge.name_id);
        }
    }

    return number_of_nodes;
}
}
}

int main(int argc, char *argv[])
{
    std::vector<osrm::extractor::QueryNode> coordinate_list;
    osrm::util::LogPolicy::GetInstance().Unmute();

    // enable logging
    if (argc < 2)
    {
        osrm::util::SimpleLogger().Write(logWARNING) << "usage:\n" << argv[0] << " <osrm>";
        return EXIT_FAILURE;
    }

    std::vector<osrm::tools::TarjanEdge> graph_edge_list;
    auto number_of_nodes = osrm::tools::loadGraph(argv[1], coordinate_list, graph_edge_list);

    tbb::parallel_sort(graph_edge_list.begin(), graph_edge_list.end());
    const auto graph = std::make_shared<osrm::tools::TarjanGraph>(number_of_nodes, graph_edge_list);
    graph_edge_list.clear();
    graph_edge_list.shrink_to_fit();

    osrm::util::SimpleLogger().Write() << "Starting SCC graph traversal";

    auto tarjan =
        osrm::util::make_unique<osrm::extractor::TarjanSCC<osrm::tools::TarjanGraph>>(graph);
    tarjan->Run();
    osrm::util::SimpleLogger().Write() << "identified: " << tarjan->GetNumberOfComponents()
                                       << " many components";
    osrm::util::SimpleLogger().Write() << "identified " << tarjan->GetSizeOneCount()
                                       << " size 1 SCCs";

    // output
    TIMER_START(SCC_RUN_SETUP);

    // remove files from previous run if exist
    osrm::tools::deleteFileIfExists("component.dbf");
    osrm::tools::deleteFileIfExists("component.shx");
    osrm::tools::deleteFileIfExists("component.shp");

    OGRRegisterAll();

    const char *psz_driver_name = "ESRI Shapefile";
    auto *po_driver = OGRSFDriverRegistrar::GetRegistrar()->GetDriverByName(psz_driver_name);
    if (nullptr == po_driver)
    {
        throw osrm::util::exception("ESRI Shapefile driver not available");
    }
    auto *po_datasource = po_driver->CreateDataSource("component.shp", nullptr);

    if (nullptr == po_datasource)
    {
        throw osrm::util::exception("Creation of output file failed");
    }

    auto *po_srs = new OGRSpatialReference();
    po_srs->importFromEPSG(4326);

    auto *po_layer = po_datasource->CreateLayer("component", po_srs, wkbLineString, nullptr);

    if (nullptr == po_layer)
    {
        throw osrm::util::exception("Layer creation failed.");
    }
    TIMER_STOP(SCC_RUN_SETUP);
    osrm::util::SimpleLogger().Write() << "shapefile setup took "
                                       << TIMER_MSEC(SCC_RUN_SETUP) / 1000. << "s";

    uint64_t total_network_length = 0;
    osrm::util::Percent percentage(graph->GetNumberOfNodes());
    TIMER_START(SCC_OUTPUT);
    for (const NodeID source : osrm::util::irange(0u, graph->GetNumberOfNodes()))
    {
        percentage.PrintIncrement();
        for (const auto current_edge : graph->GetAdjacentEdgeRange(source))
        {
            const auto target = graph->GetTarget(current_edge);

            if (source < target || SPECIAL_EDGEID == graph->FindEdge(target, source))
            {
                total_network_length +=
                    100 * osrm::util::coordinate_calculation::greatCircleDistance(
                              coordinate_list[source], coordinate_list[target]);

                BOOST_ASSERT(current_edge != SPECIAL_EDGEID);
                BOOST_ASSERT(source != SPECIAL_NODEID);
                BOOST_ASSERT(target != SPECIAL_NODEID);

                const unsigned size_of_containing_component =
                    std::min(tarjan->GetComponentSize(tarjan->GetComponentID(source)),
                             tarjan->GetComponentSize(tarjan->GetComponentID(target)));

                // edges that end on bollard nodes may actually be in two distinct components
                if (size_of_containing_component < 1000)
                {
                    OGRLineString line_string;
                    line_string.addPoint(
                        static_cast<double>(osrm::util::toFloating(coordinate_list[source].lon)),
                        static_cast<double>(osrm::util::toFloating(coordinate_list[source].lat)));
                    line_string.addPoint(
                        static_cast<double>(osrm::util::toFloating(coordinate_list[target].lon)),
                        static_cast<double>(osrm::util::toFloating(coordinate_list[target].lat)));

                    OGRFeature *po_feature = OGRFeature::CreateFeature(po_layer->GetLayerDefn());

                    po_feature->SetGeometry(&line_string);
                    if (OGRERR_NONE != po_layer->CreateFeature(po_feature))
                    {
                        throw osrm::util::exception("Failed to create feature in shapefile.");
                    }
                    OGRFeature::DestroyFeature(po_feature);
                }
            }
        }
    }
    OGRSpatialReference::DestroySpatialReference(po_srs);
    OGRDataSource::DestroyDataSource(po_datasource);
    TIMER_STOP(SCC_OUTPUT);
    osrm::util::SimpleLogger().Write()
        << "generating output took: " << TIMER_MSEC(SCC_OUTPUT) / 1000. << "s";

    osrm::util::SimpleLogger().Write()
        << "total network distance: " << static_cast<uint64_t>(total_network_length / 100 / 1000.)
        << " km";

    osrm::util::SimpleLogger().Write() << "finished component analysis";
    return EXIT_SUCCESS;
} catch(...) {

}


/*!
@file
Defines `boost::hana::Group`.

@copyright Louis Dionne 2013-2016
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_HANA_CONCEPT_GROUP_HPP
#define BOOST_HANA_CONCEPT_GROUP_HPP

#include <boost/hana/fwd/concept/group.hpp>

#include <boost/hana/config.hpp>
#include <boost/hana/core/default.hpp>
#include <boost/hana/core/tag_of.hpp>
#include <boost/hana/minus.hpp>
#include <boost/hana/negate.hpp>

BOOST_HANA_NAMESPACE_BEGIN
    template <typename G>
    struct Group {
        using Tag = typename tag_of<G>::type;
        static constexpr bool value = !is_default<negate_impl<Tag>>::value ||
                                      !is_default<minus_impl<Tag, Tag>>::value;
    };
BOOST_HANA_NAMESPACE_END

#endif // !BOOST_HANA_CONCEPT_GROUP_HPP


#include "stdafx.h"

#include <boost\test\unit_test.hpp>

#include <geometry\geometry.hpp>

#include <boost\gil\extension\io_new\png_all.hpp>

using namespace boost;
using namespace gil;
//using namespace boost::gil::opencv;

template< typename Geometry, typename View >
struct draw {};

template< typename View, typename Point >
struct draw< geometry::box< Point >, View > { static void d() {} };

BOOST_AUTO_TEST_CASE( test_geometry )
{
	typedef geometry::point_xy<double> P;

	geometry::box< P > b( P( 10, 10 )
	                    , P( 35, 70 )
	                    );

    rgb8_image_t img;
    int i;
    //draw( i, view( img ) );

    draw< geometry::box< P >, double >::d();

    //write_view( "..\\out\\geometry.png", view( dst ), png_tag() );
}


#include <Eigen/Core>
#include <Eigen/Geometry>
#include <string>
#include <vector>

#include "boost/filesystem.hpp"
#include "boost/program_options.hpp"

#include <pcl/io/pcd_io.h>
#include <pcl/common/transforms.h>

#include "modules/common/log.h"
#include "modules/localization/msf/common/io/pcl_point_types.h"
#include "modules/localization/msf/common/io/velodyne_utility.h"

bool ParseCommandLine(int argc, char* argv[],
                      boost::program_options::variables_map* vm) {
  boost::program_options::options_description desc("Allowd options");
  desc.add_options()("help", "product help message")
      ("pcd_folders", boost::program_options::value<std::vector<std::string>>()
                          ->multitoken()
                          ->composing()
                          ->required(),
       "pcd folders(repeated)")(
          "pose_files",
          boost::program_options::value<std::vector<std::string>>()
              ->multitoken()
              ->composing()
              ->required(),
          "pose files(repeated)");
  try {
    boost::program_options::store(
        boost::program_options::parse_command_line(argc, argv, desc), *vm);
    if (vm->count("help")) {
      std::cerr << desc << std::endl;
      return false;
    }
    boost::program_options::notify(*vm);
  } catch (std::exception& e) {
    std::cerr << "Error" << e.what() << std::endl;
    std::cerr << desc << std::endl;
    return false;
  } catch (...) {
    std::cerr << "Unknown error!" << std::endl;
    return false;
  }
  return true;
}

int main(int argc, char** argv) {
  boost::program_options::variables_map boost_args;
  if (!ParseCommandLine(argc, argv, &boost_args)) {
    std::cerr << "Parse input command line failed." << std::endl;
    return -1;
  }

  const std::vector<std::string> pcd_folder_pathes =
      boost_args["pcd_folders"].as<std::vector<std::string>>();
  const std::vector<std::string> pose_files =
      boost_args["pose_files"].as<std::vector<std::string>>();
  if (pcd_folder_pathes.size() != pose_files.size()) {
    std::cerr << "The count of pcd folders is not equal pose files"
              << std::endl;
    return -1;
  }

  const unsigned int num_trials = pcd_folder_pathes.size();

  // load all poses
  std::cerr << "Pcd folders are as follows:" << std::endl;
  for (std::size_t i = 0; i < num_trials; ++i) {
    std::cerr << pcd_folder_pathes[i] << std::endl;
  }
  std::vector<std::vector<Eigen::Affine3d>> ieout_poses(num_trials);
  std::vector<std::vector<double>> time_stamps(num_trials);
  std::vector<std::vector<unsigned int>> pcd_indices(num_trials);
  for (std::size_t i = 0; i < pose_files.size(); ++i) {
    apollo::localization::msf::velodyne::LoadPcdPoses(
        pose_files[i], &ieout_poses[i], &time_stamps[i], &pcd_indices[i]);
  }

  Eigen::Affine3d initial_pose_inv;

  pcl::PointCloud<apollo::localization::msf::velodyne::PointXYZIT>::Ptr result_cloud(new pcl::PointCloud<apollo::localization::msf::velodyne::PointXYZIT>);

  for (unsigned int trial = 0; trial < num_trials; ++trial) {
    for (unsigned int frame_idx = 0; frame_idx < ieout_poses[trial].size();
         ++frame_idx) {
      unsigned int trial_frame_idx = frame_idx;
      const std::vector<Eigen::Affine3d>& poses = ieout_poses[trial];

      apollo::localization::msf::velodyne::VelodyneFrame velodyne_frame;
      std::string pcd_file_path;
      std::ostringstream ss;
      ss << pcd_indices[trial][frame_idx];
      pcd_file_path = pcd_folder_pathes[trial] + "/" + ss.str() + ".pcd";
      const Eigen::Affine3d& pcd_pose = poses[trial_frame_idx];

      if (trial_frame_idx == 0) {
          initial_pose_inv = pcd_pose.inverse();
      }

      pcl::PointCloud<apollo::localization::msf::velodyne::PointXYZIT>::Ptr cloud(new pcl::PointCloud<apollo::localization::msf::velodyne::PointXYZIT>);
      if (pcl::io::loadPCDFile(pcd_file_path, *cloud) >= 0) {
        pcl::transformPointCloud (*cloud, *cloud, pcd_pose);
        pcl::transformPointCloud (*cloud, *cloud, initial_pose_inv);

        *result_cloud+=*cloud;

      } else {
        AERROR << "Failed to load PCD file: " << pcd_file_path;
      }

      pcl::io::savePCDFile("result_pcd.pcd", *result_cloud);
      AERROR << "File " << frame_idx << " out of " << ieout_poses[trial].size();

    }
  }

  return 0;
}


// Copyright (c) 2011-2013, Pacific Biosciences of California, Inc.
//
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted (subject to the limitations in the
// disclaimer below) 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.
//
//  * Neither the name of Pacific Biosciences nor the names of its
//    contributors may be used to endorse or promote products derived
//    from this software without specific prior written permission.
//
// NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE
// GRANTED BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY PACIFIC
// BIOSCIENCES AND ITS 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 PACIFIC BIOSCIENCES OR ITS
// 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.

// Author: David Alexander

#pragma once

#include <boost/random/bernoulli_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/poisson_distribution.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <iostream>
#include <string>
#include <vector>

#include <ConsensusCore/Mutation.hpp>
#include <ConsensusCore/Features.hpp>
#include <ConsensusCore/Quiver/QvEvaluator.hpp>
#include <ConsensusCore/Read.hpp>
#include <ConsensusCore/Utils.hpp>

#include "ParameterSettings.hpp"

using ConsensusCore::Mutation;
using ConsensusCore::MutationType;
using ConsensusCore::QvEvaluator;
using ConsensusCore::QvModelParams;
using ConsensusCore::QvSequenceFeatures;
using ConsensusCore::QvRead;

template<typename RNG>
std::string
RandomSequence(RNG& rng, int length)
{
    const char* bases = "ACGT";
    boost::random::uniform_int_distribution<> indexDist(0, 3);
    std::stringstream ss;
    for (int i = 0; i < length; ++i)
    {
        ss << bases[indexDist(rng)];
    }
    return ss.str();
}

template<typename RNG>
float*
RandomQvArray(RNG& rng, int length)
{
    float* array = new float[length];
    boost::random::uniform_int_distribution<> indexDist(0, 127);
    for (int i = 0; i < length; ++i)
    {
        array[i] = static_cast<float>(indexDist(rng));
    }
    return array;
}

template<typename RNG>
float*
RandomTagArray(RNG& rng, int length)
{
    std::string seq = RandomSequence(rng, length);
    float* array = new float[length];
    for (int i = 0; i < length; ++i)
    {
        array[i] = static_cast<float>(seq[i]);
    }
    return array;
}

template<typename RNG>
int
RandomPoissonDraw(RNG& rng, int mean)
{
    boost::random::poisson_distribution<> dist(mean);
    return dist(rng);
}

template<typename RNG>
bool
RandomBernoulliDraw(RNG& rng, float p)
{
    boost::random::bernoulli_distribution<> dist(p);
    return dist(rng);
}

template<typename RNG>
QvEvaluator
RandomQvEvaluator(RNG& rng, int length)
{
    std::string tpl = RandomSequence(rng, length);

    int readLength = RandomPoissonDraw(rng, length);
    std::string seq = RandomSequence(rng, readLength);

    float* insQv = RandomQvArray(rng, readLength);
    float* subsQv = RandomQvArray(rng, readLength);
    float* delQv = RandomQvArray(rng, readLength);
    float* delTag = RandomTagArray(rng, readLength);
    float* mergeQv = RandomQvArray(rng, readLength);

    QvSequenceFeatures f(seq, insQv, subsQv, delQv, delTag, mergeQv);
    QvRead read(f, "anonymous", "unknown");

    delete[] insQv;
    delete[] subsQv;
    delete[] delQv;
    delete[] delTag;
    delete[] mergeQv;

    bool pinStart = RandomBernoulliDraw(rng, 0.5);
    bool pinEnd = RandomBernoulliDraw(rng, 0.5);
    return QvEvaluator(read, tpl, TestingParams(), pinStart, pinEnd);
}

template<typename RNG>
std::vector<int>
RandomSampleWithoutReplacement(RNG& rng, int n, int k)
{
    // Random sample of k elements from [0..n) without replacement
    std::vector<int> draws;
    boost::random::uniform_int_distribution<> indexDist(0, n - 1);
    while (draws.size() < k) {
        int draw = indexDist(rng);
        if (std::find(draws.begin(), draws.end(), draw) == draws.end())
        {
            draws.push_back(draw);
        }
    }
}

typedef boost::mt19937 Rng;


#include <iostream>
#include <fstream>
#include <mutex>
#include <condition_variable>
#include <queue>
#include <iostream>
#include <zlib.h>
#include <omp.h>
#include <thread>
#include <valarray>
#include <vector>
#include <iomanip>

#include <boost/program_options.hpp>
#include <boost/iostreams/device/mapped_file.hpp>
#include <boost/iostreams/stream.hpp>
#include <boost/asio.hpp>

#include "./include/seq.h"
#include "./include/kmer.h"

using namespace std;

namespace po = boost::program_options;
namespace bio = boost::iostreams;
namespace basio = boost::asio;

mutex mux;
queue<string> reads_queue;
condition_variable condition;
volatile bool terminate_threads;

class ProgressDisplay
{
private:
    int total = 0;
    int progress = 0;
public:
    ProgressDisplay(int total)
    {
        this->total = total;
    }

    void operator++(int)
    {
        this->operator++();
    }

    void operator++()
    {
        this->progress++;
        this->print();
    }

    void end()
    {
        this->progress=this->total;
        cout << "Completed " << fixed << setprecision(2) << 100.00 << "%       " << endl << flush;
    }

    void print()
    {
        float percentage = 100.0 * static_cast<float>(this->progress)/static_cast<float>(this->total);
        cout << "Completed " << fixed << setprecision(2) << percentage << "%             \r" << flush;
    }
};

void off_load_process(string &output, KmerCounter &kc, int &threads)
{
    string seq;
    vector<string> batch;
    ofstream outfs(output, ios::out);
    size_t batch_size;

    while (true)
    {
        {
            unique_lock<mutex> lock(mux);

            while (reads_queue.size() > 0)
            {
                seq = reads_queue.front();
                batch.push_back(seq);
                reads_queue.pop();

                if (batch.size() == 10000)
                {
                    break;
                }
            }
        }

        condition.notify_all();

        if (batch.size() > 0)
        {
            batch_size = batch.size();
            vector<valarray<double>> results(batch_size);
            ostringstream outss;
            outss.precision(6);
            outss << fixed;

#pragma omp parallel for num_threads(threads) schedule(dynamic, 1)
            for (size_t i = 0; i < batch_size; i++)
            {
                results[i] = kc.count_kmers(batch[i]);
            }

            for (auto dvec : results)
            {
                for (size_t j = 0; j < dvec.size(); j++)
                {
                    outss << dvec[j];
                    if (j < dvec.size() - 1)
                    {
                        outss << ' ';
                    }
                }
                outss << '\n';
            }

            outfs << outss.str();
            outss.clear();

            results.clear();
            batch.clear();
        }

        {
            unique_lock<mutex> lock(mux);
            if (terminate_threads && reads_queue.size() == 0)
            {
                break;
            }
        }
    }

    outfs.close();
}

void io_thread(SeqReader &reader)
{
    Seq seq;
    int count = 0;

    while (reader.get_seq(seq))
    {
        {
            unique_lock<mutex> lock(mux);
            condition.wait(lock, [] { return reads_queue.size() < 10000; });
            reads_queue.push(seq.seq_string);
        }
        count++;

        cout << "Loaded Reads " << count << "     \r" << flush;
    }

    cout << endl;

    terminate_threads = true;
}

void run(string &input, string &output, int &ksize, int &threads)
{
    SeqReader reader(input);
    KmerCounter kc(ksize);

    cout << "Counting sequences" << endl;
    size_t total_reads = reader.get_seq_count();
    cout << total_reads <<  " sequences found" << endl;
    size_t per_line_size = kc.kmer_counts_length * (8 + 1);
    size_t estimated_file_size = total_reads * per_line_size; // space after each value + newline (9 ASCII chars per value)

    bio::mapped_file_params params;
    params.path = output;
    params.new_file_size = estimated_file_size;
    params.flags = bio::mapped_file::mapmode::readwrite;
    bio::mapped_file_sink mmout(params);

    asio::thread_pool pool(threads);
    ProgressDisplay pd(total_reads);
    mutex reader_mux;

    for (int _ = 0; _ < threads * 5; _++)
    {
        asio::post(pool, [&reader_mux, &estimated_file_size, &reader, &pd, &mmout, &kc, &per_line_size]() {
            bool has_read = true;
            Seq seq;
            auto sptr = mmout.begin();

            while (true)
            {
                {
                    unique_lock<mutex> lock(reader_mux);
                    has_read = reader.get_seq(seq);
                    pd++;
                }

                if (has_read)
                {
                    // process the read
                    valarray<double> dvec = kc.count_kmers(seq.seq_string);
                    ostringstream outss;
                    outss.precision(6);
                    outss << fixed;

                    for (size_t j = 0; j < dvec.size(); j++)
                    {
                        outss << dvec[j];
                        if (j < dvec.size() - 1)
                        {
                            outss << ' ';
                        }
                    }
                    outss << '\n';

                    memcpy(sptr + seq.seq_id * per_line_size, outss.str().c_str(), outss.str().size());
                }
                else
                {
                    break;
                }
            }
        });
    }
    pool.join();
    mmout.close();
    pd.end();
}

int main(int ac, char **av)
{
    int ksize, threads;
    string input, output;

    po::options_description desc("Seq2Vec fast sequence vectorization");

    desc.add_options()("help,h", "show help message");
    desc.add_options()("file,f", po::value<string>(&input)->required(), "input file path");
    desc.add_options()("output,o", po::value<string>(&output)->required(), "output vectors path");
    desc.add_options()("k-size,k", po::value<int>(&ksize)->default_value(3), "set k-mer size");
    desc.add_options()("threads,t", po::value<int>(&threads)->default_value(8), "set thread count");

    po::variables_map vm;
    po::store(po::parse_command_line(ac, av, desc), vm);

    if (vm.count("help") || ac == 1)
    {
        cout << desc << "\n";
        return 1;
    }

    po::notify(vm);

    cout << "Starting Seq2Vec sequence vectorization" << endl;
    run(input, output, ksize, threads);

    return 0;
}


/*
 * beam.cc
 *
 *  Copyright (C) 2013 Diamond Light Source
 *
 *  Author: James Parkhurst
 *
 *  This code is distributed under the BSD license, a copy of which is
 *  included in the root directory of this package.
 */
#include <boost/python.hpp>
#include <boost/python/def.hpp>
#include <string>
#include <sstream>
#include <scitbx/constants.h>
#include <dxtbx/model/beam.h>
#include <dxtbx/model/boost_python/to_from_dict.h>
#include <scitbx/array_family/boost_python/flex_wrapper.h>

namespace dxtbx { namespace model { namespace boost_python {

  using namespace boost::python;
  using scitbx::deg_as_rad;
  using scitbx::rad_as_deg;

  std::string beam_to_string(const Beam &beam) {
    std::stringstream ss;
    ss << beam;
    return ss.str();
  }

  struct BeamPickleSuite : boost::python::pickle_suite {
    static boost::python::tuple getinitargs(const Beam &obj) {
      return boost::python::make_tuple(obj.get_sample_to_source_direction(),
                                       obj.get_wavelength(),
                                       obj.get_divergence(),
                                       obj.get_sigma_divergence(),
                                       obj.get_polarization_normal(),
                                       obj.get_polarization_fraction(),
                                       obj.get_flux(),
                                       obj.get_transmission());
    }

    static boost::python::tuple getstate(boost::python::object obj) {
      const Beam &beam = boost::python::extract<const Beam &>(obj)();
      return boost::python::make_tuple(obj.attr("__dict__"),
                                       beam.get_s0_at_scan_points());
    }

    static void setstate(boost::python::object obj, boost::python::tuple state) {
      Beam &beam = boost::python::extract<Beam &>(obj)();
      DXTBX_ASSERT(boost::python::len(state) == 2);

      // restore the object's __dict__
      boost::python::dict d =
        boost::python::extract<boost::python::dict>(obj.attr("__dict__"))();
      d.update(state[0]);

      // restore the internal state of the C++ object
      scitbx::af::const_ref<vec3<double> > s0_list =
        boost::python::extract<scitbx::af::const_ref<vec3<double> > >(state[1]);
      beam.set_s0_at_scan_points(s0_list);
    }

    static bool getstate_manages_dict() {
      return true;
    }
  };

  static Beam *make_beam(vec3<double> sample_to_source,
                         double wavelength,
                         double divergence,
                         double sigma_divergence,
                         bool deg) {
    Beam *beam = NULL;
    if (deg) {
      beam = new Beam(sample_to_source,
                      wavelength,
                      deg_as_rad(divergence),
                      deg_as_rad(sigma_divergence));
    } else {
      beam = new Beam(sample_to_source, wavelength, divergence, sigma_divergence);
    }
    return beam;
  }

  static Beam *make_beam_w_s0(vec3<double> s0,
                              double divergence,
                              double sigma_divergence,
                              bool deg) {
    Beam *beam = NULL;
    if (deg) {
      beam = new Beam(s0, deg_as_rad(divergence), deg_as_rad(sigma_divergence));
    } else {
      beam = new Beam(s0, divergence, sigma_divergence);
    }
    return beam;
  }

  static Beam *make_beam_w_all(vec3<double> sample_to_source,
                               double wavelength,
                               double divergence,
                               double sigma_divergence,
                               vec3<double> polarization_normal,
                               double polarization_fraction,
                               double flux,
                               double transmission,
                               bool deg) {
    Beam *beam = NULL;
    if (deg) {
      beam = new Beam(sample_to_source,
                      wavelength,
                      deg_as_rad(divergence),
                      deg_as_rad(sigma_divergence),
                      polarization_normal,
                      polarization_fraction,
                      flux,
                      transmission);
    } else {
      beam = new Beam(sample_to_source,
                      wavelength,
                      divergence,
                      sigma_divergence,
                      polarization_normal,
                      polarization_fraction,
                      flux,
                      transmission);
    }
    return beam;
  }

  static double get_divergence(const Beam &beam, bool deg) {
    double divergence = beam.get_divergence();
    return deg ? rad_as_deg(divergence) : divergence;
  }

  static double get_sigma_divergence(const Beam &beam, bool deg) {
    double sigma_divergence = beam.get_sigma_divergence();
    return deg ? rad_as_deg(sigma_divergence) : sigma_divergence;
  }

  static void set_divergence(Beam &beam, double divergence, bool deg) {
    beam.set_divergence(deg ? deg_as_rad(divergence) : divergence);
  }

  static void set_sigma_divergence(Beam &beam, double sigma_divergence, bool deg) {
    beam.set_sigma_divergence(deg ? deg_as_rad(sigma_divergence) : sigma_divergence);
  }

  static void rotate_around_origin(Beam &beam,
                                   vec3<double> axis,
                                   double angle,
                                   bool deg) {
    double angle_rad = deg ? deg_as_rad(angle) : angle;
    beam.rotate_around_origin(axis, angle_rad);
  }

  static void Beam_set_s0_at_scan_points_from_tuple(Beam &beam,
                                                    boost::python::tuple l) {
    scitbx::af::shared<vec3<double> > s0_list;
    for (std::size_t i = 0; i < boost::python::len(l); ++i) {
      vec3<double> s0 = boost::python::extract<vec3<double> >(l[i]);
      s0_list.push_back(s0);
    }
    beam.set_s0_at_scan_points(s0_list.const_ref());
  }

  static void Beam_set_s0_at_scan_points_from_list(Beam &beam, boost::python::list l) {
    scitbx::af::shared<vec3<double> > s0_list;
    for (std::size_t i = 0; i < boost::python::len(l); ++i) {
      vec3<double> s0 = boost::python::extract<vec3<double> >(l[i]);
      s0_list.push_back(s0);
    }
    beam.set_s0_at_scan_points(s0_list.const_ref());
  }

  template <>
  boost::python::dict to_dict<Beam>(const Beam &obj) {
    boost::python::dict result;
    result["direction"] = obj.get_sample_to_source_direction();
    result["wavelength"] = obj.get_wavelength();
    result["divergence"] = rad_as_deg(obj.get_divergence());
    result["sigma_divergence"] = rad_as_deg(obj.get_sigma_divergence());
    result["polarization_normal"] = obj.get_polarization_normal();
    result["polarization_fraction"] = obj.get_polarization_fraction();
    result["flux"] = obj.get_flux();
    result["transmission"] = obj.get_transmission();
    if (obj.get_num_scan_points() > 0) {
      boost::python::list l;
      scitbx::af::shared<vec3<double> > s0_at_scan_points = obj.get_s0_at_scan_points();
      for (scitbx::af::shared<vec3<double> >::iterator it = s0_at_scan_points.begin();
           it != s0_at_scan_points.end();
           ++it) {
        l.append(boost::python::make_tuple((*it)[0], (*it)[1], (*it)[2]));
      }
      result["s0_at_scan_points"] = l;
    }
    return result;
  }

  template <>
  Beam *from_dict<Beam>(boost::python::dict obj) {
    Beam *b = new Beam(
      boost::python::extract<vec3<double> >(obj["direction"]),
      boost::python::extract<double>(obj["wavelength"]),
      deg_as_rad(boost::python::extract<double>(obj.get("divergence", 0.0))),
      deg_as_rad(boost::python::extract<double>(obj.get("sigma_divergence", 0.0))),
      boost::python::extract<vec3<double> >(
        obj.get("polarization_normal", vec3<double>(0.0, 1.0, 0.0))),
      boost::python::extract<double>(obj.get("polarization_fraction", 0.999)),
      boost::python::extract<double>(obj.get("flux", 0)),
      boost::python::extract<double>(obj.get("transmission", 1)));
    if (obj.has_key("s0_at_scan_points")) {
      boost::python::list s0_at_scan_points =
        boost::python::extract<boost::python::list>(obj["s0_at_scan_points"]);
      Beam_set_s0_at_scan_points_from_list(*b, s0_at_scan_points);
    }
    return b;
  }

  void export_beam() {
    // Export BeamBase
    class_<BeamBase, boost::noncopyable>("BeamBase", no_init)
      .def("get_sample_to_source_direction", &BeamBase::get_sample_to_source_direction)
      .def("set_direction", &BeamBase::set_direction)
      .def("get_wavelength", &BeamBase::get_wavelength)
      .def("set_wavelength", &BeamBase::set_wavelength)
      .def("get_s0", &BeamBase::get_s0)
      .def("set_s0", &BeamBase::set_s0)
      .def("get_unit_s0", &BeamBase::get_unit_s0)
      .def("set_unit_s0", &BeamBase::set_unit_s0)
      .def("get_divergence", &get_divergence, (arg("deg") = true))
      .def("set_divergence", &set_divergence, (arg("divergence"), arg("deg") = true))
      .def("get_sigma_divergence", &get_sigma_divergence, (arg("deg") = true))
      .def("set_sigma_divergence",
           &set_sigma_divergence,
           (arg("sigma_divergence"), arg("deg") = true))
      .def("get_polarization_normal", &BeamBase::get_polarization_normal)
      .def("set_polarization_normal", &BeamBase::set_polarization_normal)
      .def("get_polarization_fraction", &BeamBase::get_polarization_fraction)
      .def("set_polarization_fraction", &BeamBase::set_polarization_fraction)
      .def("get_flux", &BeamBase::get_flux)
      .def("set_flux", &BeamBase::set_flux)
      .def("get_transmission", &BeamBase::get_transmission)
      .def("set_transmission", &BeamBase::set_transmission)
      .add_property("num_scan_points", &BeamBase::get_num_scan_points)
      .def("get_num_scan_points", &BeamBase::get_num_scan_points)
      .def("set_s0_at_scan_points", &BeamBase::set_s0_at_scan_points)
      .def("set_s0_at_scan_points", &Beam_set_s0_at_scan_points_from_tuple)
      .def("set_s0_at_scan_points", &Beam_set_s0_at_scan_points_from_list)
      .def("get_s0_at_scan_points", &BeamBase::get_s0_at_scan_points)
      .def("get_s0_at_scan_point", &BeamBase::get_s0_at_scan_point)
      .def("reset_scan_points", &BeamBase::reset_scan_points)
      .def("rotate_around_origin",
           &rotate_around_origin,
           (arg("axis"), arg("angle"), arg("deg") = true))
      .def("__eq__", &BeamBase::operator==)
      .def("__ne__", &BeamBase::operator!=)
      .def("is_similar_to",
           &BeamBase::is_similar_to,
           (arg("other"),
            arg("wavelength_tolerance") = 1e-6,
            arg("direction_tolerance") = 1e-6,
            arg("polarization_normal_tolerance") = 1e-6,
            arg("polarization_fraction_tolerance") = 1e-6));

    // Export Beam : BeamBase
    class_<Beam, boost::shared_ptr<Beam>, bases<BeamBase> >("Beam")
      .def(init<const Beam &>())
      .def(init<vec3<double>, double>((arg("direction"), arg("wavelength"))))
      .def(init<vec3<double> >((arg("s0"))))
      .def("__init__",
           make_constructor(&make_beam,
                            default_call_policies(),
                            (arg("direction"),
                             arg("wavelength"),
                             arg("divergence"),
                             arg("sigma_divergence"),
                             arg("deg") = true)))
      .def(
        "__init__",
        make_constructor(
          &make_beam_w_s0,
          default_call_policies(),
          (arg("s0"), arg("divergence"), arg("sigma_divergence"), arg("deg") = true)))
      .def("__init__",
           make_constructor(&make_beam_w_all,
                            default_call_policies(),
                            (arg("direction"),
                             arg("wavelength"),
                             arg("divergence"),
                             arg("sigma_divergence"),
                             arg("polarization_normal"),
                             arg("polarization_fraction"),
                             arg("flux"),
                             arg("transmission"),
                             arg("deg") = true)))
      .def("__str__", &beam_to_string)
      .def("to_dict", &to_dict<Beam>)
      .def("from_dict", &from_dict<Beam>, return_value_policy<manage_new_object>())
      .staticmethod("from_dict")
      .def_pickle(BeamPickleSuite());

    scitbx::af::boost_python::flex_wrapper<Beam>::plain("flex_Beam");
  }

}}}  // namespace dxtbx::model::boost_python


#ifndef HERMIT_SPIRIT_QI_RFC1123_DATE_HPP
#define HERMIT_SPIRIT_QI_RFC1123_DATE_HPP

#include <iostream>
#include <vector>
#include <string>
#include <boost/date_time/date_defs.hpp>
#include <boost/date_time/gregorian/gregorian.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/date_time/gregorian_calendar.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix.hpp>

namespace hermit {
  namespace spirit {
    namespace qi {
      template <typename InputIterator>
        class rfc1123_date :
          public boost::spirit::qi::grammar< InputIterator, boost::posix_time::ptime() > {
            public:
              rfc1123_date() : rfc1123_date::base_type( root ) {
                namespace qi = boost::spirit::qi;
                namespace dt = boost::date_time;
                namespace phx = boost::phoenix;
                root = ( wkday >> ',' >> qi::omit[ +qi::standard::space ] >>
                         date >> qi::omit[ +qi::standard::space ] >> time >> qi::omit[ +qi::standard::space ] >>
                         zone )[
                  qi::_pass = qi::_1 == phx::bind( &boost::gregorian::date::day_of_week, &qi::_2 ),
                  qi::_val = phx::construct< boost::posix_time::ptime >( qi::_2,
                    qi::_3 + qi::_4
                  )
                ];
                date = ( dec2_p >> qi::omit[ +qi::standard::space ] >> month >> qi::omit[ +qi::standard::space ] >> dec4_p )[
                  qi::_pass = qi::_1 <= phx::bind( boost::gregorian::gregorian_calendar::end_of_month_day, qi::_3, qi::_2 ),
                  qi::_val = phx::construct< boost::gregorian::date >( qi::_3, qi::_2, qi::_1 )
                ];
                time = ( dec2_p >> ':' >> dec2_p >> ':' >> dec2_p )[
                  qi::_pass = qi::_1 < 24u && qi::_2 < 60u && qi::_3 < 60u,
                  qi::_val = phx::construct< boost::posix_time::time_duration >( qi::_1, qi::_2, qi::_3 )
                ];
                wkday.add( "Mon", dt::Monday )( "Tue", dt::Tuesday )( "Wed", dt::Wednesday )( "Thu", dt::Thursday )
                         ( "Fri", dt::Friday )( "Sat", dt::Saturday )( "Sun", dt::Sunday );
                month.add( "Jan", dt::Jan )( "Feb", dt::Feb )( "Mar", dt::Mar )( "Apr", dt::Apr )
                         ( "May", dt::May )( "Jun", dt::Jun )( "Jul", dt::Jul )( "Aug", dt::Aug )
                         ( "Sep", dt::Sep )( "Oct", dt::Oct )( "Nov", dt::Nov )( "Dec", dt::Dec );
                zone_name.add( "UT", boost::posix_time::time_duration( 0, 0, 0 ) )
                             ( "GMT", boost::posix_time::time_duration( 0, 0, 0 ) )
                             ( "EST", boost::posix_time::time_duration( -5, 0, 0 ) )
                             ( "EDT", boost::posix_time::time_duration( -4, 0, 0 ) )
                             ( "CST", boost::posix_time::time_duration( -6, 0, 0 ) )
                             ( "CDT", boost::posix_time::time_duration( -5, 0, 0 ) )
                             ( "MST", boost::posix_time::time_duration( -7, 0, 0 ) )
                             ( "MDT", boost::posix_time::time_duration( -6, 0, 0 ) )
                             ( "PST", boost::posix_time::time_duration( -8, 0, 0 ) )
                             ( "PDT", boost::posix_time::time_duration( -7, 0, 0 ) )
                             ( "Z", boost::posix_time::time_duration( 0, 0, 0 ) )
                             ( "A", boost::posix_time::time_duration( -1, 0, 0 ) )
                             ( "B", boost::posix_time::time_duration( -2, 0, 0 ) )
                             ( "C", boost::posix_time::time_duration( -3, 0, 0 ) )
                             ( "D", boost::posix_time::time_duration( -4, 0, 0 ) )
                             ( "E", boost::posix_time::time_duration( -5, 0, 0 ) )
                             ( "F", boost::posix_time::time_duration( -6, 0, 0 ) )
                             ( "G", boost::posix_time::time_duration( -7, 0, 0 ) )
                             ( "H", boost::posix_time::time_duration( -8, 0, 0 ) )
                             ( "I", boost::posix_time::time_duration( -9, 0, 0 ) )
                             ( "K", boost::posix_time::time_duration( -10, 0, 0 ) )
                             ( "L", boost::posix_time::time_duration( -11, 0, 0 ) )
                             ( "M", boost::posix_time::time_duration( -12, 0, 0 ) )
                             ( "N", boost::posix_time::time_duration( 1, 0, 0 ) )
                             ( "O", boost::posix_time::time_duration( 2, 0, 0 ) )
                             ( "P", boost::posix_time::time_duration( 3, 0, 0 ) )
                             ( "Q", boost::posix_time::time_duration( 4, 0, 0 ) )
                             ( "R", boost::posix_time::time_duration( 5, 0, 0 ) )
                             ( "S", boost::posix_time::time_duration( 6, 0, 0 ) )
                             ( "T", boost::posix_time::time_duration( 7, 0, 0 ) )
                             ( "U", boost::posix_time::time_duration( 8, 0, 0 ) )
                             ( "V", boost::posix_time::time_duration( 9, 0, 0 ) )
                             ( "W", boost::posix_time::time_duration( 10, 0, 0 ) )
                             ( "X", boost::posix_time::time_duration( 11, 0, 0 ) )
                             ( "Y", boost::posix_time::time_duration( 12, 0, 0 ) );
                zone_pos = ( '+' >> qi::omit[ *qi::standard::space ] >> dece2_p >> dece2_p )[
                  qi::_pass = qi::_2 < 60u,
                  qi::_val = phx::construct< boost::posix_time::time_duration >( qi::_1, qi::_2, 0 )
                ];
                zone_neg = ( '-' >> qi::omit[ *qi::standard::space ] >> dece2_p >> dece2_p )[
                  qi::_pass = qi::_2 < 60u,
                  qi::_val = -phx::construct< boost::posix_time::time_duration >( qi::_1, qi::_2, 0 )
                ];
                zone = zone_name|zone_pos|zone_neg;
              }
            private:
              boost::spirit::qi::uint_parser<unsigned int, 10, 2, 2> dece2_p;
              boost::spirit::qi::uint_parser<unsigned int, 10, 1, 2> dec2_p;
              boost::spirit::qi::uint_parser<unsigned int, 10, 1, 4> dec4_p;
              boost::spirit::qi::rule< InputIterator, boost::posix_time::ptime() > root;
              boost::spirit::qi::rule< InputIterator, boost::gregorian::date() > date;
              boost::spirit::qi::rule< InputIterator, boost::posix_time::time_duration() > time;
              boost::spirit::qi::rule< InputIterator, boost::posix_time::time_duration() > zone;
              boost::spirit::qi::rule< InputIterator, boost::posix_time::time_duration() > zone_pos;
              boost::spirit::qi::rule< InputIterator, boost::posix_time::time_duration() > zone_neg;
              boost::spirit::qi::symbols< char, boost::date_time::weekdays > wkday;
              boost::spirit::qi::symbols< char, boost::posix_time::time_duration > zone_name;
              boost::spirit::qi::symbols< char, boost::date_time::months_of_year > month;
      };
    }
  }
}

#endif


#ifndef _RYSQ_CUDA_HPP_
#define _RYSQ_CUDA_HPP_

#include <vector>

// #include <boost/numeric/ublas/matrix.hpp>
#include "rysq/core.hpp"
#include "rysq/eri.hpp"
#include "rysq/fock.hpp"

#include "rysq/cuda-memory.hpp"
#include "rysq/cuda-matrix.hpp"

namespace rysq {
    namespace cuda {

	std::vector<int> get_devices();
	void set_device(size_t device = 0);
	void thread_exit();

	struct thread {
	    thread(size_t device = 0) { set_device(device); }
	    ~thread() { thread_exit(); }
	};

	namespace detail {
	    class Eri;
	    class Fock;
	}

	typedef cuda::array<Center> Centers;

	struct Quartets : cuda::array<Int4> {
	    Quartets() {
		min.assign(1);
		max.assign(0);
	    }
	    template<typename T>
	    explicit Quartets(T begin, T end)
		: vector_(begin, end) {
		initialize();
	    }
	    explicit Quartets(const std::vector<Int4> &centers)
		: vector_(centers) {
		initialize();
	    }
	    void assign(const std::vector<Int4> &centers) {
		vector_ = centers;
		initialize();
	    }
	    typedef std::vector<Int4>::const_iterator const_iterator;
	    const_iterator begin() const { return vector_.begin(); }
	    const_iterator end() const { return vector_.end(); }
	    boost::array<int,4> max, min;
	private:
	    typedef cuda::array<Int4> base_type;
	    std::vector<Int4> vector_;
	    void initialize() {
		max.assign(0);
		min.assign(1);
		if (!vector_ . empty()) {
		    max = vector_.front();
		    min = max;
		}
		typedef std::vector<Int4>::const_iterator iterator;
		for (iterator it = vector_.begin(); it < vector_.end(); ++ it) {
		    // std:: cout << *it << std::endl;
		    for (int i = 0; i < 4; ++i) {
			max[i] = std::max(max[i], (*it)[i]);
			min[i] = std::min(min[i], (*it)[i]);
		    }
		}
		base_type::assign(vector_);
	    }
	};

	struct Eri {
	    Eri(const rysq::Quartet<rysq::Shell> &shells);
	    ~Eri();
	    operator bool() const { return kernel_ != NULL; }
	    void operator()(const cuda::Centers &centers,
			    const cuda::Quartets &quartets,
			    cuda::array <double> &Eri,
			    const Parameters &parameters);
	private:
	    detail::Eri *kernel_;
	};

	typedef cuda::block_matrix<double> density_matrix;
	typedef cuda::block_matrix<double> fock_matrix;

	struct Fock {
	    typedef hf::matrix_ptr_set < fock_matrix> fock_matrix_set;
	    typedef hf::matrix_ptr_set < density_matrix> density_matrix_set;
	    Fock(const rysq::Quartet<rysq::Shell> &shells);
	    ~Fock();
	    operator bool() const { return kernel_ != NULL; }
	    // operator bool() const { return  kernel_ != NULL; }
	    void operator()(const cuda::Centers &centers,
			    const cuda::Quartets &quartets,
			    density_matrix_set D, fock_matrix_set F,
			    const Parameters &parameters);
	private:
	    detail::Fock *kernel_;
	    size_t block_[4];
	};

    }
}

#endif /* _RYSQ_CUDA_HPP_ */


#ifndef MAC_TIME_TRACKER_TIME_HPP
#define MAC_TIME_TRACKER_TIME_HPP

#include <chrono>
#include <ctime>   // for std::localtime()
#include <iomanip> // for std::put_time()
#include <iostream>
#include <string>

#include <boost/lexical_cast.hpp>

#include <mac_time_tracker/io.hpp>

namespace mac_time_tracker {

class Time : public std::chrono::system_clock::time_point, public Writable {
private:
  using Base = std::chrono::system_clock::time_point;

public:
  // Constructors
  using Base::Base;
  Time(const Base &base) : Base(base) {}
  Time(Base &&base) : Base(base) {}
  template <class Duration>
  Time(const std::chrono::time_point<clock, Duration> &t)
      : Base(std::chrono::time_point_cast<Base>(t)) {}

  // A shortcut to Time::clock::now()
  static Time now() { return clock::now(); }

  using Writable::toStr;
  std::string toStr(const std::string &fmt) const {
    const std::time_t t = clock::to_time_t(*this);
    return boost::lexical_cast<std::string>(std::put_time(std::localtime(&t), fmt.c_str()));
  }

  static std::string defaultFormat() {
    // a format like "Y-M-D H:M:S", which is based on ISO 8601
    return "%F %T";
  }

private:
  virtual void write(std::ostream &os) const override { os << toStr(defaultFormat()); }
};

} // namespace mac_time_tracker

#endif


#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/args.hpp>
#include <boost/python/overloads.hpp>
#include <mmtbx/building/loop_closure/ccd.h>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/copy_non_const_reference.hpp>
#include <boost/python/copy_const_reference.hpp>

namespace mmtbx { namespace building { namespace loop_closure {
namespace {

  void init_module()
  {
    using namespace boost::python;
    typedef return_value_policy<return_by_value> rbv;
    typedef return_value_policy<copy_const_reference> ccr;
    typedef return_value_policy<copy_non_const_reference> cncr;
    typedef default_call_policies dcp;

    class_<ccd_cpp> ("ccd_cpp", no_init)
      .def(init< scitbx::af::tiny<scitbx::vec3<double>, 3 >,
        iotbx::pdb::hierarchy::root & ,
        scitbx::af::tiny<size_t, 3>,
        const int&,
        const double&>(
          (arg("fixed_ref_atoms"),
           arg("moving_h"),
           arg("moving_ref_atoms_iseqs"),
           arg("max_number_of_iterations")=500,
           arg("needed_rmsd")=0.1)))
      .def("_find_angle", &ccd_cpp::_find_angle,
          (arg("axis_point_1"),
           arg("axis_point_2")))
      .def("_modify_angle", &ccd_cpp::_modify_angle,
          (arg("angle")))

      .add_property("early_exit",
          make_getter(&ccd_cpp::early_exit, rbv()),
          make_setter(&ccd_cpp::early_exit, dcp()))
      .add_property("resulting_rmsd",
          make_getter(&ccd_cpp::resulting_rmsd, rbv()),
          make_setter(&ccd_cpp::resulting_rmsd, dcp()))
      .add_property("needed_rmsd",
          make_getter(&ccd_cpp::needed_rmsd, rbv()),
          make_setter(&ccd_cpp::needed_rmsd, dcp()))
      .add_property("max_number_of_iterations",
          make_getter(&ccd_cpp::max_number_of_iterations, rbv()),
          make_setter(&ccd_cpp::max_number_of_iterations, dcp()))
      .add_property("r",
          make_getter(&ccd_cpp::r, rbv()),
          make_setter(&ccd_cpp::r, dcp()))
      .add_property("convergence_diff",
          make_getter(&ccd_cpp::convergence_diff, rbv()),
          make_setter(&ccd_cpp::convergence_diff, dcp()))
      .add_property("fixed_ref_atoms",
          make_getter(&ccd_cpp::fixed_ref_atoms, rbv()))
      .add_property("moving_ref_atoms_iseqs",
          make_getter(&ccd_cpp::moving_ref_atoms_iseqs, rbv()))
      .add_property("moving_h", make_getter(&ccd_cpp::moving_h, cncr()))
    ;
  }

} // namespace <anonymous>
}}} // namespace mmtbx::building::loop_closure

BOOST_PYTHON_MODULE(mmtbx_building_loop_closure_ext)
{
  mmtbx::building::loop_closure::init_module();
}