// Copyright 2015-2022 The ALMA Project Developers
//
// 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.

/// @file
/// Definitions corresponding to qpoint_grid.hpp.

#include <map>
#include <unordered_set>
#if BOOST_VERSION >= 106700
#include <boost/container_hash/hash.hpp>
#else
#include <boost/functional/hash.hpp>
#endif
#include <utilities.hpp>
#include <exceptions.hpp>
#include <qpoint_grid.hpp>

namespace std {
/// Trivial implementation of std::hash for arrays,
/// required to create an unordered_set of arrays.
template <typename T, std::size_t S> struct hash<std::array<T, S>> {
    std::size_t operator()(const array<T, S>& key) const {
        hash<T> backend;
        std::size_t nruter = 0;

        for (auto& e : key)
            boost::hash_combine(nruter, backend(e));
        return nruter;
    }
};
} // namespace std

namespace alma {
Gamma_grid::Gamma_grid(const Crystal_structure& poscar,
                       const Symmetry_operations& symms,
                       const Harmonic_ifcs& force_constants,
                       int _na,
                       int _nb,
                       int _nc)
    : na(_na), nb(_nb), nc(_nc), nqpoints(_na * _nb * _nc),
      rlattvec(poscar.rlattvec),
      dq(rlattvec.array().rowwise() /
         Eigen::Map<Eigen::Array3i>(
             std::array<int, 3>({{_na, _nb, _nc}}).data())
             .cast<double>()
             .transpose()) {
    if (std::min({_na, _nb, _nc}) <= 0)
        throw value_error("all grid dimensions must be positive");
    this->initialize_cpos();
    this->fill_equivalences(symms);
    this->fill_map(symms);
    Dynamical_matrix_builder builder(poscar, symms, force_constants);
    boost::mpi::communicator world;
    auto my_spectrum = this->compute_my_spectrum(builder, symms, world);
    std::vector<decltype(my_spectrum)> all_spectra;
    boost::mpi::all_gather(world, my_spectrum, all_spectra);

    for (auto& s : all_spectra)
        this->spectrum.insert(this->spectrum.end(), s.begin(), s.end());
}

Gamma_grid::Gamma_grid(const Crystal_structure& poscar,
                       const Symmetry_operations& symms,
                       const Harmonic_ifcs& force_constants,
                       const Dielectric_parameters& born,
                       int _na,
                       int _nb,
                       int _nc)
    : na(_na), nb(_nb), nc(_nc), nqpoints(_na * _nb * _nc),
      rlattvec(poscar.rlattvec),
      dq(rlattvec.array().rowwise() /
         Eigen::Map<Eigen::Array3i>(
             std::array<int, 3>({{_na, _nb, _nc}}).data())
             .cast<double>()
             .transpose()) {
    if (std::min({_na, _nb, _nc}) <= 0)
        throw value_error("all grid dimensions must be positive");
    this->initialize_cpos();
    this->fill_equivalences(symms);
    this->fill_map(symms);
    Dynamical_matrix_builder builder(poscar, symms, force_constants, born);
    boost::mpi::communicator world;
    auto my_spectrum = this->compute_my_spectrum(builder, symms, world);
    std::vector<decltype(my_spectrum)> all_spectra;
    boost::mpi::all_gather(world, my_spectrum, all_spectra);

    for (auto& s : all_spectra)
        this->spectrum.insert(this->spectrum.end(), s.begin(), s.end());
}

void Gamma_grid::initialize_cpos() {
    this->cpos.resize(3, this->nqpoints);
    std::size_t iq = 0;

    for (auto ia = 0; ia < this->na; ++ia)
        for (auto ib = 0; ib < this->nb; ++ib)
            for (auto ic = 0; ic < this->nc; ++ic) {
                this->cpos(0, iq) = ia;
                this->cpos(1, iq) = ib;
                this->cpos(2, iq) = ic;
                ++iq;
            }
    this->cpos.row(0) /= this->na;
    this->cpos.row(1) /= this->nb;
    this->cpos.row(2) /= this->nc;
    this->cpos = this->rlattvec * this->cpos;
}

std::vector<Spectrum_at_point> Gamma_grid::compute_my_spectrum(
    const Dynamical_matrix_builder& factory,
    const Symmetry_operations& symms,
    const boost::mpi::communicator& communicator) {
    auto nprocs = communicator.size();
    auto my_id = communicator.rank();
    auto limits = my_jobs(this->nqpoints, nprocs, my_id);

    std::vector<Spectrum_at_point> my_spectrum;
    my_spectrum.reserve(limits[1] - limits[0]);

    // Compute the spectrum for each q point assigned to this
    // MPI process.
    for (auto iq = limits[0]; iq < limits[1]; ++iq) {
        my_spectrum.emplace_back(*factory.get_spectrum(this->cpos.col(iq)));
    }
    // Symmetrize the group velocities at each point.
    for (auto iq = limits[0]; iq < limits[1]; ++iq) {
        my_spectrum[iq - limits[0]].vg =
            this->copy_symmetry(
                    iq, symms, my_spectrum[iq - limits[0]].vg.matrix())
                .array();
    }

    return my_spectrum;
}

void Gamma_grid::fill_map(const Symmetry_operations& symms) {
    auto nops = symms.get_nsym();
    Eigen::Vector3d q;
    Eigen::Vector3d newq;
    Eigen::Vector3i intq;

    for (std::size_t iq = 0; iq < this->nqpoints; ++iq) {
        // A value of nqpoints in the this->symmetry map
        // signals an invalid symmetry operation.
        std::vector<std::size_t> images(2 * nops, this->nqpoints);
        auto indices = this->one_to_three(iq);
        q << indices[0] / static_cast<double>(this->na),
            indices[1] / static_cast<double>(this->nb),
            indices[2] / static_cast<double>(this->nc);

        // Try all the symmetry operations on each q point.
        for (decltype(nops) iop = 0; iop < nops; ++iop) {
            newq = symms.rotate_q(q, iop);
            newq(0) *= this->na;
            newq(1) *= this->nb;
            newq(2) *= this->nc;

            for (auto i = 0; i < 3; ++i)
                intq(i) = static_cast<int>(std::round(newq(i)));

            // Discard an operation if it does not map the q point
            // onto another valid q point. Some symmetry operations
            // may be incompatible with the q-point grid.
            if (!almost_equal(0., (newq - intq.cast<double>()).squaredNorm()))
                continue;
            // Compute the index of the q point obtained as the
            // image of q through the symmetry operation, as well
            // as the index of the q point obtained through time
            // reversal symmetry. Append both to the images vector.
            images[2 * iop] = this->three_to_one({{intq(0), intq(1), intq(2)}});
            images[2 * iop + 1] =
                this->three_to_one({{-intq(0), -intq(1), -intq(2)}});
        }
        this->symmetry_map.emplace_back(images);
    }
}

void Gamma_grid::fill_equivalences(const Symmetry_operations& symms) {
    auto nops = symms.get_nsym();
    Eigen::Vector3d q;
    Eigen::Vector3d newq;
    Eigen::Vector3i intq;

    this->equivalences.emplace_back(std::vector<std::size_t>{0});

    // For each q point.
    for (std::size_t iq = 1; iq < this->nqpoints; ++iq) {
        auto indices = this->one_to_three(iq);
        q << indices[0] / static_cast<double>(this->na),
            indices[1] / static_cast<double>(this->nb),
            indices[2] / static_cast<double>(this->nc);
        // Try all the symmetry operations.
        bool found = false;

        for (decltype(nops) iop = 0; iop < nops; ++iop) {
            newq = symms.rotate_q(q, iop);
            newq(0) *= this->na;
            newq(1) *= this->nb;
            newq(2) *= this->nc;

            for (auto i = 0; i < 3; ++i)
                intq(i) = static_cast<int>(std::round(newq(i)));

            // Discard an operation if it does not map the q point
            // onto another valid q point. Some symmetry operations
            // may be incompatible with the q-point grid.
            if (!almost_equal(0., (newq - intq.cast<double>()).squaredNorm()))
                continue;
            // Compute the index of the q point obtained as the
            // image of q through the symmetry operation.
            auto candidate1 = this->three_to_one({{intq(0), intq(1), intq(2)}});
            // And the index of the q point obtained through time
            // reversal symmetry.
            auto candidate2 =
                this->three_to_one({{-intq(0), -intq(1), -intq(2)}});

            // If either of candidate1 or candidate2 are the first
            // elements of an existing equivalence class, add iq
            // to that class.
            for (auto& c : this->equivalences)
                if ((candidate1 == c[0]) || (candidate2 == c[0])) {
                    c.emplace_back(iq);
                    found = true;
                    break;
                }

            if (found)
                break;
        }

        // If iq cannot be assigned to any existing equivalence class,
        // start a new one.
        if (!found) {
            auto size = this->equivalences.size();
            this->equivalences.resize(size + 1);
            this->equivalences[size].emplace_back(iq);
        }
    }

    this->fill_parentlookup();
}

void Gamma_grid::fill_parentlookup() {
    this->parentlookup.resize(this->nqpoints);

    for (std::size_t nclass = 0; nclass < this->equivalences.size(); nclass++) {
        for (std::size_t nmember = 0;
             nmember < this->equivalences[nclass].size();
             nmember++) {
            this->parentlookup.at(this->equivalences[nclass].at(nmember)) =
                this->equivalences[nclass].at(0);
        }
    }
}

std::size_t Gamma_grid::getParentIdx(std::size_t iq) const {
    if (iq >= this->nqpoints)
        throw value_error("invalid q point index");

    return this->parentlookup.at(iq);
}

std::size_t Gamma_grid::getSymIdxToParent(std::size_t iq) const {
    if (iq >= this->nqpoints)
        throw value_error("invalid q point index");

    std::size_t iq_parent = this->parentlookup.at(iq);

    std::size_t i;

    for (i = 0; i < this->symmetry_map[iq].size(); ++i) {
        auto iq_image = this->symmetry_map[iq][i];
        if (iq_image == iq_parent)
            return i;
    }
    throw exception("some point is not in symmetry_map");
}

Gamma_grid::Gamma_grid(const Crystal_structure& poscar,
                       const Symmetry_operations& symms,
                       int _na,
                       int _nb,
                       int _nc)
    : na(_na), nb(_nb), nc(_nc), nqpoints(_na * _nb * _nc),
      rlattvec(poscar.rlattvec),
      dq(rlattvec.array().rowwise() /
         Eigen::Map<Eigen::Array3i>(
             std::array<int, 3>({{_na, _nb, _nc}}).data())
             .cast<double>()
             .transpose()) {
    if (std::min({_na, _nb, _nc}) <= 0)
        throw value_error("all grid dimensions must be positive");
    this->initialize_cpos();
    this->fill_equivalences(symms);
    this->fill_map(symms);
}

std::vector<std::array<std::size_t, 2>> Gamma_grid::equivalent_qpairs(
    const std::array<std::size_t, 2>& original) const {
    auto iq1 = original[0];
    auto iq2 = original[1];

    if ((iq1 >= this->nqpoints) || (iq2 >= this->nqpoints))
        throw value_error("invalid q point index");

    std::unordered_set<std::array<std::size_t, 2>> unique;

    // Loop over the images of both q points looking
    // for all unique equivalent pairs.
    for (std::size_t i = 0; i < this->symmetry_map[0].size(); ++i) {
        auto jq1 = this->symmetry_map[iq1][i];
        auto jq2 = this->symmetry_map[iq2][i];
        std::array<std::size_t, 2> candidate({{jq1, jq2}});
        unique.emplace(candidate);
    }

    return std::vector<std::array<std::size_t, 2>>(unique.begin(),
                                                   unique.end());
}

std::vector<std::array<std::size_t, 3>> Gamma_grid::equivalent_qtriplets(
    const std::array<std::size_t, 3>& original) const {
    auto iq1 = original[0];
    auto iq2 = original[1];
    auto iq3 = original[2];

    if ((iq1 >= this->nqpoints) || (iq2 >= this->nqpoints) ||
        (iq3 >= this->nqpoints))
        throw value_error("invalid q point index");

    std::unordered_set<std::array<std::size_t, 3>> unique;

    // Loop over the images of all three q points looking
    // for all unique equivalent triplets.
    for (std::size_t i = 0; i < this->symmetry_map[0].size(); ++i) {
        auto jq1 = this->symmetry_map[iq1][i];
        auto jq2 = this->symmetry_map[iq2][i];
        auto jq3 = this->symmetry_map[iq3][i];
        std::array<std::size_t, 3> candidate({{jq1, jq2, jq3}});
        if (jq1 < this->nqpoints && jq2 < this->nqpoints &&
            jq3 < this->nqpoints) {
            unique.emplace(candidate);
        }
    }

    return std::vector<std::array<std::size_t, 3>>(unique.begin(),
                                                   unique.end());
}

std::vector<Triangle> Gamma_grid::get_triangles(std::size_t ia) const {
    std::vector<Triangle> triangles;
    int itriangle = 0;

    for (int ib = 0; ib < this->nb; ++ib)
        for (int ic = 0; ic < this->nc; ++ic) {
            std::array<int, 3> indices({{static_cast<int>(ia), ib, ic}});
            std::size_t i000 =
                this->three_to_one({{indices[0], indices[1], indices[2]}});
            std::size_t i010 =
                this->three_to_one({{indices[0], indices[1] + 1, indices[2]}});
            std::size_t i001 =
                this->three_to_one({{indices[0], indices[1], indices[2] + 1}});
            std::size_t i011 = this->three_to_one(
                {{indices[0], indices[1] + 1, indices[2] + 1}});
            triangles.emplace_back(Triangle({{i000, i010, i011}}));
            triangles.emplace_back(Triangle({{i000, i001, i011}}));
            itriangle += 2;
        }
    return triangles;
}
} // namespace alma


/*
 * MyPIDCntrllr.cpp
 *
 *  Created on: 11 set 2017
 *      Author: andrea
 */

#include <agents/MyPIDCntrllr.h>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/uuid/uuid_io.hpp>
#include <boost/log/trivial.hpp>
#include <events/MyIMUSample.h>
#include <events/MyBaroSample.h>
#include <events/MyOutMotors.h>
#include <events/MyYPRError.h>
#include <events/MyRCSample.h>
#include <events/MyMotorsDisarmed.h>
#include <events/MyMotorsArmed.h>
#include <iostream>
#include <syslog.h>

#define pi 3.1415926
#define pi2 1.5707963

RangeFloat MyPIDCntrllr::TARGET_RANGES[] = { RangeFloat(-62.0f, 62.0f), // roll
                                             RangeFloat(-62.0f, 62.0f), // pitch
                                             RangeFloat(-125.0f, 125.0f), // yaw
                                             RangeFloat(1000.0f, 2000.0f), // thrust
                                             RangeFloat(0.0f, 100.0f), // aux1
                                             RangeFloat(0.0f, 100.0f)  // aux2
        };
ValueFloat MyPIDCntrllr::TARGET_VALUES[] = {
        ValueFloat(0.0f, MyPIDCntrllr::TARGET_RANGES[ROLL_POS]),
        ValueFloat(0.0f, MyPIDCntrllr::TARGET_RANGES[PITCH_POS]),
        ValueFloat(0.0f, MyPIDCntrllr::TARGET_RANGES[YAW_POS]),
        ValueFloat(1000.0f, MyPIDCntrllr::TARGET_RANGES[THRUST_POS]),
        ValueFloat(0.0f, MyPIDCntrllr::TARGET_RANGES[AUX1_POS]),
        ValueFloat(0.0f, MyPIDCntrllr::TARGET_RANGES[AUX2_POS])
};
int8_t MyPIDCntrllr::QUATERNION_DIRECTION_RPY[] = { 1, -1, 1 };
int8_t MyPIDCntrllr::RC_DIRECTION_RPY[] = { 1, -1, 1 };
float MyPIDCntrllr::FREQUENCY = 100.0f;
RangeFloat MyPIDCntrllr::INTEGRAL_RANGE = RangeFloat(-10.0f, 10.0f);
RangeFloat MyPIDCntrllr::ALTITUDE_RANGE = RangeFloat(-20000.0f, 20000.0f);
using namespace std;

MyPIDCntrllr::MyPIDCntrllr(boost::shared_ptr<MyEventBus> bus,
                           vector<MyEvent::EventType> acceptedEventTypes) :
        MyAgent(bus, acceptedEventTypes), initialized(false), armed(false), yawErr(
                1.0f, 4, 10, 3, INTEGRAL_RANGE), pitchErr(1.0f, 4, 10, 10,
                                                          INTEGRAL_RANGE), rollErr(
                1.0f, 4, 10, 10, INTEGRAL_RANGE), altitudeBuff(1.0f, 20, 2, 2,
                                                          ALTITUDE_RANGE)
{
    this->clean();
    /*
     * Una configurazione che sembra andare:
     * keRoll/kePitch = 1.11f;
     * keDRoll/keDPitch = 9.8f;
     * keIRoll/keIPitch = 0.04080f;
     * keYaw = 2.41f; <-- con questo qualcosa non va. Senza di esso il decollo OK, ma gira su sé stesso.
     * Modificare controllo Yaw in modo che identifichi la velocità di rotazione e non i gradi target
     */

    keRoll = 1.40f; //1.11f; //1.48f //1.90f; //3.2f; //0.45f;
    keIRoll = 0.0f; //0.04080f; //0.026f; //0.000523f; //0.028f; //0.000523f;
    keDRoll = 11.30f; // 9.8f; // 5.56f ok senza yaw; //3.0f; // 3.8f; //5.5f; //15.5f; // 4.0f; //0.012f; //2.0f;

    kePitch = 1.40; // 1.11f; //1.48f //1.90f; //3.2f; //0.45f;
    keIPitch = 0.0f; //0.0480f; //0.026f; //0.000523f; //0.028f; //0.000523f;
    keDPitch = 11.30f; // 9.8f;//5.56f ok senza yaw; //3.0f; // 3.8f; //5.5f; //15.5f; //4.0f; //0.012f; //2.0f;

    keYaw = 1.91f; //1.5f; // 8.0f; //0.05f;
    keIYaw = 0.0f; //0.116f;
    keDYaw = 0.0f; //1.3f; // 6.3f;

    // TODO: usare parametro diverso per yaw. La rotazione richiede molti più giri
    // modificare di conseguenza la funzione calcOutput
    deg2MicrosFactor = 200.0f; //350.0f;
    deg2MicrosYawFactor = 200.0f; //60.0f;

    baroData.altitude = 0.0f;
    baroData.pressure = 0;
    baroData.temperature = 0.0f;
    imuSampleTimeStampMillis = 0;
}

MyPIDCntrllr::~MyPIDCntrllr()
{

}

void MyPIDCntrllr::initialize()
{
    initialized = true;
    syslog(LOG_INFO, "mydrone: MyPIDCntrllr: initialization OK");

}

MyPIDCntrllr::YPRT MyPIDCntrllr::calcYPRData(boost::math::quaternion<float> q)
{
    YPRT result = { };
    float real = q.real();
    float x = q.R_component_2();
    float y = q.R_component_3();
    float z = q.R_component_4();

    result.yaw = QUATERNION_DIRECTION_RPY[YAW_POS]
            * atan2(2.0f * (real * z + x * y), 1.0f - 2.0f * (y * y + z * z))
            * 57.295779513f;
    result.pitch = QUATERNION_DIRECTION_RPY[PITCH_POS]
            * asin(2.0f * real * y - 2.0 * z * x) * 57.295779513f;
    result.roll = QUATERNION_DIRECTION_RPY[ROLL_POS]
            * atan2(2.0f * (real * x + y * z), 1.0f - 2.0f * (x * x + y * y))
            * 57.295779513f;
//	syslog(LOG_INFO, "YPR: y(%3.2f), p(%3.2f), r(%3.2f)", result.yaw, result.pitch, result.roll);
    return result;
}

void MyPIDCntrllr::calcErr(YPRT &yprtReq, YPRT &yprtReal)
{
    // considero errore limitato a 10 deg.
    float yawErrTmp = std::min<float>(
            10.0f,
            std::max<float>(-10.0f, yprtReal.yaw - yprtReq.yaw));
    yawErr.push(yawErrTmp);
    pitchErr.push(
            std::min<float>(
                    10.0f,
                    std::max<float>(-10.0f, yprtReal.pitch - yprtReq.pitch)));
    rollErr.push(
            std::min<float>(
                    10.0f,
                    std::max<float>(-10.0f, yprtReal.roll - yprtReq.roll)));
//	syslog(LOG_INFO, "EYPRT: y(%3.5f), p(%3.5f,%3.5f), r(%3.5f,%3.5f)", yawErr.getMean(), pitchErr.getMean(), pitchErr.getIntegral(), rollErr.getMean(), rollErr.getIntegral());
//    syslog(LOG_INFO, "EYPRT: p(%3.5f, %3.5f, %3.5f, %3.5f), r(%3.5f, %3.5f, %3.5f, %3.5f), t(%5.5f)", yprtReal.pitch, pitchErr.getMean(), pitchErr.getIntegral(), pitchErr.getDerivate(), yprtReal.roll, rollErr.getMean(), rollErr.getIntegral(), rollErr.getDerivate(), yprtReal.thrust);

}

// The YPR input is the distance from real to target data with PID correction.
// the thrust is absolute value
MyPIDCntrllr::YPRT MyPIDCntrllr::calcCorrection(YPRT &yprt)
{
    YPRT result = { 0.0f, 0.0f, 0.0f, 0.0f };
    float yawCorr = yawErr.getMean() * keYaw + yawErr.getIntegral() * keIYaw
            + yawErr.getDerivate() * keDYaw;
    float pitchCorr = pitchErr.getMean() * kePitch
            + pitchErr.getIntegral() * keIPitch
            + pitchErr.getDerivate() * keDPitch;
    float rollCorr = rollErr.getMean() * keRoll
            + rollErr.getIntegral() * keIRoll + rollErr.getDerivate() * keDRoll;

// FIXME: non mi pare funzioni
//    result.yaw = yprt.yaw - yawCorr;
//    result.pitch = yprt.pitch - pitchCorr;
//    result.roll = yprt.roll - rollCorr;
//    result.thrust = yprt.thrust;

    result.yaw = -yawCorr;
    result.pitch = -pitchCorr;
    result.roll = -rollCorr;
    result.thrust = yprt.thrust;

    // syslog(LOG_INFO, "y=(%5.5f), pI=(%5.5f), rI=(%5.5f), t=(%5.5f)", yawCorr, pitchErr.getIntegral(), rollErr.getIntegral(), yprt.thrust);

    return result;
}

MyPIDCntrllr::YPRT MyPIDCntrllr::calcDelta(YPRT &yprt1, YPRT &yprt2)
{
    YPRT result = yprt1 - yprt2;
    return result;
}

// TODO: spostare su MyMotorsAgent
MyPIDCntrllr::PIDOutput MyPIDCntrllr::calcOutput(YPRT &data)
{
    /* 3      1
         \  /
          \/
          /\
	     /  \
	   2      4
     */
    // F3 e F4 girano in senso orario (CW)
    // F1 + F3 - F2 - F4 = pitch
    // F2 + F3 - F1 - F4 = roll
    // F3 + F4 - F1 - F2 = yaw
    // F1 + F2 + F3 + F4 = thrust
    // +--+--+--+--+   +--+   +--+
    // |-1|-1| 1| 1|   |F1|   |Y |
    // +--+--+--+--+   +--+   +--+
    // | 1|-1| 1|-1|   |F2|   |P |
    // +--+--+--+--+ * +--+ = +--+
    // |-1| 1| 1|-1|   |F3|   |R |
    // +--+--+--+--+   +--+   +--+
    // | 1| 1| 1| 1|   |F4|   |T |
    // +--+--+--+--+   +--+   +--+
    /*
     -->inv(M)
     +-----+-----+-----+-----+   +---+   +----+
     |-0.25| 0.25|-0.25| 0.25|   | Y |   | F1 |
     +-----+-----+-----+-----+   +---+   +----+
     |-0.25|-0.25| 0.25| 0.25|   | P |   | F2 |
     +-----+-----+-----+-----+ * +---+ = +----+
     | 0.25| 0.25| 0.25| 0.25|   | R |   | F3 |
     +-----+-----+-----+-----+   +---+   +----+
     | 0.25|-0.25|-0.25| 0.25|   | T |   | F4 |
     +-----+-----+-----+-----+   +---+   +----+
     */
    // Transform input (delta attitude and thrust) to output (nanoseconds for motors)
    long f1 = std::max<long>(
            1000000L,
            std::min<long>(
                    2000000L,
                    std::lrint(
                            (data.thrust
                                    + ((0.25) * data.pitch * deg2MicrosFactor
                                            + (-0.25) * data.roll
                                                    * deg2MicrosFactor
                                            + (-0.25) * data.yaw
                                                    * deg2MicrosYawFactor))
                                    * 1000.0f)));
    long f2 = std::max<long>(
            1000000L,
            std::min<long>(
                    2000000L,
                    std::lrint(
                            (data.thrust
                                    + ((-0.25) * data.pitch * deg2MicrosFactor
                                            + (0.25) * data.roll
                                                    * deg2MicrosFactor
                                            + (-0.25) * data.yaw
                                                    * deg2MicrosYawFactor))
                                    * 1000.0f)));
    long f3 = std::max<long>(
            1000000L,
            std::min<long>(
                    2000000L,
                    std::lrint(
                            (data.thrust
                                    + ((0.25) * data.pitch * deg2MicrosFactor
                                            + (0.25) * data.roll
                                                    * deg2MicrosFactor
                                            + (0.25) * data.yaw
                                                    * deg2MicrosYawFactor))
                                    * 1000.0f)));
    long f4 = std::max<long>(
            1000000L,
            std::min<long>(
                    2000000L,
                    std::lrint(
                            (data.thrust
                                    + ((-0.25) * data.pitch * deg2MicrosFactor
                                            + (-0.25) * data.roll
                                                    * deg2MicrosFactor
                                            + (0.25) * data.yaw
                                                    * deg2MicrosYawFactor))
                                    * 1000.0f)));

    MyPIDCntrllr::PIDOutput result = { };
    result.front = uint16_t(f1/320); // in range [3125,6250]
    result.rear = uint16_t(f2/320);
    result.left = uint16_t(f3/320);
    result.right = uint16_t(f4/320);
//	syslog(LOG_INFO, "MOT: f(%ld), r(%ld), l(%ld), r(%ld), y(%5.5f), p(%5.5f), r(%5.5f), t(%5.5f), tt(%5.5f)", f1, f2, f3, f4, data.yaw, data.pitch, data.roll, data.thrust, targetData.thrust);
    return result;
}
void MyPIDCntrllr::sendOutput(PIDOutput &data)
{
    { // out error event
        boost::shared_ptr<MyYPRError> evOut(
                boost::make_shared<MyYPRError>(
                        this->getUuid(), prevSample.yaw, prevSample.pitch,
                        prevSample.roll, requestedData.yaw, requestedData.pitch,
                        requestedData.roll, keRoll * rollErr.getMean(),
                        keIRoll * rollErr.getIntegral(),
                        keDRoll * rollErr.getDerivate(),
                        kePitch * pitchErr.getMean(),
                        keIPitch * pitchErr.getIntegral(),
                        keDPitch * pitchErr.getDerivate(),
                        keYaw * yawErr.getMean(), keIYaw * yawErr.getIntegral(),
                        keDYaw * yawErr.getDerivate()));
        m_signal(evOut);
    }
    {
//	    syslog(LOG_INFO, "mydrone. MOTORS: front(%ld), rear(%ld), left(%ld), right(%ld)", data.front, data.rear, data.left, data.right);

        // out state motors
        boost::shared_ptr<MyOutMotors> evOut(
                boost::make_shared<MyOutMotors>(this->getUuid(), data.front,
                                                data.rear, data.left,
                                                data.right));
        m_signal(evOut);
    }
}

MyPIDCntrllr::YPRT MyPIDCntrllr::getYPRTFromTargetData()
{
    YPRT result = { };
    result.yaw = float(
            std::trunc(
                    RC_DIRECTION_RPY[YAW_POS]
                            * MyPIDCntrllr::TARGET_VALUES[YAW_POS].getValue()));
    result.pitch =
            float(std::trunc(
                    RC_DIRECTION_RPY[PITCH_POS]
                            * MyPIDCntrllr::TARGET_VALUES[PITCH_POS].getValue()));
    result.roll =
            float(std::trunc(
                    RC_DIRECTION_RPY[ROLL_POS]
                            * MyPIDCntrllr::TARGET_VALUES[ROLL_POS].getValue()));
    result.thrust = float(
            std::round(MyPIDCntrllr::TARGET_VALUES[THRUST_POS].getValue()));

    if ((result.yaw >= -1.1f) && (result.yaw <= 1.1f))
    {
        result.yaw = 0.0f;
    }
    if ((result.pitch >= -1.1f) && (result.pitch <= 1.1f))
    {
        result.pitch = 0.0f;
    }
    if ((result.roll >= -1.1f) && (result.roll <= 1.1f))
    {
        result.roll = 0.0f;
    }
    return result;
}

void MyPIDCntrllr::processImuSample(boost::math::quaternion<float> sampleQ,
                                    float yaw, float pitch, float roll,
                                    VectorFloat gravity, VectorInt16 accel,
                                    VectorInt16 linearAccel, long int elapsedMillis)
{
    YPRT sample = { };
    sample.yaw = yaw;
    sample.pitch = pitch;
    sample.roll = roll;
    sample.thrust = requestedData.thrust; // not evaluated from IMU. I force to requested.

    // inizializza stato iniziale in seguito ad un disarm
    if (prevSample.isZero() && prevExpected.isZero())
    {
        prevSample = sample;
        prevExpected = {0.0f,0.0f,0.0f,0.0f};
    }

    YPRT nextExpected = (requestedData - sample);

    float maxPitchRollDeg = MYPIDCNTRLLR_MAX_DEG_PER_SEC/10.0f;
    float maxYawDeg = MYPIDCNTRLLR_MAX_DEG_PER_SEC_YAW/10.0f;
    nextExpected.limitYPR(maxPitchRollDeg, maxYawDeg); // Distanza max da percorrere in un decimo di secondo

    /**********************************************************************
     * Non sembra andar bene ...
     * riduce troppo quando l'errore è grande
     * aumenta troppo quando è piccolo
     */
    // Calcolo la velocità di rotazione che dovrò avere al prossimo ciclo
//    float b = ((maxYawDeg + abs(nextExpected.yaw))/maxYawDeg);
//    nextExpected.yaw = nextExpected.yaw / (b * abs(nextExpected.yaw) + 1);
//    b = ((maxPitchRollDeg + abs(nextExpected.pitch))/maxPitchRollDeg);
//    nextExpected.pitch = nextExpected.pitch / (b * abs(nextExpected.pitch) + 1);
//    b = ((maxPitchRollDeg + abs(nextExpected.roll))/maxPitchRollDeg);
//    nextExpected.roll = nextExpected.roll / (b * abs(nextExpected.roll) + 1);

    // Calcolo la velocità di rotazione che dovrò avere al prossimo ciclo
    nextExpected.divideYPR(17.0f);

    YPRT deltaReal = sample - prevSample; // Distanza percorsa nel ciclo precedente
    // normalize delta on 10 millis (100Hz)
    deltaReal.divideYPR(float(elapsedMillis)/10.0f);

    // Calcolo errore solo se sono in volo
    // TODO: trovare un modo migliore ...
//    if (sample.thrust > 1350.0f)
//    {
        calcErr(prevExpected, deltaReal);
//    } else {
//        // TODO: Da togliere. Mi serve per i test
//        calcErr(prevExpected, deltaReal);
//    }

    float alfa = ((requestedData.thrust - 1000.0f)/1000.0f)*(pi2);

    nextExpected.thrust = 1000.0f + (requestedData.thrust - 1000.0f) * sin(alfa);
    prevSample = sample;
    prevExpected = nextExpected;

    /* CALCOLO INPUT
     * calculate input data for transformation function
     * La correzione è data dal target richiesto (espresso in gradi) in un ciclo di frequenza e compensata con l'errore (gained)
     */
    YPRT input = calcCorrection(nextExpected);

    // calculate transformation function
    PIDOutput output = {};
    if(requestedData.thrust >= 1010.0f) {
        output = calcOutput(input);
    } else {
        output.clean();
    }

//    syslog(LOG_INFO, "Y(%5.5f),P(%5.5f),R(%5.5f), FR(%ld),FL(%ld),RR(%ld),RL(%ld), T(%5.5f)", nextExpected.yaw, nextExpected.pitch, nextExpected.roll, output.front - std::lrint(input.thrust), output.left - std::lrint(input.thrust), output.right  - std::lrint(input.thrust),output.rear  - std::lrint(input.thrust), input.thrust);
//    syslog(LOG_INFO, "P(%5.5f),PD(%5.5f),PI(%5.5f), FR(%ld),FL(%ld),RR(%ld),RL(%ld), T(%5.5f)", pitchErr.getMean() * deg2MicrosFactor, pitchErr.getDerivate() * keDPitch * deg2MicrosFactor, pitchErr.getIntegral() * keIPitch * deg2MicrosFactor, output.front - std::lrint(input.thrust), output.left - std::lrint(input.thrust), output.right  - std::lrint(input.thrust),output.rear  - std::lrint(input.thrust), input.thrust);

    // send to motors
        this->sendOutput(output);
}
void MyPIDCntrllr::clean()
{
    yawErr.clean();
    pitchErr.clean();
    rollErr.clean();
    requestedData.clean();
    prevSample.clean();
    prevExpected.clean();
    this->armed = false;
}
void MyPIDCntrllr::disarm()
{
    this->clean();
}
void MyPIDCntrllr::arm()
{
    this->clean();
    this->armed = true;
}
void MyPIDCntrllr::processEvent(boost::shared_ptr<MyEvent> event)
{
    if (!initialized)
    {
        initialize();
    }
    if (this->getState() == MyAgentState::Active)
    {
        if (event->getType() == MyEvent::EventType::MotorsDisarmed)
        {
            syslog(LOG_INFO, "Motors Disarmed");
            this->disarm();
        }
        else if (event->getType() == MyEvent::EventType::MotorsArmed)
        {
            syslog(LOG_INFO, "Motors Armed");
            this->arm();
        }
        else if (event->getType() == MyEvent::EventType::IMUSample
                && this->armed)
        {
            boost::shared_ptr<MyIMUSample> imuSample =
                    boost::static_pointer_cast<MyIMUSample>(event);
            long int elapsedMillis = 10;
            if(imuSampleTimeStampMillis != 0) {
                elapsedMillis = imuSample->getTimestampMillis() - imuSampleTimeStampMillis;
            }
            imuSampleTimeStampMillis = imuSample->getTimestampMillis();
            boost::math::quaternion<float> q = imuSample->getQuaternion();
            this->processImuSample(q, imuSample->getYaw(),
                                   imuSample->getPitch(), imuSample->getRoll(),
                                   imuSample->getGravity(),
                                   imuSample->getAccel(),
                                   imuSample->getLinearAccel(),
                                   elapsedMillis);
        }
        else if (event->getType() == MyEvent::EventType::RCSample)
        {
            boost::shared_ptr<MyRCSample> rcSample = boost::static_pointer_cast<
                    MyRCSample>(event);
            MyPIDCntrllr::TARGET_VALUES[ROLL_POS].setPercentValue(
                    (*rcSample).getRollPercent());
            MyPIDCntrllr::TARGET_VALUES[PITCH_POS].setPercentValue(
                    (*rcSample).getPitchPercent());
            MyPIDCntrllr::TARGET_VALUES[YAW_POS].setPercentValue(
                    (*rcSample).getYawPercent());
            MyPIDCntrllr::TARGET_VALUES[THRUST_POS].setPercentValue(
                    (*rcSample).getThrustPercent());
            MyPIDCntrllr::TARGET_VALUES[AUX1_POS].setPercentValue(
                    (*rcSample).getAux1Percent());
            MyPIDCntrllr::TARGET_VALUES[AUX2_POS].setPercentValue(
                    (*rcSample).getAux2Percent());
            requestedData = this->getYPRTFromTargetData();
//            syslog(LOG_INFO, "ReqData(485) RPY(%5.5f,%5.5f,%5.5f)", requestedData.roll, requestedData.pitch, requestedData.yaw);
//            float v1 = MyPIDCntrllr::TARGET_VALUES[AUX2_POS].getValue();

//            float vv1 = MyPIDCntrllr::TARGET_VALUES[AUX1_POS].getValue()/10.0f;
//            if(abs(vv1 - keYaw) > 0.1f) {
//                keYaw = vv1;
//                syslog(LOG_INFO, "K: E(%5.5f)",keYaw);
//            }

//            float vv1 = MyPIDCntrllr::TARGET_VALUES[AUX1_POS].getValue()/1000.0f;
//            if(abs(vv1 - keIRoll) > 0.001f) {
//                keIRoll = vv1;
//                keIPitch = vv1;
//                syslog(LOG_INFO, "K: E(%5.5f)",keIRoll);
//            }

//            float v1 = MyPIDCntrllr::TARGET_VALUES[AUX2_POS].getValue();
//            float vv1 = MyPIDCntrllr::TARGET_VALUES[AUX1_POS].getValue()/10.0f;
//            if(abs(v1 - keDRoll) > 0.1f || abs(vv1 - keRoll) > 0.1f) {
//                keDRoll = v1;
//                keDPitch = keDRoll;
//                keRoll = vv1;
//                kePitch = keRoll;
//                syslog(LOG_INFO, "K: E(%5.5f),D(%5.5f)",keRoll, keDRoll);
//            }

        }
        else if (event->getType() == MyEvent::EventType::BaroSample)
        {
            // TODO: rivedere completamente
//            boost::shared_ptr<MyBaroSample> baroSample =
//                    boost::static_pointer_cast<MyBaroSample>(event);
//            // TODO: da spostare in metodo ad hoc per BaroPID ...
//            float prevAlt = this->altitudeBuff.getMean();
//            this->altitudeBuff.push(baroSample->getAltitude());
//            baroData.altitude = this->altitudeBuff.getMean();
//            baroData.pressure = baroSample->getPressure();
//            baroData.temperature = baroSample->getTemperature();
//            baroData.speedMetersPerSecond = (baroData.altitude - prevAlt)*1000.0f/baroSample->getDTimeMillis();
        }
        else if (event->getType() == MyEvent::EventType::IMUSample)
        {
//            boost::shared_ptr<MyIMUSample> imuSample =
//                    boost::static_pointer_cast<MyIMUSample>(event);
//            syslog(LOG_INFO, "Y(%5.5f),P(%5.5f),R(%5.5f)", imuSample->getYaw(), imuSample->getPitch(), imuSample->getRoll());

        }
        else
        {
            // skip events
//            syslog(LOG_INFO, "Skipped event");
        }
    }
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2014-2021.
// Modifications copyright (c) 2014-2021 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)

#ifndef BOOST_GEOMETRY_TEST_TO_SVG_HPP
#define BOOST_GEOMETRY_TEST_TO_SVG_HPP

#include <fstream>

#include <boost/geometry/algorithms/detail/overlay/get_turns.hpp>
#include <boost/geometry/algorithms/detail/overlay/self_turn_points.hpp>
#include <boost/geometry/algorithms/detail/overlay/traversal_info.hpp>
#include <boost/geometry/algorithms/detail/overlay/debug_turn_info.hpp>
#include <boost/geometry/algorithms/detail/overlay/enrichment_info.hpp>
#include <boost/geometry/algorithms/detail/overlay/enrich_intersection_points.hpp>
#include <boost/geometry/algorithms/detail/relate/turns.hpp>

#include <boost/geometry/io/svg/svg_mapper.hpp>
#include <boost/geometry/io/wkt/read.hpp>

#include <string_from_type.hpp>

template <typename Turns, typename Mapper>
inline void turns_to_svg(Turns const& turns, Mapper& mapper)
{
    namespace bg = boost::geometry;

    // Simple map to avoid two texts at same place (note that can still overlap!)
    std::map<std::pair<int, int>, int> offsets;
    int index = 0;
    int const margin = 5;

    for (auto const& turn : turns)
    {
        using coordinate_type
            = typename bg::coordinate_type<decltype(turn.point)>::type;
        int lineheight = 10;
        mapper.map(turn.point, "fill:rgb(255,128,0);"
                               "stroke:rgb(0,0,0);stroke-width:1", 3);

        {
            coordinate_type half = 0.5;
            coordinate_type ten = 10;
            // Map characteristics
            // Create a rounded off point
            std::pair<int, int> p
                    = std::make_pair(
                          boost::numeric_cast<int>(half
                                                   + ten * bg::get<0>(turn.point)),
                          boost::numeric_cast<int>(half
                                                   + ten * bg::get<1>(turn.point))
                          );
            std::string style =  "fill:rgb(0,0,0);font-family:Arial;font-size:12px";

            if (turn.discarded)
            {
                style =  "fill:rgb(92,92,92);font-family:Arial;font-size:10px";
                lineheight = 6;
            }

            {
                std::ostringstream out;
                out << index
                    << ": " << bg::method_char(turn.method)
                    << (turn.discarded ? " (discarded)" : "")
                    << (turn.blocked() ? " (blocked)" : "")
                    << '\n';

                out << bg::operation_char(turn.operations[0].operation)
                        <<": seg: " << turn.operations[0].seg_id.source_index
                       << ' ' << turn.operations[0].seg_id.multi_index
                       << ' ' << turn.operations[0].seg_id.ring_index
                       << ' ' << turn.operations[0].seg_id.segment_index << ", ";
                out << "other: " << turn.operations[1].seg_id.source_index
                    << ' ' << turn.operations[1].seg_id.multi_index
                    << ' ' << turn.operations[1].seg_id.ring_index
                    << ' ' << turn.operations[1].seg_id.segment_index;

                out << '\n';

                out << bg::operation_char(turn.operations[1].operation)
                        << ": seg: " << turn.operations[1].seg_id.source_index
                        << ' ' << turn.operations[1].seg_id.multi_index
                        << ' ' << turn.operations[1].seg_id.ring_index
                        << ' ' << turn.operations[1].seg_id.segment_index << ", ";
                out << "other: " << turn.operations[0].seg_id.source_index
                    << ' ' << turn.operations[0].seg_id.multi_index
                    << ' ' << turn.operations[0].seg_id.ring_index
                    << ' ' << turn.operations[0].seg_id.segment_index;

                offsets[p] += lineheight;
                int offset = offsets[p];
                offsets[p] += lineheight * 3;
                mapper.text(turn.point, out.str(), style, margin, offset, lineheight);
            }
            index++;
        }
    }
}

struct to_svg_assign_policy
{
    static bool const include_no_turn = false;
    static bool const include_degenerate = false;
    static bool const include_opposite = false;
    static bool const include_start_turn = false;
};

template <typename G1, typename G2, typename G3>
inline void to_svg(G1 const& g1, G2 const& g2, G3 const& g3,
                   std::string const& caseid)
{
    namespace bg = boost::geometry;

    using point_type = typename bg::point_type<G1>::type;
    using coordinate_type = typename bg::coordinate_type<point_type>::type;

    std::ostringstream filename;
    filename << "case_"
        << caseid << "_"
        << string_from_type<coordinate_type>::name()
#if defined(BOOST_GEOMETRY_USE_RESCALING)
        << "_rescaled"
#endif
        << ".svg";

    std::ofstream svg(filename.str());

    bg::svg_mapper<point_type> mapper(svg, 500, 500);

    mapper.add(g1);
    mapper.add(g2);
    mapper.add(g3);

    std::string const green = "rgb(153,204,0)";
    std::string const blue = "rgb(51,51,153)";
    std::string const red = "rgb(255,0,0)";
    if (bg::util::is_linestring<G1>::value)
    {
        auto style = [](auto const fo, auto const& c)
            { return "stroke:" + c + ";stroke-width:3;stroke-opacity:"
                    + std::to_string(fo) + ";"; };
        mapper.map(g1, style(0.5, green));
        mapper.map(g2, style(0.3, blue));
        mapper.map(g3, style(0.5, red));
    }
    else
    {
        auto style = [](auto const fo, auto const& c)
            { return "fill-opacity:" + std::to_string(fo) + ";fill:" + c
              + ";stroke:rgb(153,204,0);stroke-width:1;stroke-opacity:0.7;"; };
        mapper.map(g1, style(0.5, green));
        mapper.map(g2, style(0.3, blue));
        mapper.map(g3, style(0.5, red));
    }

    // GET TURNS

    using strategy_type
        = typename bg::strategies::relate::services::default_strategy
            <G1, G2>::type;

    using ratio_type = bg::segment_ratio<coordinate_type>;

    using turn_type = bg::detail::overlay::turn_info
        <
            point_type,
            ratio_type,
            bg::detail::overlay::turn_operation_linear<point_type, ratio_type>
        >;

    strategy_type strategy;

    std::deque<turn_type> turns;
    bg::detail::get_turns::no_interrupt_policy interrupt_policy;

    using turn_policy = bg::detail::get_turns::get_turn_info_type
        <
            G1, G2, to_svg_assign_policy
        >;

    bg::detail::relate::turns::get_turns
        <
            G1, G2, turn_policy
        >::apply(turns, g1, g2, interrupt_policy, strategy);

    {
        using less = bg::detail::relate::turns::less
            <
                0,
                bg::detail::relate::turns::less_op_xxx_linear
                    <
                        0, bg::detail::relate::turns::op_to_int<>
                    >,
                typename bg::cs_tag<G1>::type
            >;
        std::sort(boost::begin(turns), boost::end(turns), less());
    }

    turns_to_svg(turns, mapper);
}

#endif // BOOST_GEOMETRY_TEST_TO_SVG_HPP


#pragma once

#include <boost/operators.hpp>
#include <cinttypes>
#include <limits>
#include <type_traits>

namespace wbtree {
template <typename IntT, typename TagT, typename = void> class Strong;

template <typename Type, typename TagT, typename> class Strong {
  static_assert(std::numeric_limits<Type>::is_integer || std::is_same_v<Type, bool>,
                "only integer types and bool type are supported");
};

template <typename IntT, typename TagT>
class Strong<IntT, TagT, std::enable_if_t<std::numeric_limits<IntT>::is_integer>>
    // Use baseclass chaining to maintain standard layout
    : public boost::totally_ordered<
          Strong<IntT, TagT>,
          boost::integer_arithmetic<
              Strong<IntT, TagT>,
              boost::bitwise<Strong<IntT, TagT>,
                             boost::unit_steppable<Strong<IntT, TagT>,
                                                   boost::shiftable<Strong<IntT, TagT>>>>>> {
public:
  using Self = Strong<IntT, TagT>;
  using Int = IntT;
  using Tag = TagT;

  Strong() = default;
  explicit constexpr Strong(Int v) : val(v) {}
  template <typename Result> explicit constexpr operator Result() const {
    return static_cast<Result>(val);
  }

  constexpr auto get() const { return val; }

  template <class CharT, class Traits>
  friend auto operator<<(std::basic_ostream<CharT, Traits> &os, const Strong &i)
      -> std::basic_ostream<CharT, Traits> & {
    return os << i.val;
  }

  template <class CharT, class Traits>
  friend inline auto operator>>(std::basic_istream<CharT, Traits> &is, Strong &i)
      -> std::basic_istream<CharT, Traits> & {
    return is >> i.val;
  }

  constexpr auto operator<(const Strong &x) const -> bool { return val < x.val; }
  constexpr auto operator==(const Strong &x) const -> bool { return val == x.val; }

  constexpr auto operator+=(const Strong &x) -> Strong & {
    val += x.val;
    return *this;
  }
  constexpr auto operator-=(const Strong &x) -> Strong & {
    val -= x.val;
    return *this;
  }
  constexpr auto operator*=(const Strong &x) -> Strong & {
    val *= x.val;
    return *this;
  }
  constexpr auto operator/=(const Strong &x) -> Strong & {
    val /= x.val;
    return *this;
  }
  constexpr auto operator%=(const Strong &x) -> Strong & {
    val %= x.val;
    return *this;
  }
  constexpr auto operator|=(const Strong &x) -> Strong & {
    val |= x.val;
    return *this;
  }
  constexpr auto operator&=(const Strong &x) -> Strong & {
    val &= x.val;
    return *this;
  }
  constexpr auto operator^=(const Strong &x) -> Strong & {
    val ^= x.val;
    return *this;
  }
  constexpr auto operator<<=(const Strong &x) -> Strong & {
    val <<= x.val;
    return *this;
  }
  constexpr auto operator>>=(const Strong &x) -> Strong & {
    val >>= x.val;
    return *this;
  }
  constexpr auto operator++() -> Strong & {
    ++val;
    return *this;
  }
  constexpr auto operator--() -> Strong & {
    --val;
    return *this;
  }

  constexpr auto operator+() const { // unary plus
    return *this;
  }
  constexpr auto operator-() const { // unary minus
    return Strong(0) - *this;
  }

private:
  Int val = 0;
};

template <typename TagT>
class Strong<bool, TagT> : public boost::equality_comparable<Strong<bool, TagT>> {
public:
  using Self = Strong<bool, TagT>;
  using Int = bool;
  using Tag = TagT;

  explicit constexpr Strong() = default;
  explicit constexpr Strong(bool v) : val(v) {}
  explicit constexpr operator bool() const { return val; }

  constexpr auto operator==(const Strong &x) const -> bool { return val == x.val; }
  constexpr auto operator!() const -> bool { return !val; }

private:
  bool val = false;
};
} // namespace wbtree

namespace std {
template <typename Int, typename Tag> struct numeric_limits<wbtree::Strong<Int, Tag>> {
  static constexpr auto is_specialized = true;
  static constexpr auto is_signed = std::numeric_limits<Int>::is_signed;
  static constexpr auto is_integer = std::numeric_limits<Int>::is_integer;
  static constexpr auto is_exact = std::numeric_limits<Int>::is_exact;
  static constexpr auto has_infinity = std::numeric_limits<Int>::has_infinity;
  static constexpr auto has_quiet_NaN = std::numeric_limits<Int>::has_quiet_NaN;
  static constexpr auto has_signaling_NaN = std::numeric_limits<Int>::has_signaling_NaN;
  static constexpr auto has_denorm = std::numeric_limits<Int>::has_denorm;
  static constexpr auto has_denorm_loss = std::numeric_limits<Int>::has_denorm_loss;
  static constexpr auto round_style = std::numeric_limits<Int>::round_style;
  static constexpr auto is_iec559 = std::numeric_limits<Int>::is_iec559;
  static constexpr auto is_bounded = std::numeric_limits<Int>::is_bounded;
  static constexpr auto is_modulo = std::numeric_limits<Int>::is_modulo;
  static constexpr auto digits = std::numeric_limits<Int>::digits;
  static constexpr auto digits10 = std::numeric_limits<Int>::digits10;
  static constexpr auto max_digits10 = std::numeric_limits<Int>::max_digits10;
  static constexpr auto radix = std::numeric_limits<Int>::radix;
  static constexpr auto min_exponent = std::numeric_limits<Int>::min_exponent;
  static constexpr auto min_exponent10 = std::numeric_limits<Int>::min_exponent10;
  static constexpr auto max_exponent = std::numeric_limits<Int>::max_exponent;
  static constexpr auto max_exponent10 = std::numeric_limits<Int>::max_exponent10;
  static constexpr auto traps = std::numeric_limits<Int>::traps;
  static constexpr auto tinyness_before = std::numeric_limits<Int>::tinyness_before;

  static constexpr auto min() { return wbtree::Strong<Int, Tag>(std::numeric_limits<Int>::min()); }
  static constexpr auto lowest() {
    return wbtree::Strong<Int, Tag>(std::numeric_limits<Int>::lowest());
  }
  static constexpr auto max() { return wbtree::Strong<Int, Tag>(std::numeric_limits<Int>::max()); }
  static constexpr auto epsilon() {
    return wbtree::Strong<Int, Tag>(std::numeric_limits<Int>::epsilon());
  }
  static constexpr auto round_error() {
    return wbtree::Strong<Int, Tag>(std::numeric_limits<Int>::round_error());
  }
  static constexpr auto infinity() {
    return wbtree::Strong<Int, Tag>(std::numeric_limits<Int>::infinity());
  }
  static constexpr auto quiet_NaN() {
    return wbtree::Strong<Int, Tag>(std::numeric_limits<Int>::quiet_NaN());
  }
  static constexpr auto signaling_NaN() {
    return wbtree::Strong<Int, Tag>(std::numeric_limits<Int>::signaling_NaN());
  }
  static constexpr auto denorm_min() {
    return wbtree::Strong<Int, Tag>(std::numeric_limits<Int>::denorm_min());
  }
};
} // namespace std


/*
 * MIT License
 *
 * Copyright (c) 2020 Robert Grupp
 *
 * 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 "xregRayCastLineIntOCL.h"

#include <boost/compute/utility/source.hpp>

#include "xregExceptionUtils.h"
#include "xregAssert.h"

namespace  // un-named
{

//////////////////////////////////////////////////////////////////////

const char* kRAY_CASTING_LINE_INT_OPENCL_SRC = BOOST_COMPUTE_STRINGIZE_SOURCE(

__kernel void xregLineIntegralKernel(const RayCastArgs args,
                                     __global const float4* det_pts,
                                     image3d_t vol_tex,
                                     __global float* dst_line_integral_sums,
                                     __global const float16* cam_to_itk_phys_xforms,
                                     __global const ulong* cam_model_for_proj,
                                     __global const float4* cam_focal_pts)
{
  const ulong idx = get_global_id(0);

  const ulong num_rays = args.num_projs * args.num_det_pts;

  if (idx < num_rays)
  {
    const ulong proj_idx   = idx / args.num_det_pts;
    const ulong det_pt_idx = idx - (proj_idx * args.num_det_pts);
    const ulong cam_idx    = cam_model_for_proj[proj_idx];

    const float4 focal_pt_wrt_cam = cam_focal_pts[cam_idx];

    const sampler_t sampler = CLK_NORMALIZED_COORDS_FALSE | CLK_ADDRESS_CLAMP | CLK_FILTER_LINEAR;

    const float4 tex_coords_off = (float4) (0.5f, 0.5f, 0.5f, 0);

    const float16 xform_cam_to_itk_idx = xregFrm4x4Composition(args.itk_phys_pt_to_itk_idx_xform,
                                                               cam_to_itk_phys_xforms[proj_idx]);

    const float4 pinhole_wrt_itk_idx = xregFrm4x4XformFloat4Pt(xform_cam_to_itk_idx, focal_pt_wrt_cam);

    const float4 cur_det_pt_wrt_cam = det_pts[(cam_idx * args.num_det_pts) + det_pt_idx];

    const float4 pinhole_to_det_wrt_itk_idx = xregFrm4x4XformFloat4Pt(xform_cam_to_itk_idx, cur_det_pt_wrt_cam)
                                                - pinhole_wrt_itk_idx;

    // check intersection
    float2 t = xregLineSegmentRectIntersect(xregFloat4HmgToFloat3(args.img_aabb_min),
                                            xregFloat4HmgToFloat3(args.img_aabb_max),
                                            xregFloat4HmgToFloat3(pinhole_wrt_itk_idx),
                                            xregFloat4HmgToFloat3(pinhole_to_det_wrt_itk_idx));
    // t.x indicates the first intersection with the volume along the source to detector line,
    // t.y indicates the exit of the volume along the source to detector line

    // a float4 is returned from read_imagef, but for our image all components
    // are equal
    float4 dst_val = XREG_LINE_INT_KERNEL_INIT;

    const float4 start_pt_wrt_itk_idx = pinhole_wrt_itk_idx + (t.x * pinhole_to_det_wrt_itk_idx);

    const float pinhole_to_det_len_wrt_itk_idx  = xregFloat4HmgNorm(pinhole_to_det_wrt_itk_idx);
    const float intersect_len_wrt_itk_idx = (t.y - t.x) * pinhole_to_det_len_wrt_itk_idx;

    const float4 focal_pt_to_det_wrt_cam = cur_det_pt_wrt_cam - focal_pt_wrt_cam;
    // NOTE: since we are transforming a vector with a scaling transform (points to indices),
    //       the output vector may not have the same norm, thus we take the norm
    const float step_len_wrt_itk_idx = xregFloat4HmgNorm(
                  xregFrm4x4XformFloat4Vec(xform_cam_to_itk_idx,
                      (focal_pt_to_det_wrt_cam / xregFloat4HmgNorm(focal_pt_to_det_wrt_cam)) * args.step_size));

    const ulong num_steps = (ulong)(intersect_len_wrt_itk_idx / step_len_wrt_itk_idx);

    float4 cur_cont_vol_idx = (float4) (start_pt_wrt_itk_idx.x, start_pt_wrt_itk_idx.y, start_pt_wrt_itk_idx.z, 0);

    cur_cont_vol_idx += tex_coords_off;

    // "/ pinhole_to_det_len_wrt_itk_idx" makes pinhole_to_det_wrt_itk_idx a unit vector
    const float scale_to_step = step_len_wrt_itk_idx / pinhole_to_det_len_wrt_itk_idx;

    const float4 step_vec_wrt_itk_idx = pinhole_to_det_wrt_itk_idx * scale_to_step;

    for (ulong step_idx = 0; step_idx <= num_steps; ++step_idx)
    {
      dst_val = XREG_LINE_INT_KERNEL_OP(dst_val, read_imagef(vol_tex, sampler, cur_cont_vol_idx));

      cur_cont_vol_idx += step_vec_wrt_itk_idx;
    }

    dst_line_integral_sums[idx] = XREG_LINE_INT_KERNEL_OP(dst_val.x, dst_line_integral_sums[idx]);
  }
}

);

//////////////////////////////////////////////////////////////////////

}  // un-named

xreg::RayCasterLineIntOCL::RayCasterLineIntOCL(const boost::compute::device& dev)
  : RayCasterOCL(dev)
{ }

xreg::RayCasterLineIntOCL::RayCasterLineIntOCL(const boost::compute::context& ctx,
                                               const boost::compute::command_queue& queue)
  : RayCasterOCL(ctx, queue)
{ }

void xreg::RayCasterLineIntOCL::allocate_resources()
{
  namespace bc = boost::compute;

  RayCasterOCL::allocate_resources();

  std::string kernel_op_ocl_src;

  switch (this->kernel_id())
  {
    case kRAY_CAST_LINE_INT_SUM_KERNEL:
      kernel_op_ocl_src = "\n\n#define XREG_LINE_INT_KERNEL_INIT 0\n"
                          "#define XREG_LINE_INT_KERNEL_OP(X,Y) (X) + (Y)\n\n";
      break;
    case kRAY_CAST_LINE_INT_MAX_KERNEL:
      kernel_op_ocl_src = "\n\n#define XREG_LINE_INT_KERNEL_INIT -FLT_MAX\n"
                          "#define XREG_LINE_INT_KERNEL_OP(X,Y) max((X),(Y))\n\n";
      break;
    default:
      xregThrow("Unsupported Line Integral Kernel!");
  }

  // Build the line integral ray casting program
  std::stringstream ss;
  ss << RayCastBaseOCLStr()
     << kernel_op_ocl_src
     << kRAY_CASTING_LINE_INT_OPENCL_SRC;

  bc::program prog = bc::program::create_with_source(ss.str(), ctx_);
  prog.build();

  dev_kernel_ = prog.create_kernel("xregLineIntegralKernel");
}

void xreg::RayCasterLineIntOCL::compute(const size_type vol_idx)
{
  xregASSERT(this->resources_allocated_);

  compute_helper_pre_kernels(vol_idx);

  // setup kernel arguments and launch

  dev_kernel_.set_arg(0, sizeof(ray_cast_kernel_args_), &ray_cast_kernel_args_);

  dev_kernel_.set_arg(1, det_pts_dev_);

  dev_kernel_.set_arg(2, vol_texs_dev_[vol_idx]);

  dev_kernel_.set_arg(3, *proj_pixels_dev_to_use_);

  dev_kernel_.set_arg(4, cam_to_itk_phys_xforms_dev_);

  dev_kernel_.set_arg(5, cam_model_for_proj_dev_);

  dev_kernel_.set_arg(6, focal_pts_dev_);

  std::size_t global_work_size = ray_cast_kernel_args_.num_det_pts * this->num_projs_;

  cmd_queue_.enqueue_nd_range_kernel(dev_kernel_,
                                     1, // dim
                                     0, // null offset -> start at 0
                                     &global_work_size,
                                     0  // passing null lets open CL pick a local size
                                    ).wait();

  compute_helper_post_kernels(vol_idx);
}


#include <boost/numeric/odeint/external/nt2/nt2_algebra_dispatcher.hpp>


// Copyright 2015-2020 Tier IV, Inc. All rights reserved.
//
// 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.

#include <boost/algorithm/clamp.hpp>
#include <memory>
#include <scenario_simulator_exception/exception.hpp>
#include <string>
#include <traffic_simulator/behavior/vehicle/behavior_tree.hpp>
#include <traffic_simulator/behavior/vehicle/follow_lane_sequence/yield_action.hpp>
#include <traffic_simulator/math/catmull_rom_spline.hpp>
#include <vector>

namespace entity_behavior
{
namespace vehicle
{
namespace follow_lane_sequence
{
YieldAction::YieldAction(const std::string & name, const BT::NodeConfiguration & config)
: entity_behavior::VehicleActionNode(name, config)
{
}

const boost::optional<openscenario_msgs::msg::Obstacle> YieldAction::calculateObstacle(
  const openscenario_msgs::msg::WaypointsArray & waypoints)
{
  if (!distance_to_stop_target_) {
    return boost::none;
  }
  if (distance_to_stop_target_.get() < 0) {
    return boost::none;
  }
  traffic_simulator::math::CatmullRomSpline spline(waypoints.waypoints);
  if (distance_to_stop_target_.get() > spline.getLength()) {
    return boost::none;
  }
  openscenario_msgs::msg::Obstacle obstacle;
  obstacle.type = obstacle.ENTITY;
  obstacle.s = distance_to_stop_target_.get();
  return obstacle;
}

const openscenario_msgs::msg::WaypointsArray YieldAction::calculateWaypoints()
{
  if (!entity_status.lanelet_pose_valid) {
    THROW_SIMULATION_ERROR("failed to assign lane");
  }
  if (entity_status.action_status.twist.linear.x >= 0) {
    openscenario_msgs::msg::WaypointsArray waypoints;
    double horizon =
      boost::algorithm::clamp(entity_status.action_status.twist.linear.x * 5, 20, 50);
    traffic_simulator::math::CatmullRomSpline spline(hdmap_utils->getCenterPoints(route_lanelets));
    waypoints.waypoints = spline.getTrajectory(
      entity_status.lanelet_pose.s, entity_status.lanelet_pose.s + horizon, 1.0);
    return waypoints;
  } else {
    return openscenario_msgs::msg::WaypointsArray();
  }
}

boost::optional<double> YieldAction::calculateTargetSpeed()
{
  if (!distance_to_stop_target_) {
    return boost::none;
  }
  double rest_distance =
    distance_to_stop_target_.get() - (vehicle_parameters.bounding_box.dimensions.x) - 10;
  if (rest_distance < calculateStopDistance()) {
    if (rest_distance > 0) {
      return std::sqrt(2 * 5 * rest_distance);
    } else {
      return 0;
    }
  }
  return entity_status.action_status.twist.linear.x;
}

BT::NodeStatus YieldAction::tick()
{
  getBlackBoardValues();
  if (request != "none" && request != "follow_lane") {
    return BT::NodeStatus::FAILURE;
  }
  if (!driver_model.see_around) {
    return BT::NodeStatus::FAILURE;
  }
  if (!entity_status.lanelet_pose_valid) {
    return BT::NodeStatus::FAILURE;
  }
  const auto right_of_way_entities = getRightOfWayEntities(route_lanelets);
  if (right_of_way_entities.empty()) {
    if (!target_speed) {
      target_speed = hdmap_utils->getSpeedLimit(route_lanelets);
    }
    setOutput("updated_status", calculateEntityStatusUpdated(target_speed.get()));
    const auto waypoints = calculateWaypoints();
    if (waypoints.waypoints.empty()) {
      return BT::NodeStatus::FAILURE;
    }
    const auto obstacle = calculateObstacle(waypoints);
    setOutput("waypoints", waypoints);
    setOutput("obstacle", obstacle);
    return BT::NodeStatus::SUCCESS;
  }
  distance_to_stop_target_ = getYieldStopDistance(route_lanelets);
  target_speed = calculateTargetSpeed();
  if (!target_speed) {
    target_speed = hdmap_utils->getSpeedLimit(route_lanelets);
  }
  setOutput("updated_status", calculateEntityStatusUpdated(target_speed.get()));
  const auto waypoints = calculateWaypoints();
  if (waypoints.waypoints.empty()) {
    return BT::NodeStatus::FAILURE;
  }
  const auto obstacle = calculateObstacle(waypoints);
  setOutput("waypoints", waypoints);
  setOutput("obstacle", obstacle);
  return BT::NodeStatus::RUNNING;
}
}  // namespace follow_lane_sequence
}  // namespace vehicle
}  // namespace entity_behavior


#include <ctime>

#include <boost/date_time/posix_time/posix_time.hpp>

#include "datetime.hpp"

using namespace std;

namespace pgl {
	int64_t get_unix_time(const int year, const int month, const int day, const int hours, const int minutes,
		const int seconds) {
		const boost::posix_time::ptime epoch(boost::gregorian::date(1970, 1, 1));
		const boost::posix_time::ptime date_time(boost::gregorian::date(static_cast<const unsigned short>(year),
				static_cast<const unsigned short>(month), static_cast<const unsigned short>(day)),
			boost::posix_time::time_duration(hours, minutes, seconds));
		return (date_time - epoch).total_seconds();
	}

	void get_boost_ptime_from_unix_time(const int64_t unix_time, boost::posix_time::ptime& ptime) {
		ptime = boost::posix_time::ptime(boost::gregorian::date(1970, 1, 1), boost::posix_time::seconds(unix_time));
	}

	datetime::datetime(const int year, const int month, const int day): datetime(year, month, day, 0, 0, 0) {}

	datetime::datetime(const int year, const int month, const int day, const int hour, const int minute,
		const int second): unix_time_(get_unix_time(year, month, day, hour, minute, second)) {}

	int datetime::year() const {
		boost::posix_time::ptime ptime;
		get_boost_ptime_from_unix_time(unix_time_, ptime);
		return ptime.date().year();
	}

	int datetime::month() const {
		boost::posix_time::ptime ptime;
		get_boost_ptime_from_unix_time(unix_time_, ptime);
		return ptime.date().month();
	}

	int datetime::day() const {
		boost::posix_time::ptime ptime;
		get_boost_ptime_from_unix_time(unix_time_, ptime);
		return ptime.date().day();
	}

	int datetime::hour() const {
		boost::posix_time::ptime ptime;
		get_boost_ptime_from_unix_time(unix_time_, ptime);
		return static_cast<int>(ptime.time_of_day().hours());
	}

	int datetime::minute() const {
		boost::posix_time::ptime ptime;
		get_boost_ptime_from_unix_time(unix_time_, ptime);
		return static_cast<int>(ptime.time_of_day().minutes());
	}

	int datetime::second() const {
		boost::posix_time::ptime ptime;
		get_boost_ptime_from_unix_time(unix_time_, ptime);
		return static_cast<int>(ptime.time_of_day().seconds());
	}

	size_t datetime::get_hash() const {
		return boost::hash_value(unix_time_);
	}

	bool datetime::operator<(const datetime& other) const {
		return unix_time_ < other.unix_time_;
	}

	bool datetime::operator==(const datetime& other) const {
		return unix_time_ == other.unix_time_;
	}

	datetime datetime::now() {
		const auto date_time = boost::posix_time::second_clock::universal_time();
		const auto date = date_time.date();
		const auto time = date_time.time_of_day();
		return datetime(date.year(), date.month(), date.day(), static_cast<int>(time.hours()),
			static_cast<int>(time.minutes()), static_cast<int>(time.seconds()));
	}

	string get_now_datetime_string() {
		const auto date_time = boost::posix_time::microsec_clock::universal_time();
		ostringstream oss;
		// deleting of std::locale::facet mey not be necessary.
		auto facet = new boost::posix_time::time_facet();
		facet->format("%Y-%m-%d %H:%M:%S.%f UTC");
		oss.imbue(locale(locale::classic(), facet));
		oss << date_time;
		return oss.str();
	}
}


// Copyright (c) 2017 Franka Emika GmbH; Bristol Robotics Laboratory (UK).
// Use of this source code is governed by the Apache-2.0 license, see LICENSE

/*****************************************************************************
Author: Yuan (Vincent) Guan
Email: yuan2.guan@brl.ac.uk / vincentguan@ieee.org
 For questions, comments or bug reports, please send email or check the Github
 repo at:
    https://github.com/VinceGuan/Franka_Teleop/tree/main/examples
File:
  HD_Franks_Simple_Motion_Mirror.cpp
Description:
  an example showing how to teleoperate the Franka Panda Robot using 3DSystem
  Touch haptic device. This application is currently a LTS version.
*******************************************************************************/

#include <cstdlib>
#include <cstdio>
#include <iostream>
#include <chrono>
#include <thread>

#include <HD/hd.h>
#include <HDU/hdu.h>
#include <HDU/hduError.h>
#include <HDU/hduVector.h>

#include <array>
#include <cmath>
#include <functional>
#include <SDL2/SDL.h>

#include <Eigen/Dense>
#include <Eigen/SVD>
#include <Eigen/QR>

#include <franka/duration.h>
#include <franka/exception.h>
#include <franka/model.h>
#include <franka/robot.h>

#include "examples_common.h"

/* <Graphics> */
#if defined(WIN32)
# include <windows.h>
#endif

#if defined(WIN32) || defined(linux)
# include <GL/glut.h>
#elif defined(__APPLE__)
# include <GLUT/glut.h>
#endif

/* <Static> */
static HHD ghHD = HD_INVALID_HANDLE;
static HDSchedulerHandle hUpdateDeviceCallback = HD_INVALID_HANDLE;
static bool isRunning = true;

/* <Struct> */
typedef struct
{
  hduVector3Dd start_position;
  hduVector3Dd Delta_position;
  HDdouble Robot_transform[16];
  HDdouble HD_transform[16];
  hduVector3Dd anchor;
  HDboolean isAnchorActive;
  hduVector3Dd Feedback_Force;
} HapticState;

/* <Function> */
void exitHandler();
void initHD();
void Check_KeyEvent();
//
HDCallbackCode HDCALLBACK hapticCallback(void *pUserData);

/* <Global> */
//Eigen::MatrixXd pinv_Eigen_SVD(Eigen::Map<const Eigen::Matrix<double, 6, 7>> map);
Eigen::MatrixXd pinv_Eigen_SVD(Eigen::MatrixXd &origin);
// double time_now = 0;
SDL_Event Event;
HapticState state;
SDL_Window *window = nullptr;

/**
 * @Application HD_Franks_Simple_Motion_Mirror.cpp
 *  is an example showing how to teleoperate the Franka Panda Robot using 3DSystem Touch
 *  haptic device.
 *
 * @Warning Before executing this example, make sure there is enough space in front of the robot.
 */

int main(int argc, char** argv)
{
  // Check whether the required arguments were passed
  if (argc != 2) {
    std::cerr << "Usage: " << argv[0] << " <robot-hostname>" << std::endl;
    return -1;
  }
  /// -----------------------------------------------------------------------------------
  /////////////////////////////////// <START> Init window ///////////////////////////////
  if(SDL_Init(SDL_INIT_VIDEO) < 0) {
    std::cout << "SDL Video Initialisation Error: " << SDL_GetError() << std::endl;
  }
  else {
    window = SDL_CreateWindow("A Window Title",SDL_WINDOWPOS_CENTERED, SDL_WINDOWPOS_CENTERED,640,480,SDL_WINDOW_SHOWN);
    if (window == nullptr) {
      std::cout << "SDL Window Initialisation Error: " << SDL_GetError() << std::endl;
    }
    else {
      SDL_UpdateWindowSurface(window);
    }
  }
  //////////////////////////////////// <END> Init window ////////////////////////////////

  /// -----------------------------------------------------------------------------------
  ////////////////// <START> Register the Exit signal handle ////////////////////////////
  if (std::atexit(exitHandler) != 0) {
    std::cout << "Failed to register the Exit Handler" << std::endl;
  }
  /////////////////// <END> Register the Exit signal handle ///////////////////////

  /// -----------------------------------------------------------------------------------
  ///////////////////////// <START> Creating new thread ///////////////////////
  std::thread Check_KeyEvent_thread(Check_KeyEvent);
  ////////////////////////// <END> Creating new thread ////////////////////////

  /// -----------------------------------------------------------------------------------
  ///////////////////////// <START> Init HD API and find device ///////////////////////
  initHD();
  std::cout << "HDAPI Initialised." << std::endl;
  ///////////////////////// <START> Init HD API and find device ///////////////////////

  /// -----------------------------------------------------------------------------------

  // TODO: raise a flag of state data ready. after that we can do the control.
  std::cout << state.Delta_position << std::endl;

  /// -----------------------------------------------------------------------------------
  ////////////////////////// <START> Initiate Stiffness and Damping matrix ////////////////////////
  // Compliance parameters
  const double translational_stiffness{1000.0};
  const double rotational_stiffness{30.0};
  const double null_stiffness{20.0};
  Eigen::MatrixXd stiffness(6, 6), damping(6, 6);
  stiffness.setZero();
  stiffness.topLeftCorner(3, 3) << translational_stiffness * Eigen::MatrixXd::Identity(3, 3);
  stiffness.bottomRightCorner(3, 3) << rotational_stiffness * Eigen::MatrixXd::Identity(3, 3);
  // Toggle this to have a higher stiffness in the z-axis.
  //stiffness(2,2) = 10 * stiffness(2,2);
  damping.setZero();
  damping.topLeftCorner(3, 3) << 2.0 * sqrt(translational_stiffness) *
                                 Eigen::MatrixXd::Identity(3, 3);
  damping.bottomRightCorner(3, 3) << 2.0 * sqrt(rotational_stiffness) *
                                     Eigen::MatrixXd::Identity(3, 3);
  ////////////////////////// <END> Initiate Stiffness and Damping matrix //////////////////////////
  /// -----------------------------------------------------------------------------------

  try {
    // connect to robot
    franka::Robot robot(argv[1]);
    setDefaultBehavior(robot);
    // load the kinematics and dynamics model
    franka::Model model = robot.loadModel();
    // get the robot initial state
    franka::RobotState initial_state = robot.readOnce();

    // equilibrium point is the initial position
    // TODO: Usage of Eigen::Map (reuse memory of a variable as Eigen type)
    Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data()));
    Eigen::Vector3d position_d(initial_transform.translation());
    Eigen::Quaterniond orientation_d(initial_transform.linear());

    /// -----------------------------------------------------------------------------------
    ////////////////////////// <START> For sake of Debugging /////////////////////////////
    std::cout << position_d << std::endl;
    std::cout << orientation_d.w() << "-w "
              << orientation_d.x() << "-x "
              << orientation_d.y() << "-y "
              << orientation_d.z() << "-z "
              << std::endl;

    Eigen::Affine3d F2EE_transform(Eigen::Matrix4d::Map(initial_state.F_T_EE.data()));
    Eigen::Vector3d F2EE_position_d(F2EE_transform.translation());
    Eigen::Quaterniond F2EE_orientation_d(F2EE_transform.linear());

    std::cout << F2EE_position_d << std::endl;
    std::cout << F2EE_orientation_d.w() << "-w "
              << F2EE_orientation_d.x() << "-x "
              << F2EE_orientation_d.y() << "-y "
              << F2EE_orientation_d.z() << "-z "
              << std::endl;

    // TODO: Clear this after usage for debugging purpose.
    std::cout << "... Debugging. Please press Enter to continue.";
    std::cin.ignore();
    ////////////////////////// <END> For sake of Debugging /////////////////////////////

    /// -----------------------------------------------------------------------------------
    // set collision behavior
    robot.setCollisionBehavior({{200.0, 200.0, 200.0, 200.0, 200.0, 200.0, 200.0}},
                               {{200.0, 200.0, 200.0, 200.0, 200.0, 200.0, 200.0}},
                               {{200.0, 200.0, 200.0, 200.0, 200.0, 200.0}},
                               {{200.0, 200.0, 200.0, 200.0, 200.0, 200.0}});

    double time = 0.0;
    std::array<double, 16> initial_pose;
    while (1) {
      try {
        /// -----------------------------------------------------------------------------------
        ////////////////// <START> Define torque controller callback ////////////////////////
        // define callback for the torque control loop
        std::function<franka::Torques(const franka::RobotState&, franka::Duration)>
            impedance_control_callback = [&](const franka::RobotState& robot_state,
                                             franka::Duration duration) -> franka::Torques {
          // Get starting timepoint
          auto start = std::chrono::high_resolution_clock::now();

          // get state variables
          std::array<double, 7> coriolis_array = model.coriolis(robot_state);
          std::array<double, 42> jacobian_array =
              model.zeroJacobian(franka::Frame::kEndEffector, robot_state);

          // convert to Eigen
          Eigen::Map<const Eigen::Matrix<double, 7, 1>> coriolis(coriolis_array.data());
          Eigen::Map<const Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());
          Eigen::Map<const Eigen::Matrix<double, 7, 1>> q(robot_state.q.data());
          Eigen::Map<const Eigen::Matrix<double, 7, 1>> dq(robot_state.dq.data());
          Eigen::Map<const Eigen::Matrix<double, 6, 1>> tau_ext(robot_state.O_F_ext_hat_K.data());
          Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data()));
          Eigen::Affine3d transform_hd;
          Eigen::Affine3d transform_offset;
          Eigen::Vector3d position(transform.translation());
          Eigen::Quaterniond orientation(transform.linear());

          if (time == 0.0) {
            initial_pose = robot_state.O_T_EE_c;
          }

          time += duration.toSec();

          position_d(0) = initial_transform.translation()[0] + 0.002 * state.Delta_position[0];
          position_d(1) = initial_transform.translation()[1] - 0.002 * state.Delta_position[2];
          position_d(2) = initial_transform.translation()[2] + 0.002 * state.Delta_position[1];

          // TODO: Orientation
          if (state.isAnchorActive) {
             transform_hd = Eigen::Matrix4d::Map(state.HD_transform);
//             std::cout << transform.linear() << "  " << std::endl;
             Eigen::AngleAxisd rot_90_x (M_PI/2, Eigen::Vector3d(1, 0, 0));
             orientation_d = rot_90_x.toRotationMatrix() * transform_hd.linear() * rot_90_x.toRotationMatrix();
//             orientation_d = (initial_transform * transform.linear()).linear();
//             Eigen::Transform<double,3,2> t = Eigen::AngleAxis(&rotation_vector);
             std::cout << rot_90_x.toRotationMatrix() * transform_hd.linear() * rot_90_x.toRotationMatrix() << std::endl;

           // std::cout <<  transform_hd.linear()  << std::endl;
          }

          /// compute error to desired equilibrium pose
          /// position error
          Eigen::Matrix<double, 6, 1> error;
          error.head(3) << position - position_d;

          /// orientation error
          // "difference" quaternion
          // std::cout << orientation_d.coeffs() << std::endl;
          if (orientation_d.coeffs().dot(orientation.coeffs()) < 0.0) {
            orientation.coeffs() << -orientation.coeffs();
          }
          // "difference" quaternion
          Eigen::Quaterniond error_quaternion(orientation.inverse() * orientation_d);
          error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z();
          // Transform to base frame
          error.tail(3) << -transform.linear() * error.tail(3);

          // TODO: null space
          double null_error;
          null_error = q(0) - 0.0;
          double task_null = null_stiffness * null_error + 2.0 * dq(0);

          Eigen::MatrixXd I = Eigen::MatrixXd::Identity(7, 7);
          Eigen::MatrixXd pinv_jacobian =
              jacobian.transpose() * (jacobian * jacobian.transpose()).inverse();
//                Eigen::MatrixXd pinv_jacobian = pinv_Eigen_SVD(jacob);
//           jacobian.completeOrthogonalDecomposition().pseudoInverse();

          // compute control
          Eigen::VectorXd tau_task(7), tau_d(7);

          // Spring damper system with damping ratio=1
          tau_task << jacobian.transpose() * (-stiffness * error - damping * (jacobian * dq));
          // tau_task.setZero();

          Eigen::MatrixXd q_null = Eigen::MatrixXd::Zero(7, 1);
          q_null(0) = -task_null;

          tau_d << tau_task + coriolis +
                       (I - jacobian.transpose() * pinv_jacobian.transpose()) * 2.0 * q_null;

          //////////////////////////////////////////
          // try this on first and then try SVD to compute.
          // jacobian.completeOrthogonalDecomposition().pseudoInverse();
          // pinv_Eigen_SVD(jacobian);
          ////////////////////////////////////////////

          // TODO: Assign the feedback force
          /*Eigen::Matrix<double, 6 ,1> Wrench_ext;
    //      Wrench_ext = (stiffness * error + damping * (jacobian * dq));
          Wrench_ext = jacobian * tau_d;
          const double force_feedback_ratio(-0.5);
          state.Feedback_Force.set(force_feedback_ratio * Wrench_ext[0], force_feedback_ratio *
    Wrench_ext[2], force_feedback_ratio * Wrench_ext[1]);
    //      std::cout << state.Feedback_Force.magnitude() << std::endl;*/

          Eigen::Matrix<double, 6, 1> Wrench_ext(tau_ext);
          const double force_feedback_ratio(-0.1);
          state.Feedback_Force.set(force_feedback_ratio * Wrench_ext[0],
                                   force_feedback_ratio * Wrench_ext[2],
                                   force_feedback_ratio * Wrench_ext[1]);

          std::array<double, 7> tau_d_array{};
          Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d;

          // Toggle this to enter the guiding mode
          // tau_d_array = {{0,0,0,0,0,0,0}};

          if (!isRunning) {
            std::cout << "Here finishsssss" << std::endl;
            return franka::MotionFinished(franka::Torques(tau_d_array));
          }

          auto stop = std::chrono::high_resolution_clock::now();

          auto dur = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);

//          std::cout << dur.count() << std::endl;

          return tau_d_array;
        };

        ///////////////////// <END> Define torque controller callback /////////////////////////

        /// -----------------------------------------------------------------------------------
        ////////////////// <START> Ask the user to start the control loop /////////////////////
        // start real-time control loop
        //    std::cout << "WARNING: Collision thresholds are set to high values. "
        //              << "Make sure you have the user stop at hand!" << std::endl
        //              << "After starting try to push the robot and see how it reacts." << std::endl
        //              << "Press Enter to continue..." << std::endl;
        //    std::cin.ignore();

        // pass the controller callback function ptr to the control
        robot.control(impedance_control_callback);
        /////////////////// <END> Ask the user to start the control loop /////////////////////

      } catch (const franka::Exception& ex) {
        // print exception
        std::cout << ex.what() << std::endl;
        std::cout << "Running error recovery..." << std::endl;
        robot.automaticErrorRecovery();
      }
    }

    /// -----------------------------------------------------------------------------------
//    while (isRunning) {
//      // TODO:wait....
//      std::cout << "is running3" << std::endl;
//    }

  } catch (const franka::Exception& ex) {
    // print exception
    std::cout << ex.what() << std::endl;
  }

  std::cout << "Please wait, we will quit the program..." << std::endl;
  //return 0;
  std::exit(0);
}

/**
 * @Function Check_KeyEvent
 *  Function used to monitor the keyboard events.
 * @in Null
 * @return Null
 */
void Check_KeyEvent ()
{
  std::cout << "is running1" << std::endl;
  while (isRunning) {
    while (SDL_PollEvent(&Event) != 0) {
      if (Event.type == SDL_KEYDOWN) {
        switch (Event.key.keysym.sym) {
          case SDLK_q:
            std::cout << "is running2" << SDL_PollEvent(&Event) << std::endl;
            isRunning = false;
            std::exit(0);
            //break;
        }
      }
    }
  }
}

/**
 * @Function exitHandler()
 *  The handler get called when the application is exiting to shutdown the HD API.
 */
void exitHandler()
{
  std::cout << "On Exit ..." << std::endl;
  hdStopScheduler();
  hdUnschedule(hUpdateDeviceCallback);
  if (ghHD != HD_INVALID_HANDLE)
  {
    hdDisableDevice(ghHD);
    ghHD = HD_INVALID_HANDLE;
  }
  std::cout << "Safely quit the haptic device ..." << std::endl;
  SDL_DestroyWindow(window);
  SDL_Quit();
  std::cout << "Safely quit the SDL and destroy the window ..." << std::endl;
  std::cout << "Now quit ..." << std::endl;
}

/**
 * @Function initHD()
 *  Initializes the HDAPI.  This involves initing a device configuration, enabling
 *  forces, and scheduling a haptic thread callback for servicing the device.
 */
void initHD()
{
  // To capture any HD Error message
  HDErrorInfo error;
  // Init the HD_DEFAULT_DEVICE
  ghHD = hdInitDevice(HD_DEFAULT_DEVICE);
  if (HD_DEVICE_ERROR(error = hdGetError())) {
    hduPrintError(stderr, &error, "Failed to initialize haptic device");
    fprintf(stderr, "\nPress any key to quit.\n");
    getchar();
    exit(-1);
  }

  // Enable the force feedback
  hdEnable(HD_FORCE_OUTPUT);

  // Creating Asynchronous callback handle
  // TODO: See if can use nullptr to replace nUserData.
  hUpdateDeviceCallback = hdScheduleAsynchronous(
      hapticCallback, 0, HD_MAX_SCHEDULER_PRIORITY);

  // Start the HD scheduler.
  hdStartScheduler();
  if (HD_DEVICE_ERROR(error = hdGetError())) {
    hduPrintError(stderr, &error, "Failed to start the scheduler");
    exit(-1);
  }
}

/**
 * @Function pinv_eigen_based
 *  This is the main haptic rendering callback.  This callback will render an
 *  anchored spring force when the user presses the button.
 */
Eigen::MatrixXd pinv_Eigen_SVD(Eigen::MatrixXd &origin) {

  const float er = 0;
  // perform svd decomposition
  Eigen::JacobiSVD<Eigen::MatrixXd> svd_holder(origin, Eigen::ComputeThinU | Eigen::ComputeThinV);

  // Build SVD decomposition results
  Eigen::MatrixXd U = svd_holder.matrixU();
  Eigen::MatrixXd V = svd_holder.matrixV();
  Eigen::MatrixXd D = svd_holder.singularValues();

  // Build the S matrix
  Eigen::MatrixXd S(V.cols(), U.cols());
  S.setZero();

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

    if (D(i, 0) > er) {
      S(i, i) = 1 / D(i, 0);
    } else {
      S(i, i) = 0;
    }
  }

  // pinv_matrix = V * S * U^T
  return V * S * U.transpose();
}

/**
 * @Callback_Function hapticCallback
 *  This is the main haptic rendering callback.  This callback will render an
 *  anchored spring force when the user presses the button.
 */
HDCallbackCode HDCALLBACK hapticCallback(void *pUserData)
{
  static const HDdouble kAnchorStiffness = 0.1;
  hduVector3Dd start_position, position;
  hduVector3Dd force = { 0, 0, 0 };
  HDdouble transform[16];
  int currentButtons, lastButtons;
  HDdouble forceClamp;
  HDErrorInfo error;

  /// -----------------------------------------------------------------------------------
  ///////////////////// <START> HD frame //////////////////////
  hdBeginFrame(ghHD);

  hdGetIntegerv(HD_CURRENT_BUTTONS, &currentButtons);
  hdGetIntegerv(HD_LAST_BUTTONS, &lastButtons);
  hdGetDoublev(HD_CURRENT_POSITION, position);
  hdGetDoublev(HD_CURRENT_TRANSFORM, transform);

//  std::cout << "------------" << std::endl;
//  std::cout << transform[0] << "  " << transform[4] << "  " << transform[8] << std::endl;
//  std::cout << transform[1] << "  " << transform[5] << "  " << transform[9] << std::endl;
//  std::cout << transform[2] << "  " << transform[6] << "  " << transform[10] << std::endl;

  // Detect button state transitions.
  if ((currentButtons & HD_DEVICE_BUTTON_1) != 0 &&
      (lastButtons & HD_DEVICE_BUTTON_1) == 0)
  {
    state.isAnchorActive = HD_TRUE;
    hdGetDoublev(HD_CURRENT_POSITION, state.start_position);
  }
  else if ((currentButtons & HD_DEVICE_BUTTON_1) == 0 &&
           (lastButtons & HD_DEVICE_BUTTON_1) != 0)
  {
    state.isAnchorActive = HD_FALSE;
  }

  // Force Computing and Clamping
  if (state.isAnchorActive)
  {
    hdGetDoublev(HD_CURRENT_POSITION, position);
    hdGetDoublev(HD_CURRENT_TRANSFORM, state.HD_transform);

    // Compute force that will attact the device towards the anchor position:
    // F = k * (anchor - position)
    hduVecSubtract(force, state.anchor, position);
    hduVecScaleInPlace(force, kAnchorStiffness);

    force = state.Feedback_Force;
    hduVecScaleInPlace(force, 1.0);

    // Clamp the force.
    hdGetDoublev(HD_NOMINAL_MAX_CONTINUOUS_FORCE, &forceClamp);
    if (hduVecMagnitude(force) > forceClamp)
    {
      hduVecNormalizeInPlace(force);
      hduVecScaleInPlace(force, forceClamp);
    }
  }
  else
  {
    force.set(0,0,0);
    position.set(0,0,0);
    state.start_position.set(0,0,0);
  }

  // Update the haptic device state
  hduVecSubtract(state.Delta_position, position, state.start_position);

  // Render the force
  hdSetDoublev(HD_CURRENT_FORCE, force);
  //std::cout << force << std::endl;

  //time_now += 0.001;

  hdEndFrame(ghHD);
  ///////////////////// <END> HD frame //////////////////////

  /// -----------------------------------------------------------------------------------
  if (HD_DEVICE_ERROR(error = hdGetError()))
  {
    if (hduIsForceError(&error))
    {
      /* Disable the anchor following the force error. */
      //
    }
    else
    {
      /* This is likely a more serious error, so bail. */
      hduPrintError(stderr, &error, "Error during haptic rendering");
      exit(-1);
    }
  }

  return HD_CALLBACK_CONTINUE;
}

/* --------- End of the File ----------- */


/*******************************************************************************
 *
 * MIT License
 *
 * Copyright (c) 2017 Advanced Micro Devices, Inc.
 *
 * 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 <miopen/solver.hpp>

#include <miopen/env.hpp>
#include <miopen/kernel_build_params.hpp>
#include <miopen/stringutils.hpp>
#include <miopen/conv/compiled_in_parameters.hpp>
#include <miopen/conv/data_invoke_params.hpp>
#include <miopen/conv/tensors.hpp>

#include <boost/any.hpp>

MIOPEN_DECLARE_ENV_VAR(MIOPEN_DEBUG_AMD_WINOGRAD_3X3)

namespace miopen {
namespace solver {

bool ConvBinWinograd3x3U::IsApplicable(const ConvolutionContext& params) const
{
    if(miopen::IsDisabled(MIOPEN_DEBUG_AMD_WINOGRAD_3X3{}))
        return false;
    if(!params.Is2d())
        return false;
    if(!(params.direction.IsForward() || params.direction.IsBackwardData()))
        return false;
    if(!(params.rmv.IsV2orV3() && params.use_asm_kernels))
        return false;

    const auto name = params.GetStream().GetDeviceName();
    if(!(name == "gfx803" || name == "gfx900" || name == "gfx906" || name == "gfx908"))
        return false;

    // Check if kernel is suitable for the problem description
    // and able to correctly run with given parameters.
    const auto device_is_gfx8         = StartsWith(name, "gfx8");
    const auto grid_workgroup_count_x = params.GetStream().GetMaxComputeUnits();
    if(!params.IsLayoutDefault())
    {
        return false;
    }

    // clang-format off
    return params.pad_w == 1
        && params.pad_h == 1
        && params.kernel_size_w == 3
        && params.kernel_size_h == 3
        && params.kernel_stride_w == 1
        && params.kernel_stride_h == 1
        && params.kernel_dilation_w == 1
        && params.kernel_dilation_h == 1
        && params.batch_sz < std::pow(2, 16)
        && params.n_inputs < std::pow(2, 16)
        && params.n_outputs < std::pow(2, 16)
        && params.in_height < std::pow(2, 16)
        && params.in_width < std::pow(2, 16)
        && grid_workgroup_count_x < std::pow(2, 16)
        && (params.n_inputs * params.in_height * params.in_width) <= std::pow(2, 28)
        && (params.n_outputs * params.in_height * params.in_width) <= std::pow(2, 28)
        && (params.n_inputs * params.kernel_size_w * params.kernel_size_h) <= std::pow(2, 28)
        && (params.n_outputs * params.kernel_size_w * params.kernel_size_h) <= std::pow(2, 28)
        && params.n_inputs % 2 == 0
        && params.n_inputs >= (device_is_gfx8 ? 16 : 18)
        && params.IsFp32()
        && params.group_counts == 1
        && params.in_layout == "NCHW";
        /// && (isForwardDirection() ? _weights_layout == "KCHW" : _weights_layout == "CKHW" )
        /// Actually, K<->C flpping is controlled by separate flag, so we can support either
        /// layout in both directions.
    // clang-format on
}

ConvSolution ConvBinWinograd3x3U::GetSolution(const ConvolutionContext& params) const
{
    ConvSolution result;
    const auto n_groups = params.GetStream().GetMaxComputeUnits();
    const auto name     = params.GetStream().GetDeviceName();

    KernelInfo kernel;

    kernel.g_wk.clear();
    kernel.g_wk.push_back(512 * n_groups);
    kernel.g_wk.push_back(1);
    kernel.g_wk.push_back(1);

    kernel.l_wk.clear();
    kernel.l_wk.push_back(512);
    kernel.l_wk.push_back(1);
    kernel.l_wk.push_back(1);

    kernel.kernel_name = "miopenSp3AsmConv3x3F";

    KernelBuildParameters options{
        {"ROCM_METADATA_VERSION", params.rmv.UseV3() ? 5 : 4},
    };
    kernel.comp_options = options.GenerateFor(kbp::GcnAsm{});

    if(StartsWith(name, "gfx8"))
        kernel.kernel_file = "conv_3x3_wheel_alpha_v3_0b.s";
    else if(StartsWith(name, "gfx9"))
        kernel.kernel_file = "conv_3x3_wheel_alpha_v7_0_3b.s";
    else
        MIOPEN_THROW("Unsupported device.");

    result.construction_params.push_back(kernel);

    const auto is_forward = params.direction.IsForward();

    result.invoker_factory = [=](const std::vector<Kernel>& kernels) {
        constexpr int F_REVERSE_R = 1 << 0;
        constexpr int F_REVERSE_S = 1 << 1;
        constexpr int F_FLIP_K_C  = 1 << 2;
        // These are not used yet. Nevertheless let's keep as a shader documentation.
        // constexpr int F_FLIP_DATA_N_C = 1 << 3; // Unsupported in f3x2.
        // constexpr int F_FLIP_OUT_N_K = 1 << 4; // Unsupported in f3x2.
        // constexpr int L_F_ADDR_INDIRECT  = 1 << 6;
        // constexpr int L_F_BIAS  = 1 << 7;
        // constexpr int L_F_LEAKY_RELU  = 1 << 8;

        // not used in this particular kernel
        // constexpr int L_F_NKC_STRIDES = 1 << 9;

        int flags         = is_forward ? 0 : F_REVERSE_R + F_REVERSE_S + F_FLIP_K_C;
        int reserved      = 0;
        int* reserved_ptr = nullptr;
        int N, C, H, W, K, n_groups_, out_H, out_W, R, S, pad_H, pad_W;
        GetCompiledInParameters(
            params, &N, &C, &H, &W, &K, &n_groups_, &out_H, &out_W, &R, &S, &pad_H, &pad_W);
        MIOPEN_LOG_I2(" N=" << N << " C=" << C << " H=" << H << " W=" << W << " K=" << K
                            << " n_groups=" << n_groups_ << " flags=" << flags << " R=" << R
                            << " S=" << S << " pad_H=" << pad_H << " pad_W=" << pad_W
                            << " out_H=" << out_H << " out_W=" << out_W);

        return [=](const Handle& handle, const AnyInvokeParams& ctx) {
            const auto k        = handle.Run(kernels[0]);
            const auto& fwd_ctx = ctx.CastTo<conv::DataInvokeParams>();
            const auto& tensors = fwd_ctx.tensors;

            k(N,
              C,
              H,
              W,
              K,
              n_groups_,
              flags,
              reserved,
              tensors.in,
              tensors.w,
              tensors.out,
              reserved_ptr);
        };
    };

    return result;
}
} // namespace solver
} // namespace miopen


#include "Map.hpp"
#include "utility/MathHelper.hpp"

#include <boost/python.hpp>
#include <memory>
#include <string>
#include <thread>
#include <vector>

namespace py = boost::python;

namespace
{
py::list python_find_path(const pathfind::Map& map, float start_x,
                          float start_y, float start_z, float stop_x,
                          float stop_y, float stop_z)
{
    py::list result;

    const math::Vertex start {start_x, start_y, start_z};
    const math::Vertex stop {stop_x, stop_y, stop_z};

    std::vector<math::Vertex> path;

    if (map.FindPath(start, stop, path))
        for (auto const& point : path)
            result.append(py::make_tuple(point.X, point.Y, point.Z));

    return result;
}

py::tuple load_adt_at(pathfind::Map& map, float x, float y)
{
    int adt_x, adt_y;

    math::Convert::WorldToAdt({x, y, 0.f}, adt_x, adt_y);

    if (!map.HasADT(adt_x, adt_y))
        throw std::runtime_error("Requested ADT does not exist for map");

    if (!map.LoadADT(adt_x, adt_y))
        throw std::runtime_error("Failed to load requested ADT");

    return py::make_tuple(adt_x, adt_y);
}

py::list python_query_heights(const pathfind::Map& map, float x, float y)
{
    py::list result;

    std::vector<float> height_values;

    if (map.FindHeights(x, y, height_values))
        for (auto const& z : height_values)
            result.append(z);

    return result;
}

py::tuple get_zone_and_area(pathfind::Map& map, float x, float y, float z)
{
    math::Vertex p {x, y, z};
    unsigned int zone = -1, area = -1;
    // TODO: check return value and figure out how to return None
    map.ZoneAndArea(p, zone, area);
    return py::make_tuple(zone, area);
}
} // namespace

BOOST_PYTHON_MODULE(pathfind)
{
    py::class_<pathfind::Map, boost::noncopyable>(
        "Map", py::init<const std::string&, const std::string&>())
        .def("load_all_adts", &pathfind::Map::LoadAllADTs)
        .def("load_adt_at", &load_adt_at)
        .def("find_path", &python_find_path)
        .def("query_z", &python_query_heights)
        .def("get_zone_and_area", &get_zone_and_area);
}


#ifndef STAN_MATH_PRIM_SCAL_FUN_ROUND_HPP
#define STAN_MATH_PRIM_SCAL_FUN_ROUND_HPP

#include <stan/math/prim/scal/fun/boost_policy.hpp>
#include <stan/math/prim/scal/fun/is_nan.hpp>
#include <boost/math/special_functions/round.hpp>
#include <limits>

namespace stan {
namespace math {

/**
 * Return the closest integer to the specified argument, with
 * halfway cases rounded away from zero.
 *
 * @param x Argument.
 * @return The rounded value of the argument.
 */
inline double round(double x) {
  if (is_nan(x))
    return std::numeric_limits<double>::quiet_NaN();
  return boost::math::round(x, boost_policy_t());
}

/**
 * Return the closest integer to the specified argument, with
 * halfway cases rounded away from zero.
 *
 * @param x Argument.
 * @return The rounded value of the argument.
 */
inline double round(int x) { return round(static_cast<double>(x)); }

}  // namespace math
}  // namespace stan
#endif