/**
 * @file ra_model_impl.hpp
 * @author Ryan Curtin
 *
 * Implementation of the RAModel class.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#ifndef MLPACK_METHODS_RANN_RA_MODEL_IMPL_HPP
#define MLPACK_METHODS_RANN_RA_MODEL_IMPL_HPP

// In case it hasn't been included yet.
#include "ra_model.hpp"
#include <mlpack/core/math/random_basis.hpp>
#include <boost/serialization/variant.hpp>

namespace mlpack {
namespace neighbor {

//! Monochromatic search for the given RAType instance.
template<typename RAType>
void MonoSearchVisitor::operator()(RAType* ra) const
{
  if (ra)
    return ra->Search(k, neighbors, distances);
  throw std::runtime_error("no rank-approximate model initialized");
}

//! Save the parameters for the rank-approximate search.
template<typename SortPolicy>
BiSearchVisitor<SortPolicy>::BiSearchVisitor(const arma::mat& querySet,
                                 const size_t k,
                                 arma::Mat<size_t>& neighbors,
                                 arma::mat& distances,
                                 const size_t leafSize) :
    querySet(querySet),
    k(k),
    neighbors(neighbors),
    distances(distances),
    leafSize(leafSize)
{};

//! Default Bichromatic search on the given RAType instance.
template<typename SortPolicy>
template<template<typename TreeMetricType,
                  typename TreeStatType,
                  typename TreeMatType> class TreeType>
void BiSearchVisitor<SortPolicy>::operator()(RATypeT<TreeType>* ra) const
{
  if (ra)
    return ra->Search(querySet, k, neighbors, distances);
  throw std::runtime_error("no rank-approximate model initialized");
}

//! Bichromatic search on the given RAType specialized for KDTrees.
template<typename SortPolicy>
void BiSearchVisitor<SortPolicy>::operator()(RATypeT<tree::KDTree>* ra) const
{
  if (ra)
    return SearchLeaf(ra);
  throw std::runtime_error("no rank-approximate search model initialized");
}

//! Bichromatic search on the given RAType specialized for Octrees.
template<typename SortPolicy>
void BiSearchVisitor<SortPolicy>::operator()(RATypeT<tree::Octree>* ra) const
{
  if (ra)
    return SearchLeaf(ra);
  throw std::runtime_error("no rank-approximate search model initialized");
}

//! Bichromatic search on the given RAType considering the leafSize.
template<typename SortPolicy>
template<typename RAType>
void BiSearchVisitor<SortPolicy>::SearchLeaf(RAType* ra) const
{
  if (!ra->Naive() && !ra->SingleMode())
  {
    // Build a second tree and search
    Timer::Start("tree_building");
    Log::Info << "Building query tree...."<< std::endl;
    std::vector<size_t> oldFromNewQueries;
    typename RAType::Tree queryTree(std::move(querySet), oldFromNewQueries,
        leafSize);
    Log::Info << "Tree Built." << std::endl;
    Timer::Stop("tree_building");

    arma::Mat<size_t> neighborsOut;
    arma::mat distancesOut;
    ra->Search(&queryTree, k, neighborsOut, distancesOut);

    // Unmap the query points.
    distances.set_size(distancesOut.n_rows, distancesOut.n_cols);
    neighbors.set_size(neighborsOut.n_rows, neighborsOut.n_cols);
    for (size_t i = 0; i < neighborsOut.n_cols; ++i)
    {
      neighbors.col(oldFromNewQueries[i]) = neighborsOut.col(i);
      distances.col(oldFromNewQueries[i]) = distancesOut.col(i);
    }
  }
  else
  {
    // Search without building a second tree.
    ra->Search(querySet, k, neighbors, distances);
  }
}

//! Save parameters for the Train.
template<typename SortPolicy>
TrainVisitor<SortPolicy>::TrainVisitor(arma::mat&& referenceSet,
                                       const size_t leafSize) :
    referenceSet(std::move(referenceSet)),
    leafSize(leafSize)
{};

//! Default Train on the given RAType instance.
template<typename SortPolicy>
template<template<typename TreeMetricType,
                  typename TreeStatType,
                  typename TreeMatType> class TreeType>
void TrainVisitor<SortPolicy>::operator()(RATypeT<TreeType>* ra) const
{
  if (ra)
    return ra->Train(std::move(referenceSet));
  throw std::runtime_error("no rank-approximate search model initialized");
}

//! Train on the given RAType specialized for KDTrees.
template<typename SortPolicy>
void TrainVisitor<SortPolicy>::operator()(RATypeT<tree::KDTree>* ra) const
{
  if (ra)
    return TrainLeaf(ra);
  throw std::runtime_error("no rank-approximate search model initialized");
}

//! Train on the given RAType specialized for Octrees.
template<typename SortPolicy>
void TrainVisitor<SortPolicy>::operator()(RATypeT<tree::Octree>* ra) const
{
  if (ra)
    return TrainLeaf(ra);
  throw std::runtime_error("no rank-approximate search model is initialized");
}

//! Train on the given RAType considering the leafSize.
template<typename SortPolicy>
template<typename RAType>
void TrainVisitor<SortPolicy>::TrainLeaf(RAType* ra) const
{
  // Build tree, if necessary
  if (ra->Naive())
  {
    ra->Train(std::move(referenceSet));
  }
  else
  {
    std::vector<size_t> oldFromNewReferences;
    typename RAType::Tree* tree =
        new typename RAType::Tree(std::move(referenceSet), oldFromNewReferences,
        leafSize);
    ra->Train(tree);

    // Give the model ownership of the tree and the mappings.
    ra->treeOwner = true;
    ra->oldFromNewReferences = std::move(oldFromNewReferences);
  }
}

//! Exposes the SingleSampleLimit() method of the given RAType.
template<typename RAType>
size_t& SingleSampleLimitVisitor::operator()(RAType* ra) const
{
  if (ra)
    return ra->SingleSampleLimit();
  throw std::runtime_error("no rank-approximate search model is initialized");
}

//! Exposes the FirstLeafExact() method of the given RAType.
template<typename RAType>
bool& FirstLeafExactVisitor::operator()(RAType* ra) const
{
  if (ra)
    return ra->FirstLeafExact();
  throw std::runtime_error("no rank-approximate search model is initialized");
}

//! Exposes the SampleAtLeaves() method of the given RAType.
template<typename RAType>
bool& SampleAtLeavesVisitor::operator()(RAType* ra) const
{
  if (ra)
    return ra->SampleAtLeaves();
  throw std::runtime_error("no rank-approximate search model is initialized");
}

//! Exposes the Alpha() method of the given RAType instance.
template<typename RAType>
double& AlphaVisitor::operator()(RAType* ra) const
{
  if (ra)
    return ra->Alpha();
  throw std::runtime_error("no rank-approximate model is initialized");
}

//! Exposes the Tau() method of the given RAType instance.
template<typename RAType>
double& TauVisitor::operator()(RAType* ra) const
{
  if (ra)
    return ra->Tau();
  throw std::runtime_error("no rank-approximate model is initialized");
}

//! Exposes the SingleMode() method of the given RAType.
template<typename RAType>
bool& SingleModeVisitor::operator()(RAType* ra) const
{
  if (ra)
    return ra->SingleMode();
  throw std::runtime_error("no rank-approximate model is intialized");
}

//! Exposes the referenceSet of the given RAType.
template<typename RAType>
const arma::mat& ReferenceSetVisitor::operator()(RAType* ra) const
{
  if (ra)
    return ra->ReferenceSet();
  throw std::runtime_error("no rank-approximate model is intialized");
}

//! Exposes the Naive() method of the given RAType instance.
template<typename RAType>
bool& NaiveVisitor::operator()(RAType* ra) const
{
  if (ra)
    return ra->Naive();
  throw std::runtime_error("no rank-approximate search model is intialized");
}

//! For cleaning memory
template<typename RSType>
void DeleteVisitor::operator()(RSType* rs) const
{
  if (rs)
    delete rs;
}

template<typename SortPolicy>
RAModel<SortPolicy>::RAModel(const TreeTypes treeType, const bool randomBasis) :
    treeType(treeType),
    leafSize(20),
    randomBasis(randomBasis)
{
  // Nothing to do.
}

// Copy constructor.
template<typename SortPolicy>
RAModel<SortPolicy>::RAModel(const RAModel& other) :
    treeType(other.treeType),
    leafSize(other.leafSize),
    randomBasis(other.randomBasis),
    q(other.q),
    raSearch(other.raSearch)
{
  // Nothing to do.
}

// Move constructor.
template<typename SortPolicy>
RAModel<SortPolicy>::RAModel(RAModel&& other) :
    treeType(other.treeType),
    leafSize(other.leafSize),
    randomBasis(other.randomBasis),
    q(std::move(other.q)),
    raSearch(std::move(other.raSearch))
{
  // Clear other model.
  other.treeType = TreeTypes::KD_TREE;
  other.leafSize = 20;
  other.randomBasis = false;
  other.raSearch = decltype(other.raSearch)();
}

// Copy operator.
template<typename SortPolicy>
RAModel<SortPolicy>& RAModel<SortPolicy>::operator=(const RAModel& other)
{
  // Clear current model.
  boost::apply_visitor(DeleteVisitor(), raSearch);

  treeType = other.treeType;
  leafSize = other.leafSize;
  randomBasis = other.randomBasis;
  q = other.q;
  raSearch = other.raSearch;

  return *this;
}

template<typename SortPolicy>
RAModel<SortPolicy>& RAModel<SortPolicy>::operator=(RAModel&& other)
{
  boost::apply_visitor(DeleteVisitor(), raSearch);

  treeType = other.treeType;
  leafSize = other.leafSize;
  randomBasis = other.randomBasis;
  q = std::move(other.q);
  raSearch = std::move(other.raSearch);

  // Reset other model.
  other.treeType = TreeTypes::KD_TREE;
  other.leafSize = 20;
  other.randomBasis = false;
  other.raSearch = decltype(other.raSearch)();

  return *this;
}

// Clean memory, if necessary
template<typename SortPolicy>
RAModel<SortPolicy>::~RAModel()
{
  boost::apply_visitor(DeleteVisitor(), raSearch);
}

template<typename SortPolicy>
template<typename Archive>
void RAModel<SortPolicy>::serialize(Archive& ar,
                                    const unsigned int /* version */)
{
  ar & BOOST_SERIALIZATION_NVP(treeType);
  ar & BOOST_SERIALIZATION_NVP(randomBasis);
  ar & BOOST_SERIALIZATION_NVP(q);

  // This should never happen, but just in case, be clean with memory.
  if (Archive::is_loading::value)
  {
    boost::apply_visitor(DeleteVisitor(), raSearch);
  }

  // We only need to serialize one of the kRANN objects.
  ar & BOOST_SERIALIZATION_NVP(raSearch);
}

template<typename SortPolicy>
const arma::mat& RAModel<SortPolicy>::Dataset() const
{
  return boost::apply_visitor(ReferenceSetVisitor(), raSearch);
}

template<typename SortPolicy>
bool RAModel<SortPolicy>::Naive() const
{
  return boost::apply_visitor(NaiveVisitor(), raSearch);
}

template<typename SortPolicy>
bool& RAModel<SortPolicy>::Naive()
{
  return boost::apply_visitor(NaiveVisitor(), raSearch);
}

template<typename SortPolicy>
bool RAModel<SortPolicy>::SingleMode() const
{
  return boost::apply_visitor(SingleModeVisitor(), raSearch);
}

template<typename SortPolicy>
bool& RAModel<SortPolicy>::SingleMode()
{
  return boost::apply_visitor(SingleModeVisitor(), raSearch);
}

template<typename SortPolicy>
double RAModel<SortPolicy>::Tau() const
{
  return boost::apply_visitor(TauVisitor(), raSearch);
}

template<typename SortPolicy>
double& RAModel<SortPolicy>::Tau()
{
  return boost::apply_visitor(TauVisitor(), raSearch);
}

template<typename SortPolicy>
double RAModel<SortPolicy>::Alpha() const
{
  return boost::apply_visitor(AlphaVisitor(), raSearch);
}

template<typename SortPolicy>
double& RAModel<SortPolicy>::Alpha()
{
  return boost::apply_visitor(AlphaVisitor(), raSearch);
}

template<typename SortPolicy>
bool RAModel<SortPolicy>::SampleAtLeaves() const
{
  return boost::apply_visitor(SampleAtLeavesVisitor(), raSearch);
}

template<typename SortPolicy>
bool& RAModel<SortPolicy>::SampleAtLeaves()
{
  return boost::apply_visitor(SampleAtLeavesVisitor(), raSearch);
}

template<typename SortPolicy>
bool RAModel<SortPolicy>::FirstLeafExact() const
{
  return boost::apply_visitor(FirstLeafExactVisitor(), raSearch);
}

template<typename SortPolicy>
bool& RAModel<SortPolicy>::FirstLeafExact()
{
  return boost::apply_visitor(FirstLeafExactVisitor(), raSearch);
}

template<typename SortPolicy>
size_t RAModel<SortPolicy>::SingleSampleLimit() const
{
  return boost::apply_visitor(SingleSampleLimitVisitor(), raSearch);
}

template<typename SortPolicy>
size_t& RAModel<SortPolicy>::SingleSampleLimit()
{
  return boost::apply_visitor(SingleSampleLimitVisitor(), raSearch);
}

template<typename SortPolicy>
size_t RAModel<SortPolicy>::LeafSize() const
{
  return leafSize;
}

template<typename SortPolicy>
size_t& RAModel<SortPolicy>::LeafSize()
{
  return leafSize;
}

template<typename SortPolicy>
typename RAModel<SortPolicy>::TreeTypes RAModel<SortPolicy>::TreeType() const
{
  return treeType;
}

template<typename SortPolicy>
typename RAModel<SortPolicy>::TreeTypes& RAModel<SortPolicy>::TreeType()
{
  return treeType;
}

template<typename SortPolicy>
bool RAModel<SortPolicy>::RandomBasis() const
{
  return randomBasis;
}

template<typename SortPolicy>
bool& RAModel<SortPolicy>::RandomBasis()
{
  return randomBasis;
}

template<typename SortPolicy>
void RAModel<SortPolicy>::BuildModel(arma::mat&& referenceSet,
                                     const size_t leafSize,
                                     const bool naive,
                                     const bool singleMode)
{
  // Initialize random basis, if necessary.
  if (randomBasis)
  {
    Log::Info << "Creating random basis..." << std::endl;
    math::RandomBasis(q, referenceSet.n_rows);
  }

  // Clean memory, if necessary.
  boost::apply_visitor(DeleteVisitor(), raSearch);

  this->leafSize = leafSize;

  if (randomBasis)
    referenceSet = q * referenceSet;

  if (!naive)
  {
    Timer::Start("tree_building");
    Log::Info << "Building reference tree..." << std::endl;
  }

  switch (treeType)
  {
    case KD_TREE:
      raSearch = new RAType<SortPolicy, tree::KDTree>(naive, singleMode);
      break;
    case COVER_TREE:
      raSearch = new RAType<SortPolicy, tree::StandardCoverTree>(naive,
          singleMode);
      break;
    case R_TREE:
      raSearch = new RAType<SortPolicy, tree::RTree>(naive, singleMode);
      break;
    case R_STAR_TREE:
      raSearch = new RAType<SortPolicy, tree::RStarTree>(naive, singleMode);
      break;
    case X_TREE:
      raSearch = new RAType<SortPolicy, tree::XTree>(naive, singleMode);
      break;
    case HILBERT_R_TREE:
      raSearch = new RAType<SortPolicy, tree::HilbertRTree>(naive, singleMode);
      break;
    case R_PLUS_TREE:
      raSearch = new RAType<SortPolicy, tree::RPlusTree>(naive, singleMode);
      break;
    case R_PLUS_PLUS_TREE:
      raSearch = new RAType<SortPolicy, tree::RPlusPlusTree>(naive,
          singleMode);
      break;
    case UB_TREE:
      raSearch = new RAType<SortPolicy, tree::UBTree>(naive, singleMode);
      break;
    case OCTREE:
      raSearch = new RAType<SortPolicy, tree::Octree>(naive, singleMode);
      break;
  }

  TrainVisitor<SortPolicy> tn(std::move(referenceSet), leafSize);
  boost::apply_visitor(tn, raSearch);

  if (!naive)
  {
    Timer::Stop("tree_building");
    Log::Info << "Tree built." << std::endl;
  }
}

template<typename SortPolicy>
void RAModel<SortPolicy>::Search(arma::mat&& querySet,
                                 const size_t k,
                                 arma::Mat<size_t>& neighbors,
                                 arma::mat& distances)
{
  // Apply the random basis if necessary.
  if (randomBasis)
    querySet = q * querySet;

  Log::Info << "Searching for " << k << " approximate nearest neighbors with ";
  if (!Naive() && !SingleMode())
    Log::Info << "dual-tree rank-approximate " << TreeName() << " search...";
  else if (!Naive())
    Log::Info << "single-tree rank-approximate " << TreeName() << " search...";
  else
    Log::Info << "brute-force (naive) rank-approximate search...";
  Log::Info << std::endl;

  BiSearchVisitor<SortPolicy> search(querySet, k, neighbors, distances,
      leafSize);
  boost::apply_visitor(search, raSearch);
}

template<typename SortPolicy>
void RAModel<SortPolicy>::Search(const size_t k,
                                 arma::Mat<size_t>& neighbors,
                                 arma::mat& distances)
{
  Log::Info << "Searching for " << k << " approximate nearest neighbors with ";
  if (!Naive() && !SingleMode())
    Log::Info << "dual-tree rank-approximate " << TreeName() << " search...";
  else if (!Naive())
    Log::Info << "single-tree rank-approximate " << TreeName() << " search...";
  else
    Log::Info << "brute-force (naive) rank-approximate search...";
  Log::Info << std::endl;

  MonoSearchVisitor search(k, neighbors, distances);
  boost::apply_visitor(search, raSearch);
}

template<typename SortPolicy>
std::string RAModel<SortPolicy>::TreeName() const
{
  switch (treeType)
  {
    case KD_TREE:
      return "kd-tree";
    case COVER_TREE:
      return "cover tree";
    case R_TREE:
      return "R tree";
    case R_STAR_TREE:
      return "R* tree";
    case X_TREE:
      return "X tree";
    case HILBERT_R_TREE:
      return "Hilbert R tree";
    case R_PLUS_TREE:
      return "R+ tree";
    case R_PLUS_PLUS_TREE:
      return "R++ tree";
    case UB_TREE:
      return "UB tree";
    case OCTREE:
      return "octree";
    default:
      return "unknown tree";
  }
}

} // namespace neighbor
} // namespace mlpack

#endif


/*
 * $Revision: 567 $ $Date: 2011-02-24 13:28:02 -0800 (Thu, 24 Feb 2011) $
 *
 * Copyright by Astos Solutions GmbH, Germany
 *
 * this file is published under the Astos Solutions Free Public License
 * For details on copyright and terms of use see
 * http://www.astos.de/Astos_Solutions_Free_Public_License.html
 */

#include "TrajectoryGeometry.h"
#include "Trajectory.h"
#include "RenderContext.h"
#include "Material.h"
#include "Debug.h"
#include <curveplot/curveplot.h>
#include <Eigen/LU>
#include <algorithm>

using namespace vesta;
using namespace Eigen;
using namespace std;

TrajectoryGeometry::TrajectoryGeometry() :
    m_color(Spectrum(1.0f, 1.0f, 1.0f)),
    m_opacity(1.0f),
    m_curvePlot(0),
    m_startTime(0.0),
    m_endTime(0.0),
    m_boundingRadius(0.0),
    m_displayedPortion(Entire),
    m_windowDuration(0.0),
    m_windowLead(0.),
    m_fadeFraction(0.0),
    m_lineWidth(1.0f)
{
    // Make trajectories splittable by default in order to prevent
    // clipping artifacts.
    setClippingPolicy(SplitToPreventClipping);
}

TrajectoryGeometry::~TrajectoryGeometry()
{
    delete m_curvePlot;
}

void
TrajectoryGeometry::render(RenderContext& rc, double clock) const
{
    if (!m_curvePlot)
    {
        return;
    }

    bool fade = false;
    double startTime = m_startTime;
    double endTime = m_endTime;
    switch (m_displayedPortion)
    {
    case StartToCurrentTime:
        endTime = clock;
        break;
    case CurrentTimeToEnd:
        startTime = clock;
        break;
    case WindowBeforeCurrentTime:
        endTime = clock + m_windowLead;
        startTime = clock + m_windowLead - m_windowDuration;
        fade = m_fadeFraction > 0.0;
        break;
    default:
        break;
    }

    // Abort now if there's nothing to draw
    if (endTime <= startTime)
    {
        return;
    }

    // Skip drawing trajectories that are less than a pixel in size. This should be done by
    // UniverseRenderer, except that visualizers (which is where TrajectoryGeometry is typically used)
    // aren't size culled.
    float projectedSize = (boundingSphereRadius() / float(rc.modelview().translation().norm())) / rc.pixelSize();
    if (projectedSize < 0.5f)
    {
        return;
    }

    const Frustum& frustum = rc.frustum();

    // Get a high precision modelview matrix; the full transformation is stored at single precision,
    // but the camera space position is stored at double precision.
    Transform3d modelview = rc.modelview().cast<double>();
    Vector3d t = rc.modelTranslation();
    modelview.matrix().col(3) = Vector4d(t.x(), t.y(), t.z(), 1.0);

    if (m_frame.isValid())
    {
        modelview = modelview * m_frame->orientation(clock);
    }

    // Set the model view matrix to identity, as the curveplot module performs all transformations in
    // software using double precision.
    rc.pushModelView();
    rc.identityModelView();

    glLineWidth(m_lineWidth);
    double subdivisionThreshold = rc.pixelSize() * 30.0;
    if (fade)
    {
        rc.setVertexInfo(VertexSpec::PositionColor);
        Material material;
        material.setDiffuse(Spectrum::White());
        rc.bindMaterial(&material);

        double fadeStartTime = startTime;
        double fadeEndTime = fadeStartTime + m_windowDuration * m_fadeFraction;
        glEnable(GL_BLEND);
        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
        m_curvePlot->renderFaded(modelview,
                                 -frustum.nearZ, -frustum.farZ, frustum.planeNormals, // viewFrustum
                                 subdivisionThreshold,
                                 startTime, endTime,
                                 Vector4f(m_color.red(), m_color.green(), m_color.blue(), 1.0f),
                                 fadeStartTime, fadeEndTime);
        glDisable(GL_BLEND);
    }
    else
    {
        rc.setVertexInfo(VertexSpec::Position);
        Material material;
        material.setEmission(m_color);
        rc.bindMaterial(&material);

        m_curvePlot->render(modelview,
                            -frustum.nearZ, -frustum.farZ, frustum.planeNormals, // viewFrustum
                            subdivisionThreshold,
                            startTime, endTime);
    }
    glLineWidth(1.0f);

    rc.popModelView();
}

/** Add a new sample to the trajectory. If this is not the first sample, the time t must
  * be greater than the time of the last added sample; if not, the sample is discarded.
  *
  * \param t time in seconds since J2000.0
  * \param s state vector
  */
void
TrajectoryGeometry::addSample(double t, const StateVector& s)
{
    if (!m_curvePlot)
    {
        m_curvePlot = new CurvePlot();
    }

    if (m_curvePlot->sampleCount() == 0)
    {
        m_startTime = t;
        m_endTime = t;
    }

    if (m_curvePlot->sampleCount() == 0 || t > m_curvePlot->endTime())
    {
        CurvePlotSample sample;
        sample.t = t;
        sample.position = s.position();
        sample.velocity = s.velocity();
        m_curvePlot->addSample(sample);

        m_boundingRadius = std::max(m_boundingRadius, s.position().norm());
        m_endTime = t;
    }
}

/** Remove all trajectory plot samples.
  */
void
TrajectoryGeometry::clearSamples()
{
    // Throw out the previous trajectory
    if (m_curvePlot)
    {
        delete m_curvePlot;
        m_curvePlot = NULL;
    }

    m_boundingRadius = 0.0;
    m_startTime = 0.0;
    m_endTime = 0.0;
}

class TrajectorySampleGenerator : public TrajectoryPlotGenerator
{
public:
    TrajectorySampleGenerator(const Trajectory* trajectory) :
        m_trajectory(trajectory)
    {
    }

    StateVector state(double t) const
    {
        return m_trajectory->state(t);
    }

    double startTime() const
    {
        return m_trajectory->startTime();
    }

    double endTime() const
    {
        return m_trajectory->endTime();
    }

private:
    const Trajectory* m_trajectory;
};

/** Automatically add samples to the trajectory plot. Samples of the specified trajectory
  * are calculated at regular intervals between the startTime and endTime. Any existing samples
  * in the trajectory plot are replaced.
  */
void
TrajectoryGeometry::computeSamples(const Trajectory* trajectory, double startTime, double endTime, unsigned int steps)
{
    // Abort if we're asked to plot a null trajectory
    if (!trajectory)
    {
        return;
    }

    TrajectorySampleGenerator gen(trajectory);
    computeSamples(&gen, startTime, endTime, steps);
}

/** Automatically add samples to the trajectory plot. Samples of the specified trajectory
  * are calculated at regular intervals between the startTime and endTime.
  */
void
TrajectoryGeometry::updateSamples(const Trajectory* trajectory, double startTime, double endTime, unsigned int steps)
{
    // Abort if we're asked to plot a null trajectory
    if (!trajectory)
    {
        return;
    }

    TrajectorySampleGenerator gen(trajectory);
    updateSamples(&gen, startTime, endTime, steps);
}

/** Automatically add samples to the trajectory plot. States from the specified generator
  * are calculated at regular intervals between the startTime and endTime. Any existing samples
  * in the trajectory plot are replaced.
  */
void
TrajectoryGeometry::computeSamples(const TrajectoryPlotGenerator* generator, double startTime, double endTime, unsigned int steps)
{
    // Abort if we're asked to use a null generator
    if (!generator)
    {
        return;
    }

    clearSamples();

    startTime = max(generator->startTime(), startTime);
    endTime = min(generator->endTime(), endTime);

    // Nothing to plot if end is before start
    if (endTime <= startTime)
    {
        return;
    }

    m_curvePlot = new CurvePlot();

    m_startTime = startTime;
    m_endTime = endTime;
    double dt = (endTime - startTime) / steps;

    for (unsigned int i = 0; i <= steps; ++i)
    {
        double t = m_startTime + i * dt;
        StateVector state = generator->state(t);
        addSample(t, state);
    }

    // Adjust the bounding radius slightly to prevent culling when the
    // trajectory lies barely inside the view frustum.
    m_boundingRadius *= 1.1;
}

/** Automatically add samples to the trajectory plot. Samples of the specified
  * generator are calculated at regular intervals between the startTime and endTime.
  */
void
TrajectoryGeometry::updateSamples(const TrajectoryPlotGenerator* generator, double startTime, double endTime, unsigned int steps)
{
    // Abort if we're asked to use a null generator
    if (!generator)
    {
        return;
    }

    if (!m_curvePlot)
    {
        // Trajectory hasn't been created yet; initialize it for the specified time range
        computeSamples(generator, startTime, endTime, steps);
        return;
    }

    double dt = (endTime - startTime) / (steps - 1);
    double windowStartTime = max(generator->startTime(), startTime - dt);
    double windowEndTime = min(generator->endTime(), endTime + dt);

    if (endTime <= m_curvePlot->startTime() || startTime >= m_curvePlot->endTime())
    {
        computeSamples(generator, windowStartTime, windowEndTime, steps);
    }
    else
    {
        if (startTime < m_curvePlot->startTime())
        {
            // Add samples at the beginning
            for (double t = m_curvePlot->startTime() - dt; t > windowStartTime; t -= dt)
            {
                t = max(t, windowStartTime);
                StateVector sv = generator->state(t);

                CurvePlotSample sample;
                sample.t = t;
                sample.position = sv.position();
                sample.velocity = sv.velocity();
                m_curvePlot->addSample(sample);
                m_boundingRadius = std::max(m_boundingRadius, sv.position().norm());
            }
        }

        if (endTime > m_curvePlot->endTime())
        {
            // Add samples at the end
            for (double t = m_curvePlot->endTime() + dt; t < windowEndTime; t += dt)
            {
                t = min(t, windowEndTime);
                StateVector sv = generator->state(t);

                CurvePlotSample sample;
                sample.t = t;
                sample.position = sv.position();
                sample.velocity = sv.velocity();
                m_curvePlot->addSample(sample);
                m_boundingRadius = std::max(m_boundingRadius, sv.position().norm());
            }
        }

        // Remove samples
        m_curvePlot->removeSamplesAfter(windowEndTime);
        m_curvePlot->removeSamplesBefore(windowStartTime);
    }

    m_startTime = windowStartTime;
    m_endTime = windowEndTime;
}


/*
Copyright 2020 The OneFlow Authors. 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 <gtest/gtest.h>
#include "oneflow/core/ep/test/primitive/primitive_test.h"
#include "oneflow/core/ep/include/primitive/memset.h"
#include "oneflow/core/ep/include/primitive/memcpy.h"
#include "oneflow/core/ep/include/primitive/add.h"
#include <Eigen/Core>

namespace oneflow {

namespace ep {

namespace primitive {

namespace test {

namespace {

template<DataType data_type, typename T, size_t n>
void TestAdd(DeviceManagerRegistry* registry, const std::set<DeviceType>& device_types) {
  constexpr size_t max_arity = 10;
  using Matrix = Eigen::Matrix<T, 1, n>;
  std::vector<Matrix> srcs(max_arity);
  std::vector<Matrix> dsts(max_arity);
  for (size_t i = 0; i < max_arity; ++i) {
    srcs[i] = Matrix::Random();
    if (i == 0) {
      dsts[i] = Matrix::Zero();
    } else {
      dsts[i] = srcs[i - 1] + dsts[i - 1];
    }
  }
  const size_t vector_size = n * sizeof(T);
  for (const auto& device_type : device_types) {
    auto device = registry->GetDevice(device_type, 0);
    std::vector<void*> host_srcs(max_arity);
    std::vector<void*> device_srcs(max_arity);
    std::vector<void*> host_dsts(max_arity);
    std::vector<void*> device_dsts(max_arity);
    AllocationOptions pinned_options;
    pinned_options.SetPinnedDevice(device_type, 0);
    AllocationOptions device_options;
    for (size_t i = 0; i < max_arity; ++i) {
      CHECK_JUST(device->AllocPinned(pinned_options, &host_srcs[i], vector_size));
      CHECK_JUST(device->AllocPinned(pinned_options, &host_dsts[i], vector_size));
      CHECK_JUST(device->Alloc(device_options, &device_srcs[i], vector_size));
      CHECK_JUST(device->Alloc(device_options, &device_dsts[i], vector_size));
    }
    ep::test::StreamGuard stream(device.get());
    std::unique_ptr<Add> add = NewPrimitive<AddFactory>(device_type, data_type);
    ASSERT_TRUE(add.operator bool());
    std::unique_ptr<Memcpy> h2d = NewPrimitive<MemcpyFactory>(device_type, MemcpyKind::kHtoD);
    std::unique_ptr<Memcpy> d2h = NewPrimitive<MemcpyFactory>(device_type, MemcpyKind::kDtoH);
    ASSERT_TRUE(d2h.operator bool());
    ASSERT_TRUE(h2d.operator bool());
    for (size_t i = 0; i < max_arity; ++i) {
      std::memcpy(host_srcs[i], srcs[i].data(), vector_size);
      h2d->Launch(stream.stream(), device_srcs[i], host_srcs[i], vector_size);
    }
    for (size_t i = 2; i < max_arity; ++i) {
      add->Launch(stream.stream(), device_srcs.data(), i, device_dsts.at(i), n);
    }
    for (size_t i = 2; i < max_arity; ++i) {
      d2h->Launch(stream.stream(), host_dsts[i], device_dsts[i], vector_size);
    }
    CHECK_JUST(stream.stream()->Sync());
    for (size_t i = 2; i < max_arity; ++i) {
      auto res = Eigen::Map<Matrix, Eigen::Unaligned>(reinterpret_cast<T*>(host_dsts[i]), n);
      ASSERT_TRUE(dsts[i].template isApprox(res));
    }
    for (size_t i = 0; i < max_arity; ++i) {
      device->FreePinned(pinned_options, host_srcs[i]);
      device->FreePinned(pinned_options, host_dsts[i]);
      device->Free(device_options, device_srcs[i]);
      device->Free(device_options, device_dsts[i]);
    }
  }
}

}  // namespace

TEST_F(PrimitiveTest, TestAdd) {
  TestAdd<DataType::kDouble, double, 1024>(&device_manager_registry_, available_device_types_);
  TestAdd<DataType::kFloat, float, 1024>(&device_manager_registry_, available_device_types_);
  TestAdd<DataType::kFloat16, Eigen::half, 1024>(&device_manager_registry_,
                                                 available_device_types_);
}

}  // namespace test

}  // namespace primitive

}  // namespace ep

}  // namespace oneflow


#include <boost/format.hpp>
#include <facter/execution/execution.hpp>
#include <facter/facts/fact_map.hpp>
#include <facter/facts/fact.hpp>
#include <facter/facts/scalar_value.hpp>
#include <facter/facts/posix/uptime_resolver.hpp>
#include <facter/util/file.hpp>
#include <facter/util/string.hpp>
#include <re2/re2.h>

using namespace std;
using boost::format;
using namespace facter::util;
using namespace facter::execution;

namespace facter { namespace facts { namespace posix {

    uptime_resolver::uptime_resolver() :
        fact_resolver(
            "uptime",
            {
                fact::uptime,
                fact::uptime_days,
                fact::uptime_hours,
                fact::uptime_seconds
            })
    {
    }

    void uptime_resolver::resolve_facts(fact_map& facts)
    {
        // Resolve all uptime-related facts
        resolve_uptime_seconds(facts);  // must be first b/c the following facts are based on this
        resolve_uptime_hours(facts);
        resolve_uptime_days(facts);
        resolve_uptime(facts);
    }

    void uptime_resolver::resolve_uptime_seconds(fact_map& facts)
    {
        int value = executable_uptime();
        facts.add(fact::uptime_seconds, make_value<integer_value>(value));
    }

    void uptime_resolver::resolve_uptime_hours(fact_map& facts)
    {
        auto uptime_seconds = facts.get<integer_value>(fact::uptime_seconds, false);
        if (!uptime_seconds) {
            return;
        }
        facts.add(fact::uptime_hours, make_value<integer_value>(uptime_seconds->value() / (60 * 60)));
    }

    void uptime_resolver::resolve_uptime_days(fact_map& facts)
    {
        auto uptime_seconds = facts.get<integer_value>(fact::uptime_seconds, false);
        if (!uptime_seconds) {
            return;
        }
        facts.add(fact::uptime_days, make_value<integer_value>(uptime_seconds->value() / (60 * 60 * 24)));
    }

    void uptime_resolver::resolve_uptime(fact_map& facts)
    {
        auto uptime_seconds = facts.get<integer_value>(fact::uptime_seconds, false);
        if (!uptime_seconds) {
            return;
        }
        int seconds = uptime_seconds->value();

        int days    = seconds / (60 * 60 * 24);
        int hours   = (seconds / (60 * 60)) % 24;
        int minutes = (seconds / 60) % 60;

        string value;
        switch (days) {
            case 0:
                value = (format("%d:%02d hours") % hours % minutes).str();
                break;
            case 1:
                value = "1 day";
                break;
            default:
                value = (format("%d days") % days).str();
        }
        facts.add(fact::uptime, make_value<string_value>(move(value)));
    }

    // call the uptime executable
    int uptime_resolver::executable_uptime()
    {
        string uptime_output = execute("uptime");
        if (uptime_output.empty()) {
            return 0;
        }
        return parse_executable_uptime(uptime_output);
    }

    // parse the output from the uptime executable
    int uptime_resolver::parse_executable_uptime(string const& output)
    {
        // This regex parsing is directly ported from facter:
        // https://github.com/puppetlabs/facter/blob/2.0.1/lib/facter/util/uptime.rb#L42-L60

        int days, hours, minutes;

        if (RE2::PartialMatch(output, "(\\d+) day(?:s|\\(s\\))?,\\s+(\\d+):(\\d+)", &days, &hours, &minutes)) {
            return 86400 * days + 3600 * hours + 60 * minutes;
        } else if (RE2::PartialMatch(output, "(\\d+) day(?:s|\\(s\\))?,\\s+(\\d+) hr(?:s|\\(s\\))?,", &days, &hours)) {
            return 86400 * days + 3600 * hours;
        } else if (RE2::PartialMatch(output, "(\\d+) day(?:s|\\(s\\))?,\\s+(\\d+) min(?:s|\\(s\\))?,", &days, &minutes)) {
            return 86400 * days + 60 * minutes;
        } else if (RE2::PartialMatch(output, "(\\d+) day(?:s|\\(s\\))?,", &days)) {
            return 86400 * days;
        } else if (RE2::PartialMatch(output, "up\\s+(\\d+):(\\d+),", &hours, &minutes)) {
            return 3600 * hours + 60 * minutes;
        } else if (RE2::PartialMatch(output, "(\\d+) hr(?:s|\\(s\\))?,", &hours)) {
            return 3600 * hours;
        } else if (RE2::PartialMatch(output, "(\\d+) min(?:s|\\(s\\))?,", &minutes)) {
            return 60 * minutes;
        } else {
           return 0;
        }
    }

}}}  // namespace facter::facts::posix


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-boolean
    This function object conditionally returns @c x (respectively @ref Zero)
    if @c c is @ref True (respectively  @ref False)

    @par Header <boost/simd/function/if_else_zero.hpp>

    @par Example:

      @snippet if_else_zero.cpp if_else_zero

    @par Possible output:

      @snippet if_else_zero.txt if_else_zero

  **/
  Value1 if_else_zero(Value0 const& c, Value1 const& t);
} }
#endif

#include <boost/simd/function/scalar/if_else_zero.hpp>
#include <boost/simd/function/simd/if_else_zero.hpp>

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_BITWISE_FUNCTIONS_SIMD_COMMON_RSHR_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_BITWISE_FUNCTIONS_SIMD_COMMON_RSHR_HPP_INCLUDED
#include <boost/simd/toolbox/bitwise/functions/rshr.hpp>
#include <boost/simd/sdk/meta/cardinal_of.hpp>
#include <boost/simd/include/functions/simd/is_gtz.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/shift_left.hpp>
#include <boost/simd/include/functions/simd/shift_right.hpp>
#include <boost/simd/include/functions/simd/unary_minus.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION_IF ( boost::simd::tag::rshr_, tag::cpu_, (A0)(A1)(X)
                                , (boost::mpl::equal_to < boost::simd::meta::cardinal_of<A0>
                                                        , boost::simd::meta::cardinal_of<A1>
                                                        >
                                  )
                                , ((simd_<arithmetic_<A0>,X>))
                                  ((simd_<integer_<A1>,X>))
                       )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(2)
    {
      return if_else(is_gtz(a1),shr(a0, a1),shl(a0, -a1));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION_IF ( boost::simd::tag::rshr_, tag::cpu_, (A0)(A1)(X)
                                , (boost::mpl::equal_to < boost::simd::meta::cardinal_of<A0>
                                                        , boost::simd::meta::cardinal_of<A1>
                                                        >
                                  )
                                , ((simd_<arithmetic_<A0>,X>))
                                  ((simd_<unsigned_<A1>,X>))
                       )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(2)
    {
      return shr(a0, a1);
    }
  };

} } }

#endif


#include <boost/test/unit_test.hpp>
#include <vector>

#include <physics/objects/Vehicle.h>
#include <physics/World.h>
#include <physics/objects/ObjectsFactory.h>

using namespace Physics;
using namespace Objects;

struct VehicleTestFixture {
	VehicleTestFixture() :
		w(b2Vec2(0.0f, 9.8f), 4, 8),
		obj(ObjectsFactory::init(w.getWorld())),
		heights(8, 1.0f){
	}
	World w;
	ObjectsFactory& obj;
	std::vector<float> heights;
};

BOOST_FIXTURE_TEST_SUITE(VehicleTests, VehicleTestFixture);

BOOST_AUTO_TEST_CASE(check_creation_position)
{
	Vehicle v(Vector2(1.1f, 2.1f), heights, 0.1f, 0.1f);
	BOOST_CHECK_EQUAL(v.getPosition().x, 1.1f);
	BOOST_CHECK_EQUAL(v.getPosition().y, 2.1f);
}

BOOST_AUTO_TEST_CASE(check_wheels_radius)
{
	Vehicle v(Vector2(1.1f, 2.1f), heights, 1.1f, 2.1f);
	BOOST_CHECK_EQUAL(v.getFrontWheel().getRadius(), 1.1f);
	BOOST_CHECK_EQUAL(v.getBackWheel().getRadius(), 2.1f);
}

BOOST_AUTO_TEST_CASE(check_body_vertices)
{
	Vehicle v(Vector2(1.1f, 2.1f), heights, 1.1f, 2.1f);
	auto& vert = v.getBody().getVertices();

	BOOST_CHECK_EQUAL(vert[0].x, 0.0f);
	BOOST_CHECK_EQUAL(vert[0].y, 1.0f);

	BOOST_CHECK_EQUAL(vert[1].x, -1.0f);
	BOOST_CHECK_EQUAL(vert[1].y, 1.0f);

	BOOST_CHECK_EQUAL(vert[2].x, -1.0f);
	BOOST_CHECK_EQUAL(vert[2].y, 0.0f);

	BOOST_CHECK_EQUAL(vert[3].x, -1.0f);
	BOOST_CHECK_EQUAL(vert[3].y, -1.0f);

	BOOST_CHECK_EQUAL(vert[4].x, 0.0f);
	BOOST_CHECK_EQUAL(vert[4].y, -1.0f);

	BOOST_CHECK_EQUAL(vert[5].x, 1.0f);
	BOOST_CHECK_EQUAL(vert[5].y, -1.0f);

	BOOST_CHECK_EQUAL(vert[6].x, 1.0f);
	BOOST_CHECK_EQUAL(vert[6].y, 0.0f);

	BOOST_CHECK_EQUAL(vert[7].x, 1.0f);
	BOOST_CHECK_EQUAL(vert[7].y, 1.0f);
}

BOOST_AUTO_TEST_SUITE_END();


#include <cstdio>
#include <limits>

#include <sstream>
#include <iomanip>

#include <boost/lexical_cast.hpp>

#include "asserts.hpp"
#include "as_function_params.hpp"
#include "as_object.hpp"
#include "as_value.hpp"
#include "swf_environment.hpp"
#include "swf_player.hpp"

namespace swf
{
	as_value_ptr as_value::from_bool(bool value)
	{
		return std::make_shared<as_value>(value);
	}

	as_value::as_value(as_value_ptr get, as_value_ptr set)
		: type_(ValueType::PROPERTY),
		  b_(false),
		  d_(0),
		  o_(nullptr),
		  p_(std::make_shared<as_property>(get, set)),
		  flags_(PropertyFlags::NONE)
	{
	}

	as_value::as_value(as_native_function_type fn)
		: type_(ValueType::OBJECT),
		  b_(false),
		  d_(0),
		  flags_(PropertyFlags::NONE)
	{
		o_ = as_native_function::create(weak_player_ptr(), fn);
	}

	const char*	as_value::to_string() const
	{
		return to_std_string().c_str();
	}

	std::string as_value::to_std_string() const
	{
		switch(type_) {
			case ValueType::UNDEFINED: return "undefined";
			case ValueType::BOOLEAN: return b_ ? "true" : "false";
			case ValueType::NUMERIC: {
				if(d_ == std::numeric_limits<double>::quiet_NaN()
					|| d_ == std::numeric_limits<double>::signaling_NaN()) {
					return "NaN";
				} else {
					std::stringstream ss_fixed;
					ss_fixed << std::setprecision(14) << std::ios::fixed << d_;
					std::string s_fix = ss_fixed.str();
					std::stringstream ss_sci;
					ss_fixed << std::setprecision(14) << std::ios::scientific << d_;
					std::string s_sci = ss_sci.str();
					if(s_sci.length() < s_fix.length()) {
						return s_sci;
					}
					return s_fix;
				}
			}
			case ValueType::STRING: return s_;
			case ValueType::NULL_VALUE: return "null";
			case ValueType::OBJECT: return o_->to_string();
			case ValueType::PROPERTY:
				/*
				convert property p_ to a string.
				*/
			default: ASSERT_LOG(false, "to_string() bad type: " << static_cast<int>(type_));
		}
		return "undefined";
	}

	as_value operator+(const as_value& v1, const as_value& v2)
	{
		auto p1 = v1.to_primitive();
		auto p2 = v2.to_primitive();
		if(p1.type_ == ValueType::STRING || p2.type_ == ValueType::STRING) {
			return as_value(p1.to_string() + p2.to_std_string());
		}
		return as_value(p1.to_number() + p2.to_number());
	}

	as_value as_value::to_primitive(HintType hint) const
	{
		if(type_ == ValueType::OBJECT && o_ != nullptr) {
			return o_->default_value(hint);
		} else if(type_ == ValueType::PROPERTY) {
			ASSERT_LOG(false, "XXX todo PROPERTY::to_primitive");
		}
		return *this;
	}

	int32_t as_value::to_int32()
	{
		double num = to_number();
		if(num == std::numeric_limits<double>::quiet_NaN()
			|| num == std::numeric_limits<double>::signaling_NaN()
			|| abs(num) == 0
			|| abs(num) == std::numeric_limits<double>::infinity()) {
			return 0;
		}
		double pos_int = (num > 0 ? 1 : -1) * floor(abs(num));
		return static_cast<int32_t>(fmod(pos_int, 4294967296));
	}

	int as_value::to_integer()
	{
		/// Need to double check this against the standard.
		double num = to_number();
		if(num == std::numeric_limits<double>::quiet_NaN()
			|| num == std::numeric_limits<double>::signaling_NaN()
			|| abs(num) == 0
			|| abs(num) == std::numeric_limits<double>::infinity()) {
			return 0;
		}
		return static_cast<int>((num > 0 ? 1 : -1) * floor(abs(num)));
	}

	double as_value::to_number()
	{
		switch(type_) {
			case ValueType::UNDEFINED:		return std::numeric_limits<double>::infinity();
			case ValueType::BOOLEAN:		return b_ ? 1 : 0;
			case ValueType::NUMERIC:		return d_;
			case ValueType::NULL_VALUE:		return 0;
			case ValueType::OBJECT:			return o_->to_number();
			case ValueType::PROPERTY: {
				ASSERT_LOG(false, "XXX todo PROPERTY::to_number");
			}
		}
		ASSERT_LOG(type_ != ValueType::STRING, "FATAL: unknown type_ value: " << static_cast<int>(type_));
		// String case -- we do thing the lazy way, not the compliant way.
		double num = std::numeric_limits<double>::infinity();
		try {
			num = boost::lexical_cast<double>(s_);
		} catch(boost::bad_lexical_cast&) {
			std::cerr << "Caught a bad floating point cast from " << s_ << " assuming infinity" << std::endl;
		}
		return num;
	}

	bool as_value::to_boolean()
	{
		// XXX check for correctness.
		return to_integer() != 0;
	}

	as_object_ptr as_value::to_object()
	{
		switch(type_) {
			case ValueType::UNDEFINED:
			case ValueType::BOOLEAN:
			case ValueType::NUMERIC:
			case ValueType::NULL_VALUE:
			case ValueType::STRING:
				break;
			case ValueType::OBJECT:			return o_;
			case ValueType::PROPERTY: {

				ASSERT_LOG(false, "XXX todo PROPERTY::to_object");
			}
		}
		return nullptr;
	}

	as_value_ptr as_value::clone()
	{
		return std::make_shared<as_value>(*this);
	}

	void as_value::set_property(const as_value_ptr& value)
	{
		ASSERT_LOG(is_property(), "Attempt to set property value on non-property.");
		p_->set(o_, value);
	}

	as_value_ptr as_value::get_property() const
	{
		ASSERT_LOG(is_property(), "Attempt to get property value on non-property.");
		return p_->get(o_);
	}

	as_value_ptr as_value::get_property(const as_value_ptr& primitive) const
	{
		ASSERT_LOG(is_property(), "Attempt to get property value on non-property.");
		return p_->get(primitive);
	}

	void as_value::set_property_target(const as_object_ptr& target)
	{
		ASSERT_LOG(is_property(), "Attempt to set property target on non-property.");
		o_ = target;
	}

	as_property::as_property(const as_value_ptr& get, const as_value_ptr& set)
	{
		get_ = std::dynamic_pointer_cast<as_function>(get->to_object());
		set_ = std::dynamic_pointer_cast<as_function>(set->to_object());
	}

	void as_property::set(const as_object_ptr& target, const as_value_ptr& value)
	{
		if(target) {
			if(set_) {
				auto env = environment::create(target->get_player());
				env->push(value);
				(*set_)(function_params(as_value::create(target), env, 1, env->get_top_index()));
			}
		} else {
			LOG_WARN("Tried to set property on null target.");
		}
	}

	as_value_ptr as_property::get(const as_object_ptr& target)
	{
		as_value_ptr value = as_value::create();
		if(target) {
			if(get_) {
				auto env = environment::create(target->get_player());
				value = (*get_)(function_params(as_value::create(target), env, 0, 0));
			}
		} else {
			LOG_WARN("Tried to set property on null target.");
		}
		return value;
	}

	as_value_ptr as_property::get(const as_value_ptr& primitive)
	{
		if(get_) {
			return (*get_)(function_params(primitive, nullptr, 0, 0));
		}
		return nullptr;
	}

	as_function_ptr as_value::to_function()
	{
		if(type_ == ValueType::OBJECT && o_ != nullptr) {
			// XXX This is a little icky -- maybe as_object should have a virtual operator().
			return std::dynamic_pointer_cast<as_function>(o_);
		}
		LOG_WARN("couldn't convert as_value to function.");
		return nullptr;
	}

	as_value_ptr as_value::find_property(const std::string& name)
	{
		switch(type_) {
			case ValueType::UNDEFINED:
			case ValueType::NULL_VALUE:
			case ValueType::PROPERTY:
				break;
			case ValueType::STRING:
				return player::get_builtin_string_method(name);
			case ValueType::BOOLEAN:
				return player::get_builtin_boolean_method(name);
			case ValueType::NUMERIC:
				return player::get_builtin_numeric_method(name);
			case ValueType::OBJECT: {
				if(o_) {
					return o_->get_member(name);
				}
				break;
			}
		}
		return nullptr;
	}
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_BITWISE_FUNCTIONS_SIMD_SSE_SSE2_SHRI_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_BITWISE_FUNCTIONS_SIMD_SSE_SSE2_SHRI_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT
#include <boost/simd/toolbox/bitwise/functions/shri.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/functions/simd/bitwise_and.hpp>
#include <boost/simd/include/functions/simd/bitwise_or.hpp>
#include <boost/simd/include/constants/int_splat.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/sdk/meta/make_dependent.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::shri_
                                    , boost::simd::tag::sse2_
                                    , (A0)(A1)
                                    , ((simd_<type8_<A0>,boost::simd::tag::sse_>))
                                      (scalar_< integer_<A1> >)
                                    )
  {
    typedef A0 result_type;
    typedef typename meta::make_dependent<int32_t,A0>::type int_t;

    BOOST_SIMD_FUNCTOR_CALL(2)
    {
      typedef native<int_t, boost::simd::tag::sse_> gen_type;
      result_type const Mask1 =  bitwise_cast<result_type>(boost::simd::integral_constant<gen_type, 0x00ff00ff>());
      result_type const Mask2 =  bitwise_cast<result_type>(boost::simd::integral_constant<gen_type, 0xff00ff00>());
      result_type tmp  = b_and(a0, Mask1);
      result_type tmp1 = _mm_srli_epi16(tmp, a1);
      tmp1 = b_and(tmp1, Mask1);
      tmp = b_and(a0, Mask2);
      result_type tmp3 = _mm_srli_epi16(tmp, a1);
      return tmp1 | b_and(tmp3, Mask2);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::shri_
                                    , boost::simd::tag::sse2_
                                    , (A0)(A1)
                                    , ((simd_<type32_<A0>,boost::simd::tag::sse_>))
                                      (scalar_< integer_<A1> >)
                                    )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(2)
    {
      typedef typename dispatch::meta::as_integer<A0>::type sint;
      sint const that = _mm_srli_epi32(bitwise_cast<sint>(a0), int(a1));
      return bitwise_cast<A0>(that);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::shri_
                                    , boost::simd::tag::sse2_
                                    , (A0)(A1)
                                    , ((simd_<type64_<A0>,boost::simd::tag::sse_>))
                                      (scalar_< integer_<A1> >)
                                    )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(2)
    {
      typedef typename dispatch::meta::as_integer<A0>::type sint;
      sint const that = _mm_srli_epi64(bitwise_cast<sint>(a0), int(a1));
      return bitwise_cast<result_type>(that);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::shri_, boost::simd::tag::sse2_
                                    , (A0)(A1)
                                    , ((simd_<type16_<A0>,boost::simd::tag::sse_>))
                                      (scalar_< integer_<A1> >)
                                    )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(2)
    {
      return _mm_srli_epi16(a0, int(a1));
    }
  };
} } }

#endif
#endif


/*
	This software is distributed under MIT License, which means:
		- Do whatever you want
		- Please keep this notice and include the license file to your project
		- I provide no warranty

	Created by Kyrylo Sovailo (github.com/Meta-chan, k.sovailo@gmail.com)
	Reinventing bicycles since 2020
*/

#include "../header/p6_construction.hpp"
#include "../header/p6_linear_material.hpp"
#include "../header/p6_nonlinear_material.hpp"
#include "../header/p6_file.hpp"
#include <cassert>
#include <Eigen>

class p6::Construction::Vector : public Eigen::Vector<p6::real, Eigen::Dynamic>
{
public:
	using Eigen::Vector<p6::real, Eigen::Dynamic>::Vector;
};

class p6::Construction::Matrix : public Eigen::Matrix<p6::real, Eigen::Dynamic, Eigen::Dynamic> {};

p6::uint p6::Construction::create_node() noexcept
{
	assert(!_simulation);
	Node node;
	node.freedom = 0;
	node.coord = Coord(0.0, 0.0);
	node.angle = 0.0;
	node.coord_simulated = Coord(0.0, 0.0);
	_node.push_back(node);
	return _node.size() - 1;
}

void p6::Construction::delete_node(uint node) noexcept
{
	assert(!_simulation);
	for (uint i = _stick.size() - 1; i != (uint)-1; i--)
	{
		if (_stick[i].node[0] == node || _stick[i].node[1] == node)
		{
			_stick.erase(_stick.begin() + i);
		}
		else
		{
			if (_stick[i].node[0] > node) _stick[i].node[0]--;
			if (_stick[i].node[1] > node) _stick[i].node[1]--;
		}
	}
	for (uint i = _force.size() - 1; i != (uint)-1; i--)
	{
		if (_force[i].node == node) _force.erase(_force.begin() + i);
		else if (_force[i].node > node) _force[i].node--;
	}
	_node.erase(_node.begin() + node);
}

void p6::Construction::set_node_coord(uint node, Coord coord) noexcept
{
	assert(!_simulation);
	assert(coord.x == coord.x);
	assert(abs(coord.x) != std::numeric_limits<real>::infinity());
	assert(coord.y == coord.y);
	assert(abs(coord.y) != std::numeric_limits<real>::infinity());
	_node[node].coord = coord;
}

void p6::Construction::set_node_freedom(uint node, unsigned char freedom) noexcept
{
	assert(!_simulation);
	assert(freedom <= 2);
	_node[node].freedom = freedom;
}

void p6::Construction::set_node_rail_angle(uint node, real angle) noexcept
{
	assert(!_simulation);
	assert(_node[node].freedom == 1);
	assert(angle == angle);
	assert(abs(angle) != std::numeric_limits<real>::infinity());
	_node[node].angle = angle;
}

p6::uint p6::Construction::get_node_count() const noexcept
{
	return _node.size();
}

p6::Coord p6::Construction::get_node_coord(uint node) const noexcept
{
	return _simulation ? _node[node].coord_simulated : _node[node].coord;
}

unsigned char p6::Construction::get_node_freedom(uint node) const noexcept
{
	return _node[node].freedom;
}

p6::real p6::Construction::get_node_rail_angle(uint node) const noexcept
{
	assert(_node[node].freedom == 1);
	return _node[node].angle;
}

p6::uint p6::Construction::create_stick(const uint node[2]) noexcept
{
	assert(!_simulation);
	assert(node[0] != node[1]);
	assert(node[0] < _node.size());
	assert(node[1] < _node.size());
	for (uint i = 0; i < _stick.size(); i++)
	{
		if((_stick[i].node[0] == node[0] && _stick[i].node[0] == node[1])
		&& (_stick[i].node[1] == node[1] && _stick[i].node[1] == node[1]))
			return i;
	}

	Stick stick;
	stick.node[0] = node[0];
	stick.node[1] = node[1];
	stick.material = (uint)-1;
	stick.area = 0.0;
	_stick.push_back(stick);
	return _stick.size() - 1;
}

void p6::Construction::delete_stick(uint stick) noexcept
{
	assert(!_simulation);
	_stick.erase(_stick.begin() + stick);
}

void p6::Construction::set_stick_material(uint stick, uint material) noexcept
{
	assert(!_simulation);
	_stick[stick].material = material;
}

void p6::Construction::set_stick_area(uint stick, real area) noexcept
{
	assert(!_simulation);
	assert(area == area);
	_stick[stick].area = area;
}

p6::uint p6::Construction::get_stick_count() const noexcept
{
	return _stick.size();
}

p6::uint p6::Construction::get_stick_material(uint stick) const noexcept
{
	return _stick[stick].material;
}

p6::real p6::Construction::get_stick_area(uint stick) const noexcept
{
	return _stick[stick].area;
}

void p6::Construction::get_stick_node(uint stick, uint node[2]) const noexcept
{
	node[0] = _stick[stick].node[0];
	node[1] = _stick[stick].node[1];
}

p6::real p6::Construction::get_stick_length(uint stick) const noexcept
{
	const Node *node[2] = { &_node[_stick[stick].node[0]], &_node[_stick[stick].node[1]] };
	return _simulation ?
		node[0]->coord_simulated.distance(node[1]->coord_simulated) :
		node[0]->coord.distance(node[1]->coord);
}

p6::real p6::Construction::get_stick_strain(uint stick) const noexcept
{
	assert(_simulation);
	const Node *node[2] = { &_node[_stick[stick].node[0]], &_node[_stick[stick].node[1]] };
	return (
		node[0]->coord_simulated.distance(node[1]->coord_simulated) /
		node[0]->coord.distance(node[1]->coord)
		) - 1.0;
}

p6::real p6::Construction::get_stick_force(uint stick) const noexcept
{
	assert(_simulation);
	return _stick[stick].area * _material[_stick[stick].material]->stress(get_stick_strain(stick));
}

p6::uint p6::Construction::create_force(uint node) noexcept
{
	assert(!_simulation);
	assert(node < _node.size());
	Force force;
	force.node = node;
	force.direction = Coord(0.0, 0.0);
	_force.push_back(force);
	return _force.size() - 1;
}

void p6::Construction::delete_force(uint force) noexcept
{
	assert(!_simulation);
	_force.erase(_force.begin() + force);
}

void p6::Construction::set_force_direction(uint force, Coord direction) noexcept
{
	assert(!_simulation);
	assert(direction.x == direction.x);
	assert(direction.y == direction.y);
	_force[force].direction = direction;
}

p6::uint p6::Construction::get_force_count() const noexcept
{
	return _force.size();
}

p6::Coord p6::Construction::get_force_direction(uint force) const noexcept
{
	return _force[force].direction;
}

p6::uint p6::Construction::get_force_node(uint force) const noexcept
{
	return _force[force].node;
}

p6::uint p6::Construction::create_linear_material(const String name, real modulus)
{
	assert(!_simulation);
	for (uint i = 0; i < _material.size(); i++)
	{
		if (name == _material[i]->name())
		{
			Material *material = new LinearMaterial(name, modulus);
			delete _material[i];
			_material[i] = material;
			return i;
		}
	}
	_material.push_back(new LinearMaterial(name, modulus));
	return _material.size() - 1;
}

p6::uint p6::Construction::create_nonlinear_material(const String name, const String formula)
{
	assert(!_simulation);
	for (uint i = 0; i < _material.size(); i++)
	{
		if (name == _material[i]->name())
		{
			Material *material = new NonlinearMaterial(name, formula);
			delete _material[i];
			_material[i] = material;
			return i;
		}
	}

	_material.push_back(new NonlinearMaterial(name, formula));
	return _material.size() - 1;
}

void p6::Construction::delete_material(uint material) noexcept
{
	assert(!_simulation);
	for (uint i = 0; i < _stick.size(); i++)
	{
		if (_stick[i].material == material) _stick[i].material = (uint)-1;
	}
	delete _material[material];
	_material.erase(_material.begin() + material);
}

p6::uint p6::Construction::get_material_count() const noexcept
{
	return _material.size();
}

p6::String p6::Construction::get_material_name(uint material) const noexcept
{
	return _material[material]->name();
}

p6::Material::Type p6::Construction::get_material_type(uint material) const noexcept
{
	return _material[material]->type();
}

p6::real p6::Construction::get_material_modulus(uint material) const noexcept
{
	assert(_material[material]->type() == Material::Type::linear);
	return ((LinearMaterial*)_material[material])->modulus();
}

p6::String p6::Construction::get_material_formula(uint material) const noexcept
{
	assert(_material[material]->type() == Material::Type::nonlinear);
	return ((NonlinearMaterial*)_material[material])->formula();
}

void p6::Construction::save(const String filepath) const
{
	//Open file
	OutputFile file(filepath);
	if (!file.ok()) throw std::runtime_error("File cannot be opened for write");
	Header header;
	header.node = _node.size();
	header.stick = _stick.size();
	header.force = _force.size();
	header.material = _material.size();
	file.write(&header, sizeof(Header));

	//Nodes
	for (uint i = 0; i < _node.size(); i++)
	{
		file.write(&_node[i], sizeof(StaticNode));
	}

	//Sticks
	for (uint i = 0; i < _stick.size(); i++)
	{
		file.write(&_stick[i], sizeof(Stick));
	}

	//Forces
	for (uint i = 0; i < _force.size(); i++)
	{
		file.write(&_force[i], sizeof(Force));
	}

	//Materials
	for (uint i = 0; i < _material.size(); i++)
	{
		//Name
		String name = _material[i]->name();
		uint len = name.size();
		file.write(&len, sizeof(uint));
		file.write(name.data(), len);

		//Type
		Material::Type type = _material[i]->type();
		file.write(&type, sizeof(Material::Type));

		if (type == Material::Type::linear)
		{
			//Modulus
			real modulus = ((LinearMaterial*)_material[i])->modulus();
			file.write(&modulus, sizeof(real));
		}
		else
		{
			//Formula
			String formula = ((NonlinearMaterial*)_material[i])->formula();
			len = formula.size();
			file.write(&len, sizeof(uint));
			file.write(formula.data(), len);
		}
	}
}

void p6::Construction::load(const String filepath)
{
	//Open file
	InputFile file(filepath);
	if (!file.ok()) throw std::runtime_error("File cannot be opened for read");
	Header header, sample;
	file.read(&header, sizeof(Header));
	if (memcmp(header.signature, sample.signature, 8) != 0) throw ("Invalid file format");

	//Nodes
	_node.resize(header.node);
	for (uint i = 0; i < _node.size(); i++)
	{
		file.read(&_node[i], sizeof(StaticNode));
	}

	//Sticks
	_stick.resize(header.stick);
	for (uint i = 0; i < _stick.size(); i++)
	{
		file.read(&_stick[i], sizeof(Stick));
	}

	//Forces
	_force.resize(header.force);
	for (uint i = 0; i < _force.size(); i++)
	{
		file.read(&_force[i], sizeof(Force));
	}

	//Materials
	for (uint i = 0; i < _material.size(); i++) delete _material[i];
	_material.resize(header.material);
	for (uint i = 0; i < _material.size(); i++)
	{
		//Name
		uint len;
		file.read(&len, sizeof(uint));
		String name(len, '\0');
		file.read(&name[0], len);

		//Type
		Material::Type type;
		file.read(&type, sizeof(Material::Type));

		if (type == Material::Type::linear)
		{
			//Modulus
			real modulus;
			file.read(&modulus, sizeof(real));
			_material[i] = new LinearMaterial(name, modulus);
		}
		else
		{
			//Formula
			file.read(&len, sizeof(uint));
			String formula(len, '\0');
			file.read(&formula[0], len);
			_material[i] = new NonlinearMaterial(name, formula);
		}
	}
}

void p6::Construction::import(const String filepath)
{
	//Open file
	InputFile file(filepath);
	if (!file.ok()) throw std::runtime_error("File cannot be opened for read");
	Header header, sample;
	file.read(&header, sizeof(Header));
	if (memcmp(header.signature, sample.signature, 8) != 0) throw ("Invalid file format");

	uint old_node_size = _node.size();
	uint old_stick_size = _stick.size();
	uint old_force_size = _force.size();
	uint old_material_size = _material.size();

	//Nodes
	_node.resize(old_node_size + header.node);
	for (uint i = old_node_size; i < _node.size(); i++)
	{
		file.read(&_node[i], sizeof(StaticNode));
	}

	//Sticks
	_stick.resize(old_stick_size + header.stick);
	for (uint i = old_stick_size; i < _stick.size(); i++)
	{
		file.read(&_stick[i], sizeof(Stick));
		_stick[i].node[0] += old_node_size;
		_stick[i].node[1] += old_node_size;
		_stick[i].material += old_material_size;
	}

	//Forces
	_force.resize(old_force_size + header.force);
	for (uint i = old_force_size; i < _force.size(); i++)
	{
		file.read(&_force[i], sizeof(Force));
		_force[i].node += old_node_size;
	}

	//Materials
	for (uint i = 0; i < header.material; i++)
	{
		//Name
		uint len;
		file.read(&len, sizeof(uint));
		String name(len, '\0');
		file.read(&name[0], len);

		//Find existing material
		uint existing_material = (uint)-1;
		for (uint j = 0; j < old_material_size; j++)
		{
			if (_material[j]->name() == name)
			{
				existing_material = j;
				break;
			}
		}

		//Type
		Material::Type type;
		file.read(&type, sizeof(Material::Type));

		//Creating material
		Material *new_material = nullptr;
		if (type == Material::Type::linear)
		{
			//Modulus
			real modulus;
			file.read(&modulus, sizeof(real));
			if (existing_material == (uint)-1) new_material = new LinearMaterial(name, modulus);
		}
		else
		{
			//Formula
			file.read(&len, sizeof(uint));
			String formula(len, '\0');
			file.read(&formula[0], len);
			if (existing_material == (uint)-1) new_material = new NonlinearMaterial(name, formula);
		}

		//Adding material to materials
		if (existing_material == (uint)-1)
		{
			_material.push_back(new_material);
			existing_material = _material.size() - 1;
		}

		//Correcting sticks
		for (uint i = old_stick_size; i < _stick.size(); i++)
		{
			if (_stick[i].material == old_material_size + i) _stick[i].material = existing_material;
		}
	}
}

p6::uint p6::Construction::_node_equation_fx(uint free2d) const noexcept
{
	assert(free2d < _nfree2d);
	return 2 * free2d;
}

p6::uint p6::Construction::_node_equation_fy(uint free2d) const noexcept
{
	assert(free2d < _nfree2d);
	return 2 * free2d + 1;
}

p6::uint p6::Construction::_node_equation_fr(uint free1d) const noexcept
{
	assert(free1d < _nfree1d);
	return 2 * _nfree2d + free1d;
}

p6::uint p6::Construction::_equation_number() const noexcept
{
	return 2 * _nfree2d + _nfree1d;
}

p6::uint p6::Construction::_node_variable_x(uint free2d) const noexcept
{
	assert(free2d < _nfree2d);
	return 2 * free2d;
}

p6::uint p6::Construction::_node_variable_y(uint free2d) const noexcept
{
	assert(free2d < _nfree2d);
	return 2 * free2d + 1;
}

p6::uint p6::Construction::_node_variable_r(uint free1d) const noexcept
{
	assert(free1d < _nfree1d);
	return 2 * _nfree2d + free1d;
}

p6::uint p6::Construction::_variable_number() const noexcept
{
	return 2 * _nfree2d + _nfree1d;
}

void p6::Construction::_check_materials_specified() const
{
	for (uint i = 0; i < _stick.size(); i++)
	{
		assert(_stick[i].material < _material.size() || _stick[i].material == (uint)-1);
		if (_stick[i].material == (uint)-1) throw std::runtime_error("Material is not specified");
	}
}

void p6::Construction::_create_map(std::vector<uint> *node_to_free) noexcept
{
	_nfree2d = 0;
	_nfree1d = 0;
	node_to_free->resize(_node.size(), (uint)-1);
	for (uint i = 0; i < _node.size(); i++)
	{
		if (_node[i].freedom == 1)
		{
			node_to_free->at(i) = _nfree1d;
			_nfree1d++;
		}
		else if (_node[i].freedom == 2)
		{
			node_to_free->at(i) = _nfree2d;
			_nfree2d++;
		}
	}
}

p6::real p6::Construction::_get_tolerance() const noexcept
{
	real minforce = std::numeric_limits<real>::infinity();
	for (uint i = 0; i < _force.size(); i++)
	{
		real newforce = _force[i].direction.norm();
		if (newforce < minforce) minforce = newforce;
	}
	return minforce / 1000.0;
}

void p6::Construction::_create_vectors(
	const std::vector <uint> *node_to_free,
	Vector *s,
	Vector *z,
	Vector *m,
	Matrix *d) const noexcept
{
	s->resize(_variable_number());
	for (uint i = 0; i < _node.size(); i++)
	{
		if (_node[i].freedom == 1)
		{
			uint free1d = node_to_free->at(i);
			(*s)(_node_variable_r(free1d)) = 0.0;
		}
		else if (_node[i].freedom == 2)
		{
			uint free2d = node_to_free->at(i);
			(*s)(_node_variable_x(free2d)) = _node[i].coord.x;
			(*s)(_node_variable_y(free2d)) = _node[i].coord.y;
		}
	}
	z->resize(_equation_number());
	z->setZero();
	m->resize(_equation_number());
	m->setZero();
	d->resize(_equation_number(), _variable_number());
	d->setZero();
}

void p6::Construction::_set_z_to_external_forces(
	const std::vector <uint> *node_to_free,
	Vector *z) const noexcept
{
	for (uint i = 0; i < _force.size(); i++)
	{
		uint node = _force[i].node;
		if (_node[node].freedom == 1)
		{
			uint free1d = node_to_free->at(node);
			real angle = _node[node].angle;
			(*z)(_node_variable_r(free1d)) = _force[i].direction.x * cos(angle) + _force[i].direction.y * sin(angle);
		}
		else if (_node[node].freedom == 2)
		{
			uint free2d = node_to_free->at(node);
			(*z)(_node_variable_x(free2d)) = _force[i].direction.x;
			(*z)(_node_variable_y(free2d)) = _force[i].direction.y;
		}
	}
}

void p6::Construction::_set_d_to_zero(
	const std::vector <uint> *node_to_free,
	Matrix *d) const noexcept
{
	for (uint i = 0; i < _stick.size(); i++)
	{
		const uint *node = _stick[i].node;
		for (uint j = 0; j < 2; j++)
		{
			//If current point is fixed on rail
			if (_node[node[j]].freedom == 1)
			{
				//It sets own derivatives on own coordinates
				uint free1d = node_to_free->at(node[j]);
				(*d)(_node_equation_fr(free1d), _node_variable_r(free1d)) = 0.0;

				//And own derivatives on coordinates of other point
				if (_node[node[j ^ 1]].freedom == 1)
				{
					uint other_free1d = node_to_free->at(node[j ^ 1]);
					(*d)(_node_equation_fr(free1d), _node_variable_r(other_free1d)) = 0.0;
				}
				else if (_node[node[j ^ 1]].freedom == 2)
				{
					uint other_free2d = node_to_free->at(node[j ^ 1]);
					(*d)(_node_equation_fr(free1d), _node_variable_x(other_free2d)) = 0.0;
					(*d)(_node_equation_fr(free1d), _node_variable_y(other_free2d)) = 0.0;
				}
			}
			//If current point is free
			else if (_node[node[j]].freedom == 2)
			{
				//It sets own derivatives on own coordinates
				uint free2d = node_to_free->at(node[j]);
				(*d)(_node_equation_fx(free2d), _node_variable_x(free2d)) = 0.0;
				(*d)(_node_equation_fx(free2d), _node_variable_y(free2d)) = 0.0;
				(*d)(_node_equation_fy(free2d), _node_variable_x(free2d)) = 0.0;
				(*d)(_node_equation_fy(free2d), _node_variable_y(free2d)) = 0.0;

				//And own derivatives on coordinates of other point
				if (_node[node[j ^ 1]].freedom == 1)
				{
					uint other_free1d = node_to_free->at(node[j ^ 1]);
					(*d)(_node_equation_fx(free2d), _node_variable_r(other_free1d)) = 0.0;
					(*d)(_node_equation_fy(free2d), _node_variable_r(other_free1d)) = 0.0;
				}
				else if (_node[node[j ^ 1]].freedom == 2)
				{
					uint other_free2d = node_to_free->at(node[j ^ 1]);
					(*d)(_node_equation_fx(free2d), _node_variable_x(other_free2d)) = 0.0;
					(*d)(_node_equation_fx(free2d), _node_variable_y(other_free2d)) = 0.0;
					(*d)(_node_equation_fy(free2d), _node_variable_x(other_free2d)) = 0.0;
					(*d)(_node_equation_fy(free2d), _node_variable_y(other_free2d)) = 0.0;
				}
			}
		}
	}
}

p6::Coord p6::Construction::_get_delta(
	uint stick,
	const std::vector <uint> *node_to_free,
	const Vector *s) const noexcept
{
	const uint *node = _stick[stick].node;
	Coord coord[2];
	for (uint i = 0; i < 2; i++)
	{
		if (_node[node[i]].freedom == 1)
		{
			uint free1d = node_to_free->at(node[i]);
			real angle = _node[node[i]].angle;
			coord[i] = _node[node[i]].coord + Coord(cos(angle), sin(angle)) * (*s)(_node_variable_r(free1d));
		}
		else if (_node[node[i]].freedom == 2)
		{
			uint free2d = node_to_free->at(node[i]);
			coord[i] = Coord((*s)(_node_variable_x(free2d)), (*s)(_node_variable_y(free2d)));
		}
		else coord[i] = _node[node[i]].coord;
	}
	return coord[1] - coord[0];
}

void p6::Construction::_modify_z_with_stick_force(
	uint stick,
	const std::vector <uint> *node_to_free,
	const Vector *s,
	Vector *z) const noexcept
{
	const Material *material = _material[_stick[stick].material];
	const uint *node = _stick[stick].node;
	Coord delta = _get_delta(stick, node_to_free, s);
	real length = delta.norm();
	real initial_length = _node[node[0]].coord.distance(_node[node[1]].coord);
	real force = _stick[stick].area * material->stress(length / initial_length - 1.0);

	for (uint i = 0; i < 2; i++)
	{
		if (_node[node[i]].freedom == 1)
		{
			uint free1d = node_to_free->at(node[i]);
			real sign = i == 0 ? 1.0 : -1.0;
			(*z)(_node_variable_r(free1d)) +=
				cos(_node[node[i]].angle) * sign * force * delta.x / length +
				sin(_node[node[i]].angle) * sign * force * delta.y / length;
		}
		else if (_node[node[i]].freedom == 2)
		{
			uint free2d = node_to_free->at(node[i]);
			real sign = i == 0 ? 1.0 : -1.0;
			(*z)(_node_variable_x(free2d)) += sign * force * delta.x / length;
			(*z)(_node_variable_y(free2d)) += sign * force * delta.y / length;
		}
	}
}

void p6::Construction::_modify_d_with_stick_force(
	uint stick,
	const std::vector <uint> *node_to_free,
	const Vector *s,
	Matrix *d) const noexcept
{
	const uint *node = _stick[stick].node;
	const Material *material = _material[_stick[stick].material];
	Coord delta = _get_delta(stick, node_to_free, s);
	real length = delta.norm();
	real initial_length = _node[node[0]].coord.distance(_node[node[1]].coord);
	real strain = length / initial_length - 1.0;
	real force = _stick[stick].area * material->stress(strain);

	for (uint i = 0; i < 2; i++)
	{
		Coord deltaoi = (i == 0) ? delta : delta * (-1.0);

		//If current point is fixed on rail
		if (_node[node[i]].freedom == 1)
		{
			//It sets own derivatives on own coordinates
			uint free1d = node_to_free->at(node[i]);
			real anglei = _node[node[i]].angle;
			real dl_dri = (-deltaoi.x * cos(anglei) - deltaoi.y * sin(anglei)) / length;
			real df_dri = _stick[stick].area * material->derivative(strain) * dl_dri / initial_length;
			(*d)(_node_equation_fr(free1d), _node_variable_r(free1d)) += (
				cos(anglei) * ((df_dri * deltaoi.x + force * (-cos(anglei))) * length - dl_dri * force * deltaoi.x) +
				sin(anglei) * ((df_dri * deltaoi.y + force * (-sin(anglei))) * length - dl_dri * force * deltaoi.y)
				) / sqr(length);

			//And own derivatives on coordinates of other point
			if (_node[node[i ^ 1]].freedom == 1)
			{
				uint other_free1d = node_to_free->at(node[i ^ 1]);
				real angleo = _node[node[i ^ 1]].angle;
				real dl_dro = (deltaoi.x * cos(angleo) + deltaoi.y * sin(angleo)) / length;
				real df_dro = _stick[stick].area * material->derivative(strain) * dl_dro / initial_length;
				(*d)(_node_equation_fr(free1d), _node_variable_r(other_free1d)) += (
					cos(anglei) * ((df_dro * deltaoi.x + force * cos(angleo)) * length - dl_dro * force * deltaoi.x) +
					sin(anglei) * ((df_dro * deltaoi.y + force * sin(angleo)) * length - dl_dro * force * deltaoi.y)
					) / sqr(length);
			}
			else if (_node[node[i ^ 1]].freedom == 2)
			{
				uint other_free2d = node_to_free->at(node[i ^ 1]);
				real dl_dxo = deltaoi.x / length;
				real df_dxo = _stick[stick].area * material->derivative(strain) * dl_dxo / initial_length;
				(*d)(_node_equation_fr(free1d), _node_variable_x(other_free2d)) += (
					cos(anglei) * ((df_dxo * deltaoi.x + force * 1.0) * length - dl_dxo * force * deltaoi.x) +
					sin(anglei) * deltaoi.y * (df_dxo * length - dl_dxo * force)
					) / sqr(length);
				real dl_dyo = deltaoi.y / length;
				real df_dyo = _stick[stick].area * material->derivative(strain) * dl_dyo / initial_length;
				(*d)(_node_equation_fr(free1d), _node_variable_y(other_free2d)) += (
					cos(anglei) * deltaoi.x * (df_dyo * length - dl_dyo * force) +
					sin(anglei) * ((df_dyo * deltaoi.y + force * 1.0) * length - dl_dyo * force * deltaoi.y)
					) / sqr(length);
			}
		}
		//If current point is free
		else if (_node[node[i]].freedom == 2)
		{
			//It sets own derivatives on own coordinates
			uint free2d = node_to_free->at(node[i]);

			real dl_dxi = -deltaoi.x / length;
			real df_dxi = _stick[stick].area * material->derivative(strain) * dl_dxi / initial_length;
			real dfxi_dxi = ((df_dxi * deltaoi.x + force * (-1.0)) * length - dl_dxi * force * deltaoi.x) / sqr(length);
			(*d)(_node_equation_fx(free2d), _node_variable_x(free2d)) += dfxi_dxi;
			real dl_dyi = -deltaoi.y / length;
			real df_dyi = _stick[stick].area * material->derivative(strain) * dl_dyi / initial_length;
			real dfxi_dyi = deltaoi.x * (df_dyi * length - dl_dyi * force) / sqr(length);
			(*d)(_node_equation_fx(free2d), _node_variable_y(free2d)) += dfxi_dyi;
			real dfyi_dxi = deltaoi.y * (df_dxi * length - dl_dxi * force) / sqr(length);
			(*d)(_node_equation_fy(free2d), _node_variable_x(free2d)) += dfyi_dxi;
			real dfyi_dyi = ((df_dyi * deltaoi.y + force * (-1.0)) * length - dl_dyi * force * deltaoi.y) / sqr(length);
			(*d)(_node_equation_fy(free2d), _node_variable_y(free2d)) += dfyi_dyi;

			//And own derivatives on coordinates of other point
			if (_node[node[i ^ 1]].freedom == 1)
			{
				uint other_free1d = node_to_free->at(node[i ^ 1]);
				real angleo = _node[node[i ^ 1]].angle;
				real dl_dro = (deltaoi.x * cos(angleo) + deltaoi.y * sin(angleo)) / length;
				real df_dro = _stick[stick].area * material->derivative(strain) * dl_dro / initial_length;
				(*d)(_node_equation_fx(free2d), _node_variable_r(other_free1d)) +=
					((df_dro * deltaoi.x + force * cos(angleo)) * length - dl_dro * force * deltaoi.x) / sqr(length);
				(*d)(_node_equation_fy(free2d), _node_variable_r(other_free1d)) +=
					((df_dro * deltaoi.y + force * sin(angleo)) * length - dl_dro * force * deltaoi.y) / sqr(length);
			}
			else if (_node[node[i ^ 1]].freedom == 2)
			{
				uint other_free2d = node_to_free->at(node[i ^ 1]);
				(*d)(_node_equation_fx(free2d), _node_variable_x(other_free2d)) -= dfxi_dxi;
				(*d)(_node_equation_fx(free2d), _node_variable_y(other_free2d)) -= dfxi_dyi;
				(*d)(_node_equation_fy(free2d), _node_variable_x(other_free2d)) -= dfyi_dxi;
				(*d)(_node_equation_fy(free2d), _node_variable_y(other_free2d)) -= dfyi_dyi;
			}
		}
	}
}

p6::real p6::Construction::_get_residuum(const Vector *z) const noexcept
{
	real error = 0.0;
	for (int i = 0; i < z->rows(); i++)
	{
		real newabs = abs((*z)(i));
		if (newabs > error) error = newabs;
	}
	return error;
}

void p6::Construction::_apply_state_vector(
	const std::vector<uint> *node_to_free,
	const Vector *s) noexcept
{
	for (uint i = 0; i < _node.size(); i++)
	{
		if (_node[i].freedom == 1)
		{
			uint free1d = node_to_free->at(i);
			real angle = _node[i].angle;
			_node[i].coord_simulated = _node[i].coord + Coord(cos(angle), sin(angle)) * (*s)(_node_variable_r(free1d));
		}
		else if (_node[i].freedom == 2)
		{
			uint free2d = node_to_free->at(i);
			_node[i].coord_simulated = Coord((*s)(_node_variable_x(free2d)), (*s)(_node_variable_y(free2d)));
		}
		else _node[i].coord_simulated = _node[i].coord;
	}
}

bool p6::Construction::_is_adequate(
	const Vector *m,
	const std::vector <uint> *node_to_free,
	const Vector *s) const noexcept
{
	for (int i = 0; i < m->rows(); i++)
	{
		if ((*m)(i) != (*m)(i)) return false;
		for (uint i = 0; i < _stick.size(); i++)
		{
			const uint *node = _stick[i].node;
			Coord delta = _get_delta(i, node_to_free, s);
			real length = delta.norm();
			for (uint j = 0; j < 2; j++)
			{
				if (_node[node[j]].freedom == 1)
				{
					uint free1d = node_to_free->at(node[j]);
					real modification = abs((*m)(_node_equation_fr(free1d)));
					if (modification > length * 0.01) return false;
				}
				else if (_node[node[j]].freedom == 2)
				{
					uint free2d = node_to_free->at(node[j]);
					real modification = Coord((*m)(_node_equation_fx(free2d)), (*m)(_node_equation_fy(free2d))).norm();
					if (modification > length * 0.01) return false;
				}
			}
		}
	}
	return true;
}

p6::real  p6::Construction::_get_flow_coefficient(
	const std::vector <uint> *node_to_free,
	const Vector *s,
	const Vector *z) const noexcept
{
	real coef = std::numeric_limits<real>::infinity();
	for (uint i = 0; i < _stick.size(); i++)
	{
		const uint *node = _stick[i].node;
		Coord delta = _get_delta(i, node_to_free, s);
		real length = delta.norm();
		real initial_length = _node[node[0]].coord.distance(_node[node[1]].coord);
		real strain = length / initial_length - 1.0;
		real df_dl = _stick[i].area * _material[_stick[i].material]->derivative(strain) / initial_length;
		if (1.0 / df_dl < coef) coef = 1.0 / df_dl;

		for (uint j = 0; j < 2; j++)
		{
			if (_node[node[j]].freedom == 1)
			{
				uint free1d = node_to_free->at(node[j]);
				real unbalanced_force = abs((*z)(_node_equation_fr(free1d)));
				if (length / unbalanced_force < coef) coef = length / unbalanced_force;
			}
			else if (_node[node[j]].freedom == 2)
			{
				uint free2d = node_to_free->at(node[j]);
				real unbalanced_force = Coord((*z)(_node_equation_fx(free2d)), (*z)(_node_equation_fy(free2d))).norm();
				if (length / unbalanced_force < coef) coef = length / unbalanced_force;
			}
		}
	}
	return coef;
}

void p6::Construction::simulate(bool sim)
{
	if (sim == _simulation) return;
	else if (!sim) { _simulation = false; return; }

	//Checking if materials are specified
	_check_materials_specified();

	//Creating node-to-free map
	std::vector<uint> node_to_free;
	_create_map(&node_to_free);

	//Calculating tolerance
	real tolerance = _get_tolerance();

	//Creating vectors and matrixes
	Vector s;	//State vector
	Vector z;	//Should-be-zero value
	Vector m;	//Modification of state vector
	Matrix d;	//Derivative of should-be-zero value
	_create_vectors(&node_to_free, &s, &z, &m, &d);

	//Iterating
	real last_error = 0.0;
	uint not_converge_count = 0;
	while (true)
	{
		_set_z_to_external_forces(&node_to_free, &z);
		_set_d_to_zero(&node_to_free, &d);
		for (uint i = 0; i < _stick.size(); i++)
		{
			_modify_z_with_stick_force(i, &node_to_free, &s, &z);
			_modify_d_with_stick_force(i, &node_to_free, &s, &d);
		}
		real error = _get_residuum(&z);
		if (error < tolerance) break;
		else if (error < last_error) not_converge_count = 0;
		else if (++not_converge_count == 10000) throw std::runtime_error("Simulation does not converge");
		Eigen::HouseholderQR<Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic>> qr(d);
		m = qr.solve(z);
		if (_is_adequate(&m, &node_to_free, &s)) s -= m;
		else s += 0.01 * _get_flow_coefficient(&node_to_free, &s, &z) * z;
	}
	_apply_state_vector(&node_to_free, &s);

	_simulation = true;
}

p6::Construction::~Construction()
{
	for (uint i = 0; i < _material.size(); i++) delete _material[i];
}


// Copyright (c) 2015-2018 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#ifndef individual_model_hpp
#define individual_model_hpp

#include <vector>

#include <boost/optional.hpp>

#include "genotype_prior_model.hpp"
#include "core/types/haplotype.hpp"
#include "core/models/haplotype_likelihood_cache.hpp"
#include "core/types/genotype.hpp"
#include "logging/logging.hpp"

namespace octopus { namespace model {

class IndividualModel
{
public:
    struct Latents
    {
        using GenotypeProbabilityVector = std::vector<double>;
        GenotypeProbabilityVector genotype_probabilities;
    };

    struct InferredLatents
    {
        Latents posteriors;
        double log_evidence;
    };

    IndividualModel() = delete;

    IndividualModel(const GenotypePriorModel& genotype_prior_model,
                    boost::optional<logging::DebugLogger> debug_log = boost::none,
                    boost::optional<logging::TraceLogger> trace_log = boost::none);

    IndividualModel(const IndividualModel&)            = delete;
    IndividualModel& operator=(const IndividualModel&) = delete;
    IndividualModel(IndividualModel&&)                 = delete;
    IndividualModel& operator=(IndividualModel&&)      = delete;

    ~IndividualModel() = default;

    const GenotypePriorModel& prior_model() const noexcept;

    InferredLatents evaluate(const std::vector<Genotype<Haplotype>>& genotypes,
                             const HaplotypeLikelihoodCache& haplotype_likelihoods) const;

    InferredLatents evaluate(const std::vector<Genotype<Haplotype>>& genotypes,
                             const std::vector<std::vector<unsigned>>& genotype_indices,
                             const HaplotypeLikelihoodCache& haplotype_likelihoods) const;

private:
    const GenotypePriorModel& genotype_prior_model_;

    mutable boost::optional<logging::DebugLogger> debug_log_;
    mutable boost::optional<logging::TraceLogger> trace_log_;
};

} // namesapce model
} // namespace octopus

#endif


/*
 * This file is part of the DUDS project. It is subject to the BSD-style
 * license terms in the LICENSE file found in the top-level directory of this
 * distribution and at http://www.somewhere.org/somepath/license.html.
 * No part of DUDS, including this file, may be copied, modified, propagated,
 * or distributed except according to the terms contained in the LICENSE file.
 *
 * Copyright (C) 2017  Jeff Jackowski
 */
#include <duds/hardware/devices/instruments/INA219.hpp>
#include <duds/hardware/interface/linux/DevSmbus.hpp>
#include <iostream>
#include <thread>
#include <iomanip>
#include <assert.h>
#include <boost/exception/diagnostic_information.hpp>

constexpr int valw = 8;
bool quit = false;

void runtest(duds::hardware::devices::instruments::INA219 &ina) {
	std::cout.precision(5);
	std::int16_t sv, bv;
	do {
		ina.sample();
		duds::data::Quantity shnV = ina.shuntVoltage();
		duds::data::Quantity busV = ina.busVoltage();
		duds::data::Quantity busI = ina.busCurrent();
		assert(busV.unit == duds::data::units::Volt);
		assert(busI.unit == duds::data::units::Ampere);
		assert(shnV.unit == duds::data::units::Volt);
		duds::data::Quantity busP = busV * busI;
		assert(busP.unit == duds::data::units::Watt);
		ina.vals(sv, bv);
		std::cout << "Shunt: " << std::setw(valw) << shnV.value <<
			"v   Bus: " << std::setw(valw) << busV.value << "v  " <<
			std::setw(valw) << busI.value << "A  " << std::setw(valw) <<
			busP.value <<
			//'W' << std::endl;
			"W   s = " << std::setw(valw-2) << sv << " b = " <<
			std::setw(valw-2) << bv << std::endl;
		std::this_thread::sleep_for(std::chrono::seconds(1));
	} while (!quit);
}

int main(void)
try {
	std::unique_ptr<duds::hardware::interface::Smbus> smbus(
		new duds::hardware::interface::linux::DevSmbus(
			"/dev/i2c-1",
			0x40,
			duds::hardware::interface::Smbus::NoPec()
		)
	);
	duds::hardware::devices::instruments::INA219 meter(smbus, 0.1);
	std::this_thread::sleep_for(std::chrono::milliseconds(2));
	std::thread doit(runtest, std::ref(meter));
	std::cin.get();
	quit = true;
	doit.join();
} catch (...) {
	std::cerr << "ERROR: " << boost::current_exception_diagnostic_information()
	<< std::endl;
	return 1;
}


// Boost.Geometry - gis-projections (based on PROJ4)

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

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

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

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

// Last updated version of proj: 5.0.0

// Original copyright notice:

// Purpose:  Stub projection implementation for lat/long coordinates. We
//           don't actually change the coordinates, but we want proj=latlong
//           to act sort of like a projection.
// Author:   Frank Warmerdam, warmerdam@pobox.com
// Copyright (c) 2000, Frank Warmerdam

// 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.

#ifndef BOOST_GEOMETRY_PROJECTIONS_LATLONG_HPP
#define BOOST_GEOMETRY_PROJECTIONS_LATLONG_HPP

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

namespace boost { namespace geometry
{

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace latlong
    {

            /* very loosely based upon DMA code by Bradford W. Drew */

            template <typename T, typename Parameters>
            struct base_latlong_other
            {
                // FORWARD(forward)
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(Parameters const& par, T const& lp_lon, T const& lp_lat, T& xy_x, T& xy_y) const
                {
                    // TODO: in the original code a is not used
                    // different mechanism is probably used instead
                    xy_x = lp_lon / par.a;
                    xy_y = lp_lat / par.a;
                }

                // INVERSE(inverse)
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(Parameters const& par, T const& xy_x, T const& xy_y, T& lp_lon, T& lp_lat) const
                {
                    // TODO: in the original code a is not used
                    // different mechanism is probably used instead
                    lp_lat = xy_y * par.a;
                    lp_lon = xy_x * par.a;
                }

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

            };

            // Lat/long (Geodetic)
            template <typename Parameters>
            inline void setup_latlong(Parameters& par)
            {
                    par.is_latlong = 1;
                    par.x0 = 0.0;
                    par.y0 = 0.0;
            }

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

    /*!
        \brief Lat/long (Geodetic) projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Example
        \image html ex_latlong.gif
    */
    template <typename T, typename Parameters>
    struct latlong_other : public detail::latlong::base_latlong_other<T, Parameters>
    {
        template <typename Params>
        inline latlong_other(Params const& , Parameters & par)
        {
            detail::latlong::setup_latlong(par);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION_FI(srs::spar::proj_lonlat, latlong_other)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION_FI(srs::spar::proj_latlon, latlong_other)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION_FI(srs::spar::proj_latlong, latlong_other)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION_FI(srs::spar::proj_longlat, latlong_other)

        // Factory entry(s)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_ENTRY_FI(latlong_entry, latlong_other)

        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_BEGIN(latlong_init)
        {
            BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_ENTRY(lonlat, latlong_entry)
            BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_ENTRY(latlon, latlong_entry)
            BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_ENTRY(latlong, latlong_entry)
            BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_ENTRY(longlat, latlong_entry)
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_LATLONG_HPP


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_FAST_TOINT_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_FAST_TOINT_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

namespace boost { namespace simd { namespace tag
  {
    /*!
      @brief  fast_toint generic tag

      Represents the fast_toint function in generic contexts.

      @par Models:
      Hierarchy
    **/
    struct fast_toint_ : ext::elementwise_<fast_toint_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<fast_toint_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_fast_toint_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
    template<class Site>
    BOOST_FORCEINLINE generic_dispatcher<tag::fast_toint_, Site> dispatching_fast_toint_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
    {
      return generic_dispatcher<tag::fast_toint_, Site>();
    }
    template<class... Args>
    struct impl_fast_toint_;
  }
  /*!
    Convert to integer by truncation.

    @par semantic:
    For any given value @c x of type @c T:

    @code
    as_integer<T> r = fast_toint(x);
    @endcode

    The code is similar to:

    @code
    as_integer<T> r = static_cast<as_integer<T> >(x)
    @endcode

    @par Notes:

    @c fast_toint cast a floating value to the signed integer value of the same bit size.

    This is done by C casting for scalars and corresponding intrinsic in simd (if available).

    Peculiarly,  that implies that the behaviour of this function on invalid entries is
    not defined and quite unpredictable.

    (For instance it is quite frequent that the test:

    @code
    fast_toint(Inf<double>()) ==  fast_toint(1.0/0.0)
    @endcode

    will return false whilst the test:

    @code
    Inf<double>() == 1.0/0.0
    @endcode

    returns true !)

    If you intend to use nans and infs entries,  consider using fast_toints instead.
    On integral typed values, it acts as identity.

    @par Alias

    fast__fast_toint

    @param  a0

    @return      a value of the integer type associated to the input.

  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::fast_toint_, fast_toint, 1)

} }

#include <boost/simd/operator/specific/common.hpp>

#endif