#include "ag_DataConfiguration.h"

// Library headers.
#include <boost/format.hpp>

// PCRaster library headers.
#include "dal_Client.h"
#include "dal_Dal.h"
#include "dal_TableDriver.h"

// Module headers.
#include "ag_DataGuide.h"
#include "ag_VisGroup.h"
#include "ag_DataObject.h"
#include "ag_XMLViewItems.h"

/*!
  \file
  This file contains the implementation of the DataConfiguration class.
*/

namespace ag {

template<typename T>
struct DimensionMaker {
  dal::Meaning            meaning;
  dal::DiscretisationType setType;
  dal::DiscretisationType rangeType;

  dal::Dimension create(
         typename pcrxsd::RangeSetTypeTrait<T>::RangeOrSet const& f) {

    if(f.set().present()) {
      return dal::Dimension(meaning, pcrxsd::items<T>(f.set().get()));
    }
    else {
      assert(f.range().present());
      return dal::Dimension(meaning,
         T(f.range()->begin()),
         T(f.range()->end()),
         T(f.range()->increment()));
    }
  }
};

//------------------------------------------------------------------------------
// DEFINITION OF STATIC DATACONFIGURATION MEMBERS
//------------------------------------------------------------------------------

/*!
 * \todo should go into dal, but first must have definite XML Schema
 */
dal::DataSpace DataConfiguration::dataSpace(
         pcrxml::DataSpace const& xml)
{
  dal::DataSpace space;

  if(!xml.scenarios().empty()) {
    std::set<std::string> set;
    set.insert(xml.scenarios()[0].item().begin(),
         xml.scenarios()[0].item().end());
    dal::Dimension dimension(dal::Scenarios, set);

    for(size_t i = 1; i < xml.scenarios().size(); ++i) {
      set.clear();
      set.insert(xml.scenarios()[i].item().begin(),
           xml.scenarios()[i].item().end());
      dimension |= dal::Dimension(dal::Scenarios, set);
    }

    space.addDimension(dimension);
  }

  if(!xml.quantiles().empty()) {
    DimensionMaker<float> m;
    m.meaning  = dal::CumulativeProbabilities;
    m.setType  = dal::ExactDiscretisation;
    m.rangeType= dal::RegularDiscretisation;

    dal::Dimension dimension(m.create(xml.quantiles()[0]));

    for(size_t i = 1; i < xml.quantiles().size(); ++i) {
      dimension |= m.create(xml.quantiles()[i]);
    }

    space.addDimension(dimension);
  }

// if(!xml.samples().empty()) {
//   DimensionMaker m;
//   m.meaning    = dal::Samples;
//   m.setType    = dal::RegularDiscretisation; // ???? default
//   m.rangeType  = dal::RegularDiscretisation;
//    BLA BLA
//   space.addDimension(dimension);
// }

  if(!xml.timesteps().empty()) {
    DimensionMaker<size_t> m;
    m.meaning  = dal::Time;
    m.setType  = dal::RegularDiscretisation;
    m.rangeType= dal::RegularDiscretisation;

    dal::Dimension dimension(m.create(xml.timesteps()[0]));
    for(size_t i = 1; i < xml.timesteps().size(); ++i) {
      dimension |= m.create(xml.timesteps()[i]);
    }
    space.addDimension(dimension);
  }

  return space;
}

//------------------------------------------------------------------------------
// DEFINITION OF DATACONFIGURATION MEMBERS
//------------------------------------------------------------------------------

DataConfiguration::DataConfiguration(
         /// dal::Dal& dal,
         VisGroup* group,
         pcrxml::VisualisationGroup const& xmlGroup)

  : /// d_dal(dal),
    d_group(group),
    d_nrScenarios(0)

{
  // Search space is optional.
  if(xmlGroup.searchSpace().present()) {
    d_searchSpace = dataSpace(xmlGroup.searchSpace().get());
  }

  // For each data set mentioned store:
  // - name
  // - data space
  // - properties
  // - data guide

  // Loop over all data items and get data space and property information.
  // If a data space is not set, but the search space is, then this is set
  // initially.

  dal::DataSpace space;

  for(size_t i = 0; i < xmlGroup.data().size(); ++i) {
    pcrxml::AguilaData const& configuration = xmlGroup.data()[i];
    std::string name = configuration.name();
    space = configuration.dataSpace().present()
        ? dataSpace(configuration.dataSpace().get())
        : d_searchSpace;

    add(name, space, configuration);
  }

  // Loop over all views and get name / data space information.
  for(pcrxml::VisualisationGroup::view_const_iterator view =
         xmlGroup.view().begin(); view != xmlGroup.view().end(); ++view) {
    XMLViewItems const& items(*view);

    for(XMLViewItems::const_iterator name = items.begin();
         name != items.end(); ++name) {

      // If not yet added, add now
      // TODO KDJ name is not a good identifier for a data set, the name
      // TODO space combination is. This works for as long as no two datasets
      // TODO with the same name but different space are mentioned in xmlGroup.
      if(!d_dataMap2.count(*name)) {
        space = d_searchSpace;
        add(*name, space, pcrxml::AguilaData(*name));
      }
    }
  }

  // Overall space.
  space = group->dataObject().dataSpace();

  d_nrScenarios = space.hasScenarios()
         ? space.dimension(space.indexOf(dal::Scenarios)).nrCoordinates()
         : 1;

  // Send properties and datemappers found to the data object.
  for(DataMap2::const_iterator d = d_dataMap2.begin();
         d != d_dataMap2.end(); ++d) {
    DataGuide const& guide((*d).second.guide);
    pcrxml::AguilaData const& configuration((*d).second.configuration);

    if(configuration.drawProperties().present()) {
      d_group->dataObject().setXML(
         guide, configuration.drawProperties().get(), false);
    }

    // Set date mapper if needed/present.
    if(d_group->dataObject().dataSpace(guide).hasTime()) {
      // Data space stuff in data item takes precedence
      if(guide.type() == geo::STACK) {
        // TODO only supported for stacks at the moment.
        if(configuration.dataSpace().present() &&
              configuration.dataSpace()->timesteps().size() &&
              configuration.dataSpace()->timesteps()[0].dateMapper().present()) {
          d_group->dataObject().setDateMapper(guide,
              configuration.dataSpace()->timesteps()[0].dateMapper().get(),
                   false);
        }
        else if(xmlGroup.searchSpace().present() &&
              xmlGroup.searchSpace()->timesteps().size() &&
              xmlGroup.searchSpace()->timesteps()[0].dateMapper().present()) {
          d_group->dataObject().setDateMapper(guide,
              xmlGroup.searchSpace()->timesteps()[0].dateMapper().get(), false);
        }
      }
    }
  }
}

// DataConfiguration::DataConfiguration(
//     dal::Dal&     dal,
//     dal::DataSpace const& searchSpace,
//     VisGroup* group):
//      d_dal(dal),
//      d_searchSpace(searchSpace),
//      d_group(group)
// {
// }

/* NOT IMPLEMENTED
//! Copy constructor.
DataConfiguration::DataConfiguration(
         DataConfiguration const& rhs)

  : Base(rhs)

{
}
*/

DataConfiguration::~DataConfiguration()
{
}

/* NOT IMPLEMENTED
//! Assignment operator.
DataConfiguration& DataConfiguration::operator=(
         DataConfiguration const& rhs)
{
  if(this != &rhs) {
  }

  return *this;
}
*/

void DataConfiguration::add(
         std::string const& name,
         dal::DataSpace space,
         pcrxml::AguilaData const& configuration)
{
  dal::DataSpaceQueryResult result;
  boost::tie(result, boost::tuples::ignore) = dal::Client::dal().search(name,
      space, dal::NarrowSpaceWhenNeeded, dal::SearchForAllItems);

  if(!result) {
    dal::throwCannotBeOpened(name, space);
  }

  space = result.space();

  if(!space.hasScenarios()) {
    add(name, space, result, configuration);
  }
  else {
    size_t index = space.indexOf(dal::Scenarios);
    dal::Dimension const& dimension(space.dimension(index));
    // d_nameMap.insert(std::make_pair(name, name));
    dal::DataSpace narrowSpace;

    for(size_t i = 0; i < dimension.nrCoordinates(); ++i) {
      add(name, dal::dataSpaceWithNarrowedDimension(space, index, i),
         result, configuration);
    }
  }
}

void DataConfiguration::add(
         std::string const& name,
         dal::DataSpace space,
         dal::DataSpaceQueryResult const& result,
         pcrxml::AguilaData const& configuration)
{
  // TODO Use result to help
  // - VisGroup::addData(name, space)     This only forwards to:
  // - DataObject::add(name, space)
  //   - Needs:
  //     - DatasetType (in query result)
  //     -
  //   - Forwards to addStack(name, space), etc.
  //     - Forwards to DataObjectBase<Raster>.add(name, space)
  //       - Forwards to RasterDataSources::openData(name, space)
  //         - Forwards to Raster(name, space)
  //           - Forwards to DataSource(name, space)
  // to find the data faster.
  // Check whether DataObject::add(name, space) can be changed, or whether we
  // need more overloads of that one.
  assert(result);

  if(result.datasetType() != dal::TABLE) {
    d_nameMap[name].insert(name);
    d_dataMap2.insert(std::make_pair(name, DataItemInformation(
       space, configuration, d_group->addData(name, space))));
  }
  else {
    std::vector<std::string> selection;
    boost::tie(boost::tuples::ignore, selection) =
         dal::splitNameAndSelection(name);

    if(!selection.empty()) {
      // Name IS selection.
      d_nameMap[name].insert(name);
      d_dataMap2.insert(std::make_pair(name, DataItemInformation(
         space, configuration, d_group->addData(name, space))));
    }
    else {
      // Name is not a selection, but an "alias" for all columns
      // Create for each colum, a selection specification:
      //   name{1,n}, name{1,n-1}, ...,name{1,2}
      dal::Driver const* driver(dal::Client::dal().driverByDataset(name,
         space));
      assert(driver);
      std::auto_ptr<dal::Table const> table(dynamic_cast<dal::Table const*>(
         driver->open(name, result.space(), result.address())));
      assert(table.get());

      for(size_t i = table->nrCols(); i > 1; --i) {
        std::string colName =
           (boost::format("%1%{1, %2%}") % (name) % i).str();
        d_nameMap[name].insert(colName);
        d_dataMap2.insert(std::make_pair(colName, DataItemInformation(
           space, configuration, d_group->addData(colName, space))));
      }
    }
  }
}

// dal::DataSpace DataConfiguration::dataSpaceFor(
//          std::string const& name) const
// {
//    DataMap::const_iterator i=d_dataMap.find(name);
//
//    if (i != d_dataMap.end() && i->second.dataSpace().present()) {
//      return dataSpace(i->second.dataSpace().get());
//    }
//
//    dal::DataSpaceQueryResult result = d_dal.open(name, d_searchSpace);
//
//    if(!result.dataset) {
//       dal::throwCannotBeOpened(name, d_searchSpace);
//    }
//
//    return result.dataSpace;
// };

// //! if not found return a pcrxml::DrawProperties() (empty)
// pcrxml::DrawProperties DataConfiguration::drawProperties(
//          std::string const& name) const
// {
//    DataMap::const_iterator i=d_dataMap.find(name);
//    if (i != d_dataMap.end() && i->second.drawProperties().present()) {
//      return i->second.drawProperties().get();
//    }
//    return pcrxml::DrawProperties();
// }

// dal::DataSpace const& DataConfiguration::searchSpace() const
// {
//   return d_searchSpace;
// }

std::vector<std::vector<DataGuide> > DataConfiguration::guidesOfView2(
         pcrxml::AguilaView const& view) const
{
  // For each scenario a bunch of guides.
  std::vector<std::vector<DataGuide> > result(d_nrScenarios);

  XMLViewItems viewItems(view);

  // Loop over views items: names of data sets.
  for(XMLViewItems::const_iterator i = viewItems.begin(); i != viewItems.end();
         ++i) {
    // Data set names can be an alias for a set of data set names. First
    // translate the name to this set and collect the guides for each of them.
    assert(d_nameMap.find(*i) != d_nameMap.end());
    std::set<std::string> names((*d_nameMap.find(*i)).second);
    assert(!names.empty());

    std::pair<NameMap::const_iterator, NameMap::const_iterator> nameRange(
         d_nameMap.equal_range(*i));
    assert(nameRange.first != nameRange.second);

    // For this one name we now have the data set names it corresponds to.
    // Most of the time this is the same name, but it may be a set of other
    // names. A table name is unpacked to column selections, for example.
    // Aguila is an attribute visualizer and does not handle blobs of data
    // with more than one attributes.
    for(std::set<std::string>::const_iterator it = names.begin();
         it != names.end(); ++it) {
      // This is the name to handle.
      // Get data item information.
      std::pair<DataMap2::const_iterator, DataMap2::const_iterator> range(
         d_dataMap2.equal_range(*it));
      assert(range.first != range.second);

      if(std::distance(range.first, range.second) == 1) {
        // No scenarios available in this data set. Add this one data set to
        // all collections of guides.
        for(size_t i = 0; i < result.size(); ++i) {
          result[i].push_back((*range.first).second.guide);
        }
      }
      else {
        assert(static_cast<size_t>(std::distance(range.first, range.second)) ==
              d_nrScenarios);

        for(size_t i = 0; range.first != range.second; ++range.first, ++i) {
          result[i].push_back((*range.first).second.guide);
        }
      }
    }
  }

  return result;
}

// //! return the list of guides needed for \a view
// std::vector<DataGuide> DataConfiguration::guidesOfView(
//          pcrxml::AguilaView const& view) const
// {
//   XMLViewItems avi(view);
//   std::vector<DataGuide> dg;
//   for(XMLViewItems::const_iterator i=avi.begin(); i != avi.end(); ++i) {
//       GuideMap::const_iterator g=d_guideMap.find(*i);
//       assert(g!= d_guideMap.end());
//       dg.push_back(g->second);
//   }
//   return dg;
// }

// std::vector<DataGuide> DataConfiguration::tableGuides(
//          std::string const& name) const
// {
//   const dal::DataSpace tableDataSpace(dataSpaceFor(name));
//
//   std::vector<std::string> ts(tableSelections(name));
//   std::vector<DataGuide> dg;
//
//   for (size_t i=0; i < ts.size(); ++i)
//     dg.push_back(d_group->addData(ts[i],tableDataSpace));
//
//   return dg;
// }

// std::vector<std::string> DataConfiguration::tableSelections(
//          std::string const& name) const
// {
//   std::vector<std::string> ts;
//
//   // We assume that the first column in the table contains the time steps
//   // and the subsequent ones contain the time series. Each time series is
//   // treated as a separate attribute.
//
//   dal::Table* table;
//   boost::tie(table,boost::tuples::ignore,boost::tuples::ignore) =
//          d_dal.open<dal::Table>(name, dal::TABLE, dataSpaceFor(name));
//
//   if(!table || !(table->nrCols() >= 2)) {
//     // The name does not refer to a table but some other dataset. Selections
//     // are only supported for tables so we just add this name to the
//     // collection.
//     ts.push_back(name);
//   } else {
//     std::vector<std::string> selection;
//     boost::tie(boost::tuples::ignore,selection)= dal::splitNameAndSelection(name);
//
//     if(!selection.empty()) {
//       // name IS A selection
//       ts.push_back(name);
//     } else {
//       // name IS not a selection, but "alias" for all collumns
//       // create for each colum, a selection specification:
//       //    name{1,n}, name{1,n-1}, ...,name{1,2}
//       for(size_t i = table->nrCols(); i > 1; --i) {
//         std::string colName = (boost::format("%1%{1, %2%}") % (name) % i).str();
//         ts.push_back(colName);
//       }
//     }
//   }
//
//   return ts;
// }

//------------------------------------------------------------------------------
// DEFINITION OF FREE OPERATORS
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// DEFINITION OF FREE FUNCTIONS
//------------------------------------------------------------------------------

} // namespace ag


#include <stdio.h>
#include <cfloat>
#include <list>
#include <vector>
#include <math.h>
#include <iostream>
#include <algorithm>
#include <numeric>
#include <boost/algorithm/clamp.hpp>
#include <random>
#include "third_party/eigen3/unsupported/Eigen/CXX11/Tensor"
#include "unsupported/Eigen/CXX11/src/Tensor/TensorDeviceCuda.h"
//#include "unsupported/Eigen/CXX11/Tensor"
#include "tensorflow/core/framework/op.h"
#include "tensorflow/core/framework/op_kernel.h"
#include "tensorflow/core/framework/tensor_shape.h"
#include "tensorflow/core/framework/common_shape_fns.h"
#include "tensorflow/core/util/work_sharder.h"
#include "bboxes.h"
#include "wtoolkit.h"
#include "open_pose_decode_imp.h"
#include <future>

using namespace tensorflow;
using namespace std;

typedef Eigen::ThreadPoolDevice CPUDevice;
typedef Eigen::GpuDevice GPUDevice;
/*
 * gaussian_delta: 一般为2
 * keypoints: [B,N,num_points_nr,2] 相对坐标,x,y
 * glength: 有效的groundtruth instance数量
 * output_size: 输出图的大小[2]=(OH,OW)
 *
 * output:
 * output_conf_map: heatmaps [B,OH,OW,num_points_nr]
 * output_indexs: [B,N,num_points_nr] //-1表示点无效
 */
REGISTER_OP("HrNetPe")
    .Attr("T: {float,double,int32,int64}")
	.Attr("gaussian_delta:float=8.0")
    .Input("keypoints: T")
    .Input("output_size: int32")
    .Input("glength: int32")
	.Output("output_conf_map:T")
	.Output("output_indexs:int32")
	.SetShapeFn([](shape_inference::InferenceContext* c) {
            const auto input_shape0 = c->input(0);
            const auto N = c->Value(c->Dim(input_shape0,1));
            const auto points_nr = c->Value(c->Dim(input_shape0,2));
            const auto batch_size = c->Dim(input_shape0,0);
            auto shape0 = c->MakeShape({batch_size,-1,-1,points_nr});
            auto shape1 = c->MakeShape({batch_size,N,points_nr});

			c->set_output(0, shape0);
			c->set_output(1, shape1);
			return Status::OK();
			});

template <typename Device,typename T>
class HRNetPEOp: public OpKernel {
	public:
		explicit HRNetPEOp(OpKernelConstruction* context) : OpKernel(context) {
			OP_REQUIRES_OK(context, context->GetAttr("gaussian_delta", &gaussian_delta_));
		}

		void Compute(OpKernelContext* context) override
        {
            TIME_THISV1("HRNetPE");
            const Tensor &_keypoints = context->input(0);
            const Tensor &_gsize      = context->input(2);
            auto          output_size = context->input(1).template flat<int>().data();
            const auto    batch_size  = _keypoints.dim_size(0);
            const auto num_keypoints = _keypoints.dim_size(2);

            OP_REQUIRES(context, _keypoints.dims() == 4, errors::InvalidArgument("keypoints data must be 4-dimension"));
            OP_REQUIRES(context, _gsize.dims() == 1, errors::InvalidArgument("gsize data must be 1-dimension"));

            auto         keypoints       = _keypoints.template tensor<T,4>();
            auto         gsize           = _gsize.template tensor<int,1>();
            int          dims_4d0[4]     = {int(batch_size),output_size[0],output_size[1],num_keypoints};
            int          dims_3d0[3]     = {int(batch_size),_keypoints.dim_size(1),num_keypoints};
            TensorShape  outshape0;
            TensorShape  outshape1;
            Tensor      *output_conf_map = NULL;
            Tensor      *output_indexs = NULL;
            const auto   max_data_nr     = _keypoints.dim_size(1);

            TensorShapeUtils::MakeShape(dims_4d0, 4, &outshape0);
            TensorShapeUtils::MakeShape(dims_3d0, 3, &outshape1);

            OP_REQUIRES_OK(context, context->allocate_output(0, outshape0, &output_conf_map));
            OP_REQUIRES_OK(context, context->allocate_output(1, outshape1, &output_indexs));

            auto heatmaps_conf = output_conf_map->template tensor<T,4>();
            auto heatmaps_indexs = output_indexs->template tensor<int,3>();
            const auto block_size = output_size[0]*output_size[1];

            heatmaps_conf.setZero();
            heatmaps_indexs.setConstant(-1);

            for(auto i=0; i<batch_size; ++i) {
                for(auto j=0; j<gsize(i); ++j) {
                    for(auto k=0; k<num_keypoints; ++k) {
                        const auto x0 = keypoints(i,j,k,0)*(output_size[1]-1);
                        const auto y0 = keypoints(i,j,k,1)*(output_size[0]-1);

						if((x0>=0) && (y0>=0)) {
                        	draw_gaussian(heatmaps_conf,i,x0,y0,k,gaussian_delta_);
                            const auto ix0 = int(x0+0.5);
                            const auto iy0 = int(y0+0.5);
                            const auto index = ix0*num_keypoints+iy0*output_size[1]*num_keypoints+k;
                            heatmaps_indexs(i,j,k) = index;
                        }
                    }
                }
            }
        }

        template<typename DT>
        static void draw_gaussian(DT& data,int batch_index,float cx,float cy,int k,float radius)
        {
            const auto th           = 4.6052;
            const auto spread_range = radius *sqrt(2*th);
            const auto width        = data.dimension(2);
            const auto height       = data.dimension(1);
            const auto xtl          = max(0,int(cx-spread_range));
            const auto ytl          = max(0,int(cy-spread_range));
            const auto xbr          = min<int>(width,int(cx+spread_range+1));
            const auto ybr          = min<int>(height,int(cy+spread_range+1));
            const auto sigma_p      = 2 *radius*radius;

            for(auto x=xtl; x<xbr; ++x) {
                for(auto y=ytl; y<ybr; ++y) {
                    auto dx = x-cx;
                    auto dy = y-cy;
                    const auto d = (dx*dx+dy*dy);
                    const auto expv = d/sigma_p;
                    if (expv>th)
                        continue;
                    auto v = exp(-expv);
                    data(batch_index,y,x,k) = max(data(batch_index,y,x,k),v);
                }
            }
        }
	private:
       float       gaussian_delta_ = 2;
};
REGISTER_KERNEL_BUILDER(Name("HrNetPe").Device(DEVICE_CPU).TypeConstraint<float>("T"), HRNetPEOp<CPUDevice, float>);

/*
 * ans: [B,N,num_keypoints,3+tag_C]
 * det: [B,H,W,num_keypoints]
 * tag: [B,H,W,num_keypoints,tag_C]
 * output:
 * output_ans: [B,N,num_keypoints,3+tag_C] (x,y,scores,tag0,tag1,...)
 */
REGISTER_OP("HrNetRefine")
    .Attr("T: {float,double,int32,int64}")
    .Input("ans: T")
    .Input("det: T")
    .Input("tag: T")
    .Output("output_ans: T")
	.SetShapeFn([](shape_inference::InferenceContext* c) {
            const auto shape0 = c->input(0);
			c->set_output(0, shape0);
			return Status::OK();
			});

template <typename Device,typename T>
class HRNetRefine: public OpKernel {
    private:
        using Tensor1d_t = Eigen::Tensor<float,1,Eigen::RowMajor>;
        using Tensor2d_t = Eigen::Tensor<float,2,Eigen::RowMajor>;
        using Tensor3d_t = Eigen::Tensor<float,3,Eigen::RowMajor>;
        using Tensor4d_t = Eigen::Tensor<float,4,Eigen::RowMajor>;
	public:
		explicit HRNetRefine(OpKernelConstruction* context) : OpKernel(context) {
		}

		void Compute(OpKernelContext* context) override
        {
            TIME_THISV1("HRNetPE");
            const Tensor &_ans = context->input(0);
            const Tensor &_det = context->input(1);
            const Tensor &_tag = context->input(2);

            OP_REQUIRES(context, _ans.dims() == 4, errors::InvalidArgument("ans data must be 4-dimension"));
            OP_REQUIRES(context, _det.dims() == 4, errors::InvalidArgument("dete data must be 4-dimension"));
            OP_REQUIRES(context, _tag.dims() == 5, errors::InvalidArgument("tag data must be 5-dimension"));

            const auto  batch_size    = _ans.dim_size(0);
            const auto  num_keypoints = _ans.dim_size(2);
            auto        ans           = _ans.template tensor<T,4>();
            auto        det           = _det.template tensor<T,4>();
            auto        tag           = _tag.template tensor<T,5>();
            Tensor     *output_ans    = NULL;

            OP_REQUIRES_OK(context, context->allocate_output(0, _ans.shape(), &output_ans));

            auto o_ans = output_ans->template tensor<T,4>();

            o_ans.setZero();

            for(auto i=0; i<batch_size; ++i) {
                for(auto j=0; j<_ans.dim_size(1); ++j) {
                    auto n_ans = refine(ans.chip(i,0).chip(j,0),det.chip(i,0),tag.chip(i,0));
                    o_ans.chip(i,0).chip(j,0) = n_ans;
                }
            }
        }

        Tensor2d_t refine(const Tensor2d_t& keypoints,const Tensor3d_t& det,const Tensor4d_t& tag)
        {
            Tensor2d_t res_keypoints = keypoints;
            const auto num_keypoints = keypoints.dimension(0);
            const auto tag_C = keypoints.dimension(1)-3;
            Tensor2d_t tags(num_keypoints,tag_C);
            const auto H = det.dimension(0);
            const auto W = det.dimension(1);
            float tags_nr = 1e-3;

            tags.setZero();
            for(auto i=0; i<num_keypoints; ++i) {
                if(keypoints(i,2)>0.01) {
                    const int x = keypoints(i,0);
                    const int y = keypoints(i,1);

                    tags.chip(i,0) = tag.chip(y,0).chip(x,0).chip(i,0);
                    tags_nr += 1;
                }
            }

            if((tags_nr<2) || (tags_nr>=num_keypoints))
                return res_keypoints;

            Tensor1d_t _tags_mean0 = tags.sum(Eigen::array<int,1>({0}));
            Tensor1d_t _tags_mean = _tags_mean0/_tags_mean0.constant(tags_nr);
            Eigen::array<int,3> three_dims{{1, 1, tag_C}};
            Tensor3d_t _prev_tag = _tags_mean.reshape(three_dims);
            Eigen::array<int, 3> bcast({det.dimension(0), det.dimension(1),1});
            Tensor3d_t prev_tag = _prev_tag.broadcast(bcast);
            vector<tuple<float,float,float>> ans;

            for(auto i=0; i<num_keypoints; ++i) {
                Tensor2d_t tmp_det = det.chip(i,2);
                Tensor2d_t _tt0;

                if(tag.dimension(2)>1) {
                    _tt0 = (tag.chip(i,2)-prev_tag).square().sum(Eigen::array<int,1>({2})).pow(0.5);
                } else {
                    _tt0 = (tag.chip(i,2)-prev_tag).abs().reshape(Eigen::array<int,2>({det.dimension(0),det.dimension(1)}));
                }

                Tensor2d_t _tt = _tt0+_tt0.constant(0.5);
                Tensor2d_t tt = _tt.cast<int>().cast<float>();
                Tensor2d_t tmp_det2 = tmp_det-tt;
                //Tensor2d_t tmp_det2 = tmp_det-tt*tt.constant(100);
                int xx,yy;
                float y,x;

                tie(yy,xx) = argmax(tmp_det2);

                auto val = tmp_det(yy,xx);

                if(_tt0(yy,xx)>=1.0)
                    val = 0;

                y = yy;
                x = xx;

                x += 0.5;
                y += 0.5;

                if(tmp_det(yy,min<int>(xx+1,W-1))>tmp_det(yy,max<int>(xx-1,0))) {
                    x += 0.25;
                } else {
                    x -= 0.25;
                }
                if(tmp_det(min<int>(yy+1,H-1),xx)>tmp_det(max<int>(yy-1,0),xx)) {
                    y += 0.25;
                } else {
                    y -= 0.25;
                }
                ans.emplace_back(make_tuple(x,y,val));
            }
            for(auto i=0; i<num_keypoints; ++i) {
                if((std::get<2>(ans[i])>0.1) && (keypoints(i,2)<=0)) {
                    res_keypoints(i,0) = std::get<0>(ans[i]);
                    res_keypoints(i,1) = std::get<1>(ans[i]);
                    res_keypoints(i,2) = std::get<2>(ans[i]);
                }
            }
            return res_keypoints;
        }

        pair<int,int> argmax(const Tensor2d_t& tensor) {
            int row = 0;
            int col = 0;
            float max_val = tensor(0,0);
            for(auto i=0; i<tensor.dimension(0); ++i) {
                for(auto j=0; j<tensor.dimension(1); ++j) {
                    if(tensor(i,j)>max_val) {
                        row = i;
                        col = j;
                        max_val = tensor(i,j);
                    }
                }
            }
            return make_pair(row,col);
        }
};
REGISTER_KERNEL_BUILDER(Name("HrNetRefine").Device(DEVICE_CPU).TypeConstraint<float>("T"), HRNetRefine<CPUDevice, float>);


#ifndef INVKIN_H_INCLUDED
#define INVKIN_H_INCLUDED

#include <Eigen/Core>
#include <Eigen/Dense>
#include <cmath>
#include <fstream>
#include <iostream>
#include <string>
#include "pinocchio/math/rpy.hpp"
#include "pinocchio/spatial/explog.hpp"

class InvKin {
  /* Planner that outputs current and future locations of footsteps, the
     reference trajectory of the base and the position, velocity, acceleration
     commands for feet in swing phase based on the reference velocity given by
     the user and the current position/velocity of the base */

  /*private:
   // Inputs of the constructor
   double dt;       // Time step of the contact sequence (time step of the MPC)
   double dt_tsid;  // Time step of TSID
   double T_gait;   // Gait period
   double T_mpc;    // MPC period (prediction horizon)
   double h_ref;    // Reference height for the trunk
   int k_mpc;       // Number of TSID iterations for one iteration of the MPC
   bool on_solo8;   //  Whether we are working on solo8 or not

   // Predefined quantities
   double k_feedback = 0.03;  // Feedback gain for the feedback term of the
   planner double g = 9.81;           // Value of the gravity acceleartion
   double L = 0.155;          // Value of the maximum allowed deviation due to
   leg length bool is_static = false;    // Flag for static gait

   // Number of time steps in the prediction horizon
   int n_steps; // T_mpc / time step of the MPC

   // Feet index vector
   std::vector<int> feet;
   std::vector<double> t0s;
   double t_remaining = 0.0;
   double t_swing[4] = {0.0, 0.0, 0.0, 0.0};

   // Constant sized matrices
   Eigen::MatrixXd fsteps = Eigen::MatrixXd::Zero(N0_gait, 13);
   Eigen::Matrix<double, 3, 4> shoulders = Eigen::Matrix<double, 3, 4>::Zero();
   // Position of shoulders in local frame Eigen::Matrix<double, 19, 1> q_static
   = Eigen::Matrix<double, 19, 1>::Zero(); Eigen::Matrix<double, 3, 1>
   RPY_static = Eigen::Matrix<double, 3, 1>::Zero(); Eigen::Matrix<double, 1,
   12> o_feet_contact = Eigen::Matrix<double, 1, 12>::Zero();  // Feet matrix in
   world frame Eigen::Matrix<double, 3, 4> next_footstep = Eigen::Matrix<double,
   3, 4>::Zero();  // To store the result of the compute_next_footstep function
   Eigen::Matrix<double, 3, 3> R =
       Eigen::Matrix<double, 3, 3>::Zero();  // Predefined matrices for
   compute_footstep function Eigen::Matrix<double, 3, 3> R_1 =
       Eigen::Matrix<double, 3, 3>::Zero();  // Predefined matrices for
   compute_next_footstep function Eigen::Matrix<double, 3, 3> R_2 =
   Eigen::Matrix<double, 3, 3>::Zero(); Eigen::Matrix<double, N0_gait, 1> dt_cum
   = Eigen::Matrix<double, N0_gait, 1>::Zero(); Eigen::Matrix<double, N0_gait,
   1> angle = Eigen::Matrix<double, N0_gait, 1>::Zero(); Eigen::Matrix<double,
   N0_gait, 1> dx = Eigen::Matrix<double, N0_gait, 1>::Zero();
   Eigen::Matrix<double, N0_gait, 1> dy = Eigen::Matrix<double, N0_gait,
   1>::Zero(); Eigen::Matrix<double, 3, 1> q_tmp = Eigen::Matrix<double, 3,
   1>::Zero(); Eigen::Matrix<double, 3, 1> q_dxdy = Eigen::Matrix<double, 3,
   1>::Zero(); Eigen::Matrix<double, 3, 1> RPY = Eigen::Matrix<double, 3,
   1>::Zero(); Eigen::Matrix<double, 3, 1> b_v_cur = Eigen::Matrix<double, 3,
   1>::Zero(); Eigen::Matrix<double, 6, 1> b_v_ref = Eigen::Matrix<double, 6,
   1>::Zero(); Eigen::Matrix<double, 3, 1> cross = Eigen::Matrix<double, 3,
   1>::Zero(); Eigen::Matrix<double, 6, 1> vref_in = Eigen::Matrix<double, 6,
   1>::Zero();

   Eigen::Matrix<double, N0_gait, 5> gait_p = Eigen::Matrix<double, N0_gait,
   5>::Zero();  // Past gait Eigen::MatrixXd gait_f =
   Eigen::MatrixXd::Zero(N0_gait, 5);                                // Current
   and future gait Eigen::Matrix<double, N0_gait, 5> gait_f_des =
   Eigen::Matrix<double, N0_gait, 5>::Zero();  // Future desired gait

   // Time interval vector
   Eigen::Matrix<double, 1, Eigen::Dynamic> dt_vector;

   // Reference trajectory matrix of size 12 by (1 + N)  with the current state
   of
   // the robot in column 0 and the N steps of the prediction horizon in the
   others Eigen::MatrixXd xref;

   // Foot trajectory generator
   double max_height_feet = 0.05;  // * (1000/312.5);  // height * correction
   coefficient double t_lock_before_touchdown = 0.07; std::vector<TrajGen>
   myTrajGen;

   // Variables for foot trajectory generator
   int i_end_gait = 0;
   Eigen::Matrix<double, 1, 4> t_stance =
       Eigen::Matrix<double, 1, 4>::Zero();  // Total duration of current stance
   phase for each foot Eigen::Matrix<double, 2, 4> footsteps_target =
   Eigen::Matrix<double, 2, 4>::Zero(); Eigen::MatrixXd goals =
   Eigen::MatrixXd::Zero(3, 4);   // Store 3D target position for feet
   Eigen::MatrixXd vgoals = Eigen::MatrixXd::Zero(3, 4);  // Store 3D target
   velocity for feet Eigen::MatrixXd agoals = Eigen::MatrixXd::Zero(3, 4);  //
   Store 3D target acceleration for feet Eigen::Matrix<double, 6, 4> mgoals =
       Eigen::Matrix<double, 6, 4>::Zero();  // Storage variable for the
   trajectory generator

   Eigen::Matrix<double, 11, 4> res_gen = Eigen::Matrix<double, 11, 4>::Zero();
   // Result of the generator
   */

 private:
  // Inputs of the constructor
  double dt;  // Time step of the contact sequence (time step of the MPC)

  // Matrices initialisation
  Eigen::Matrix<double, 4, 3> feet_position_ref = Eigen::Matrix<double, 4, 3>::Zero();
  Eigen::Matrix<double, 4, 3> feet_velocity_ref = Eigen::Matrix<double, 4, 3>::Zero();
  Eigen::Matrix<double, 4, 3> feet_acceleration_ref = Eigen::Matrix<double, 4, 3>::Zero();
  Eigen::Matrix<double, 1, 4> flag_in_contact = Eigen::Matrix<double, 1, 4>::Zero();
  Eigen::Matrix<double, 3, 3> base_orientation_ref = Eigen::Matrix<double, 3, 3>::Zero();
  Eigen::Matrix<double, 1, 3> base_angularvelocity_ref = Eigen::Matrix<double, 1, 3>::Zero();
  Eigen::Matrix<double, 1, 3> base_angularacceleration_ref = Eigen::Matrix<double, 1, 3>::Zero();
  Eigen::Matrix<double, 1, 3> base_position_ref = Eigen::Matrix<double, 1, 3>::Zero();
  Eigen::Matrix<double, 1, 3> base_linearvelocity_ref = Eigen::Matrix<double, 1, 3>::Zero();
  Eigen::Matrix<double, 1, 3> base_linearacceleration_ref = Eigen::Matrix<double, 1, 3>::Zero();
  Eigen::Matrix<double, 6, 1> x_ref = Eigen::Matrix<double, 6, 1>::Zero();
  Eigen::Matrix<double, 6, 1> x = Eigen::Matrix<double, 6, 1>::Zero();
  Eigen::Matrix<double, 6, 1> dx_ref = Eigen::Matrix<double, 6, 1>::Zero();
  Eigen::Matrix<double, 6, 1> dx = Eigen::Matrix<double, 6, 1>::Zero();
  Eigen::Matrix<double, 18, 18> J = Eigen::Matrix<double, 18, 18>::Zero();
  Eigen::Matrix<double, 18, 18> invJ = Eigen::Matrix<double, 18, 18>::Zero();
  Eigen::Matrix<double, 1, 18> acc = Eigen::Matrix<double, 1, 18>::Zero();
  Eigen::Matrix<double, 1, 18> x_err = Eigen::Matrix<double, 1, 18>::Zero();
  Eigen::Matrix<double, 1, 18> dx_r = Eigen::Matrix<double, 1, 18>::Zero();

  Eigen::Matrix<double, 4, 3> pfeet_err = Eigen::Matrix<double, 4, 3>::Zero();
  Eigen::Matrix<double, 4, 3> vfeet_ref = Eigen::Matrix<double, 4, 3>::Zero();
  Eigen::Matrix<double, 4, 3> afeet = Eigen::Matrix<double, 4, 3>::Zero();
  Eigen::Matrix<double, 1, 3> e_basispos = Eigen::Matrix<double, 1, 3>::Zero();
  Eigen::Matrix<double, 1, 3> abasis = Eigen::Matrix<double, 1, 3>::Zero();
  Eigen::Matrix<double, 1, 3> e_basisrot = Eigen::Matrix<double, 1, 3>::Zero();
  Eigen::Matrix<double, 1, 3> awbasis = Eigen::Matrix<double, 1, 3>::Zero();

  Eigen::MatrixXd ddq = Eigen::MatrixXd::Zero(18, 1);
  Eigen::MatrixXd q_step = Eigen::MatrixXd::Zero(18, 1);
  Eigen::MatrixXd dq_cmd = Eigen::MatrixXd::Zero(18, 1);

  // Gains
  double Kp_base_orientation = 100.0;
  double Kd_base_orientation = 2.0 * std::sqrt(Kp_base_orientation);

  double Kp_base_position = 100.0;
  double Kd_base_position = 2.0 * std::sqrt(Kp_base_position);

  double Kp_flyingfeet = 1000.0;
  double Kd_flyingfeet = 5.0 * std::sqrt(Kp_flyingfeet);

 public:
  InvKin();
  InvKin(double dt_in);

  Eigen::Matrix<double, 1, 3> cross3(Eigen::Matrix<double, 1, 3> left, Eigen::Matrix<double, 1, 3> right);

  Eigen::MatrixXd refreshAndCompute(const Eigen::MatrixXd &x_cmd, const Eigen::MatrixXd &contacts,
                                    const Eigen::MatrixXd &goals, const Eigen::MatrixXd &vgoals, const Eigen::MatrixXd &agoals,
                                    const Eigen::MatrixXd &posf, const Eigen::MatrixXd &vf, const Eigen::MatrixXd &wf, const Eigen::MatrixXd &af,
                                    const Eigen::MatrixXd &Jf, const Eigen::MatrixXd &posb, const Eigen::MatrixXd &rotb, const Eigen::MatrixXd &vb,
                                    const Eigen::MatrixXd &ab, const Eigen::MatrixXd &Jb);
  Eigen::MatrixXd computeInvKin(const Eigen::MatrixXd &posf, const Eigen::MatrixXd &vf, const Eigen::MatrixXd &wf, const Eigen::MatrixXd &af,
                                const Eigen::MatrixXd &Jf, const Eigen::MatrixXd &posb, const Eigen::MatrixXd &rotb, const Eigen::MatrixXd &vb, const Eigen::MatrixXd &ab,
                                const Eigen::MatrixXd &Jb);
  Eigen::MatrixXd get_q_step();
  Eigen::MatrixXd get_dq_cmd();
  /*void Print();

  int create_walk();
  int create_trot();
  int create_gait_f();
  int roll(int k);
  int compute_footsteps(Eigen::MatrixXd q_cur, Eigen::MatrixXd v_cur,
  Eigen::MatrixXd v_ref); double get_stance_swing_duration(int i, int j, double
  value); int compute_next_footstep(int i, int j); int
  getRefStates(Eigen::MatrixXd q, Eigen::MatrixXd v, Eigen::MatrixXd vref,
  double z_average); int update_target_footsteps(); int
  update_trajectory_generator(int k, double h_estim); int run_planner(int k,
  const Eigen::MatrixXd &q, const Eigen::MatrixXd &v, const Eigen::MatrixXd
  &b_vref, double h_estim, double z_average);*/

  // Accessors (to retrieve C data from Python)
  /*Eigen::MatrixXd get_xref();
  Eigen::MatrixXd get_fsteps();
  Eigen::MatrixXd get_gait();
  Eigen::MatrixXd get_goals();
  Eigen::MatrixXd get_vgoals();
  Eigen::MatrixXd get_agoals();*/
};

template<typename _Matrix_Type_>
_Matrix_Type_ pseudoInverse(const _Matrix_Type_ &a, double epsilon = std::numeric_limits<double>::epsilon())
{
	Eigen::JacobiSVD< _Matrix_Type_ > svd(a ,Eigen::ComputeThinU | Eigen::ComputeThinV);
	double tolerance = epsilon * static_cast<double>(std::max(a.cols(), a.rows())) *svd.singularValues().array().abs()(0);
	return svd.matrixV() *  (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0).matrix().asDiagonal() * svd.matrixU().adjoint();
}

#endif  // INVKIN_H_INCLUDED


/* output_encoder_test.cc
   Jeremy Barnes, 18 May 2010
   Copyright (c) 2010 Jeremy Barnes.  All rights reserved.

   Test of the output encoder.
*/

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK
#define JML_TESTING_PERCEPTRON

#include <boost/test/unit_test.hpp>
#include <boost/thread.hpp>
#include <boost/thread/barrier.hpp>
#include <boost/bind.hpp>
#include <vector>
#include <stdint.h>
#include <iostream>

#include "jml/neural/output_encoder.h"
#include "jml/neural/dense_layer.h"
#include "jml/utils/smart_ptr_utils.h"
#include "jml/utils/vector_utils.h"
#include "jml/arch/exception_handler.h"

using namespace ML;
using namespace std;

using boost::unit_test::test_suite;

BOOST_AUTO_TEST_CASE( test_output_encoder )
{
    Output_Encoder encoder;
}


//
// Copyright (c) 2019 INRIA
//

#ifndef __pinocchio_multibody_data_serialization_hpp__
#define __pinocchio_multibody_data_serialization_hpp__

#include <boost/serialization/variant.hpp>
#include <boost/serialization/vector.hpp>

#include "pinocchio/serialization/aligned-vector.hpp"
#include "pinocchio/serialization/spatial.hpp"
#include "pinocchio/serialization/eigen.hpp"
#include "pinocchio/serialization/joints.hpp"
#include "pinocchio/serialization/frame.hpp"

#define PINOCCHIO_MAKE_DATA_NVP(ar,data,field_name) \
  ar & make_nvp(#field_name,data.field_name)

namespace boost
{
  namespace serialization
  {
    template<class Archive, typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
    void serialize(Archive & ar,
                   pinocchio::DataTpl<Scalar,Options,JointCollectionTpl> & data,
                   const unsigned int /*version*/)
    {
      PINOCCHIO_MAKE_DATA_NVP(ar,data,joints);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,a);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,oa);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,a_gf);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,oa_gf);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,v);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,ov);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,f);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,of);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,h);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,oh);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,oMi);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,liMi);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,tau);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,nle);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,g);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,oMf);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,Ycrb);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,dYcrb);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,M);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,Minv);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,C);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,dHdq);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,dFdq);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,dFdv);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,dFda);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,SDinv);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,UDinv);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,IS);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,vxI);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,Ivx);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,oYcrb);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,doYcrb);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,ddq);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,Yaba);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,u);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,Ag);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,dAg);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,hg);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,dhg);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,Ig);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,Fcrb);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,lastChild);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,nvSubtree);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,start_idx_v_fromRow);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,end_idx_v_fromRow);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,U);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,D);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,Dinv);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,parents_fromRow);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,supports_fromRow);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,nvSubtree_fromRow);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,J);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,dJ);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,dVdq);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,dAdq);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,dAdv);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,dtau_dq);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,dtau_dv);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,ddq_dq);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,ddq_dv);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,iMf);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,com);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,vcom);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,acom);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,mass);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,Jcom);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,kinetic_energy);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,potential_energy);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,JMinvJt);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,lambda_c);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,torque_residual);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,dq_after);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,impulse_c);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,staticRegressor);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,bodyRegressor);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,jointTorqueRegressor);
      PINOCCHIO_MAKE_DATA_NVP(ar,data,kinematic_hessians);
    }

  } // namespace serialization
} // namespace boost

#undef PINOCCHIO_MAKE_DATA_NVP

#endif // ifndef __pinocchio_multibody_data_serialization_hpp__


// Copyright (c) 2016, Huang-Ming Huang,  Object Computing, Inc.
// All rights reserved.
//
// This file is part of mFAST.
// See the file license.txt for licensing information.
#include <cmath>
#include <cfloat>

#include <iostream>

#include <boost/multiprecision/number.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

template class boost::multiprecision::backends::cpp_dec_float<18>;

#if BOOST_VERSION >= 106200

#include <boost/functional/hash.hpp>

#if defined(_MSC_VER)
#pragma warning(disable: 4667)  //  no function template defined that matches forced instantiation
#else
// This doesn't work in MSVC, but apparently it isn't needed.
template void boost::hash_combine<boost::multiprecision::backends::cpp_dec_float<18u, int, void>::enum_fpclass_type>(unsigned long&, boost::multiprecision::backends::cpp_dec_float<18u, int, void>::enum_fpclass_type const&);
#endif
template void boost::hash_combine<bool>(unsigned long&, bool const&);
template void boost::hash_combine<int>(unsigned long&, int const&);
template void boost::hash_combine<unsigned int>(unsigned long&, unsigned int const&);
#endif


/*=============================================================================
    Copyright (c) 2001-2011 Joel de Guzman
    Copyright (c) 2001-2011 Hartmut Kaiser
    Copyright (c) 2014 Thomas Bernard

    Distributed under the Boost Software License, Version 1.0. (See accompanying
    file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
==============================================================================*/
#if !defined(SPIRIT_X3_REPEAT_APRIL_16_2014_0848AM)
#define SPIRIT_X3_REPEAT_APRIL_16_2014_0848AM

#include <boost/spirit/home/x3/core/parser.hpp>
#include <boost/spirit/home/x3/operator/kleene.hpp>

namespace boost { namespace spirit { namespace x3 { namespace detail
{
    template <typename T>
    struct exact_count // handles repeat(exact)[p]
    {
        typedef T type;
        bool got_max(T i) const { return i >= exact_value; }
        bool got_min(T i) const { return i >= exact_value; }

        T const exact_value;
    };

    template <typename T>
    struct finite_count // handles repeat(min, max)[p]
    {
        typedef T type;
        bool got_max(T i) const { return i >= max_value; }
        bool got_min(T i) const { return i >= min_value; }

        T const min_value;
        T const max_value;
    };

    template <typename T>
    struct infinite_count // handles repeat(min, inf)[p]
    {
        typedef T type;
        bool got_max(T /*i*/) const { return false; }
        bool got_min(T i) const { return i >= min_value; }

        T const min_value;
    };
}}}}

namespace boost { namespace spirit { namespace x3
{
    template<typename Subject, typename RepeatCountLimit>
    struct repeat_directive : unary_parser<Subject, repeat_directive<Subject,RepeatCountLimit>>
    {
        typedef unary_parser<Subject, repeat_directive<Subject,RepeatCountLimit>> base_type;
        static bool const is_pass_through_unary = true;
        static bool const handles_container = true;

        repeat_directive(Subject const& subject, RepeatCountLimit const& repeat_limit_)
          : base_type(subject)
          , repeat_limit(repeat_limit_)
        {}

        template<typename Iterator, typename Context
          , typename RContext, typename Attribute>
        bool parse(
            Iterator& first, Iterator const& last
          , Context const& context, RContext& rcontext, Attribute& attr) const
        {
            Iterator local_iterator = first;
            typename RepeatCountLimit::type i{};
            for (/**/; !repeat_limit.got_min(i); ++i)
            {
                if (!detail::parse_into_container(
                      this->subject, local_iterator, last, context, rcontext, attr))
                    return false;
            }

            first = local_iterator;
            // parse some more up to the maximum specified
            for (/**/; !repeat_limit.got_max(i); ++i)
            {
                if (!detail::parse_into_container(
                      this->subject, first, last, context, rcontext, attr))
                    break;
            }
            return true;
        }

        RepeatCountLimit repeat_limit;
    };

    // Infinite loop tag type
    struct inf_type {};
    const inf_type inf = inf_type();

    struct repeat_gen
    {
        template<typename Subject>
        auto operator[](Subject const& subject) const
        {
            return *as_parser(subject);
        }

        template <typename T>
        struct repeat_gen_lvl1
        {
            repeat_gen_lvl1(T&& repeat_limit_)
              : repeat_limit(repeat_limit_)
            {}

            template<typename Subject>
            repeat_directive< typename extension::as_parser<Subject>::value_type, T>
            operator[](Subject const& subject) const
            {
                return { as_parser(subject),repeat_limit };
            }

            T repeat_limit;
        };

        template <typename T>
        repeat_gen_lvl1<detail::exact_count<T>>
        operator()(T const exact) const
        {
            return { detail::exact_count<T>{exact} };
        }

        template <typename T>
        repeat_gen_lvl1<detail::finite_count<T>>
        operator()(T const min_val, T const max_val) const
        {
            return { detail::finite_count<T>{min_val,max_val} };
        }

        template <typename T>
        repeat_gen_lvl1<detail::infinite_count<T>>
        operator()(T const min_val, inf_type const &) const
        {
            return { detail::infinite_count<T>{min_val} };
        }
    };

    auto const repeat = repeat_gen{};
}}}

namespace boost { namespace spirit { namespace x3 { namespace traits
{
    template <typename Subject, typename RepeatCountLimit, typename Context>
    struct attribute_of<x3::repeat_directive<Subject,RepeatCountLimit>, Context>
      : build_container<typename attribute_of<Subject, Context>::type> {};
}}}}

#endif


// Copyright © 2016-2019 Thomas Nagler and Thibault Vatter
//
// This file is part of the vinecopulib library and licensed under the terms of
// the MIT license. For a copy, see the LICENSE file in the root directory of
// vinecopulib or https://vinecopulib.github.io/vinecopulib/.

#pragma once

#include <memory>

#include <Eigen/Dense>
#include <vinecopulib/bicop/family.hpp>

namespace vinecopulib {
//! @brief An abstract class for bivariate copula families
//!
//! This class is used in the implementation underlying the Bicop class.
//! Users should not use AbstractBicop or derived classes directly, but
//! always work with the Bicop interface.
class AbstractBicop
{
    friend class Bicop;

public:
    virtual ~AbstractBicop() = 0;

protected:
    // Factories
    static std::shared_ptr <AbstractBicop> create(
        BicopFamily family = BicopFamily::indep,
        const Eigen::MatrixXd &parameters = Eigen::MatrixXd());

    // Getters and setters
    BicopFamily get_family() const;

    std::string get_family_name() const;

    double get_loglik() const;

    void set_loglik(const double loglik = NAN);

    virtual Eigen::MatrixXd get_parameters() const = 0;

    virtual Eigen::MatrixXd get_parameters_lower_bounds() const = 0;

    virtual Eigen::MatrixXd get_parameters_upper_bounds() const = 0;

    virtual void set_parameters(const Eigen::MatrixXd &parameters) = 0;

    virtual void flip() = 0;

    // Virtual methods
    virtual void fit(const Eigen::Matrix<double, Eigen::Dynamic, 2> &data,
                     std::string method,
                     double mult,
                     const Eigen::VectorXd& weights) = 0;

    virtual double calculate_npars() = 0;

    virtual double parameters_to_tau(const Eigen::MatrixXd &parameters) = 0;

    Eigen::VectorXd pdf(const Eigen::Matrix<double, Eigen::Dynamic, 2> &u);

    double loglik(const Eigen::Matrix<double, Eigen::Dynamic, 2> &u,
              const Eigen::VectorXd weights = Eigen::VectorXd());

    virtual Eigen::VectorXd pdf_raw(
        const Eigen::Matrix<double, Eigen::Dynamic, 2> &u) = 0;

    virtual Eigen::VectorXd cdf(
        const Eigen::Matrix<double, Eigen::Dynamic, 2> &u) = 0;

    virtual Eigen::VectorXd hfunc1(
        const Eigen::Matrix<double, Eigen::Dynamic, 2> &u) = 0;

    virtual Eigen::VectorXd hfunc2(
        const Eigen::Matrix<double, Eigen::Dynamic, 2> &u) = 0;

    virtual Eigen::VectorXd hinv1(
        const Eigen::Matrix<double, Eigen::Dynamic, 2> &u) = 0;

    virtual Eigen::VectorXd hinv2(
        const Eigen::Matrix<double, Eigen::Dynamic, 2> &u) = 0;

    virtual Eigen::MatrixXd tau_to_parameters(const double &tau) = 0;
    Eigen::MatrixXd no_tau_to_parameters(const double &);

    // Misc methods
    Eigen::VectorXd hinv1_num(
        const Eigen::Matrix<double, Eigen::Dynamic, 2> &u);

    Eigen::VectorXd hinv2_num(
        const Eigen::Matrix<double, Eigen::Dynamic, 2> &u);

    // Data members
    BicopFamily family_;
    double loglik_;
};

//! A shared pointer to an object of class AbstracBicop.
typedef std::shared_ptr <AbstractBicop> BicopPtr;

}

#include <vinecopulib/bicop/implementation/abstract.ipp>


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

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

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/slice.hpp>
#include <boost/simd/detail/dispatch/meta/upgrade.hpp>

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

  BOOST_DISPATCH_OVERLOAD ( sum_
                          , (typename A0)
                          , bs::avx2_
                          , bs::pack_<bd::ints64_<A0>, bs::avx_>
                         )
  {
    BOOST_FORCEINLINE typename A0::value_type operator()(const A0 & a0) const BOOST_NOEXCEPT
    {
      return sum(slice_low(a0) + slice_high(a0));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( sum_
                          , (typename A0)
                          , bs::avx2_
                          , bs::pack_<bd::ints32_<A0>, bs::avx_>
                         )
  {
    BOOST_FORCEINLINE typename A0::value_type operator()(const A0 & a0) const BOOST_NOEXCEPT
    {
      A0  acc = _mm256_add_epi32(a0, _mm256_srli_si256(a0, 8));
          acc = _mm256_add_epi32(acc, _mm256_srli_si256(acc, 4));
      return _mm256_extract_epi32(acc, 0) +  _mm256_extract_epi32(acc, 4);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( sum_
                          , (typename A0)
                          , bs::avx2_
                          , bs::pack_<bd::ints16_<A0>, bs::avx_>
                         )
  {
    BOOST_FORCEINLINE typename A0::value_type operator()(const A0 & a0) const BOOST_NOEXCEPT
    {
      __m256i s = _mm256_hadd_epi16(a0, a0);
              s = _mm256_hadd_epi16(s, s);
              s = _mm256_hadd_epi16(s, s);
      return _mm256_extract_epi16(s, 0) + _mm256_extract_epi16(s, 15);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( sum_
                          , (typename A0)
                          , bs::avx2_
                          , bs::pack_<bd::ints8_<A0>, bs::avx_>
                         )
  {
    BOOST_FORCEINLINE typename A0::value_type operator()(const A0 & a0) const BOOST_NOEXCEPT
    {
      __m256i z = _mm256_set1_epi8(0);
      bd::upgrade_t<A0> s = _mm256_add_epi16( _mm256_unpacklo_epi8(a0, z)
                                            , _mm256_unpackhi_epi8(a0, z)
                                            );
      return sum(s);
    }
  };
} } }

#endif


// ======================================================================
// \title  FrameTransformImpl.hpp
// \author gene
// \brief  hpp file for FrameTransform component implementation class
//
// \copyright
// Copyright 2009-2015, by the California Institute of Technology.
// ALL RIGHTS RESERVED.  United States Government Sponsorship
// acknowledged. Any commercial use must be negotiated with the Office
// of Technology Transfer at the California Institute of Technology.
//
// This software may be subject to U.S. export control laws and
// regulations.  By accepting this document, the user agrees to comply
// with all U.S. export laws and regulations.  User has the
// responsibility to obtain export licenses, or other export authority
// as may be required before exporting such information to foreign
// countries or providing access to foreign persons.
// ======================================================================

#ifndef FrameTransform_HPP
#define FrameTransform_HPP

#include "Gnc/Utils/FrameTransform/FrameTransformComponentAc.hpp"

#include <Eigen/Geometry>

namespace Gnc {

  class FrameTransformComponentImpl :
    public FrameTransformComponentBase
  {

    public:

      // ----------------------------------------------------------------------
      // Construction, initialization, and destruction
      // ----------------------------------------------------------------------

      //! Construct object FrameTransform
      //!
      FrameTransformComponentImpl(
#if FW_OBJECT_NAMES == 1
          const char *const compName /*!< The component name*/
#else
          void
#endif
      );

      //! Initialize object FrameTransform
      //!
      void init(
          const NATIVE_INT_TYPE instance = 0 /*!< The instance number*/
      );

      //! Destroy object FrameTransform
      //!
      ~FrameTransformComponentImpl(void);

      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    PRIVATE:
      void parameterUpdated(FwPrmIdType id /*!< The parameter ID*/);

      void parametersLoaded();

      // ----------------------------------------------------------------------
      // Handler implementations for user-defined typed input ports
      // ----------------------------------------------------------------------

      //! Handler implementation for prmTrigger
      //!
      void prmTrigger_handler(
          const NATIVE_INT_TYPE portNum, /*!< The port number*/
          FwPrmIdType dummy
      );

      //! Handler implementation for odomInA
      //!
      void odomInA_handler(
          const NATIVE_INT_TYPE portNum, /*!< The port number*/
          ROS::nav_msgs::OdometryAccel &Odometry
      );

      //! Handler implementation for odomInB
      //!
      void odomInB_handler(
          const NATIVE_INT_TYPE portNum, /*!< The port number*/
          ROS::nav_msgs::OdometryAccel &Odometry
      );

    PRIVATE:

      // ----------------------------------------------------------------------
      // Command handler implementations
      // ----------------------------------------------------------------------

      //! Implementation for FTFO_InitParams command handler
      //!
      void FTFO_InitParams_cmdHandler(
          const FwOpcodeType opCode, /*!< The opcode*/
          const U32 cmdSeq /*!< The command sequence number*/
      );

      bool paramsInited;

      Eigen::Affine3d a_X_b; // takes point in b frame to a frame
      Eigen::Affine3d b_X_a; // takes point in a frame to b frame

      bool doRotX; // see params.xml
      bool doRotV; // see params.xml

    };

} // end namespace Gnc

#endif


/**
 * @file hmm_generate_test.cpp
 * @author Daivik Nema
 *
 * Test mlpackMain() of hmm_generate_main.cpp
 *
 * 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.
 */
#include <string>

#define BINDING_TYPE BINDING_TYPE_TEST
static const std::string testName = "HMMGenerate";

#include <mlpack/core.hpp>
#include <mlpack/core/util/mlpack_main.hpp>
#include "test_helper.hpp"
#include <mlpack/methods/hmm/hmm_model.hpp>
#include <mlpack/methods/hmm/hmm.hpp>
#include <mlpack/methods/hmm/hmm_generate_main.cpp>

#include <boost/test/unit_test.hpp>
#include "../test_tools.hpp"

#include "hmm_test_utils.hpp"

using namespace mlpack;

struct HMMGenerateTestFixture
{
 public:
  HMMGenerateTestFixture()
  {
    // Cache in the options for this program.
    CLI::RestoreSettings(testName);
  }

  ~HMMGenerateTestFixture()
  {
    // Clear the settings.
    bindings::tests::CleanMemory();
    CLI::ClearSettings();
  }
};

BOOST_FIXTURE_TEST_SUITE(HMMGenerateMainTest, HMMGenerateTestFixture);

BOOST_AUTO_TEST_CASE(HMMGenerateDiscreteHMMCheckDimensionsTest)
{
  // Load data to train a discrete HMM model with.
  arma::mat inp;
  data::Load("obs1.csv", inp);
  std::vector<arma::mat> trainSeq = {inp};

  // Initialize and train a discrete HMM model.
  HMMModel* h = new HMMModel(DiscreteHMM);
  h->PerformAction<InitHMMModel, std::vector<arma::mat>>(&trainSeq);
  h->PerformAction<TrainHMMModel, std::vector<arma::mat>>(&trainSeq);

  // Now that we have a trained HMM model, we can use it to generate a sequence
  // of states and observations - using the hmm_generate utility.
  // Load the input model to be used for inference and the length of sequence
  // to be generated.
  int length = 3;
  SetInputParam("model", h);
  SetInputParam("length", length);

  // Call to hmm_generate_main.
  mlpackMain();

  // Get the generated observation sequence. Ensure that the generated sequence
  // has the correct length (as provided in the input).
  arma::mat obsSeq = CLI::GetParam<arma::mat>("output");
  BOOST_REQUIRE_EQUAL(obsSeq.n_cols, (size_t)length);
  BOOST_REQUIRE_EQUAL(obsSeq.n_rows, (size_t)1);
  BOOST_REQUIRE_EQUAL(obsSeq.n_elem, (size_t)length);

  // Get the generated state sequence. Ensure that the generated sequence
  // has the correct length (as provided in the input).
  arma::Mat<size_t> stateSeq = CLI::GetParam<arma::Mat<size_t>>("state");
  BOOST_REQUIRE_EQUAL(stateSeq.n_cols, (size_t)length);
  BOOST_REQUIRE_EQUAL(stateSeq.n_rows, (size_t)1);
  BOOST_REQUIRE_EQUAL(stateSeq.n_elem, (size_t)length);
}

BOOST_AUTO_TEST_CASE(HMMGenerateGaussianHMMCheckDimensionsTest)
{
  // Load data to train a gaussian HMM model with.
  arma::mat inp;
  data::Load("obs1.csv", inp);
  std::vector<arma::mat> trainSeq = {inp};

  // Initialize and train a gaussian HMM model.
  HMMModel* h = new HMMModel(GaussianHMM);
  h->PerformAction<InitHMMModel, std::vector<arma::mat>>(&trainSeq);
  h->PerformAction<TrainHMMModel, std::vector<arma::mat>>(&trainSeq);

  // Now that we have a trained HMM model, we can use it to generate a sequence
  // of states and observations - using the hmm_generate utility.
  // Load the input model to be used for inference and the length of sequence
  // to be generated.
  int length = 3;
  SetInputParam("model", h);
  SetInputParam("length", length);

  // Call to hmm_generate_main.
  mlpackMain();

  // Get the generated observation sequence. Ensure that the generated sequence
  // has the correct length (as provided in the input).
  arma::mat obsSeq = CLI::GetParam<arma::mat>("output");
  BOOST_REQUIRE_EQUAL(obsSeq.n_cols, (size_t)length);
  BOOST_REQUIRE_EQUAL(obsSeq.n_rows, (size_t)1);
  BOOST_REQUIRE_EQUAL(obsSeq.n_elem, (size_t)length);

  // Get the generated state sequence. Ensure that the generated sequence
  // has the correct length (as provided in the input).
  arma::Mat<size_t> stateSeq = CLI::GetParam<arma::Mat<size_t>>("state");
  BOOST_REQUIRE_EQUAL(stateSeq.n_cols, (size_t)length);
  BOOST_REQUIRE_EQUAL(stateSeq.n_rows, (size_t)1);
  BOOST_REQUIRE_EQUAL(stateSeq.n_elem, (size_t)length);
}

BOOST_AUTO_TEST_CASE(HMMGenerateGMMHMMCheckDimensionsTest)
{
  // Initialize and train a GMM HMM model.
  HMMModel* h = new HMMModel(GaussianMixtureModelHMM);
  *(h->GMMHMM()) = HMM<GMM>(2, GMM(2, 2));

  // Manually set the components.
  h->GMMHMM()->Transition() = arma::mat("0.40 0.60; 0.60 0.40");
  h->GMMHMM()->Emission().resize(2);
  h->GMMHMM()->Emission()[0] = GMM(2, 2);
  h->GMMHMM()->Emission()[0].Weights() = arma::vec("0.3 0.7");
  h->GMMHMM()->Emission()[0].Component(0) = GaussianDistribution("4.25 3.10",
      "1.00 0.20; 0.20 0.89");
  h->GMMHMM()->Emission()[0].Component(1) = GaussianDistribution("7.10 5.01",
      "1.00 0.00; 0.00 1.01");
  h->GMMHMM()->Emission()[1] = GMM(2, 2);
  h->GMMHMM()->Emission()[1].Weights() = arma::vec("0.20 0.80");
  h->GMMHMM()->Emission()[1].Component(0) = GaussianDistribution("-3.00 -6.12",
      "1.00 0.00; 0.00 1.00");
  h->GMMHMM()->Emission()[1].Component(1) = GaussianDistribution("-4.25 -2.12",
      "1.50 0.60; 0.60 1.20");

  // Now that we have a trained HMM model, we can use it to generate a sequence
  // of states and observations - using the hmm_generate utility.
  // Load the input model to be used for inference and the length of sequence
  // to be generated.
  int length = 3;
  SetInputParam("model", h);
  SetInputParam("length", length);

  // Call to hmm_generate_main
  mlpackMain();

  // Get the generated observation sequence. Ensure that the generated sequence
  // has the correct length (as provided in the input).
  arma::mat obsSeq = CLI::GetParam<arma::mat>("output");
  BOOST_REQUIRE_EQUAL(obsSeq.n_cols, (size_t) length);
  BOOST_REQUIRE_EQUAL(obsSeq.n_rows, (size_t) 2);
  BOOST_REQUIRE_EQUAL(obsSeq.n_elem, (size_t) (length*2));

  // Get the generated state sequence. Ensure that the generated sequence
  // has the correct length (as provided in the input).
  arma::Mat<size_t> stateSeq = CLI::GetParam<arma::Mat<size_t>>("state");
  BOOST_REQUIRE_EQUAL(stateSeq.n_cols, (size_t) length);
  BOOST_REQUIRE_EQUAL(stateSeq.n_rows, (size_t) 1);
  BOOST_REQUIRE_EQUAL(stateSeq.n_elem, (size_t) length);
}

BOOST_AUTO_TEST_CASE(HMMGenerateLengthPositiveTest)
{
  // Load data to train a Gaussian Mixture Model HMM model with.
  arma::mat inp;
  data::Load("obs1.csv", inp);
  std::vector<arma::mat> trainSeq = {inp};

  // Initialize and train a HMM model.
  HMMModel* h = new HMMModel(DiscreteHMM);
  h->PerformAction<InitHMMModel, std::vector<arma::mat>>(&trainSeq);
  h->PerformAction<TrainHMMModel, std::vector<arma::mat>>(&trainSeq);

  // Set the params for the hmm_generate invocation
  // Note that the length is negative - we expect that a runtime error will be
  // raised in the call to hmm_generate_main
  int length = -3; // Invalid
  SetInputParam("model", h);
  SetInputParam("length", length);

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

BOOST_AUTO_TEST_CASE(HMMGenerateValidStartStateTest)
{
  // Load data to train a Gaussian Mixture Model HMM model with.
  arma::mat inp;
  data::Load("obs1.csv", inp);
  std::vector<arma::mat> trainSeq = {inp};

  // Initialize and train a HMM model.
  HMMModel* h = new HMMModel(DiscreteHMM);
  h->PerformAction<InitHMMModel, std::vector<arma::mat>>(&trainSeq);
  h->PerformAction<TrainHMMModel, std::vector<arma::mat>>(&trainSeq);

  // Set the params for the hmm_generate invocation
  // Note that the start state is invalid - we expect that a runtime error will
  // be raised in the call to hmm_generate_main
  int length = 3;
  int startState = 2; // Invalid
  SetInputParam("model", h);
  SetInputParam("length", length);
  SetInputParam("start_state", startState);

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

BOOST_AUTO_TEST_SUITE_END();


/***************************************************************************
*   Copyright 2012 - 2013 Advanced Micro Devices, Inc.
*
*   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.

***************************************************************************/

/*
TODO:
1. Optimize the Kernel. In this version transform and reduce are called directly which performs better.
2. Found a caveat in Multi-GPU scenario (Evergreen+Tahiti). Which basically applies to most of the routines.
*/

#if !defined( BOLT_CL_INNERPRODUCT_INL )
#define BOLT_CL_INNERPRODUCT_INL
#define USE_KERNEL 0
#pragma once

#include <boost/thread/once.hpp>
#include <boost/bind.hpp>
#include <type_traits>
#include <bolt/cl/detail/reduce.inl>
#include <bolt/cl/detail/transform.inl>

#include "bolt/cl/bolt.h"

//TBB Includes
#ifdef ENABLE_TBB
#include "bolt/btbb/inner_product.h"
#endif

namespace bolt {
    namespace cl {

namespace detail {

#if USE_KERNEL
             struct CallCompiler_InnerProduct {
                static void constructAndCompile(::cl::Kernel *masterKernel,  std::string cl_code,
                    std::string valueTypeName, std::string functorTypeName1, std::string functorTypeName2,
                    const control *ctl) {

                    const std::string instantiationString =
                        "// Host generates this instantiation string with user-specified value type and functor\n"
                        "template __attribute__((mangled_name(innerProductInstantiated)))\n"
                        "__attribute__((reqd_work_group_size(64,1,1)))\n"
                        "kernel void innerProductTemplate(\n"
                        "global " + valueTypeName + "* A,\n"
                        "global " + valueTypeName + "* B,\n"
                        "const int length,\n"
                        "global " + functorTypeName1 + "* userFunctor,\n"
                        "global " + functorTypeName2 + "* userFunctor2,\n"
                        "global " + valueTypeName + "* result,\n"
                        "local " + valueTypeName + "* scratch\n"
                        ");\n\n";

                    std::string functorTypeNames = functorTypeName1 + " , " + functorTypeName2;

                    bolt::cl::constructAndCompileString( masterKernel, "innerProduct", inner_product_kernels,
                        instantiationString, cl_code, valueTypeName, functorTypeNames, *ctl);

                };
            };

#endif

            template< typename DVInputIterator, typename OutputType, typename BinaryFunction1,typename BinaryFunction2>
            OutputType inner_product_enqueue(bolt::cl::control &ctl, const DVInputIterator& first1,
                const DVInputIterator& last1, const DVInputIterator& first2, const OutputType& init,
                const BinaryFunction1& f1, const BinaryFunction2& f2, const std::string& cl_code)
            {

                //Should we directly call transform and reduce routines or launch a separate kernel?
                typedef typename std::iterator_traits<DVInputIterator>::value_type iType;
                ::cl::Event innerproductEvent;

                cl_uint distVec = static_cast< cl_uint >( std::distance( first1, last1 ) );
                if( distVec == 0 )
                    return -1;

                device_vector< iType > tempDV( distVec, 0, CL_MEM_READ_WRITE, false, ctl );
                detail::transform_enqueue( ctl, first1, last1, first2, tempDV.begin() ,f2,cl_code);
                return detail::reduce_enqueue( ctl, tempDV.begin(), tempDV.end(), init, f1, cl_code);
                bolt::cl::wait(ctl, innerproductEvent);

            };

            /*! \brief This template function overload is used to seperate device_vector iterators from all
                other iterators
                \detail This template is called by the non-detail versions of inclusive_scan,
                it already assumes random access
             *  iterators.  This overload is called strictly for non-device_vector iterators
            */
            template<typename InputIterator, typename OutputType, typename BinaryFunction1,typename BinaryFunction2>
            OutputType inner_product_pick_iterator(bolt::cl::control &ctl,  const InputIterator& first1,
                const InputIterator& last1, const InputIterator& first2, const OutputType& init,
                const BinaryFunction1& f1,
                const BinaryFunction2& f2, const std::string& user_code, std::random_access_iterator_tag )
            {
                typedef typename std::iterator_traits<InputIterator>::value_type iType;
                size_t sz = (last1 - first1);
                if (sz == 0)
                    return -1;

                bolt::cl::control::e_RunMode runMode = ctl.getForceRunMode();  // could be dynamic choice some day.
                if(runMode == bolt::cl::control::Automatic)
                {
                     runMode = ctl.getDefaultPathToRun();
                }

				#if defined(BOLT_DEBUG_LOG)
                BOLTLOG::CaptureLog *dblog = BOLTLOG::CaptureLog::getInstance();
                #endif

                if( runMode == bolt::cl::control::SerialCpu)
                {
				    #if defined(BOLT_DEBUG_LOG)
                    dblog->CodePathTaken(BOLTLOG::BOLT_INNERPRODUCT,BOLTLOG::BOLT_SERIAL_CPU,"::Inner_Product::SERIAL_CPU");
                    #endif

                    #if defined( _WIN32 )
                           return std::inner_product(first1, last1, stdext::checked_array_iterator<iType*>(&(*first2), sz ), init, f1, f2);
                    #else
                    return std::inner_product(first1, last1, first2, init, f1, f2);
                    #endif
                }
                else if(runMode == bolt::cl::control::MultiCoreCpu)
                {
                    #ifdef ENABLE_TBB
					       #if defined(BOLT_DEBUG_LOG)
                           dblog->CodePathTaken(BOLTLOG::BOLT_INNERPRODUCT,BOLTLOG::BOLT_MULTICORE_CPU,"::Inner_Product::MULTICORE_CPU");
                           #endif
                           return bolt::btbb::inner_product(first1, last1, first2, init, f1, f2);
                    #else
                           throw std::runtime_error("MultiCoreCPU Version of inner_product not Enabled! \n");
                    #endif
                }
                else
                {

				    #if defined(BOLT_DEBUG_LOG)
                    dblog->CodePathTaken(BOLTLOG::BOLT_INNERPRODUCT,BOLTLOG::BOLT_OPENCL_GPU,"::Inner_Product::OPENCL_GPU");
                    #endif

                    // Use host pointers memory since these arrays are only read once - no benefit to copying.

                    // Map the input iterator to a device_vector
                    device_vector< iType > dvInput( first1, last1, CL_MEM_USE_HOST_PTR | CL_MEM_READ_ONLY, ctl );
                    device_vector< iType > dvInput2( first2, sz, CL_MEM_USE_HOST_PTR|CL_MEM_READ_ONLY, true, ctl );

                    // Map the output iterator to a device_vector
                    //Make the fourth argument as true; We will use it as a temporary buffer
                    //for copying the transform result to reduce
                    //device_vector< iType > dvOutput( result, sz, CL_MEM_USE_HOST_PTR|CL_MEM_READ_WRITE, false, ctl );

                    return inner_product_enqueue( ctl, dvInput.begin( ), dvInput.end( ), dvInput2.begin( ),
                                                   init, f1, f2, user_code );

                    // This should immediately map/unmap the buffer
                    //dvOutput.data( );
                }
            }

            // This template is called by the non-detail versions of inclusive_scan,
            // it already assumes random access iterators
            // This is called strictly for iterators that are derived from device_vector< T >::iterator
            template<typename DVInputIterator, typename OutputType, typename BinaryFunction1, typename BinaryFunction2>
            OutputType inner_product_pick_iterator(bolt::cl::control &ctl,  const DVInputIterator& first1,
                const DVInputIterator& last1,const DVInputIterator& first2,const OutputType& init,
                const BinaryFunction1&f1,const BinaryFunction2& f2, const std::string& user_code,
                bolt::cl::device_vector_tag )
            {

                size_t sz = (last1 - first1);

                typedef typename std::iterator_traits< DVInputIterator >::value_type iType1;
                bolt::cl::control::e_RunMode runMode = ctl.getForceRunMode();  // could be dynamic choice some day.
                if(runMode == bolt::cl::control::Automatic)
                {
                     runMode = ctl.getDefaultPathToRun();
                }
                #if defined(BOLT_DEBUG_LOG)
                BOLTLOG::CaptureLog *dblog = BOLTLOG::CaptureLog::getInstance();
                #endif

                if( runMode == bolt::cl::control::SerialCpu)
                {
				    #if defined(BOLT_DEBUG_LOG)
                    dblog->CodePathTaken(BOLTLOG::BOLT_INNERPRODUCT,BOLTLOG::BOLT_SERIAL_CPU,"::Inner_Product::SERIAL_CPU");
                    #endif

                    typename bolt::cl::device_vector< iType1 >::pointer firstPtr =  first1.getContainer( ).data( );
                    typename bolt::cl::device_vector< iType1 >::pointer first2Ptr =  first2.getContainer( ).data( );

                    #if defined( _WIN32 )
                       return std::inner_product(  &firstPtr[ first1.m_Index ],
                                                &firstPtr[ last1.m_Index ],
                                                stdext::make_checked_array_iterator( &first2Ptr[ first2.m_Index ], sz),
                                                init, f1, f2);
                    #else
                    return std::inner_product(  &firstPtr[ first1.m_Index ],
                                                &firstPtr[ last1.m_Index ],
                                                &first2Ptr[ first2.m_Index ], init, f1, f2);
                    #endif
                }
                else if(runMode == bolt::cl::control::MultiCoreCpu)
                {
                #ifdef ENABLE_TBB
				   #if defined(BOLT_DEBUG_LOG)
                   dblog->CodePathTaken(BOLTLOG::BOLT_INNERPRODUCT,BOLTLOG::BOLT_MULTICORE_CPU,"::Inner_Product::MULTICORE_CPU");
                   #endif

                   typename bolt::cl::device_vector< iType1 >::pointer firstPtr =  first1.getContainer( ).data( );
                   typename bolt::cl::device_vector< iType1 >::pointer first2Ptr =  first2.getContainer( ).data( );
                    return bolt::btbb::inner_product(  &firstPtr[ first1.m_Index ],  &firstPtr[ last1.m_Index ],
                                                &first2Ptr[ first2.m_Index ], init, f1, f2);
                #else
                           throw std::runtime_error("MultiCoreCPU Version of inner_product not Enabled! \n");
                #endif
                }
                else
                {
				    #if defined(BOLT_DEBUG_LOG)
                    dblog->CodePathTaken(BOLTLOG::BOLT_INNERPRODUCT,BOLTLOG::BOLT_OPENCL_GPU,"::Inner_Product::OPENCL_GPU");
                    #endif
                    return inner_product_enqueue( ctl, first1, last1, first2, init, f1, f2, user_code );
                }
            }

            // This template is called by the non-detail versions of inclusive_scan,
            // it already assumes random access iterators
            // This is called strictly for iterators that are derived from device_vector< T >::iterator
            template<typename DVInputIterator, typename OutputType, typename BinaryFunction1,typename BinaryFunction2>
            OutputType inner_product_pick_iterator(bolt::cl::control &ctl,  const DVInputIterator& first1,
                const DVInputIterator& last1, const DVInputIterator& first2, const OutputType& init,
                const BinaryFunction1& f1, const BinaryFunction2& f2, const std::string& user_code,
                bolt::cl::fancy_iterator_tag )
            {
                typedef typename std::iterator_traits<DVInputIterator>::value_type iType;
                size_t sz = std::distance( first1, last1 );

                bolt::cl::control::e_RunMode runMode = ctl.getForceRunMode();  // could be dynamic choice some day.
                if(runMode == bolt::cl::control::Automatic)
                {
                     runMode = ctl.getDefaultPathToRun();
                }
                #if defined(BOLT_DEBUG_LOG)
                BOLTLOG::CaptureLog *dblog = BOLTLOG::CaptureLog::getInstance();
                #endif

                if( runMode == bolt::cl::control::SerialCpu)
                {
				    #if defined(BOLT_DEBUG_LOG)
                    dblog->CodePathTaken(BOLTLOG::BOLT_INNERPRODUCT,BOLTLOG::BOLT_SERIAL_CPU,"::Inner_Product::SERIAL_CPU");
                    #endif

                    #if defined( _WIN32 )
                    return std::inner_product(  first1,
                                                last1,
                                                first2,
                                                init, f1, f2  );
                    #else
                           return std::inner_product(  first1,
                                                last1,
                                                first2,
                                                init, f1, f2  );
                    #endif

                }
                else if(runMode == bolt::cl::control::MultiCoreCpu)
                {
                    #ifdef ENABLE_TBB
					       #if defined(BOLT_DEBUG_LOG)
                           dblog->CodePathTaken(BOLTLOG::BOLT_INNERPRODUCT,BOLTLOG::BOLT_MULTICORE_CPU,"::Inner_Product::MULTICORE_CPU");
                           #endif
                           return bolt::btbb::inner_product(first1, last1, first2, init, f1, f2);
                    #else
                           throw std::runtime_error("MultiCoreCPU Version of inner_product not Enabled! \n");
                    #endif
                }
                else
                {
				    #if defined(BOLT_DEBUG_LOG)
                    dblog->CodePathTaken(BOLTLOG::BOLT_INNERPRODUCT,BOLTLOG::BOLT_OPENCL_GPU,"::Inner_Product::OPENCL_GPU");
                    #endif
                    return inner_product_enqueue( ctl, first1, last1, first2, init, f1, f2, user_code );
                }
            }

            template<typename InputIterator, typename OutputType, typename BinaryFunction1, typename BinaryFunction2>
            OutputType inner_product_detect_random_access( bolt::cl::control& ctl, const InputIterator& first1,
                const InputIterator& last1, const InputIterator& first2, const OutputType& init,
                const BinaryFunction1& f1,
                const BinaryFunction2& f2, const std::string& user_code, std::random_access_iterator_tag )
            {
                return inner_product_pick_iterator( ctl, first1, last1, first2, init, f1, f2, user_code,
                typename std::iterator_traits< InputIterator >::iterator_category( ) );
            };

	    // Wrapper that uses default control class, iterator interface
            template<typename InputIterator, typename OutputType, typename BinaryFunction1, typename BinaryFunction2>
            OutputType inner_product_detect_random_access( bolt::cl::control& ctl, const InputIterator& first1,
                const InputIterator& last1, const InputIterator& first2, const OutputType& init,
                const BinaryFunction1& f1, const BinaryFunction2& f2, const std::string& user_code,
                std::input_iterator_tag )
            {
                //  TODO:  It should be possible to support non-random_access_iterator_tag iterators,
                //  if we copied the data
                //  to a temporary buffer.  Should we?
                static_assert(std::is_same< InputIterator, std::input_iterator_tag >::value  , "Bolt only supports random access iterator types" );
            };

        }//End OF detail namespace

        // default control, two-input transform, std:: iterator
        template<typename InputIterator, typename OutputType, typename BinaryFunction1, typename BinaryFunction2>
         OutputType inner_product(bolt::cl::control& ctl, InputIterator first1, InputIterator last1,
         InputIterator first2, OutputType init, BinaryFunction1 f1, BinaryFunction2 f2,
         const std::string& user_code )
        {
            return detail::inner_product_detect_random_access( ctl, first1, last1, first2, init, f1, f2, user_code,
                typename std::iterator_traits< InputIterator >::iterator_category( ) );
        }

        // default control, two-input transform, std:: iterator
        template<typename InputIterator, typename OutputType, typename BinaryFunction1, typename BinaryFunction2>
        OutputType inner_product( InputIterator first1, InputIterator last1, InputIterator first2, OutputType init,
            BinaryFunction1 f1, BinaryFunction2 f2, const std::string& user_code )
        {
            return detail::inner_product_detect_random_access( control::getDefault(), first1, last1, first2, init, f1,
                f2, user_code, typename std::iterator_traits< InputIterator >::iterator_category( ) );
        }
        template<typename InputIterator, typename OutputType>
        OutputType inner_product(bolt::cl::control& ctl,InputIterator first1,InputIterator last1,InputIterator first2,
            OutputType init, const std::string& user_code )
        {
            typedef typename std::iterator_traits<InputIterator>::value_type iType;
            return detail::inner_product_detect_random_access(ctl, first1,last1,first2,init,bolt::cl::plus< iType >( ),
                bolt::cl::multiplies< iType >( ), user_code, typename std::iterator_traits<InputIterator>::iterator_category());
        }

        // default control, two-input transform, std:: iterator
        template<typename InputIterator, typename OutputType>
        OutputType inner_product( InputIterator first1, InputIterator last1, InputIterator first2, OutputType init,
            const std::string& user_code )
        {
            typedef typename std::iterator_traits<InputIterator>::value_type iType;
            return detail::inner_product_detect_random_access( control::getDefault(), first1, last1, first2, init,
                bolt::cl::plus< iType >( ), bolt::cl::multiplies< iType >( ), user_code,
                typename std::iterator_traits< InputIterator >::iterator_category( ) );
        }

    }//end of cl namespace
};//end of bolt namespace

#endif


/*
 Copyright (C) 2017 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <qle/instruments/payment.hpp>

#include <ql/cashflows/fixedratecoupon.hpp>
#include <ql/cashflows/simplecashflow.hpp>

#include <boost/make_shared.hpp>

using namespace QuantLib;

namespace QuantExt {

Payment::Payment(const Real amount, const Currency& currency, const Date& date) : currency_(currency) {
    cashflow_ = boost::shared_ptr<SimpleCashFlow>(new SimpleCashFlow(amount, date));
}

bool Payment::isExpired() const { return cashflow_->hasOccurred(); }

void Payment::setupExpired() const { Instrument::setupExpired(); }

void Payment::setupArguments(PricingEngine::arguments* args) const {
    Payment::arguments* arguments = dynamic_cast<Payment::arguments*>(args);
    QL_REQUIRE(arguments, "wrong argument type in deposit");
    arguments->currency = currency_;
    arguments->cashflow = cashflow_;
}

void Payment::fetchResults(const PricingEngine::results* r) const {
    Instrument::fetchResults(r);
    const Payment::results* results = dynamic_cast<const Payment::results*>(r);
    QL_REQUIRE(results, "wrong result type");
}

void Payment::arguments::validate() const {
    // always valid
}

void Payment::results::reset() { Instrument::results::reset(); }
} // namespace QuantExt