/**
 * @file ns_model_impl.hpp
 * @author Ryan Curtin
 *
 * This is a model for nearest or furthest neighbor search.  It is useful in
 * that it provides an easy way to serialize a model, abstracts away the
 * different types of trees, and also reflects the NeighborSearch API and
 * automatically directs to the right tree type.
 *
 * 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_NEIGHBOR_SEARCH_NS_MODEL_IMPL_HPP
#define MLPACK_METHODS_NEIGHBOR_SEARCH_NS_MODEL_IMPL_HPP

// In case it hasn't been included yet.
#include "ns_model.hpp"

#include <boost/serialization/variant.hpp>

namespace mlpack {
namespace neighbor {

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

//! Save parameters for bichromatic neighbor 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,
                                             const double tau,
                                             const double rho) :
    querySet(querySet),
    k(k),
    neighbors(neighbors),
    distances(distances),
    leafSize(leafSize),
    tau(tau),
    rho(rho)
{}

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

//! Bichromatic neighbor search on the given NSType specialized for KDTrees.
template<typename SortPolicy>
void BiSearchVisitor<SortPolicy>::operator()(NSTypeT<tree::KDTree>* ns) const
{
  if (ns)
    return SearchLeaf(ns);
  throw std::runtime_error("no neighbor search model initialized");
}

//! Bichromatic neighbor search on the given NSType specialized for BallTrees.
template<typename SortPolicy>
void BiSearchVisitor<SortPolicy>::operator()(NSTypeT<tree::BallTree>* ns) const
{
  if (ns)
    return SearchLeaf(ns);
  throw std::runtime_error("no neighbor search model initialized");
}

//! Bichromatic neighbor search specialized for SPTrees.
template<typename SortPolicy>
void BiSearchVisitor<SortPolicy>::operator()(SpillKNN* ns) const
{
  if (ns)
  {
    if (ns->SearchMode() == DUAL_TREE_MODE)
    {
      // For Dual Tree Search on SpillTrees, the queryTree must be built with
      // non overlapping (tau = 0).
      typename SpillKNN::Tree queryTree(std::move(querySet), 0 /* tau*/,
          leafSize, rho);
      ns->Search(queryTree, k, neighbors, distances);
    }
    else
      ns->Search(querySet, k, neighbors, distances);
  }
  else
    throw std::runtime_error("no neighbor search model initialized");
}

//! Bichromatic neighbor search specialized for octrees.
template<typename SortPolicy>
void BiSearchVisitor<SortPolicy>::operator()(NSTypeT<tree::Octree>* ns) const
{
  if (ns)
    return SearchLeaf(ns);
  throw std::runtime_error("no neighbor search model initialized");
}

//! Bichromatic neighbor search on the given NSType considering the leafSize.
template<typename SortPolicy>
template<typename NSType>
void BiSearchVisitor<SortPolicy>::SearchLeaf(NSType* ns) const
{
  if (ns->SearchMode() == DUAL_TREE_MODE)
  {
    std::vector<size_t> oldFromNewQueries;
    typename NSType::Tree queryTree(std::move(querySet), oldFromNewQueries,
        leafSize);

    arma::Mat<size_t> neighborsOut;
    arma::mat distancesOut;
    ns->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
    ns->Search(querySet, k, neighbors, distances);
}

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

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

//! Train on the given NSType specialized for KDTrees.
template<typename SortPolicy>
void TrainVisitor<SortPolicy>::operator()(NSTypeT<tree::KDTree>* ns) const
{
  if (ns)
    return TrainLeaf(ns);
  throw std::runtime_error("no neighbor search model initialized");
}

//! Train on the given NSType specialized for BallTrees.
template<typename SortPolicy>
void TrainVisitor<SortPolicy>::operator()(NSTypeT<tree::BallTree>* ns) const
{
  if (ns)
    return TrainLeaf(ns);
  throw std::runtime_error("no neighbor search model initialized");
}

//! Train specialized for SPTrees.
template<typename SortPolicy>
void TrainVisitor<SortPolicy>::operator()(SpillKNN* ns) const
{
  if (ns)
  {
    if (ns->SearchMode() == NAIVE_MODE)
      ns->Train(std::move(referenceSet));
    else
    {
      typename SpillKNN::Tree tree(std::move(referenceSet), tau, leafSize, rho);
      ns->Train(std::move(tree));
    }
  }
  else
    throw std::runtime_error("no neighbor search model initialized");
}

//! Train specialized for Octrees.
template<typename SortPolicy>
void TrainVisitor<SortPolicy>::operator()(NSTypeT<tree::Octree>* ns) const
{
  if (ns)
    return TrainLeaf(ns);
  throw std::runtime_error("no neighbor search model initialized");
}

//! Train on the given NSType considering the leafSize.
template<typename SortPolicy>
template<typename NSType>
void TrainVisitor<SortPolicy>::TrainLeaf(NSType* ns) const
{
  if (ns->SearchMode() == NAIVE_MODE)
    ns->Train(std::move(referenceSet));
  else
  {
    std::vector<size_t> oldFromNewReferences;
    typename NSType::Tree referenceTree(std::move(referenceSet),
        oldFromNewReferences, leafSize);
    ns->Train(std::move(referenceTree));
    // Set the mappings.
    ns->oldFromNewReferences = std::move(oldFromNewReferences);
  }
}

//! Return the search mode.
template<typename NSType>
NeighborSearchMode& SearchModeVisitor::operator()(NSType* ns) const
{
  if (ns)
    return ns->SearchMode();
  throw std::runtime_error("no neighbor search model initialized");
}

//! Expose the Epsilon method of the given NSType.
template<typename NSType>
double& EpsilonVisitor::operator()(NSType* ns) const
{
  if (ns)
    return ns->Epsilon();
  throw std::runtime_error("no neighbor search model initialized");
}

//! Expose the referenceSet of the given NSType.
template<typename NSType>
const arma::mat& ReferenceSetVisitor::operator()(NSType* ns) const
{
  if (ns)
    return ns->ReferenceSet();
  throw std::runtime_error("no neighbor search model initialized");
}

//! Clean memory, if necessary.
template<typename NSType>
void DeleteVisitor::operator()(NSType* ns) const
{
  if (ns)
    delete ns;
}

/**
 * Initialize the NSModel with the given type and whether or not a random
 * basis should be used.
 */
template<typename SortPolicy>
NSModel<SortPolicy>::NSModel(TreeTypes treeType, bool randomBasis) :
    treeType(treeType),
    leafSize(20),
    tau(0),
    rho(0.7),
    randomBasis(randomBasis)
{
  // Nothing to do.
}

template<typename SortPolicy>
NSModel<SortPolicy>::NSModel(const NSModel& other) :
    treeType(other.treeType),
    leafSize(other.leafSize),
    tau(other.tau),
    rho(other.rho),
    randomBasis(other.randomBasis),
    q(other.q),
    nSearch(other.nSearch)
{
  // Nothing to do.
}

template<typename SortPolicy>
NSModel<SortPolicy>::NSModel(NSModel&& other) :
    treeType(other.treeType),
    leafSize(other.leafSize),
    tau(other.tau),
    rho(other.rho),
    randomBasis(other.randomBasis),
    q(std::move(other.q)),
    nSearch(other.nSearch)
{
  // Reset parameters of the other model.
  other.treeType = TreeTypes::KD_TREE;
  other.leafSize = 20;
  other.tau = 0;
  other.rho = 0.7;
  other.randomBasis = false;
  other.nSearch = decltype(other.nSearch)();
}

template<typename SortPolicy>
NSModel<SortPolicy>& NSModel<SortPolicy>::operator=(const NSModel& other)
{
  boost::apply_visitor(DeleteVisitor(), nSearch);

  treeType = other.treeType;
  leafSize = other.leafSize;
  tau = other.tau;
  rho = other.rho;
  randomBasis = other.randomBasis;
  q = other.q;
  nSearch = other.nSearch;

  return *this;
}

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

  treeType = other.treeType;
  leafSize = other.leafSize;
  tau = other.tau;
  rho = other.rho;
  randomBasis = other.randomBasis;
  q = std::move(other.q);
  // Copy the pointer and type.
  nSearch = other.nSearch;

  // Reset parameters of the other model.
  other.treeType = TreeTypes::KD_TREE;
  other.leafSize = 20;
  other.tau = 0;
  other.rho = 0.7;
  other.randomBasis = false;
  other.nSearch = decltype(other.nSearch)();

  return *this;
}

//! Clean memory, if necessary.
template<typename SortPolicy>
NSModel<SortPolicy>::~NSModel()
{
  boost::apply_visitor(DeleteVisitor(), nSearch);
}

//! Serialize the kNN model.
template<typename SortPolicy>
template<typename Archive>
void NSModel<SortPolicy>::serialize(Archive& ar, const unsigned int version)
{
  ar & BOOST_SERIALIZATION_NVP(treeType);
  // Backward compatibility: older versions of NSModel didn't include these
  // parameters.
  if (version > 0)
  {
    ar & BOOST_SERIALIZATION_NVP(leafSize);
    ar & BOOST_SERIALIZATION_NVP(tau);
    ar & BOOST_SERIALIZATION_NVP(rho);
  }
  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(), nSearch);

  ar & BOOST_SERIALIZATION_NVP(nSearch);
}

//! Expose the dataset.
template<typename SortPolicy>
const arma::mat& NSModel<SortPolicy>::Dataset() const
{
  return boost::apply_visitor(ReferenceSetVisitor(), nSearch);
}

//! Access the search mode.
template<typename SortPolicy>
NeighborSearchMode NSModel<SortPolicy>::SearchMode() const
{
  return boost::apply_visitor(SearchModeVisitor(), nSearch);
}

//! Modify the search mode.
template<typename SortPolicy>
NeighborSearchMode& NSModel<SortPolicy>::SearchMode()
{
  return boost::apply_visitor(SearchModeVisitor(), nSearch);
}

template<typename SortPolicy>
double NSModel<SortPolicy>::Epsilon() const
{
  return boost::apply_visitor(EpsilonVisitor(), nSearch);
}

template<typename SortPolicy>
double& NSModel<SortPolicy>::Epsilon()
{
  return boost::apply_visitor(EpsilonVisitor(), nSearch);
}

//! Build the reference tree.
template<typename SortPolicy>
void NSModel<SortPolicy>::BuildModel(arma::mat&& referenceSet,
                                     const size_t leafSize,
                                     const NeighborSearchMode searchMode,
                                     const double epsilon)
{
  this->leafSize = leafSize;
  // Initialize random basis if necessary.
  if (randomBasis)
  {
    Log::Info << "Creating random basis..." << std::endl;
    while (true)
    {
      // [Q, R] = qr(randn(d, d));
      // Q = Q * diag(sign(diag(R)));
      arma::mat r;
      if (arma::qr(q, r, arma::randn<arma::mat>(referenceSet.n_rows,
              referenceSet.n_rows)))
      {
        arma::vec rDiag(r.n_rows);
        for (size_t i = 0; i < rDiag.n_elem; ++i)
        {
          if (r(i, i) < 0)
            rDiag(i) = -1;
          else if (r(i, i) > 0)
            rDiag(i) = 1;
          else
            rDiag(i) = 0;
        }

        q *= arma::diagmat(rDiag);

        // Check if the determinant is positive.
        if (arma::det(q) >= 0)
          break;
      }
    }
  }

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

  // Do we need to modify the reference set?
  if (randomBasis)
    referenceSet = q * referenceSet;

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

  switch (treeType)
  {
    case KD_TREE:
      nSearch = new NSType<SortPolicy, tree::KDTree>(searchMode, epsilon);
      break;
    case COVER_TREE:
      nSearch = new NSType<SortPolicy, tree::StandardCoverTree>(searchMode,
          epsilon);
      break;
    case R_TREE:
      nSearch = new NSType<SortPolicy, tree::RTree>(searchMode, epsilon);
      break;
    case R_STAR_TREE:
      nSearch = new NSType<SortPolicy, tree::RStarTree>(searchMode, epsilon);
      break;
    case BALL_TREE:
      nSearch = new NSType<SortPolicy, tree::BallTree>(searchMode, epsilon);
      break;
    case X_TREE:
      nSearch = new NSType<SortPolicy, tree::XTree>(searchMode, epsilon);
      break;
    case HILBERT_R_TREE:
      nSearch = new NSType<SortPolicy, tree::HilbertRTree>(searchMode, epsilon);
      break;
    case R_PLUS_TREE:
      nSearch = new NSType<SortPolicy, tree::RPlusTree>(searchMode, epsilon);
      break;
    case R_PLUS_PLUS_TREE:
      nSearch = new NSType<SortPolicy, tree::RPlusPlusTree>(searchMode,
          epsilon);
      break;
    case VP_TREE:
      nSearch = new NSType<SortPolicy, tree::VPTree>(searchMode, epsilon);
      break;
    case RP_TREE:
      nSearch = new NSType<SortPolicy, tree::RPTree>(searchMode, epsilon);
      break;
    case MAX_RP_TREE:
      nSearch = new NSType<SortPolicy, tree::MaxRPTree>(searchMode, epsilon);
      break;
    case SPILL_TREE:
      nSearch = new SpillKNN(searchMode, epsilon);
      break;
    case UB_TREE:
      nSearch = new NSType<SortPolicy, tree::UBTree>(searchMode, epsilon);
      break;
    case OCTREE:
      nSearch = new NSType<SortPolicy, tree::Octree>(searchMode, epsilon);
      break;
  }

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

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

//! Perform neighbor search.  The query set will be reordered.
template<typename SortPolicy>
void NSModel<SortPolicy>::Search(arma::mat&& querySet,
                                 const size_t k,
                                 arma::Mat<size_t>& neighbors,
                                 arma::mat& distances)
{
  // We may need to map the query set randomly.
  if (randomBasis)
    querySet = q * querySet;

  Log::Info << "Searching for " << k << " neighbors with ";

  switch (SearchMode())
  {
    case NAIVE_MODE:
      Log::Info << "brute-force (naive) search..." << std::endl;
      break;
    case SINGLE_TREE_MODE:
      Log::Info << "single-tree " << TreeName() << " search..." << std::endl;
      break;
    case DUAL_TREE_MODE:
      Log::Info << "dual-tree " << TreeName() << " search..." << std::endl;
      break;
    case GREEDY_SINGLE_TREE_MODE:
      Log::Info << "greedy single-tree " << TreeName() << " search..."
          << std::endl;
      break;
  }

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

//! Perform neighbor search.
template<typename SortPolicy>
void NSModel<SortPolicy>::Search(const size_t k,
                                 arma::Mat<size_t>& neighbors,
                                 arma::mat& distances)
{
  Log::Info << "Searching for " << k << " neighbors with ";

  switch (SearchMode())
  {
    case NAIVE_MODE:
      Log::Info << "brute-force (naive) search..." << std::endl;
      break;
    case SINGLE_TREE_MODE:
      Log::Info << "single-tree " << TreeName() << " search..." << std::endl;
      break;
    case DUAL_TREE_MODE:
      Log::Info << "dual-tree " << TreeName() << " search..." << std::endl;
      break;
    case GREEDY_SINGLE_TREE_MODE:
      Log::Info << "greedy single-tree " << TreeName() << " search..."
          << std::endl;
      break;
  }

  if (Epsilon() != 0 && SearchMode() != NAIVE_MODE)
    Log::Info << "Maximum of " << Epsilon() * 100 << "% relative error."
        << std::endl;

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

//! Get the name of the tree type.
template<typename SortPolicy>
std::string NSModel<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 BALL_TREE:
      return "ball 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 SPILL_TREE:
      return "Spill tree";
    case VP_TREE:
      return "vantage point tree";
    case RP_TREE:
      return "random projection tree (mean split)";
    case MAX_RP_TREE:
      return "random projection tree (max split)";
    case UB_TREE:
      return "UB tree";
    case OCTREE:
      return "octree";
    default:
      return "unknown tree";
  }
}

} // namespace neighbor
} // namespace mlpack

#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_IEEE_FUNCTIONS_SIMD_COMMON_PREDECESSOR_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SIMD_COMMON_PREDECESSOR_HPP_INCLUDED
#include <boost/simd/toolbox/ieee/functions/predecessor.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/constants/minf.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/valmin.hpp>
#include <boost/simd/include/functions/simd/seladd.hpp>
#include <boost/simd/include/functions/simd/is_not_equal.hpp>
#include <boost/simd/include/functions/simd/is_equal.hpp>
#include <boost/simd/include/functions/simd/prev.hpp>
#include <boost/simd/include/functions/simd/abs.hpp>
#include <boost/simd/include/functions/simd/bitinteger.hpp>
#include <boost/simd/include/functions/simd/bitfloating.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::predecessor_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<arithmetic_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return seladd(neq(a0, Valmin<A0>()), a0, Mone<A0>());
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::predecessor_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<floating_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
        return prev(a0);
      }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::predecessor_, tag::cpu_
                            , (A0)(A1)(X)
                            , ((simd_<integer_<A0>,X>))((simd_<integer_<A1>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(2)
    {
      return selsub( le(Valmin<A0>()+boost::simd::abs(a1), a0), a0, boost::simd::abs(a1));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::predecessor_, tag::cpu_
                            , (A0)(A1)(X)
                            , ((simd_<floating_<A0>,X>))((simd_<integer_<A1>,X>))
                            )
  {

    typedef A0 result_type;

    BOOST_SIMD_FUNCTOR_CALL(2)
    {
      return select(eq(a0, boost::simd::Minf<A0>()), a0,  bitfloating(bitinteger(a0)-boost::simd::abs(a1)));
    }
  };
} } }

#endif


#include "ros/ros.h"
#include "hlpr_trac_ik/IKHandler.h"
#include <trac_ik/trac_ik.hpp>
#include <geometry_msgs/Pose.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
#include <string>
#include <Eigen/Core>
#include <urdf/model.h>

TRAC_IK::TRAC_IK *solver;
KDL::Chain chain;
KDL::ChainFkSolverPos_recursive *fk_solver;
urdf::Model *robot_model;

bool getPose(hlpr_trac_ik::IKHandler::Request  &req,
         hlpr_trac_ik::IKHandler::Response &res)
{

  // Flag if we failed
  bool succeed = true;

  // Pull out of we want to be verbose
  bool verbose = req.verbose;

  // Get the number of poses requested
  int poseLen = req.origin.size();

  //convert seed pose to KDL::JntArray format
  KDL::JntArray prev(poseLen);
  std::string seedStr = "";
  for (int i = 0; i < poseLen; i++) {
    seedStr += std::to_string(req.origin[i]) + " ";
    prev(i) = req.origin[i];
  }

  int len = req.goals.size();
  std::vector<geometry_msgs::Pose> goals = req.goals;
  for(int p = 0; p < len; p++) {
    //convert goal to KDL::Frame format
    std::string goalStr = "";
    KDL::Vector goalPos(goals[p].position.x, goals[p].position.y, goals[p].position.z);
    KDL::Rotation goalRot = KDL::Rotation::Quaternion(goals[p].orientation.x, goals[p].orientation.y, goals[p].orientation.z, goals[p].orientation.w);
    KDL::Frame goalPose(goalRot,goalPos);
    goalStr += std::to_string(goals[p].position.x) + " " + std::to_string(goals[p].position.y) + " " + std::to_string(goals[p].position.z)
        +  " " + std::to_string(goals[p].orientation.x) + " " + std::to_string(goals[p].orientation.y)
        + " " + std::to_string(goals[p].orientation.z) + " " + std::to_string(goals[p].orientation.w);

    if (verbose){
      ROS_INFO("request: seed=%s goal=%s", seedStr.c_str(), goalStr.c_str());
    }

    //set tolerances and send IK request
    KDL::Vector posTol(req.tolerance[0],req.tolerance[1],req.tolerance[2]);
    float max = std::numeric_limits<float>::max();
    KDL::Vector rotTol(req.tolerance[3],req.tolerance[4],req.tolerance[5]);
    KDL::Twist tol(posTol, rotTol);

    int status = solver->CartToJnt(prev, goalPose, prev);
    //int status = solver->CartToJnt(prev, goalPose, prev, tol);
    KDL::Frame fkPose;
    fk_solver->JntToCart(prev,fkPose);

    if(status < 0){
      ROS_WARN("HLP-R IK failed");
      succeed = false;
    }

    //add IK result to service response
    std::string poseStr = "";
    trajectory_msgs::JointTrajectoryPoint pt;
    for(int i = 0; i < poseLen; i++) {
      double val = prev.data[i];
      pt.positions.push_back(val);
      poseStr += std::to_string(val) += " ";
    }
    res.poses.push_back(pt);
    res.success = succeed;
    ROS_INFO("sending back response: %s", poseStr.c_str());
  }

  // Add joint names to response message
  std::vector<KDL::Segment> chain_segs = chain.segments;
  boost::shared_ptr<const urdf::Joint> joint;
  for(unsigned int i = 0; i < chain_segs.size(); ++i) {
    joint = robot_model->getJoint(chain_segs[i].getJoint().getName());
    if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
      res.joint_names.push_back(chain_segs[i].getJoint().getName());
  }

  return true;
}

int main(int argc, char **argv)
{
  // Initialize the ik service
  ros::init(argc, argv, "hlpr_trac_ik_server");
  ros::NodeHandle n;
  ros::NodeHandle nh("~"); // Private handler for parameters

  // Store parameters from the param server
  std::string _base_chain;
  std::string _end_chain;
  double _timeout_in_secs;
  double _error;
  bool _dist_flag;

  // Get parameters for the IK solver
  nh.param("base_chain", _base_chain, std::string("linear_actuator_link"));
  nh.param("end_chain", _end_chain, std::string("right_ee_link"));
  nh.param("timeout_in_sec", _timeout_in_secs, 0.005);
  nh.param("error", _error, 1e-5);
  nh.param("distance_flag", _dist_flag, false);

  // Create track IK solver

  //solver = new TRAC_IK::TRAC_IK("linear_actuator_link", "j2s7s300_ee_link", "/robot_description", 0.005, 1e-5, TRAC_IK::Distance);

  if (_dist_flag){
    solver = new TRAC_IK::TRAC_IK(_base_chain, _end_chain, "/robot_description", _timeout_in_secs, _error, TRAC_IK::Distance);
  } else {
    solver = new TRAC_IK::TRAC_IK(_base_chain, _end_chain, "/robot_description", _timeout_in_secs, _error, TRAC_IK::Speed);
  }

  bool valid;
  valid = solver->getKDLChain(chain);
  if (!valid) {
    ROS_ERROR("There was no valid KDL chain found");
    return false;
  }

  fk_solver = new KDL::ChainFkSolverPos_recursive(chain);

  robot_model = new urdf::Model();
  std::string xml_string;

  std::string urdf_xml,full_urdf_xml;
  std::string urdf_param("/robot_description");
  nh.param("urdf_xml",urdf_xml,urdf_param);
  nh.searchParam(urdf_xml,full_urdf_xml);
  ROS_DEBUG_NAMED("trac_ik","Reading xml file from parameter server");
  if (!nh.getParam(full_urdf_xml, xml_string))
    {
      ROS_FATAL_NAMED("trac_ik","Could not load the xml from parameter server: %s", urdf_xml.c_str());
      return false;
    }

  nh.param(full_urdf_xml,xml_string,std::string());
  robot_model->initString(xml_string);

  // Create the service call
  ros::ServiceServer service = n.advertiseService("hlpr_trac_ik", getPose);
  ROS_INFO("IK wrapper ready.");
  ros::spin();
  delete solver;
  return 0;
}


#ifndef STAN_MATH_TORSTEN_CPT_HPP
#define STAN_MATH_TORSTEN_CPT_HPP

#include <Eigen/Dense>
#include <boost/math/tools/promotion.hpp>
#include <stan/math/torsten/to_array_2d.hpp>
#include <stan/math/torsten/pmx_solve_ode.hpp>
#include <stan/math/prim/err/check_positive_finite.hpp>
#include <stan/math/prim/err/invalid_argument.hpp>
#include <stan/math/torsten/ev_solver.hpp>
#include <stan/math/torsten/ev_manager.hpp>
#include <stan/math/torsten/pmx_check.hpp>
#include <stan/math/torsten/pmx_onecpt_model.hpp>
#include <stan/math/torsten/pmx_twocpt_model.hpp>
#include <string>
#include <vector>

namespace torsten {

  template<template<class> class model_t>
  struct PMXCheckCPTModelParam {
    template<typename T>
    void operator()(const std::vector<std::vector<T> >& pMatrix) {}
  };

  /**
   * check validity of one-cpt model param 2d array
   *
   */
  template<>
  struct PMXCheckCPTModelParam<PMXOneCptModel> {
    template<typename T>
    void operator()(const std::vector<std::vector<T> >& pMatrix) {
      const char* fun = "PMXOneCptModel";
      for (size_t i = 0; i < pMatrix.size(); ++i) {
        stan::math::check_positive_finite(fun, "CL", pMatrix[i][0]);
        stan::math::check_positive_finite(fun, "V2", pMatrix[i][1]);
        stan::math::check_nonnegative(fun, "ka", pMatrix[i][2]);
        stan::math::check_finite(fun, "ka", pMatrix[i][2]);
      }
    }
  };

  /**
   * check validity of two-cpt model param 2d array
   *
   */
  template<>
  struct PMXCheckCPTModelParam<PMXTwoCptModel> {
    template<typename T>
    void operator()(const std::vector<std::vector<T> >& pMatrix) {
      const char* fun = "PMXTwoCptModel";
      for (size_t i = 0; i < pMatrix.size(); ++i) {
        stan::math::check_positive_finite(fun, "CL", pMatrix[i][0]);
        stan::math::check_positive_finite(fun, "Q", pMatrix[i][1]);
        stan::math::check_positive_finite(fun, "V2", pMatrix[i][2]);
        stan::math::check_positive_finite(fun, "V3", pMatrix[i][3]);
        stan::math::check_nonnegative(fun, "ka", pMatrix[i][4]);
        stan::math::check_finite(fun, "ka", pMatrix[i][4]);
      }
    }
  };

  template<template<class> class model_t>
  struct PMXSolveCPT {
    /**
     * Computes the predicted amounts in each compartment at each event
     * for a compartment model with analytical solution, currently
     * supporting 1 & 2 cpt models.
     *
     * @tparam T0 type of scalar for time of events.
     * @tparam T1 type of scalar for amount at each event.
     * @tparam T2 type of scalar for rate at each event.
     * @tparam T3 type of scalar for inter-dose inteveral at each event.
     * @tparam T4 type of scalars for the model parameters.
     * @tparam T5 type of scalar for bio-variability F.
     * @tparam T6 type of scalar for lag times.
     * @param[in] pMatrix parameters at each event
     * @param[in] time times of events
     * @param[in] amt amount at each event
     * @param[in] rate rate at each event
     * @param[in] ii inter-dose interval at each event
     * @param[in] evid event identity:
     *                    (0) observation
     *                    (1) dosing
     *                    (2) other
     *                    (3) reset
     *                    (4) reset AND dosing
     * @param[in] cmt compartment number at each event
     * @param[in] addl additional dosing at each event
     * @param[in] ss steady state approximation at each event (0: no, 1: yes)
     * @return a matrix with predicted amount in each compartment
     *         at each event.
     */
    template <typename T0, typename T1, typename T2, typename T3, typename T4,
              typename... Ts>
    stan::matrix_return_t<T0, T1, T2, T3, T4, Ts...>
    static solve(TORSTEN_PMX_FUNC_EVENTS_ARGS,
                 const std::vector<std::vector<T4> >& pMatrix,
                 const std::vector<std::vector<Ts> >&... event_array_2d_params) {
      using std::vector;
      using Eigen::Dynamic;
      using Eigen::Matrix;
      using boost::math::tools::promote_args;
      using stan::math::check_positive_finite;

      static const char* function("pmx_solve_cpt");
      torsten::pmx_check(time, amt, rate, ii, evid, cmt, addl, ss, pMatrix, function);
      torsten::pmx_check(time.size(), model_t<double>::Ncmt, event_array_2d_params...);
      PMXCheckCPTModelParam<model_t> param_check;
      param_check(pMatrix);

      using ER = NONMENEventsRecord<T0, T1, T2, T3>;
      using EM = EventsManager<ER, NonEventParameters<T0, T4, std::vector, std::tuple<Ts...> >>;
      const ER events_rec(model_t<T4>::Ncmt, time, amt, rate, ii, evid, cmt, addl, ss);

      Matrix<typename EM::T_scalar, -1, -1> pred(EM::nCmt(events_rec), events_rec.num_event_times());

      using model_type = model_t<typename EM::T_par>;
      EventSolver<model_type, EM> pr;
      pr.pred(0, events_rec, pred, dsolve::PMXAnalyiticalIntegrator(), pMatrix, event_array_2d_params...);
      return pred;
    }

    /**
     * Overload function to allow user to pass an std::vector for
     * pMatrix/bioavailability/tlag. This indicates the
     * entire population share these parameters.
     */
    template <typename T0, typename T1, typename T2, typename T3,
              typename T_par, typename T_biovar, typename T_tlag,
              typename = require_any_not_std_vector_t<T_par, T_biovar, T_tlag> >
    stan::matrix_return_t<T0, T1, T2, T3, T_par, T_biovar, T_tlag>
    static solve(TORSTEN_PMX_FUNC_EVENTS_ARGS,
                 const std::vector<T_par>& pMatrix,
                 const std::vector<T_biovar>& biovar,
                 const std::vector<T_tlag>& tlag) {
      return solve(time, amt, rate, ii, evid, cmt, addl, ss,
                   torsten::to_array_2d(pMatrix),
                   torsten::to_array_2d(biovar),
                   torsten::to_array_2d(tlag));
    }

    /**
     * Overload function to allow user to pass an std::vector for
     * pMatrix/bioavailability. This indicates the
     * entire population share these parameters.
     */
    template <typename T0, typename T1, typename T2, typename T3,
              typename T_par, typename T_biovar,
              typename = require_any_not_std_vector_t<T_par, T_biovar> >
    stan::matrix_return_t<T0, T1, T2, T3, T_par, T_biovar>
    static solve(TORSTEN_PMX_FUNC_EVENTS_ARGS,
                 const std::vector<T_par>& pMatrix,
                 const std::vector<T_biovar>& biovar) {
      return solve(time, amt, rate, ii, evid, cmt, addl, ss, torsten::to_array_2d(pMatrix), torsten::to_array_2d(biovar));
    }

    /**
     * Overload function to allow user to pass an std::vector for
     * pMatrix. This indicates the
     * entire population share these parameters.
     */
    template <typename T0, typename T1, typename T2, typename T3,
              typename T_par,
              typename = require_any_not_std_vector_t<T_par> >
    stan::matrix_return_t<T0, T1, T2, T3, T_par>
    static solve(TORSTEN_PMX_FUNC_EVENTS_ARGS,
                 const std::vector<T_par>& pMatrix) {
      return solve(time, amt, rate, ii, evid, cmt, addl, ss, torsten::to_array_2d(pMatrix));
    }

  };
}

#endif


#include <stdio.h>
#include <fstream>
#include <cmath>
#include <malloc.h>
#include <armadillo>
#include <stdlib.h>
#include <functions.h>
#include <stdbool.h>
#include <string.h>
char *outFile;

char* combine(char *a,char *b)
{
  char *c=(char*)malloc(strlen(a) + strlen(b) + 1);
  strcpy(c,a);
  strcat(c,b);
  return c;
}

#define FOR(i,n) for(i=0;i<n;i++)
#define error(i) printf(i);
using namespace arma;
using namespace std;
typedef double DD;
typedef long LL;

typedef struct{
	LL rows;
	LL columns;
	DD** matrix;
} MAT;

typedef struct{
    DD delta;
    LL nofIter;
    DD C;
    DD Cinit;
    DD Cfinal;
    LL nofNn;
    DD sigma;
    DD opt_tb;
    LL maxiter;
    DD tolfun;
    DD s;
    DD* splits;
} Opt;

typedef struct{
	DD tolX;
	LL length;
	DD tolFun;

} Opt2;

typedef struct{
    DD pathNorm;
    LL nofNn;
    DD annEps;
    LL rbf;
    DD sigma;
} Param;

#include "CalcNnDists.c"
#include "GraphDistKernelC.c"
#include "MDS.c"

typedef struct{
	DD obj;
	DD b;
	MAT Yu;
	MAT w;
} Output_Primal;

DD** Multiply_Matrices(DD **Mat1, DD **Mat2, LL M, LL Q, LL P)
{
	LL i, c, d, k;
	DD Sum = 0;
	DD** Temp = (DD**)calloc(M, sizeof(DD*));
	FOR(i, M)
		Temp[i] = (DD*)calloc(Q, sizeof(DD));
	FOR(c, M)
	{
		FOR(d, Q)
		{
			FOR(k, P)
			{
				Sum += (Mat1[c][k] * Mat2[k][d]);
			}
			Temp[c][d] = Sum;
			Sum = 0;
		}
	}
	return Temp;
}

DD** Transpose(DD **Mat1, LL Rows, LL Columns)
{
	LL i, j;
	DD** Temp = (DD**)calloc(Columns, sizeof(DD*));
	for (i = 0; i<Columns; i++)
	{
		Temp[i] = (DD*)calloc(Rows, sizeof(DD));
	}
	for (i = 0; i<Rows; i++)
	{
		for (j = 0; j<Columns; j++)
		{
			Temp[j][i] = Mat1[i][j];
		}
	}
	return Temp;
}
#include "minimize.c"
Output_Primal primal_tsvm(MAT X, MAT Y, MAT w0, Opt opt);
double ** train_one_split(MAT Xnldr, double *Yl, long Yl_rows, long m0, Opt opt, long classes);

DD** LDS(DD **Xl, LL d0, LL m0, DD **Xu, LL d1, LL m1, DD *Yl, LL Yl_rows, LL classes, DD rho, Opt opt)
{
	/*
       --> Run the Low Density Separation algorithm as described in
       --> "Semi-supervised classification by Low Density Separation" by O. Chapelle and A. Zien
	   --> Xl:  d0 x m0 matrix with the labeled points
	   --> Xu:  d1 x m1 matrix with the unlabeled points
	   --> Yl:  column vector of length n containing the labels (+1 or -1 for binary)
	   --> rho: constant
	   --> opt: optional structure containing the (optional) fields,
	   -->	   C:       the soft margin parameter relative to 1/var^2
	   -->                  [default = 1]
	   -->     nofNn:   number of NN in the graph construction
	   -->               [default = 0, i.e. fully connected graph]
	   -->     sigma:   the width of the RBF kernel
	   -->     delta:   threshold
	   -->     nofIter: number of iterations for C* to reach C [default = 10]
	   -->     Cinit:   initial value for C* relative to C [default = 0.01]
	   -->     Cfinal:  final value of C* relative to C [default = 1]
	   -->     maxiter: maximum number of iterations in each gradient
	   -->              descent (multiplied by nb of variables) [default = 3]
	   -->     tolfun:  stopping criterion on the function value
	   -->              (relative to C) [default = 1e-5]
   */
	LL i, j, k, K;
	Param param;
	MAT D2, NN;
	opt.delta = 0.1;
	opt.nofIter = 10;
	opt.C = 0.390;
	opt.Cinit = 0.01;
	opt.Cfinal = 1;
	opt.nofNn = 100;
	opt.sigma = INFINITY;
	opt.opt_tb = 1;
	opt.maxiter = 3;
	opt.tolfun = exp(-5);
	if((1>opt.nofIter)|(opt.C<0)|((opt.Cinit<0)&&(opt.Cinit>opt.Cfinal))|(opt.sigma<0))error("Assertion error");
	LL m = m0 + m1;
	if((opt.nofNn<0)|(opt.nofNn>m))error("Assertion error");
	DD** mat = (DD**)calloc(m, sizeof(DD*));
	FOR(i, m)mat[i] = (DD*)calloc(d0, sizeof(DD));
	MAT X;
	X.matrix = mat; X.rows = m; X.columns = d0;
	FOR(i, X.rows)
	{
		FOR(j, X.columns)
		{
			if (i < m0)
			{
				X.matrix[i][j] = Xl[j][i];
			}
			else{
				X.matrix[i][j] = Xu[j][i - m0];
			}
		}
	}
	K = classes;
	//===================================================
	//    ****** Compute the full distance graph ******
	//===================================================

	output t = CalcNnDists(X.matrix, X.rows, X.columns, opt.nofNn);
	D2 = t.D2; NN = t.NN;
	param.pathNorm = rho;
	param.sigma = INFINITY;
	param.nofNn = opt.nofNn;
	DD* idx = (DD*)calloc(m, sizeof(DD));
	FOR(i, m)
	{
		idx[i] = i + 1;
	}
	MAT E2 = graphDistKernelC(X, D2, NN, param, idx, m);
    FOR(i,X.rows)free(X.matrix[i]);free(X.matrix);
    FOR(i,opt.nofNn){free(D2.matrix[i]);free(NN.matrix[i]);}free(D2.matrix);free(NN.matrix);
    D2.rows=0;D2.columns=0;NN.rows=0;NN.columns=0;
	if (opt.nofNn != 0)
	{
		DD** new_E2 = (DD**)calloc(E2.rows, sizeof(DD*));
		DD max1 = E2.matrix[0][0];
		FOR(i, E2.rows)
		{
			new_E2[i] = (DD*)calloc(E2.columns, sizeof(DD));
			FOR(j, E2.columns)
			{
				new_E2[i][j] = min(E2.matrix[i][j], E2.matrix[j][i]);
				if ((new_E2[i][j] > max1) && (!isinf(new_E2[i][j])))
				{
					max1 = new_E2[i][j];
				}
			}
		}
		FOR(i, E2.rows)free(E2.matrix[i]);free(E2.matrix);
		E2.matrix = new_E2;
		FOR(i, E2.rows)
		{
			FOR(j, E2.columns)
			{
				if (isinf(E2.matrix[i][j]))E2.matrix[i][j] = 2 * max1;
			}
		}
	}
	DD* E2_array = (DD*)calloc(E2.rows*E2.columns, sizeof(DD));
	bool isinf_E2 = false;
	FOR(i, E2.columns)
	{
		FOR(j, E2.rows)
		{
			if (isinf(E2.matrix[j][i]))isinf_E2 = true;
			E2_array[i*E2.rows + j] = E2.matrix[j][i];
		}
	}
	//===========================================================================
	//        ********* Compute the new kernel, do the MDS reduction **********
	//===========================================================================

	double defaultSigma = calcDefaultSigma(E2_array, 2, E2.columns*E2.rows);
	double sigma = opt.sigma * defaultSigma;
	Output_mds mds;
	if (isinf(sigma))
	{
		if (isinf_E2)
		{
			error("Cannot do the MDS: The graph is not connected");
		}
		mds = MDS(E2.matrix, E2.rows, E2.columns, opt.delta);
	}
	else
	{
		param.rbf = 1;// set RBF name equal to Gauss
		//=================================================
		//       ****** Calculate RBF Kernel *******
		//=================================================
		double **K = calcRbfKernel(E2.matrix, E2.rows, E2.columns, param.rbf, sigma, 1);
		FOR(i, E2.rows)
		{
			FOR(j, E2.columns)
			{
				K[i][j] = 2 - 2 * K[i][j];
			}
		}
		mds = MDS(K, E2.rows, E2.columns, opt.delta);
	}
	double **cumsum = 0, sum = 0;
	cumsum = (DD**)calloc(E2.rows, sizeof(DD*));
	FOR(i, E2.rows){
		cumsum[i] = (DD*)calloc(1, sizeof(DD));
		if (i == 0)cumsum[i][0] = mds.eigen[i];
		else cumsum[i][0] = mds.eigen[i] + cumsum[i - 1][0];
	}
	sum = cumsum[E2.rows - 1][0];
	LL nb_comp;
	if (opt.delta >= 0)
	{
		FOR(i, E2.rows)
		{
			if ((1 - cumsum[i][0] < opt.delta)&(mds.eigen[i] < opt.delta*mds.eigen[0]))
			{
				nb_comp = i;
				break;
			}
		}
	}
	else
	{
		nb_comp = E2.rows;
	}
	double **Xnldr = (DD**)calloc(mds.Y.rows, sizeof(DD*));
	double *mean_Xnldr = (DD*)calloc(nb_comp, sizeof(DD));
	FOR(i, mds.Y.rows)
	{
		Xnldr[i] = (DD*)calloc(nb_comp, sizeof(DD));
		// Keep only the first components
		FOR(j, nb_comp){
			Xnldr[i][j] = mds.Y.matrix[i][j];
			mean_Xnldr[j] += mds.Y.matrix[i][j];
			if (i == mds.Y.rows - 1){
				mean_Xnldr[j] = (mean_Xnldr[j])/ mds.Y.rows;
			}
		}
	}
	mds.Y.columns = nb_comp;
	mds.Y.matrix = Xnldr;
	double sum_var = 0;
	FOR(i, nb_comp)
	{
		double var = 0;
		FOR(j, mds.Y.rows)
		{
			var += pow(Xnldr[j][i] - mean_Xnldr[i], 2);
		}
		sum_var += var / (mds.Y.rows - 1);
	}
	// Default value of C = invert of the variance
	double defaultC = 1 / sum_var;
	double C = opt.C*defaultC; opt.Cinit = C*opt.Cinit; opt.Cfinal = C*opt.Cfinal; opt.C = C;
	opt.s = 3;
	//============================================
	//       ******* Train the TSVM ********
	//============================================
	double **Yu = train_one_split(mds.Y, Yl,Yl_rows,m0, opt, classes);
	return Yu;
}
double ** train_one_split(MAT Xnldr, double *Yl,long Yl_rows,long m0, Opt opt,long classes){
	MAT Y0,w0;
	w0.matrix = NULL;
	Y0.matrix = (DD**)calloc(Xnldr.rows, sizeof(DD*)); Y0.rows = Xnldr.rows; Y0.columns = 1;
	long i, j;
	if (classes == 2){
		if (Yl_rows > m0)
		{
			opt.Cinit = opt.Cfinal; opt.nofIter = 1;
		}
		else{
			FOR(i, Y0.rows)
			{
				Y0.matrix[i] = (DD*)calloc(1, sizeof(DD));
				if (i < Yl_rows)Y0.matrix[i][0] = Yl[i];
			}
			Output_Primal pr = primal_tsvm(Xnldr, Y0, w0, opt);
			int count=0;
            FOR(i,pr.Yu.rows)
			{
				double boo=(pr.Yu.matrix[i][0]>0)?1:-1;
                                if(boo==1){
                                 printf("%ld Kareena \n",i);
                                }
                                else{
                                 printf("%ld Amir \n",i);
                                 }
            }
            printf("\n total %d\n\n",count);
               return pr.Yu.matrix;
              }
	}
	else{
		MAT new_Y;
		DD final_obj=0;
		Output_Primal pr;
		new_Y.rows = Xnldr.rows - Yl_rows; new_Y.columns = classes;
		new_Y.matrix = (DD**)calloc(new_Y.rows, sizeof(DD*));
		FOR(i, new_Y.rows)new_Y.matrix[i] = (DD*)calloc(new_Y.columns, sizeof(DD));
		FOR(i, Y0.rows)
		{
			Y0.matrix[i] = (DD*)calloc(1, sizeof(DD));
		}
		FOR(i, classes)
		{
			FOR(j, Yl_rows)
			{
				if (Yl[j] == i+1)
					Y0.matrix[j][0] = 1;
				else Y0.matrix[j][0] = -1;
			}
			pr = primal_tsvm(Xnldr, Y0, w0, opt);
			FOR(j, new_Y.rows)new_Y.matrix[j][i] = pr.Yu.matrix[j][0];
			final_obj += pr.obj;
		}
        int count=0;
        FOR(i,new_Y.rows){
                        LL index=0;
                        FOR(j,new_Y.columns)
                        {
                          if(new_Y.matrix[i][j]>new_Y.matrix[i][index])index=j;
                        }
                        new_Y.matrix[i][0]=index+1;
                }
        FILE *fp;
        char *new_string;
        new_string=combine(outFile,"/../results.txt");
        fp=fopen(new_string,"w");
        FOR(i,new_Y.rows)
		{
			fprintf(fp,"%lf\n",new_Y.matrix[i][0]);
        }
        fclose(fp);
	}
			return NULL;
}
void SVD(double*** return_mat,MAT inp)
{
  // Calculate Singular Value Decomposition using Armadillo library
  long i ,j;
  fmat A=randu<fmat>(inp.columns,inp.rows);
  double **my_mat=(double**)calloc(inp.columns,sizeof(double*));
  for(i=0;i<inp.columns;i++)
  {
   my_mat[i]=(double*)calloc(inp.columns,sizeof(double));
   for(j=0;j<inp.rows;j++)A(i,j)=inp.matrix[j][i];
  }
  fmat U;
  fvec S;
  fmat V;
  svd(U, S, V, A);
  for(i=0;i<inp.columns;i++){
    for(j=0;j<inp.columns;j++)
    {
     my_mat[i][j]=U(i,j);
     }
  }
  *return_mat=my_mat;
}
Output_Primal primal_tsvm(MAT X, MAT Y, MAT w0, Opt opt)
{
	//======================================================
	//   **** Solve the TSVM problem in the primal****
	//======================================================

	MAT new_X;
	new_X.matrix = (double**)calloc(X.rows, sizeof(double*));
	new_X.rows = X.rows; new_X.columns = X.columns + 1;
	double  C, C2, exponent, cbal = 0, sum_vbal = 0, tolfun2;
	MAT Xu, R, vbal;
	vbal.matrix = (double**)calloc(1, sizeof(double*)); vbal.rows = 1; vbal.columns = X.rows;
	vbal.matrix[0] = (double*)calloc(vbal.columns, sizeof(double));
	long i, j, zeros = 0, n, maxiter2;
	for (i = 0; i < new_X.rows; i++)
	{
		vbal.matrix[0][i] = 1;
		new_X.matrix[i] = (double*)calloc(new_X.columns, sizeof(double));
		FOR(j, new_X.columns - 1)new_X.matrix[i][j] = X.matrix[i][j];
		new_X.matrix[i][new_X.columns - 1] = 1;
	}
	for (i = 0; i < Y.rows; i++){ if (Y.matrix[i][0] == 0)zeros++; }
	Xu.matrix = (double**)calloc(zeros, sizeof(double*));
	Xu.rows = zeros;
	Xu.columns = new_X.columns;
	for (i = 0; i < Xu.rows; i++)
	{
		Xu.matrix[i] = (double*)calloc(Xu.columns, sizeof(double));
	}
	zeros = 0;
	for (i = 0; i < Y.rows; i++)
	{
		if (Y.matrix[i][0] == 0)
		{
			for (j = 0; j < Xu.columns; j++)
			{
				Xu.matrix[zeros][j] = new_X.matrix[i][j];
			}
			zeros++;
		}
	}
	Xu.matrix = Transpose(Xu.matrix, Xu.rows, Xu.columns); i = Xu.rows; Xu.rows = Xu.columns; Xu.columns = i;
	exponent = pow(opt.Cfinal / opt.Cinit, 1 / (opt.nofIter + 1));
	C = opt.C;
	C2 = opt.Cinit;
	n = new_X.columns;
	if (zeros > 0)
	{
		for (i = 0; i < Y.rows; i++)
		{
			if (Y.matrix[i][0] != 0)
			{
				vbal.matrix[0][i] = 0;
			}
		}
	}
	vbal.matrix = Multiply_Matrices(vbal.matrix, new_X.matrix, vbal.rows, new_X.columns, vbal.columns); vbal.columns = new_X.columns;
	j = 0;
	for (i = 0; i < Y.rows; i++)
	{
		if (Y.matrix[i][0] != 0)
		{
			j += 1;
			cbal += Y.matrix[i][0];
		}
	}
	for (i = 0; i < vbal.columns; i++)sum_vbal += vbal.matrix[0][i];
	cbal = (cbal / j)*sum_vbal;
    R.rows = vbal.columns; R.columns = vbal.columns;
	SVD(&R.matrix,vbal);
	DD** newR = (double**)calloc(R.rows, sizeof(double*));
	for (i = 0; i < R.rows; i++)
	{
		newR[i] = (double*)calloc(1, sizeof(double));
		newR[i][0] = R.matrix[i][0];
	}
	MAT w;
	w.matrix = Multiply_Matrices(vbal.matrix, newR, 1, 1, R.rows);
	DD w1 = cbal / w.matrix[0][0];
	if (w0.matrix == NULL)
	{
		w.matrix = (double**)calloc(n - 1, sizeof(double*));
		for (i = 0; i < n - 1; i++)
		{
			w.matrix[i] = (double*)calloc(1, sizeof(double));
			for (j = 0; j < 1; j++)w.matrix[i][j] = 0;
		}
		w.rows = n - 1; w.columns = 1;
	}
	else
	{
		DD** new_newR = (double**)calloc(R.rows, sizeof(double*));
		for (i = 0; i < R.rows; i++)
		{
			new_newR[i] = (double*)calloc(R.columns - 1, sizeof(double));
			for (j = 0; j < R.columns - 1; j++)
			{
				new_newR[i][j] = R.matrix[i][j + 1];
			}
		}
		w.matrix = Multiply_Matrices(Transpose(new_newR, R.rows, R.columns - 1), w0.matrix, R.columns - 1, w0.rows, R.rows);
		w.rows = R.columns - 1; w.columns = w0.rows;
	}
	maxiter2 = opt.maxiter*new_X.rows;
	tolfun2 = opt.tolfun*C;
	Opt2 opt2;
	opt2.length = maxiter2;
	opt2.tolX = 0;
	opt2.tolFun = tolfun2;
	DD* y_temp = (DD*)calloc(Y.rows, sizeof(DD));
	FOR(i, Y.rows)
	{
		y_temp[i] = Y.matrix[i][0];
	}
	bool FLAG;
	Output_Obj  oo;
	FOR(i, opt.nofIter)
	{
		C2 = C2*exponent;
		Output_mini tt = minimize(w, w1, y_temp, Y.rows, C, C2, opt.s, new_X, Xu, R, opt2);
		oo = obj_fun(w, w1, y_temp, Y.rows, C, C2, opt.s,new_X,Xu,R);
		FLAG = (tt.FX < opt2.length) ? 1 : 0;
	}
	DD **Temp=(DD**)calloc(w.rows+1,sizeof(DD*));
	FOR(i, w.rows + 1)
	{
		Temp[i] = (DD*)calloc(1, sizeof(DD));
		if (i !=0)Temp[i][0] = w.matrix[i-1][0];
		else Temp[i][0] = w1;
	}
	w.matrix = Multiply_Matrices(R.matrix, Temp, R.rows, 1, w.rows + 1); w.rows = R.rows; w.columns = 1;
	MAT Yu;
	Yu.matrix = Multiply_Matrices(Transpose(Xu.matrix, Xu.rows, Xu.columns), w.matrix, Xu.columns, w.columns, w.rows); Yu.rows = Xu.columns; Yu.columns = w.columns;
	Output_Primal pr;
	pr.obj = oo.obj;
	pr.b = w.matrix[w.rows - 1][0];
	pr.Yu = Yu;
	free(w.matrix[w.rows - 1]);
	w.rows = w.rows - 1;
	pr.w = w;
	return pr;
}

//==============================
//      ******  MAIN *******
//==============================

int main(int argc,char * argv[]){
  long rows=atoi(argv[3]),columns=atoi(argv[2]),columns2=atoi(argv[1])-columns,i,j;
  FILE *fp;
  char *new_string;
  new_string=combine(argv[4],"/../data/final_file_train.txt");
  fp=fopen(new_string,"r");
  DD** k=(DD**)calloc(rows,sizeof(DD*));
  FOR(i,rows)
  {
	k[i]=(DD*)calloc(columns,sizeof(DD));
  }
  FOR(j,columns)
  {
	  FOR(i,rows)
	  {
	   fscanf(fp,"%lf ",&k[i][j]);
	  }
  }
  fclose(fp);
  new_string=combine(argv[4],"/../data/final_file_test.txt");
  fp=fopen(new_string,"r");
  DD** k2=(DD**)calloc(rows,sizeof(DD*));
  FOR(i,rows)
  {
	k2[i]=(DD*)calloc(columns2,sizeof(DD));
  }
  FOR(j,columns2)
  {
	  FOR(i,rows)
	  {
	     fscanf(fp,"%lf ",&k2[i][j]);
	  }
  }
  fclose(fp);
  DD* Yl=(DD*)calloc(columns,sizeof(DD*));
  FOR(i,columns)
  {
	  if(i<columns/2)
	     Yl[i]=1;
	  else
	     Yl[i]=2;
  }
  Opt opt;
  opt.delta = 0.1;
  opt.nofIter = 10;
  opt.C = 2;
  opt.Cinit = 0.01;
  opt.Cfinal = 1;
  opt.nofNn = 50;
  opt.sigma = INFINITY;
  opt.opt_tb = 1;
  opt.maxiter = 3;
  opt.tolfun = exp(-5);
  outFile=argv[4];
  DD** y=LDS(k,rows,columns,k2,rows,columns2,Yl,columns,3,0.5,opt);
  return 0;
}


#include "fieldtrack/VelocitySourceManager.h"
#include "fieldtrack/DimensionParser.h"

#include "argus_utils/utils/ParamUtils.h"
#include "argus_utils/geometry/GeometryUtils.h"
#include "argus_utils/geometry/PoseSE3.h"
#include "argus_utils/geometry/PoseSE2.h"

#include <algorithm>

#include <boost/foreach.hpp>

namespace argus
{

  std::ostream& operator<<( std::ostream& os, const std::vector<std::string>& v )
  {
    BOOST_FOREACH( const std::string& s, v )
      {
	os << s << " ";
      }
    return os;
  }

VelocitySourceManager::VelocitySourceManager() {}

void VelocitySourceManager::Initialize( ros::NodeHandle& ph,
                                        bool twoDimensional,
                                        unsigned int filterOrder,
                                        const std::string& targetFrame,
                                        ExtrinsicsInterface::Ptr extrinsics )
{
	_filterOrder = filterOrder;

	_targetFrame = targetFrame;
	_twoDimensional = twoDimensional;
	_extrinsicsManager = extrinsics;

	std::string mode;
	GetParamRequired( ph, "mode", mode );
	_mode = StringToCovMode( mode );

	std::vector<std::string> dimSpecs;
	GetParamRequired( ph, "observed_dims", dimSpecs );

	// NOTE These are hard-coded limits for twist, IMU messages which will have
	// at most acceleration info
	// TODO Currently am not validating obsInds vs the expected message type
        //parse_dim_string( dimSpecs, twoDimensional, 2, 1 ); // Will validate dimSpecs for us
	_obsInds = parse_dim_string( dimSpecs, false, filterOrder + 1, 1 );
        //_sensorInds = parse_dim_string( sensorSpecs, false, 2, 1 );

	GetParam( ph, "min_log_likelihood", _minLogLikelihood,
	          -std::numeric_limits<double>::infinity() );

	// Construct observation matrix
	unsigned int stateDim = twoDimensional ?
	                        PoseSE2::TangentDimension :
	                        PoseSE3::TangentDimension;
	unsigned int fullDim = PoseSE3::TangentDimension * (filterOrder + 1);
	_obsMatrix = MatrixType::Zero( _obsInds.size(), fullDim );
	for( unsigned int i = 0; i < _obsInds.size(); ++i )
	{
		_obsMatrix( i, _obsInds[i] ) = 1.0;
	}

	_filterToThreeD = promote_3d_matrix( twoDimensional, filterOrder );

	// Parse covariance operating mode
	if( _mode == COV_PASS )
	{
		// Nothing to parse
	}
	else if( _mode == COV_FIXED )
	{
		_fixedCov = MatrixType( _obsInds.size(), _obsInds.size() );
		GetParamRequired( ph, "fixed_covariance", _fixedCov );
	}
	else if( _mode == COV_ADAPTIVE )
	{
		_adaptiveCov.Initialize( _obsInds.size(), ph );
	}
}

const MatrixType& VelocitySourceManager::GetIndexMatrix() const
{
	return _obsMatrix;
}

// CovarianceModel::Ptr VelocitySourceManager::InitializeModel() const
// {
// 	if( _mode == COV_PASS )
// 	{
// 		return std::make_shared<PassCovariance>();
// 	}
// 	else if( _mode == COV_FIXED )
// 	{
// 		FixedCovariance::Ptr cov = std::make_shared<FixedCovariance>();
// 		cov->Initialize( _fixedCov );
// 		// TODO Make settable
// 		cov->EnableL( false );
// 		return cov;
// 	}
// 	else if( _mode == COV_ADAPTIVE )
// 	{
// 		AdaptiveCovariance::Ptr cov = std::make_shared<AdaptiveCovariance>();
// 		// TODO HACK
// 		// TODO Read parameters correctly
// 		// TODO Enable/disable diagonal
// 		// TODO Add temporal weighting and prior
// 		cov->SetWindowSize( 10 );
// 		cov->SetDefaultValue( _adaptiveCov.GetPriorCov() );
// 		return cov;
// 	}
// 	else
// 	{
// 		throw std::runtime_error( "Invalid covariance mode" );
// 	}
// }

// void VelocitySourceManager::SetModel( const CovarianceModel& model )
// {
// 	try
// 	{
// 		if( _mode == COV_PASS )
// 		{
// 			// Nothing to do
// 		}
// 		else if( _mode == COV_FIXED )
// 		{
// 			const FixedCovariance& fcov = dynamic_cast<const FixedCovariance&>( model );
// 			_fixedCov = fcov.GetValue();
// 		}
// 		else if( _mode == COV_ADAPTIVE )
// 		{
// 			// TODO Currently nothing to do
// 		}
// 	}
// 	catch( std::bad_cast& e )
// 	{
// 		throw std::invalid_argument( "Incorrect model type: " + std::string( e.what() ) );
// 	}
// }

void VelocitySourceManager::Update( const UpdateInfo& info )
{
	if( _mode == COV_ADAPTIVE )
	{
		_adaptiveCov.Update( info );
	}
}

void VelocitySourceManager::Reset()
{
	if( _mode == COV_ADAPTIVE )
	{
		_adaptiveCov.Reset();
	}
}

bool VelocitySourceManager::CheckLogLikelihood( double ll ) const
{
	return !std::isnan(ll) && ll > _minLogLikelihood;
}

DerivObservation
VelocitySourceManager::operator()( const geometry_msgs::PoseStamped& msg )
{
	throw std::runtime_error( "VelocitySourceManager does not yet support pose differentiation" );
}

DerivObservation
VelocitySourceManager::operator()( const geometry_msgs::PoseWithCovarianceStamped& msg )
{
	throw std::runtime_error( "VelocitySourceManager does not yet support pose differentiation" );
}

DerivObservation
VelocitySourceManager::operator()( const geometry_msgs::TwistStamped& msg )
{
	VectorType derivs = MsgToTangent( msg.twist );
	MatrixType cov;
	return ProcessDerivatives( derivs, cov, msg.header.stamp,
	                           msg.header.frame_id );
}

DerivObservation
VelocitySourceManager::operator()( const geometry_msgs::TwistWithCovarianceStamped& msg )
{
	VectorType derivs = MsgToTangent( msg.twist.twist );
	MatrixType cov( 6, 6 );
	ParseMatrix( msg.twist.covariance, cov );
	return ProcessDerivatives( derivs, cov, msg.header.stamp,
	                           msg.header.frame_id );
}

DerivObservation
VelocitySourceManager::operator()( const geometry_msgs::TransformStamped& msg )
{
	throw std::runtime_error( "VelocitySourceManager does not yet support pose differentiation" );
}

DerivObservation
VelocitySourceManager::operator()( const sensor_msgs::Imu& msg )
{
	// TODO Hard-coded check on IMU valid inds
	// IMU contains angular velocities (3, 4, 5) and linear accelerations (6, 7, 8)
	BOOST_FOREACH( unsigned int ind, _obsInds )
	{
		if( ind < 3 || ind > 8 )
		{
			throw std::runtime_error("Invalid indices for IMU message");
		}
	}

	FixedVectorType<3> angVel = MsgToVector3( msg.angular_velocity );
	FixedVectorType<3> linAcc = MsgToVector3( msg.linear_acceleration );
	// orientation rotates IMU to world frame
	QuaternionType orientation = MsgToQuaternion( msg.orientation );

	MatrixType gyroCov = MatrixType( 3, 3 );
	MatrixType xlCov = MatrixType( 3, 3 );
	ParseMatrix( msg.angular_velocity_covariance, gyroCov );
	ParseMatrix( msg.linear_acceleration_covariance, xlCov );

	// Perform IMU transformations
	PoseSE3 ext = _extrinsicsManager->GetExtrinsics( msg.header.frame_id, _targetFrame, msg.header.stamp );

	// angular_vel = R * gyro_vel
	MatrixType R = ext.GetQuaternion().toRotationMatrix();
	FixedVectorType<3> bodyAngVel = R * angVel;

	// lin_acc = R * (xl_acc - ori_inv * grav - centripetal)
	FixedVectorType<3> gravity;
	gravity << 0, 0, -9.81;
	Translation3Type rTrans = ext.GetTranslation();
	FixedVectorType<3> r;
	r << rTrans.x(), rTrans.y(), rTrans.z();
	FixedVectorType<3> centri; // w x w x r
	centri = bodyAngVel.cross( bodyAngVel.cross( r ) );

	FixedVectorType<3> bodyLinAcc = R * (linAcc - orientation.inverse().toRotationMatrix() * gravity) - centri;

	// TODO Hard-coded constants
	VectorType derivs = VectorType::Zero(12);
	derivs.segment<3>(3) = bodyAngVel;
	derivs.segment<3>(6) = bodyLinAcc;

	// TODO Transform covariance properly
	MatrixType cov = MatrixType::Zero(12, 12);
	std::vector<unsigned int> gyroInds = {3,4,5};
	std::vector<unsigned int> xlInds = {6,7,8};
	PutSubmatrix( gyroCov, cov, gyroInds, gyroInds );
	PutSubmatrix( xlCov, cov, xlInds, xlInds );

	// return ProcessDerivatives( derivs, cov, msg.header.stamp, msg.header.frame_id );
	unsigned int fullDim = PoseSE3::TangentDimension * ( _filterOrder + 1 );
	MatrixType C = MatrixType::Identity( fullDim, fullDim );

	DerivObservation obs;
	obs.timestamp = msg.header.stamp;
	obs.referenceFrame = _targetFrame;
	obs.derivatives = VectorType( _obsInds.size() );
	GetSubmatrix( derivs, obs.derivatives, _obsInds );
	obs.covariance = GetCovariance( msg.header.stamp, cov );
	// We map from filter -> 3D -> reference frame -> relevant indices
	obs.C = _obsMatrix * C * _filterToThreeD;
	return obs;
}

DerivObservation
VelocitySourceManager::ProcessDerivatives( const VectorType& derivs,
                                           const MatrixType& cov,
                                           const ros::Time& stamp,
                                           const std::string& frame )
{
	PoseSE3 ext = _extrinsicsManager->GetExtrinsics( frame, _targetFrame, stamp );
	// VectorType transDerivs = TransformTangent( derivs, ext );

	unsigned int fullDim = PoseSE3::TangentDimension * ( _filterOrder + 1 );

	// C maps full 3D derivatives to reference full 3D derivatives
	MatrixType C = MatrixType::Zero( fullDim, fullDim );
	// NOTE Use inverse extrinsics adjoint to map from reference frame to sensor frame
	MatrixType adj = PoseSE3::Adjoint( ext.Inverse() );
	for( unsigned int i = 0; i < _filterOrder + 1; ++i )
	{
		unsigned int j = i * PoseSE3::TangentDimension;
		C.block<PoseSE3::TangentDimension, PoseSE3::TangentDimension>( j, j ) = adj;
	}

	DerivObservation obs;
	obs.timestamp = stamp;
	obs.referenceFrame = _targetFrame;
	obs.derivatives = VectorType( _obsInds.size() );
	GetSubmatrix( derivs, obs.derivatives, _obsInds );
	obs.covariance = GetCovariance( stamp, cov );
	// We map from filter -> 3D -> reference frame -> relevant indices
	obs.C = _obsMatrix * C * _filterToThreeD;
	return obs;
}

MatrixType VelocitySourceManager::GetCovariance( const ros::Time& stamp,
                                                 const MatrixType& recv )
{
	if( _mode == COV_PASS )
	{
		if( recv.size() == 0 )
		{
			throw std::runtime_error( "Cannot run in COV_PASS mode with non-covariance message" );
		}
		MatrixType out;
		GetSubmatrix( recv, out, _obsInds, _obsInds );
		return out;
	}
	else if( _mode == COV_FIXED )
	{
		return _fixedCov;
	}
	else if( _mode == COV_ADAPTIVE )
	{
		return _adaptiveCov.GetR( stamp );
	}
	else
	{
		throw std::runtime_error( "Unknown covariance mode!" );
	}
}
}


/***********************************************************************************************************************
*  OpenStudio(R), Copyright (c) 2008-2019, Alliance for Sustainable Energy, LLC, and other contributors. All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
*  following conditions are met:
*
*  (1) Redistributions of source code must retain the above copyright notice, this list of conditions and the following
*  disclaimer.
*
*  (2) Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following
*  disclaimer in the documentation and/or other materials provided with the distribution.
*
*  (3) Neither the name of the copyright holder nor the names of any contributors may be used to endorse or promote products
*  derived from this software without specific prior written permission from the respective party.
*
*  (4) Other than as required in clauses (1) and (2), distributions in any form of modifications or other derivative works
*  may not use the "OpenStudio" trademark, "OS", "os", or any other confusingly similar designation without specific prior
*  written permission from Alliance for Sustainable Energy, LLC.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDER(S) AND ANY CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
*  INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER(S), ANY CONTRIBUTORS, THE UNITED STATES GOVERNMENT, OR THE UNITED
*  STATES DEPARTMENT OF ENERGY, NOR ANY OF THEIR EMPLOYEES, BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
*  EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
*  USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
*  STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
*  ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
***********************************************************************************************************************/

#include "Geometry.hpp"
#include "Intersection.hpp"
#include "Transformation.hpp"
#include "Vector3d.hpp"

#include "../core/Assert.hpp"

#include <boost/math/constants/constants.hpp>

#include <polypartition/polypartition.h>

namespace openstudio{
  /// convert degrees to radians
  double degToRad(double degrees)
  {
    return degrees*boost::math::constants::pi<double>()/180.0;
  }

  /// convert radians to degrees
  double radToDeg(double radians)
  {
    return radians*180.0/boost::math::constants::pi<double>();
  }

  /// compute area from surface as Point3dVector
  boost::optional<double> getArea(const Point3dVector& points)
  {
    boost::optional<double> result;
    OptionalVector3d newall = getNewallVector(points);
    if (newall){
      result = newall->length() / 2.0;
    }
    return result;
  }

  // compute Newall vector from Point3dVector, direction is same as outward normal
  // magnitude is twice the area
  OptionalVector3d getNewallVector(const Point3dVector& points)
  {
    OptionalVector3d result;
    size_t N = points.size();
    if (N >= 3){
      Vector3d vec;
      for (unsigned i = 1; i < N-1; ++i){
        Vector3d v1 = points[i] - points[0];
        Vector3d v2 = points[i+1] - points[0];
        vec += v1.cross(v2);
      }
     result = vec;
   }
   return result;
  }

  // compute outward normal from Point3dVector
  OptionalVector3d getOutwardNormal(const Point3dVector& points)
  {
    OptionalVector3d result = getNewallVector(points);
    if (result){
      if (!result->normalize()){
        result.reset();
      }
    }
    return result;
  }

  /// compute centroid from surface as Point3dVector
  OptionalPoint3d getCentroid(const Point3dVector& points)
  {
    OptionalPoint3d result;

    if (points.size() >= 3){
      // convert to face coordinates
      Transformation alignFace = Transformation::alignFace(points);
      Point3dVector surfacePoints = alignFace.inverse()*points;

      size_t N = surfacePoints.size();
      double A = 0;
      double cx = 0;
      double cy = 0;
      for (size_t i = 0; i < N; ++i){
        double x1, x2, y1, y2;
        if (i == N-1){
          x1 = surfacePoints[i].x();
          x2 = surfacePoints[0].x();
          y1 = surfacePoints[i].y();
          y2 = surfacePoints[0].y();
        }else{
          x1 = surfacePoints[i].x();
          x2 = surfacePoints[i+1].x();
          y1 = surfacePoints[i].y();
          y2 = surfacePoints[i+1].y();
        }

        double dA = (x1*y2-x2*y1);
        A += 0.5*dA;
        cx += (x1+x2)*dA;
        cy += (y1+y2)*dA;
      }

      if (A > 0){
        // centroid in face coordinates
        Point3d surfaceCentroid(cx/(6.0*A), cy/(6.0*A), 0.0);

        // centroid
        result = alignFace*surfaceCentroid;
      }
    }
    return result;
  }

  /// reorder points to upper-left-corner convention
  Point3dVector reorderULC(const Point3dVector& points)
  {
    size_t N = points.size();
    if (N < 3){
      return Point3dVector();
    }

    // transformation to align face
    Transformation t = Transformation::alignFace(points);
    Point3dVector facePoints = t.inverse()*points;

    // find ulc index in face coordinates
    double maxY = std::numeric_limits<double>::min();
    double minX = std::numeric_limits<double>::max();
    unsigned ulcIndex = 0;
    for(unsigned i = 0; i < N; ++i){
      OS_ASSERT(std::abs(facePoints[i].z()) < 0.001);
      if ((maxY < facePoints[i].y()) || ((maxY < facePoints[i].y() + 0.00001) && (minX > facePoints[i].x()))){
        ulcIndex = i;
        maxY = facePoints[i].y();
        minX = facePoints[i].x();
      }
    }

    // no-op
    if (ulcIndex == 0){
      return points;
    }

    // create result
    Point3dVector result;
    std::copy (points.begin() + ulcIndex, points.end(), std::back_inserter(result));
    std::copy (points.begin(), points.begin() + ulcIndex, std::back_inserter(result));
    OS_ASSERT(result.size() == N);
    return result;
  }

  std::vector<Point3d> removeCollinear(const Point3dVector& points, double tol)
  {
    Transformation t = Transformation::alignFace(points);
    std::vector<Point3d> result = t*simplify(t.inverse()*points, true, tol);
    return result;
  }

  std::vector<Point3d> removeCollinearLegacy(const Point3dVector& points, double tol)
  {
    size_t N = points.size();
    if (N < 3){
      return points;
    }

    std::vector<Point3d> result;
    Point3d lastPoint = points[0];
    result.push_back(lastPoint);

    for (unsigned i = 1; i < N; ++i){
      Point3d currentPoint = points[i];
      Point3d nextPoint = points[0];
      if (i < N-1){
        nextPoint = points[i+1];
      }

      Vector3d a = (currentPoint - lastPoint);
      Vector3d b = (nextPoint - currentPoint);

      // if these fail to normalize we have zero length vectors (e.g. adjacent points)
      if (a.normalize()){
        if (b.normalize()){

          Vector3d c = a.cross(b);
          if (c.length() >= tol){
            // cross product is significant
            result.push_back(currentPoint);
            lastPoint = currentPoint;
          }else{
            // see if dot product is near -1
            double d = a.dot(b);
            if (d <= -1.0 + tol){
              // this is a line reversal
              result.push_back(currentPoint);
              lastPoint = currentPoint;
            }
          }
        }
      }
    }

    size_t iBegin = 0;
    size_t iEnd = result.size();

    bool resizeBegin = true;
    while (resizeBegin){
      resizeBegin = false;
      unsigned N = iEnd - iBegin;
      if (N > 3){
        Vector3d a = (result[iBegin] - result[iEnd - 1]);
        Vector3d b = (result[iBegin + 1] - result[iBegin]);
        if (a.normalize()){
          if (b.normalize()){
            double d = a.dot(b);
            if (d >= 1.0 - tol){
              iBegin++;
              resizeBegin = true;
            }
          } else{
            iBegin++;
            resizeBegin = true;
          }
        } else{
          iBegin++;
          resizeBegin = true;
        }
      }
    }

    bool resizeEnd = true;
    while (resizeEnd){
      resizeEnd = false;
      unsigned N = iEnd - iBegin;
      if (N > 3){
        Vector3d a = (result[iEnd - 1] - result[iEnd - 2]);
        Vector3d b = (result[iBegin] - result[iEnd - 1]);
        if (a.normalize()){
          if (b.normalize()){
            double d = a.dot(b);
            if (d >= 1.0 - tol){
              iEnd--;
              resizeEnd = true;
            }
          } else{
            iEnd--;
            resizeEnd = true;
          }
        } else{
          iEnd--;
          resizeEnd = true;
        }
      }
    }

    result = std::vector<Point3d>(result.begin() + iBegin, result.begin() + iEnd);
    return result;
  }

  double getDistance(const Point3d& point1, const Point3d& point2) {
    double dx = point1.x() - point2.x();
    double dy = point1.y() - point2.y();
    double dz = point1.z() - point2.z();
    double result = std::sqrt(dx*dx + dy*dy + dz*dz);
    return result;
  }

  double getDistancePointToLineSegment(const Point3d& point, const std::vector<Point3d>& lineSegment)
  {
    if (lineSegment.size() != 2){
      return 0;
    }

    // http://paulbourke.net/geometry/pointlineplane/

    Point3d point1 = lineSegment[0];
    Point3d point2 = lineSegment[1];

    Vector3d p2p1 = point2-point1;
    Vector3d p3p1 = point-point1;

    double d12 = p2p1.length();
    if (d12 < 1.0e-12){
      return p3p1.length();
    }

    Point3d closestPoint;
    double u = p3p1.dot(p2p1) / (d12*d12);
    if (u < 0){
      closestPoint = point1;
    }else if (u > 1){
      closestPoint = point2;
    }else{
      closestPoint = point1 + u*p2p1;
    }

    Vector3d diff = point - closestPoint;

    return diff.length();
  }

  double getDistancePointToTriangle(const Point3d& point, const std::vector<Point3d>& triangle)
  {
    if (triangle.size() != 3){
      return 0;
    }

    //Distance Between Point and Triangle in 3D
    //David Eberly
    //Geometric Tools, LLC
    //http://www.geometrictools.com/

    //T(s; t) = B+sE0+tE1

    Point3d B = triangle[0];
    Vector3d E0 = triangle[1] - triangle[0];
    Vector3d E1 = triangle[2] - triangle[0];
    Vector3d BminusP = B - point;

    double b = E0.dot(E1);

    if (std::abs(b) > 1.0-1.0E-12){
      // triangle is collinear
      return 0;
    }

    double a = E0.dot(E0);
    double c = E1.dot(E1);
    double d = E0.dot(BminusP);
    double e = E1.dot(BminusP);
    // double f = BminusP.dot(BminusP); // unused

    double det = a*c-b*b;
    double s = b*e-c*d;
    double t = b*d-a*e;

    Point3d closestPoint;

    if ( s+t <= det ) {
      if ( s < 0 ) {
        if ( t < 0 ) {
          //region 4, closest to point triangle[0]
          return getDistance(point, triangle[0]);
        } else {
          //region 3, closest to line triangle[0] to triangle[2]
          std::vector<Point3d> line;
          line.push_back(triangle[0]);
          line.push_back(triangle[2]);
          return getDistancePointToLineSegment(point, line);
        }
      } else if ( t < 0 ) {
        //region 5, closest to line triangle[0] to triangle[1]
        std::vector<Point3d> line;
        line.push_back(triangle[0]);
        line.push_back(triangle[1]);
        return getDistancePointToLineSegment(point, line);
      } else {
        //region 0, closest point is inside triangle
        double invDet = 1.0/det;
        closestPoint = B + invDet*s*E0 + invDet*t*E1;
      }
    } else {
      if ( s < 0 ) {
        //region 2, closest to point triangle[2]
        return getDistance(point, triangle[2]);
      } else if ( t < 0 ) {
        //region 6, closest to point triangle[1]
        return getDistance(point, triangle[1]);
      } else {
        //region 1, closest to line triangle[1] to triangle[2]
        std::vector<Point3d> line;
        line.push_back(triangle[1]);
        line.push_back(triangle[2]);
        return getDistancePointToLineSegment(point, line);
      }
    }

    Vector3d diff = point-closestPoint;
    return diff.length();
  }

  double getAngle(const Vector3d& vector1, const Vector3d& vector2) {
    Vector3d working1(vector1);
    working1.normalize();
    Vector3d working2(vector2);
    working2.normalize();
    return acos(working1.dot(working2));
  }

  bool circularEqual(const Point3dVector& points1, const Point3dVector& points2, double tol)
  {
    size_t N = points1.size();
    if (N != points2.size()){
      return false;
    }

    if (N == 0){
      return true;
    }

    bool result = false;

    // look for a common starting point
    for (size_t i = 0; i < N; ++i){
      if (getDistance(points1[0], points2[i]) <= tol){

        result = true;

        // check all other points
        for (unsigned j = 0; j < N; ++j){
          if (getDistance(points1[j], points2[(i + j) % N]) > tol){
            result = false;
            break;
          }
        }
      }

      if (result){
        return result;
      }
    }

    return result;
  }

  Point3d getCombinedPoint(const Point3d& point3d, std::vector<Point3d>& allPoints, double tol)
  {
    for (const Point3d& otherPoint : allPoints){
      if (std::sqrt(std::pow(point3d.x()-otherPoint.x(), 2) + std::pow(point3d.y()-otherPoint.y(), 2) + std::pow(point3d.z()-otherPoint.z(), 2)) < tol){
        return otherPoint;
      }
    }
    allPoints.push_back(point3d);
    return point3d;
  }

  std::vector<std::vector<Point3d> > computeTriangulation(const Point3dVector& vertices, const std::vector<std::vector<Point3d> >& holes, double tol)
  {
    std::vector<std::vector<Point3d> > result;

    // check input
    if (vertices.size () < 3){
      return result;
    }

    boost::optional<Vector3d> normal = getOutwardNormal(vertices);
    if (!normal || normal->z() > -0.999){
      return result;
    }

    for (const auto& hole : holes){
      normal = getOutwardNormal(hole);
      if (!normal || normal->z() > -0.999){
        return result;
      }
    }

    std::vector<Point3d> allPoints;

    // PolyPartition does not support holes which intersect the polygon or share an edge
    // if any hole is not fully contained we will use boost to remove all the holes
    bool polyPartitionHoles = true;
    for (const std::vector<Point3d>& hole : holes){
      if (!within(hole, vertices, tol)){
        // PolyPartition can't handle this
        polyPartitionHoles = false;
        break;
      }
    }

    if (!polyPartitionHoles){
      // use boost to do all the intersections
      std::vector<std::vector<Point3d> > allFaces = subtract(vertices, holes, tol);
      std::vector<std::vector<Point3d> > noHoles;
      for (const std::vector<Point3d>& face : allFaces){
        std::vector<std::vector<Point3d> > temp = computeTriangulation(face, noHoles);
        result.insert(result.end(), temp.begin(), temp.end());
      }
      return result;
    }

    // convert input to vector of TPPLPoly
    std::list<TPPLPoly> polys;

    TPPLPoly outerPoly; // must be counter-clockwise, input vertices are clockwise
    outerPoly.Init(vertices.size());
    outerPoly.SetHole(false);
    size_t n = vertices.size();
    for(size_t i = 0; i < n; ++i){

      // should all have zero z coordinate now
      double z = vertices[n-i-1].z();
      if (abs(z) > tol){
        LOG_FREE(Error, "utilities.geometry.computeTriangulation", "All points must be on z = 0 plane for triangulation methods");
        return result;
      }

      Point3d point = getCombinedPoint(vertices[n-i-1], allPoints, tol);
      outerPoly[i].x = point.x();
      outerPoly[i].y = point.y();
    }
    outerPoly.SetOrientation(TPPL_CCW);
    polys.push_back(outerPoly);

    for (const std::vector<Point3d>& holeVertices : holes){

      if (holeVertices.size () < 3){
        LOG_FREE(Error, "utilities.geometry.computeTriangulation", "Hole has fewer than 3 points, ignoring");
        continue;
      }

      TPPLPoly innerPoly; // must be clockwise, input vertices are clockwise
      innerPoly.Init(holeVertices.size());
      innerPoly.SetHole(true);
      //std::cout << "inner :";
      for(unsigned i = 0; i < holeVertices.size(); ++i){

        // should all have zero z coordinate now
        double z = holeVertices[i].z();
        if (abs(z) > tol){
          LOG_FREE(Error, "utilities.geometry.computeTriangulation", "All points must be on z = 0 plane for triangulation methods");
          return result;
        }

        Point3d point = getCombinedPoint(holeVertices[i], allPoints, tol);
        innerPoly[i].x = point.x();
        innerPoly[i].y = point.y();
      }
      innerPoly.SetOrientation(TPPL_CW);
      polys.push_back(innerPoly);
    }

    // do partitioning
    TPPLPartition pp;
    std::list<TPPLPoly> resultPolys;
    int test = pp.Triangulate_EC(&polys,&resultPolys);
    if (test == 0){
      test = pp.Triangulate_MONO(&polys, &resultPolys);
    }
    if (test == 0){
      //std::stringstream ss;
      //ss << "Vertices: " << vertices << std::endl;
      //for (const auto& hole : holes){ ss << "Hole:" << hole << std::endl; }
      //std::string testStr = ss.str();
      LOG_FREE(Error, "utilities.geometry.computeTriangulation", "Failed to partition polygon");
      return result;
    }

    // convert back to vertices
    std::list<TPPLPoly>::iterator it, itend;
    //std::cout << "Start" << std::endl;
    for(it = resultPolys.begin(), itend = resultPolys.end(); it != itend; ++it){

      it->SetOrientation(TPPL_CW);

      std::vector<Point3d> triangle;
      for (long i = 0; i < it->GetNumPoints(); ++i){
        TPPLPoint point = it->GetPoint(i);
        triangle.push_back(Point3d(point.x, point.y, 0));
      }
      //std::cout << triangle << std::endl;
      result.push_back(triangle);
    }
    //std::cout << "End" << std::endl;

    return result;
  }

  std::vector<Point3d> moveVerticesTowardsPoint(const Point3dVector& vertices, const Point3d& point, double distance)
  {
    Point3dVector result;
    for (const Point3d& vertex : vertices){
      Vector3d vector = point-vertex;
      vector.setLength(distance);
      result.push_back(vertex+vector);
    }
    return result;
  }

  std::vector<Point3d> reverse(const Point3dVector& vertices)
  {
    std::vector<Point3d> result(vertices);
    std::reverse(result.begin(), result.end());
    return result;
  }

  bool applyViewAndDaylightingGlassRatios(double viewGlassToWallRatio, double daylightingGlassToWallRatio,
    double desiredViewGlassSillHeight, double desiredDaylightingGlassHeaderHeight,
    double exteriorShadingProjectionFactor, double interiorShelfProjectionFactor,
    const Point3dVector& vertices, Point3dVector& viewVertices,
    Point3dVector& daylightingVertices, Point3dVector& exteriorShadingVertices,
    Point3dVector& interiorShelfVertices)
  {
    // check inputs for reasonableness
    double totalWWR = viewGlassToWallRatio + daylightingGlassToWallRatio;
    if (totalWWR == 0){
      // requesting no glass? remove existing windows?
      return false;
    }else if (totalWWR < 0.0 || totalWWR >= 1.0){
      return false;
    }

    boost::optional<double> grossArea = getArea(vertices);
    if (!grossArea){
      return false;
    }

    Transformation transformation = Transformation::alignFace(vertices);
    Point3dVector faceVertices = transformation.inverse() * vertices;

    if (faceVertices.empty()){
      return false;
    }

    bool doViewGlass = (viewGlassToWallRatio > 0);
    bool doDaylightGlass = (daylightingGlassToWallRatio > 0);
    bool doExteriorShading = (doViewGlass && (exteriorShadingProjectionFactor > 0));
    bool doInteriorShelf = (doDaylightGlass && (interiorShelfProjectionFactor > 0));
    bool doViewAndDaylightGlass = (doViewGlass && doDaylightGlass);

    // ignore these user arguments?
    if (!doViewGlass){
      desiredViewGlassSillHeight = 0.0;
    }
    if (!doDaylightGlass){
      desiredDaylightingGlassHeaderHeight = 0.0;
    }

    // new coordinate system has z' in direction of outward normal, y' is up
    double xmin = std::numeric_limits<double>::max();
    double xmax = std::numeric_limits<double>::min();
    double ymin = std::numeric_limits<double>::max();
    double ymax = std::numeric_limits<double>::min();
    for (const Point3d& faceVertex : faceVertices){
      xmin = std::min(xmin, faceVertex.x());
      xmax = std::max(xmax, faceVertex.x());
      ymin = std::min(ymin, faceVertex.y());
      ymax = std::max(ymax, faceVertex.y());
    }
    if ((xmin > xmax) || (ymin > ymax)){
      return false;
    }

    double oneInch = 0.0254;

    // DLM: preserve a 1" gap between window and edge to keep SketchUp happy
    double minGlassToEdgeDistance = oneInch;
    double minViewToDaylightDistance = 0;
    if (doViewAndDaylightGlass){
      minViewToDaylightDistance = oneInch;
    }

    // wall parameters
    double wallWidth = xmax - xmin;
    double wallHeight = ymax - ymin;
    double wallArea = wallWidth*wallHeight;

    if (wallWidth < 2*minGlassToEdgeDistance){
      return false;
    }

    if (wallHeight < 2*minGlassToEdgeDistance + minViewToDaylightDistance){
      return false;
    }

    // check against actual surface area to ensure this is a rectangle?
    if (std::abs(wallArea - grossArea.get()) > oneInch*oneInch){
      return false;
    }

    double maxWindowArea = wallArea - 2*wallHeight*minGlassToEdgeDistance - (wallWidth-2*minGlassToEdgeDistance)*(2*minGlassToEdgeDistance + minViewToDaylightDistance);
    double requestedViewArea = viewGlassToWallRatio*wallArea;
    double requestedDaylightingArea = daylightingGlassToWallRatio*wallArea;
    double requestedTotalWindowArea = totalWWR*wallArea;

    if (requestedTotalWindowArea > maxWindowArea){
      return false;
    }

    // view glass parameters
    double viewMinX = 0;
    double viewMinY = 0;
    double viewWidth = 0;
    double viewHeight = 0;

    // daylighting glass parameters
    double daylightingWidth = 0;
    double daylightingHeight = 0;
    double daylightingMinX = 0;
    double daylightingMinY = 0;

    // initial free parameters
    double viewWidthInset = minGlassToEdgeDistance;
    double viewSillHeight = std::max(desiredViewGlassSillHeight, minGlassToEdgeDistance);
    double daylightingWidthInset = minGlassToEdgeDistance;
    double daylightingHeaderHeight = std::max(desiredDaylightingGlassHeaderHeight, minGlassToEdgeDistance);

    bool converged = false;
    for (unsigned i = 0; i < 100; ++i){

      // view glass parameters
      viewMinX = viewWidthInset;
      viewMinY = viewSillHeight;
      viewWidth = wallWidth - 2*viewWidthInset;
      viewHeight = requestedViewArea/viewWidth;

      // daylighting glass parameters
      daylightingWidth = wallWidth - 2*daylightingWidthInset;
      daylightingHeight = requestedDaylightingArea/daylightingWidth;
      daylightingMinX = viewWidthInset;
      daylightingMinY = wallHeight - daylightingHeaderHeight - daylightingHeight;

      if (viewMinY + viewHeight + minViewToDaylightDistance > daylightingMinY){
        // windows overlap or exceed maximum size

        if (doViewAndDaylightGlass){

          // try shrinking vertical offsets
          viewSillHeight = std::max(viewSillHeight - oneInch, minGlassToEdgeDistance);
          daylightingHeaderHeight = std::max(daylightingHeaderHeight - oneInch, minGlassToEdgeDistance);

        }else if (doViewGlass){

          // solve directly
          viewSillHeight = wallHeight - minGlassToEdgeDistance - viewHeight;

          if (viewSillHeight < minGlassToEdgeDistance){
            // cannot make window this large
            return false;
          }

        }else if (doDaylightGlass){

          // solve directly
          daylightingHeaderHeight = wallHeight - minGlassToEdgeDistance - daylightingHeight;

          if (daylightingHeaderHeight < minGlassToEdgeDistance){
            // cannot make window this large
            return false;
          }

        }

      }else{

        converged = true;
        break;

      }
    }

    if (!converged){
      return false;
    }

    Point3dVector surfacePolygon;
    for (const Point3d& point : faceVertices){
      if (std::abs(point.z()) > 0.001){
        LOG_FREE(Warn, "utilities.geometry.applyViewAndDaylightingGlassRatios", "Surface point z not on plane, z =" << point.z());
      }
      surfacePolygon.push_back(Point3d(point.x(),point.y(), 0.0));
    }
    std::reverse(surfacePolygon.begin(), surfacePolygon.end());

    if (doViewGlass){
      viewVertices.push_back(Point3d(viewMinX, viewMinY + viewHeight, 0));
      viewVertices.push_back(Point3d(viewMinX, viewMinY, 0));
      viewVertices.push_back(Point3d(viewMinX + viewWidth, viewMinY, 0));
      viewVertices.push_back(Point3d(viewMinX + viewWidth, viewMinY + viewHeight, 0));

      Point3dVector windowPolygon;
      for (const Point3d& point : viewVertices){
        if (std::abs(point.z()) > 0.001){
          LOG_FREE(Warn, "utilities.geometry.applyViewAndDaylightingGlassRatios", "Surface point z not on plane, z =" << point.z());
        }
        windowPolygon.push_back(Point3d(point.x(),point.y(), 0.0));
      }

      // sub surface must be fully contained by base surface
      for (const Point3d& point : windowPolygon){
        if (!within(point, surfacePolygon, 0.001)){
          std::cout << "point: " << point << std::endl;
          std::cout << "surfacePolygon: " << surfacePolygon << std::endl;
          LOG_FREE(Debug, "utilities.geometry.applyViewAndDaylightingGlassRatios", "Surface does not fully contain SubSurface");
          return false;
        }
      }
    }

    if (doDaylightGlass){
      daylightingVertices.push_back(Point3d(daylightingMinX, daylightingMinY + daylightingHeight, 0));
      daylightingVertices.push_back(Point3d(daylightingMinX, daylightingMinY, 0));
      daylightingVertices.push_back(Point3d(daylightingMinX + daylightingWidth, daylightingMinY, 0));
      daylightingVertices.push_back(Point3d(daylightingMinX + daylightingWidth, daylightingMinY + daylightingHeight, 0));

      Point3dVector windowPolygon;
      for (const Point3d& point : daylightingVertices){
        if (std::abs(point.z()) > 0.001){
          LOG_FREE(Warn, "utilities.geometry.applyViewAndDaylightingGlassRatios", "Surface point z not on plane, z =" << point.z());
        }
        windowPolygon.push_back(Point3d(point.x(),point.y(), 0.0));
      }

      // sub surface must be fully contained by base surface
      for (const Point3d& point : windowPolygon){
        if (!within(point, surfacePolygon, 0.001)){
          LOG_FREE(Debug, "utilities.geometry.applyViewAndDaylightingGlassRatios", "Surface does not fully contain SubSurface");
          return false;
        }
      }
    }

    if (doExteriorShading) {
      exteriorShadingVertices.push_back(Point3d(viewMinX, viewMinY + viewHeight, 0));
      exteriorShadingVertices.push_back(Point3d(viewMinX, viewMinY + viewHeight, exteriorShadingProjectionFactor*viewHeight));
      exteriorShadingVertices.push_back(Point3d(viewMinX + viewWidth, viewMinY + viewHeight, exteriorShadingProjectionFactor*viewHeight));
      exteriorShadingVertices.push_back(Point3d(viewMinX + viewWidth, viewMinY + viewHeight, 0));
    }

    if (doInteriorShelf) {
      interiorShelfVertices.push_back(Point3d(daylightingMinX + daylightingWidth, daylightingMinY, 0));
      interiorShelfVertices.push_back(Point3d(daylightingMinX + daylightingWidth, daylightingMinY, -interiorShelfProjectionFactor*daylightingHeight));
      interiorShelfVertices.push_back(Point3d(daylightingMinX, daylightingMinY, -interiorShelfProjectionFactor*daylightingHeight));
      interiorShelfVertices.push_back(Point3d(daylightingMinX, daylightingMinY, 0));
    }

    // put all vertices back into input coordinate system
    viewVertices = transformation*viewVertices;
    daylightingVertices = transformation*daylightingVertices;
    exteriorShadingVertices = transformation*exteriorShadingVertices;
    interiorShelfVertices = transformation*interiorShelfVertices;

    return true;
  }

} // openstudio