/**
 * @file make_otl_blq.cpp
 *
 * @brief Program to compute the ocean tide loading from a tide grid
 *
 * With an input data containing the list of the tide file, the Green's function and the coordinate of the site of interest,
 * this code will compute the loading (response to the solid earth to the load generated by the tide) displacement to the tide.
 * It include the vertical, N-S and E-W load in amplitude and phase at the tidal frequencies.
 *
 * @version 0.2
 *
 * @todo Clean the code
 * @todo Documentation.
 *
 * @author Sebastien Allgeyer
 * @date 26/02/2021
 */

#include <iostream>
#include <fstream>

#ifdef ENABLE_PARALLELISATION
	#include "omp.h"
#endif

#include <yaml-cpp/yaml.h>
#include <boost/timer/timer.hpp>
#include <boost/program_options.hpp>
#include <boost/log/utility/setup/console.hpp>
#include <boost/log/trivial.hpp>
#include <boost/multi_array.hpp>

#include "tide.h"
#include "loading.h"
#include "utils.h"
#include "input_otl.h"
#include "load_functions.h"
#include "loadgrid.h"

using namespace std;
using namespace boost::timer;
namespace po = boost::program_options;

const int THREAD_COUNT = 8;

void program_options(int argc, char * argv[], otl_input & input)
{
	po::options_description desc{"interpolate_loading <options>"};

	// Do not set default values here, as this will overide the configuration file opitions!!!
	desc.add_options()
			("help", 			"This help message")
			("quiet", 			"Less output")
			("verbose", 		"More output")
			("grid", 	 	po::value<std::string>(),	"Loading grid netCDF file")
			("location", 	po::value<std::vector<float>>()->multitoken(), "location: lon (decimal degrees) lat (decimal degrees)")
			("xyz",        po::bool_switch()->default_value(false),  "set if the coordinates are in XYZ format")
			("code",     	po::value<std::string>(), "Station Code with or without DOMES number (ALIC 50137M0014)")
			("input",		po::value<std::string>(),	"input file containing list of stations CSV format name, lon, lat")
			("output",		po::value<std::string>(),	"Output BLQ file")

			;

	po::variables_map vm;
	// This is to be able to parse negative numbers with boost.
	po::store(po::command_line_parser(argc, argv).options(desc).style(
			po::command_line_style::unix_style ^ po::command_line_style::allow_short
	).run(), vm);
	po::notify(vm);

	if (vm.count("help")) {
		cout << "Usage: interpolate_loading [options]\n";
		cout << desc << "\n\n";
		cout << "Example: interpolate_loading --grid /<path_to>/oceantide.nc --code 'ALIC 50137M0014' --location 133.8855 -23.6701 \n\n";
		cout << "Example with input file: interpolate_loading --grid /<path_to>/oceantide.nc --input station.csv\n";
		cout << "                         Where station.csv contains station informations  \n";
		cout << "                           format: station name, longitude, latitude\n\n";
		cout << "  The file oceantide.nc contains the precalculated ocean tide loads\n";

		exit(0);
	}

	// Parser
	std::string config_f;
	std::string code_f;
	std::vector<float> location;
	bool is_ecef = vm["xyz"].as<bool>();
	if (vm.count("config"))
	{
		config_f = vm["config"].as<std::string>();
	}
	if (vm.count("location")) {
		location = vm["location"].as<std::vector<float>>();
		if (is_ecef)
		{
			if (location.size() == 3)
			{
				input.xyz_coords.push_back(location);
			} else {
				throw std::runtime_error("XYZ coordinate should have 3 values");
			}
		}
		else
		{
			input.lon.push_back(location[0]);
			input.lat.push_back(location[1]);
		}
		if (vm.count("code")) {
			input.code.push_back(vm["code"].as<std::string>());
		} else { input.code.push_back("XXXX"); }
	}

	input.tide_file.push_back(vm["grid"].as<std::string>());
	// YAML::Node config = YAML::LoadFile(config_f );

	// input.green = config["greenfunction"].as<string>();

	// YAML::Node tidefiles = config["tide"];
	// for (YAML::const_iterator it = tidefiles.begin() ; it != tidefiles.end(); ++it) {
		// input.tide_file.push_back(it->as<std::string>(""));
	// };

	vector <string> row;
	string word;
	if (vm.count("input"))
	{
		string filename = vm["input"].as<std::string>();
		std::ifstream infile(filename);
		while(infile) {
			string line;
			if (!getline(infile, line)) break;
			if (line.size()!= 0 and line.at(0) != '#')
			{
				istringstream data(line);
				row.clear();
				while (getline(data, word, ',')) {
					row.push_back(word);
				}

				if (is_ecef)
				{
					if (row.size() == 4)
					{
						std::vector<float> tmp;
						input.code.push_back(row[0]);
						tmp.push_back(stof(row[1]));
						tmp.push_back(stof(row[2]));
						tmp.push_back(stof(row[3]));
						input.xyz_coords.push_back(tmp);
					} else {
						throw std::runtime_error("xyz coordinates in the csv file should have 4 valuse \"code, x, y, z\"");
					}
				} else {
					if (row.size() == 3)
					{
						input.code.push_back(row[0]);
						input.lon.push_back(stof(row[1]));
						input.lat.push_back(stof(row[2]));
					}
				}
				row.clear();
				data.clear();
			}
		}
	}

	if (vm.count("output")) {
		input.output_blq_file = vm["output"].as<std::string>();
	} else {
		input.output_blq_file = "output.blq";
	}

	if (is_ecef)
	{
		for (int i=0; i<input.xyz_coords.size(); i++)
		{
			double tmp[3];
			double ecef[3];
			ecef[0] = input.xyz_coords[i][0];
			ecef[1] = input.xyz_coords[i][1];
			ecef[2] = input.xyz_coords[i][2];
			ecef2pos(ecef, tmp);
			input.lon.push_back(tmp[1]*180.0/M_PI);
			input.lat.push_back(tmp[0]*180.0/M_PI);
		}
	}

};

int main(int argc, char * argv[]) {
	try {
		otl_input input;
		boost::log::core::get()->set_filter(boost::log::trivial::severity >= boost::log::trivial::info);
		boost::log::add_console_log(std::cout, boost::log::keywords::format = "%Message%");

		program_options(argc, argv, input);

		BOOST_LOG_TRIVIAL(info) << " ======== INPUT PARAMETERS ======= " << "\n";
		BOOST_LOG_TRIVIAL(info) << " - Will process the following station:  " << "\n";
		for (int i = 0; i < input.code.size(); i++)
			BOOST_LOG_TRIVIAL(info) << "   * " << input.code[i] << " -- lon, lat -- " << input.lon[i] << ", "
									<< input.lat[i] << "\n";
		BOOST_LOG_TRIVIAL(info) << " - Green's function is :  " << "\n" << "   * " << input.green << "\n";
		BOOST_LOG_TRIVIAL(info) << " - Tide files is :  " << "\n";
		for (int i = 0; i < input.tide_file.size(); i++)
			BOOST_LOG_TRIVIAL(info) << "   * " << input.tide_file[i] << "\n";
		BOOST_LOG_TRIVIAL(info) << " - Output file is:  " << "\n" << "   * " << input.output_blq_file << "\n";
		BOOST_LOG_TRIVIAL(info) << " ========       END       ======= " << "\n";

		loadGrid tideinfo;

		tideinfo.set_name(input.tide_file[0]);
		tideinfo.read();

		BOOST_LOG_TRIVIAL(debug) << "there is " << tideinfo.get_nwave() << " tides\n";
		input.wave_names = tideinfo.get_wave_names();

		input.out_disp.resize(boost::extents[input.code.size()][tideinfo.get_nwave()][3]) ;
	    std::fill(input.out_disp.data(), input.out_disp.data() + input.out_disp.num_elements(), std::complex<float> (0,0));

		for (int i_sta = 0 ; i_sta < input.lat.size(); i_sta++)
		for (int i_wave = 0 ; i_wave < tideinfo.get_nwave(); i_wave ++ )
		for (int i_dir = 0 ; i_dir < 3; i_dir++ )
			input.out_disp[i_sta][i_wave][i_dir] = std::complex<float> (tideinfo.interpolate(i_wave*6 + 2*i_dir, input.lon[i_sta], input.lat[i_sta]) ,
																		tideinfo.interpolate(i_wave*6 + 2*i_dir +1, input.lon[i_sta], input.lat[i_sta])
																		);

		write_BLQ(&input, 0);

	}
	catch (std::exception &e)
	{
		std::cerr << "\n\tERROR:\n\t\t " << e.what() << "\n";
		return -1;
	}
	return 0;
}


/**
*   ReadWKT.hpp
*
*   Copyright 2015 The Climate Corporation
*   Copyright 2015 Tony Stone
*
*   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.
*
*   Created by Tony Stone on 6/13/15.
*
*   MODIFIED 2015 BY Tony Stone. Modifications licensed under Apache License, Version 2.0.
*
*/

#ifndef __ReadWKT_HPP_
#define __ReadWKT_HPP_

#include "GFGeometryCollection.hpp"
#include <boost/geometry/io/wkt/read.hpp>
#include <boost/algorithm/string/trim.hpp>
#include <boost/algorithm/string/replace.hpp>
#include <boost/algorithm/string/erase.hpp>
#include <boost/algorithm/string/find_iterator.hpp>

#include <regex>

namespace geofeatures {
    namespace io {

        template <typename Geometry, typename = typename std::enable_if<std::is_same<Geometry,GeometryCollection<>>::value>::type>
        inline void readWKT(std::string const &wkt, Geometry &geometryCollection)
        {
            static std::string GeometryCollectionToken = "GEOMETRYCOLLECTION";
            static std::regex tokensRegex("\\b(POINT|MULTIPOINT|LINESTRING|MULTILINESTRING|POLYGON|MULTIPOLYGON)", std::regex_constants::icase);
            static std::regex emptyTokenRegex("[ \\t]*EMPTY[ \\t]*", std::regex_constants::icase);

            if (!boost::istarts_with(wkt, GeometryCollectionToken)) {
                throw std::invalid_argument("Should start with 'GEOMETRYCOLLECTION'' in (" + wkt + ")");
            }

            long tokensBegin = wkt.find_first_of("(");
            long tokensEnd = wkt.find_last_of(")");

            bool openClosePerensFound = (tokensBegin != std::string::npos) && (tokensEnd != std::string::npos);

            if (tokensBegin != std::string::npos)
                tokensBegin += 1;
            else
                tokensBegin = GeometryCollectionToken.length();

            if (tokensEnd != std::string::npos)
               tokensEnd -= 1;
            else
                tokensEnd = wkt.length();

            GeometryCollection<>::value_type (*geometry)(std::string & wkt) =
                    [](std::string & wkt) -> GeometryCollection<>::value_type {

                        if (boost::istarts_with(wkt, "POINT")) {
                            geofeatures::Point geometry;

                            boost::geometry::read_wkt(wkt, geometry);

                            return geometry;
                        } else if (boost::istarts_with(wkt, "MULTIPOINT")) {
                            geofeatures::MultiPoint geometry;

                            boost::geometry::read_wkt(wkt, geometry);

                            return geometry;
                        } else if (boost::istarts_with(wkt, "LINESTRING")) {
                            geofeatures::LineString geometry;

                            boost::geometry::read_wkt(wkt, geometry);

                            return geometry;
                        } else if (boost::istarts_with(wkt, "MULTILINESTRING")) {
                            geofeatures::MultiLineString geometry;

                            boost::geometry::read_wkt(wkt, geometry);

                            return geometry;
                        } else if (boost::istarts_with(wkt, "POLYGON")) {
                            geofeatures::Polygon geometry;

                            boost::geometry::read_wkt(wkt, geometry);

                            return geometry;
                        } else if (boost::istarts_with(wkt, "MULTIPOLYGON")) {
                            geofeatures::MultiPolygon geometry;

                            boost::geometry::read_wkt(wkt, geometry);

                            return geometry;
                        }
                        return GeometryCollection<>::value_type();
                    };

            auto matchesBegin = std::sregex_iterator(wkt.begin(), wkt.end(), tokensRegex);
            auto matchesEnd = std::sregex_iterator();

            if (std::distance(matchesBegin, matchesEnd) > 0) {

                std::sregex_iterator i = matchesBegin;

                std::smatch match = *i;

                long currentToken = match.position();

                i++;

                for (; i != matchesEnd; i++) {

                    std::smatch match = *i;
                    long nextToken = match.position();

                    std::string str(wkt.substr(currentToken, nextToken - currentToken));
                    boost::algorithm::erase_last(str, ",");

                    geometryCollection.push_back(geometry(str));

                    currentToken = nextToken;
                }
                std::string str(wkt.substr(currentToken, tokensEnd - currentToken + 1));

                geometryCollection.push_back(geometry(str));

            } else {
                if (!openClosePerensFound) {

                    auto str = wkt.substr(tokensBegin, tokensEnd - tokensBegin + 1);

                    auto matches = std::sregex_iterator(str.begin(), str.end(), emptyTokenRegex);
                    if (std::distance(matches, std::sregex_iterator()) != 1) {
                        throw std::invalid_argument("Invalid wkt, expected EMPTY but got " + str);
                    }
                }
            }
        }
    } // namespace io

}   // namespace geofeatures

#endif //__ReadWKT_HPP_


/*
 * dlib_serialization.hpp
 *
 *  Created on: 01.03.2013
 *      Author: luk
 */

#ifndef UTILS_DLIB_SERIALIZATION_HPP_
#define UTILS_DLIB_SERIALIZATION_HPP_

#include <glog/logging.h>

#include <boost/serialization/split_free.hpp>

#include <dlib/matrix/matrix.h>
#include <dlib/svm/function.h>
#include <dlib/svm/one_vs_all_decision_function.h>

namespace detail {
template<typename sample_type, typename scalar_type>
struct copy_to_df_helper {
  copy_to_df_helper(dlib::any_decision_function<sample_type, scalar_type>& target_)
      : target(target_) {
  }

  dlib::any_decision_function<sample_type, scalar_type>& target;

  template<typename T>
  void operator()(const T& item) const {
    target = item;
  }
};

template <typename Archive>
struct serialize_helper
{
  Archive& _ar;
  serialize_helper(Archive& ar): _ar(ar) {}
  template <typename T>
  void operator() (const T& item) const { _ar & item; }
};
}

namespace boost
{
namespace serialization
{

//------------------------------------------------------------------------------
// dlib::matrix
template <typename Archive, typename T, long NR, long NC, typename mm, typename l>
void save(Archive & ar, const dlib::matrix<T,NR,NC,mm,l>& mat, const unsigned int version)
{
	long rows = mat.nr();
	long cols =  mat.nc();
	ar & rows;
	ar & cols;
    for (long row = 0; row < mat.nr(); ++row)
    {
    	for (long col = 0; col < mat.nc(); ++col)
    	{
    		ar & mat(row,col);
    	}
    }
}

template <typename Archive, typename T, long NR, long NC, typename mm, typename l>
void load(Archive & ar, dlib::matrix<T,NR,NC,mm,l>& mat, const unsigned int version)
{
	long rows = 0;
	long cols = 0;

    ar & rows;
    ar & cols;
    CHECK(NR == 0 || NR == rows) << "number of rows do not match";
    CHECK(NC == 0 || NC == cols) << "number of rows do not match";
    mat.set_size(rows,cols);
    for (long r = 0; r < rows; ++r)
    {
    	for (long c = 0; c < cols; ++c)
    	{
    		ar & mat(r,c);
    	}
    }
}

template <typename Archive, typename T, long NR, long NC, typename mm, typename l>
void serialize(Archive& ar, dlib::matrix<T,NR,NC,mm,l>& mat, const unsigned int version)
{
	::boost::serialization::split_free(ar, mat, version);
}

//------------------------------------------------------------------------------
// dlib::multiclass_linear_decision_function

template <typename Archive, typename K, typename R>
void serialize(Archive& ar,  dlib::multiclass_linear_decision_function<K,R>& fun, const unsigned int version)
{
  ar & fun.weights;
  ar & fun.b;
  ar & fun.labels;
}
//------------------------------------------------------------------------------
// dlib::_void
template <typename Archive>
void serialize(Archive& ar, dlib::_void& v, const unsigned int version)
{
}
//------------------------------------------------------------------------------
// dlib::null_df

template <typename Archive>
void serialize(Archive& ar, dlib::null_df& fun, const unsigned int version)
{
}
//------------------------------------------------------------------------------
// dlib::type_safe_union
template <
    typename Archive,
    typename T1, typename T2, typename T3, typename T4, typename T5,
    typename T6, typename T7, typename T8, typename T9, typename T10,
    typename T11, typename T12, typename T13, typename T14, typename T15,
    typename T16, typename T17, typename T18, typename T19, typename T20
    >
void save (
    Archive& ar,
    const dlib::type_safe_union<T1,T2,T3,T4,T5,T6,T7,T8,T9,T10, T11,T12,T13,T14,T15,T16,T17,T18,T19,T20>& item,
    const unsigned int version
)
{
  int type_identity = 0;
  // query type id!111
  if (item.template contains<T1>())
    type_identity = item.template get_type_id<T1>();
  else if (item.template contains<T2>())
    type_identity = item.template get_type_id<T2>();
  else if (item.template contains<T3>())
    type_identity = item.template get_type_id<T3>();
  else if (item.template contains<T4>())
    type_identity = item.template get_type_id<T4>();
  else if (item.template contains<T5>())
    type_identity = item.template get_type_id<T5>();
  else if (item.template contains<T6>())
    type_identity = item.template get_type_id<T6>();
  else if (item.template contains<T7>())
    type_identity = item.template get_type_id<T7>();
  else if (item.template contains<T8>())
    type_identity = item.template get_type_id<T8>();
  else if (item.template contains<T9>())
    type_identity = item.template get_type_id<T9>();
  else if (item.template contains<T10>())
    type_identity = item.template get_type_id<T10>();
  else if (item.template contains<T11>())
    type_identity = item.template get_type_id<T11>();
  else if (item.template contains<T12>())
    type_identity = item.template get_type_id<T12>();
  else if (item.template contains<T13>())
    type_identity = item.template get_type_id<T13>();
  else if (item.template contains<T14>())
    type_identity = item.template get_type_id<T14>();
  else if (item.template contains<T15>())
    type_identity = item.template get_type_id<T15>();
  else if (item.template contains<T16>())
    type_identity = item.template get_type_id<T16>();
  else if (item.template contains<T17>())
    type_identity = item.template get_type_id<T17>();
  else if (item.template contains<T18>())
    type_identity = item.template get_type_id<T18>();
  else if (item.template contains<T19>())
    type_identity = item.template get_type_id<T19>();
  else if (item.template contains<T20>())
    type_identity = item.template get_type_id<T20>();
  else
    throw std::runtime_error(
        "Can't serialize typesafe_union.  Not all types defined.");
  // serialize in the end
  ar & type_identity;
  item.apply_to_contents(::detail::serialize_helper<Archive>(ar));
}

template <
    typename Archive,
    typename T1, typename T2, typename T3, typename T4, typename T5,
    typename T6, typename T7, typename T8, typename T9, typename T10,
    typename T11, typename T12, typename T13, typename T14, typename T15,
    typename T16, typename T17, typename T18, typename T19, typename T20
    >
void load(
    Archive& ar,
    dlib::type_safe_union<T1,T2,T3,T4,T5,T6,T7,T8,T9,T10, T11,T12,T13,T14,T15,T16,T17,T18,T19,T20>& item,
    const unsigned int version
)
{
  typedef dlib::type_safe_union<T1,T2,T3,T4,T5,T6,T7,T8,T9,T10, T11,T12,T13,T14,T15,T16,T17,T18,T19,T20> tsu_type;

  int type_identity;
  ar & type_identity;
  switch (type_identity)
  {
    // swap an empty type_safe_union into item since it should be in the empty state
    case 0: tsu_type().swap(item); break;

    case 1: ar & item.template get<T1>();  break;
    case 2: ar & item.template get<T2>();  break;
    case 3: ar & item.template get<T3>();  break;
    case 4: ar & item.template get<T4>();  break;
    case 5: ar & item.template get<T5>();  break;

    case 6: ar & item.template get<T6>();  break;
    case 7: ar & item.template get<T7>();  break;
    case 8: ar & item.template get<T8>();  break;
    case 9: ar & item.template get<T9>();  break;
    case 10: ar & item.template get<T10>();  break;

    case 11: ar & item.template get<T11>();  break;
    case 12: ar & item.template get<T12>();  break;
    case 13: ar & item.template get<T13>();  break;
    case 14: ar & item.template get<T14>();  break;
    case 15: ar & item.template get<T15>();  break;

    case 16: ar & item.template get<T16>();  break;
    case 17: ar & item.template get<T17>();  break;
    case 18: ar & item.template get<T18>();  break;
    case 19: ar & item.template get<T19>();  break;
    case 20: ar & item.template get<T20>();  break;

    default: throw std::runtime_error("Corrupt data detected while deserializing type_safe_union");
  }
}

template <
    typename Archive,
    typename T1, typename T2, typename T3, typename T4, typename T5,
    typename T6, typename T7, typename T8, typename T9, typename T10,
    typename T11, typename T12, typename T13, typename T14, typename T15,
    typename T16, typename T17, typename T18, typename T19, typename T20
    >
void serialize(
    Archive& ar,
    dlib::type_safe_union<T1,T2,T3,T4,T5,T6,T7,T8,T9,T10, T11,T12,T13,T14,T15,T16,T17,T18,T19,T20>& item,
    const unsigned int version
)
{
  ::boost::serialization::split_free(ar, item, version);
}
//------------------------------------------------------------------------------
// dlib::linear_kernel
template <typename Archive, typename T>
void serialize(Archive& ar, dlib::linear_kernel<T>& kern, const unsigned int version){
}
//------------------------------------------------------------------------------
// dlib::sparse_linear_kernel
template <typename Archive, typename T>
void serialize(Archive& ar, dlib::sparse_linear_kernel<T>& kern, const unsigned int version){
}
//------------------------------------------------------------------------------
// dlib::decision_function<K>
template <typename Archive, typename K>
void serialize(Archive& ar, dlib::decision_function<K>& item, const unsigned int version){
  ar & item.alpha;
  ar & item.b;
  ar & item.kernel_function;
  ar & item.basis_vectors;
}

//------------------------------------------------------------------------------
// dlib::dlib::one_vs_all_decision_function

template <
    typename Archive,
    typename T,
    typename DF1, typename DF2, typename DF3,
    typename DF4, typename DF5, typename DF6,
    typename DF7, typename DF8, typename DF9,
    typename DF10
    >
void save(
    Archive& ar,
    const dlib::one_vs_all_decision_function<T, DF1, DF2, DF3, DF4, DF5, DF6, DF7, DF8, DF9, DF10>& fun,
    const unsigned int version) {

  dlib::type_safe_union<DF1, DF2, DF3, DF4, DF5, DF6, DF7, DF8, DF9, DF10> temp;
  typedef typename T::label_type result_type;
  typedef typename T::sample_type sample_type;
  typedef typename T::scalar_type scalar_type;
  typedef std::map<result_type, dlib::any_decision_function<sample_type, scalar_type> > binary_function_table;

  const unsigned long size = fun.get_binary_decision_functions().size();
  ar & size;

  for (typename binary_function_table::const_iterator i = fun.get_binary_decision_functions().begin();
      i != fun.get_binary_decision_functions().end();
      ++i) {
    // serialize label
    ar & i->first;

    if (i->second.template contains<DF1>())
      temp.template get<DF1>() = dlib::any_cast<DF1>(i->second);
    else if (i->second.template contains<DF2>())
      temp.template get<DF2>() = dlib::any_cast<DF2>(i->second);
    else if (i->second.template contains<DF3>())
      temp.template get<DF3>() = dlib::any_cast<DF3>(i->second);
    else if (i->second.template contains<DF4>())
      temp.template get<DF4>() = dlib::any_cast<DF4>(i->second);
    else if (i->second.template contains<DF5>())
      temp.template get<DF5>() = dlib::any_cast<DF5>(i->second);
    else if (i->second.template contains<DF6>())
      temp.template get<DF6>() = dlib::any_cast<DF6>(i->second);
    else if (i->second.template contains<DF7>())
      temp.template get<DF7>() = dlib::any_cast<DF7>(i->second);
    else if (i->second.template contains<DF8>())
      temp.template get<DF8>() = dlib::any_cast<DF8>(i->second);
    else if (i->second.template contains<DF9>())
      temp.template get<DF9>() = dlib::any_cast<DF9>(i->second);
    else if (i->second.template contains<DF10>())
      temp.template get<DF10>() = dlib::any_cast<DF10>(i->second);
    else
      throw std::runtime_error(
          "Can't serialize one_vs_all_decision_function.  Not all decision functions defined.");

    ar & temp;
  }
}

template <
typename Archive,
typename T,
typename DF1, typename DF2, typename DF3,
typename DF4, typename DF5, typename DF6,
typename DF7, typename DF8, typename DF9,
typename DF10
>
void load(
    Archive & ar,
    dlib::one_vs_all_decision_function<T,DF1,DF2,DF3,DF4,DF5,DF6,DF7,DF8,DF9,DF10>& fun,
    const unsigned int version
)
{

  typedef typename T::label_type result_type;
  typedef typename T::sample_type sample_type;
  typedef typename T::scalar_type scalar_type;
  typedef ::detail::copy_to_df_helper<sample_type, scalar_type> copy_to;

  unsigned long size;
  ar & size;

  typedef std::map<result_type, dlib::any_decision_function<sample_type, scalar_type> > binary_function_table;
  binary_function_table dfs;

  result_type label;
  dlib::type_safe_union<DF1,DF2,DF3,DF4,DF5,DF6,DF7,DF8,DF9,DF10> temp;
  for (unsigned long i = 0; i < size; ++i)
  {
    ar & label;
    ar & temp;
    if (temp.template contains<dlib::null_df>()){
      throw std::runtime_error("A sub decision function of unknown type was encountered.");
    }
    temp.apply_to_contents(copy_to(dfs[label]));
  }

  fun = dlib::one_vs_all_decision_function<T,DF1,DF2,DF3,DF4,DF5,DF6,DF7,DF8,DF9,DF10>(dfs);
}

template <
    typename Archive,
    typename T,
    typename DF1, typename DF2, typename DF3,
    typename DF4, typename DF5, typename DF6,
    typename DF7, typename DF8, typename DF9,
    typename DF10
    >
void serialize(Archive& ar, dlib::one_vs_all_decision_function<T,DF1,DF2,DF3,DF4,DF5,DF6,DF7,DF8,DF9,DF10>& fun, const unsigned int version)
{
  ::boost::serialization::split_free(ar, fun, version);
}

} /* namespace serialization */
} /* namespace boost */
#endif /* UTILS_DLIB_SERIALIZATION_HPP_ */


// Copyright (c) 2019 Graphcore Ltd. All rights reserved.
#include <algorithm>
#include <cmath>
#include <cstdint>
#include <iostream>
#include <limits>
#include <memory>
#include <random>
#include <unordered_map>

#include <boost/math/distributions/chi_squared.hpp>
#include <boost/program_options.hpp>

#include <poplar/CSRFunctions.hpp>
#include <poplar/Engine.hpp>
#include <poplar/IPUModel.hpp>
#include <poplar/RandomSeed.hpp>
#include <poplar/Target.hpp>
#include <poplibs_support/Compiler.hpp>
#include <poplibs_support/TestDevice.hpp>
#include <poplibs_test/Pass.hpp>
#include <poplibs_test/Util.hpp>
#include <poplibs_test/exceptions.hpp>
#include <popops/ElementWise.hpp>
#include <popops/codelets.hpp>
#include <poprand/RandomGen.hpp>
#include <poprand/codelets.hpp>
#include <poputil/TileMapping.hpp>
#include <poputil/Util.hpp>
#include <poputil/VertexTemplates.hpp>

#define FLOAT_REL_TOL 1e-5
#define HALF_REL_TOL 1e-3

const poplar::OptionFlags options;

using namespace poplar;
using namespace poplar::program;
using namespace poplibs_test::util;
using namespace poputil;
using namespace poplibs_support;
using poplibs_test::Pass;

template <typename T, bool deviceHalf>
static void
readAndConvertTensor(const Target &target, Engine &eng,
                     const std::string &handle, std::vector<T> &out,
                     std::size_t N,
                     typename std::enable_if<!deviceHalf, int>::type = 0) {
  eng.readTensor(handle, out.data(), &out[N]);
}

template <typename T, bool deviceHalf = false>
static void readAndConvertTensor(
    const Target &target, Engine &eng, const std::string &handle,
    std::vector<T> &out, std::size_t N,
    typename std::enable_if<std::is_same<T, float>::value && deviceHalf,
                            int>::type = 0) {
  std::vector<char> buf(target.getTypeSize(HALF) * N);
  eng.readTensor(handle, buf.data(), buf.data() + buf.size());
  copyDeviceHalfToFloat(target, buf.data(), out.data(), N);
}

template <typename T, bool deviceHalf>
static void
convertAndWriteTensor(const Target &target, Engine &eng,
                      const std::string &handle, const std::vector<T> &in,
                      std::size_t N,
                      typename std::enable_if<!deviceHalf, int>::type = 0) {
  eng.writeTensor(handle, in.data());
}

template <typename T, bool deviceHalf = false>
static void convertAndWriteTensor(
    const Target &target, Engine &eng, const std::string &handle,
    const std::vector<T> &in, std::size_t N,
    typename std::enable_if<std::is_same<T, float>::value && deviceHalf,
                            int>::type = 0) {
  std::vector<char> buf(target.getTypeSize(HALF) * N);
  copyFloatToDeviceHalf(target, in.data(), buf.data(), N);
  eng.writeTensor(handle, buf.data());
}

template <typename T>
static bool validateUniform(const std::vector<T> &mat, unsigned int inSize,
                            double minVal, double maxVal, double percentError) {
  bool boundsMet = true;
  double mean = 0;
  double maxSeen = minVal, minSeen = maxVal;
  // compute mean and variance and check bounds
  for (auto r = 0U; r != inSize; ++r) {
    if (mat[r] < minVal || mat[r] > maxVal) {
      boundsMet = false;
      std::cerr << "bounds not met at [" << r << "] ";
      std::cerr << mat[r] << "\n";
    }
    if (maxSeen < mat[r])
      maxSeen = mat[r];
    if (minSeen > mat[r])
      minSeen = mat[r];
    mean += mat[r];
  }
  mean /= inSize;

  double variance = 0;
  for (auto r = 0U; r != inSize; ++r) {
    double err = mat[r] - mean;
    variance += err * err;
  }

  double stdDev = std::sqrt(variance / (inSize - 1));

  const double dist = (maxVal - minVal) / 2;
  const double actualMean = (minVal + maxVal) / 2.0;

  const bool meanTest = mean >= (actualMean - dist * percentError / 100) &&
                        mean <= (actualMean + dist * percentError / 100);

  const double rStdDev = stdDev / dist;
  bool stdDevTest = rStdDev <= ((1 + percentError / 100) / std::sqrt(3.0)) &&
                    rStdDev >= ((1 - percentError / 100) / std::sqrt(3.0));
  // ignore stddev test for int. It is easy to derive if needed
  if (std::is_same<T, int>::value) {
    stdDevTest = true;
  }

  if (!meanTest) {
    std::cerr << "mean test failed : actual " << actualMean << " estimated ";
    std::cerr << mean << "\n";
  }
  if (!stdDevTest) {
    std::cerr << "std dev test failed : ratio " << rStdDev << "\n";
  }
  if (false) {
    std::cerr << "limit [" << minVal << ":" << maxVal << "]; range [" << minSeen
              << ":" << maxSeen << "]; mean " << mean << "; sd  " << stdDev
              << "\n";
  }
  // Add further tests if needed
  auto failed = !(boundsMet && meanTest && stdDevTest);
  if (failed) {
    std::cerr << "Validation of Uniform failed\n";
  }
  return failed;
}

template <typename T>
static bool validateLogUniform(const std::vector<T> &mat, unsigned int inSize,
                               double minVal, double maxVal,
                               double rejectionThreshold, double base,
                               double percentError) {
  double logMinVal = std::log(minVal);
  double logMaxVal = std::log(maxVal);

  // If we're using integers, do a chi-squared test on the integer bins
  if (std::is_same<T, int>::value) {
    // Get frequencies of samples
    std::unordered_map<int, unsigned int> counter;
    for (const auto &sample : mat) {
      if (counter.find(sample) == counter.end()) {
        counter[sample] = 1;
      } else {
        counter[sample]++;
      }
    }

    // Calculate chi_squared critical value with expected function
    double critVal = 0.0;
    double logRange = logMaxVal - logMinVal;
    for (const auto &count : counter) {
      T bin = count.first;
      unsigned int freq = count.second;
      double expected = inSize * (std::log((bin + 1.0) / bin) / logRange);
      critVal += std::pow(freq - expected, 2) / expected;
    }

    // Find p value with chi squared cdf
    boost::math::chi_squared chi_dist(maxVal - minVal - 1);
    double p = boost::math::cdf(chi_dist, critVal);
    // The null hypothesis is that the samples come from a log-uniform dist.
    // We reject this null hypothesis if the p value is > some number
    bool failed = p > (1.0 - rejectionThreshold);
    if (failed) {
      std::cerr << "Log Uniform did not pass chi-squared distribution test\n";
      return failed;
    }
  }

  // Helper to handle non-default bases
  double tolerance = 1e-7;
  float lnBase = std::log(base);
  bool useNaturalLog = std::abs(1.0 - lnBase) < tolerance;
  auto adjustForBase = [&useNaturalLog, &lnBase](double x) {
    return useNaturalLog ? x : (x / lnBase);
  };

  // Check the log of the matrix is uniformly distributed
  // Note: important to do this calculation in double so logMat doesn't go OOB
  // on [logMinVal, logMaxVal] since the bounds were calculated in double.
  std::vector<double> logMat;
  for (const T item : mat) {
    double logItem = adjustForBase(std::log(static_cast<double>(item)));
    logMat.push_back(logItem);
  }
  // Note: Here we try to reverse the exponential in the logUniform call to
  // recover the original uniform distribution in [log(minVal), log(maxVal)] and
  // check its validity. However, the reversal is not perfect -
  // x ~= log(cast(exp(x))) will be biased toward outType representable values
  // in log-space, particularly for integers. Therefore we need a slightly
  // higher tolerance.
  bool failed = validateUniform(logMat, inSize, adjustForBase(logMinVal),
                                adjustForBase(logMaxVal), percentError);
  if (failed) {
    std::cerr << "Expected log of Log Uniform distribution to be uniformly"
              << " distributed, but it was not\n";
  }
  return failed;
}

template <typename T>
static bool validateBernoulli(const std::vector<T> &mat, unsigned int inSize,
                              float prob, double percentError) {
  bool validEvents = true;
  double probEst = 0;
  // compute mean and variance and check bounds
  for (auto r = 0U; r != inSize; ++r) {
    if (mat[r] != 1 && mat[r] != 0) {
      validEvents = false;
      std::cerr << "invalid event at [" << r << "] ";
      std::cerr << mat[r] << "\n";
    }
    probEst += static_cast<double>(mat[r]);
  }
  probEst /= inSize;
  const bool probTest = probEst >= (prob - percentError / 100) &&
                        probEst <= (prob + percentError / 100);

  if (!probTest) {
    std::cerr << "probability test failed : actual " << prob << " estimated ";
    std::cerr << probEst << "\n";
  }
  // Add further tests if needed
  auto failed = !(validEvents && probTest);
  if (failed) {
    std::cerr << "Validation of Bernoulli failed\n";
  }
  return failed;
}

template <typename T>
static bool validateNormal(const std::vector<T> &mat, unsigned int inSize,
                           float actualMean, float actualStdDev,
                           double percentError) {
  double mean = 0;
  // compute mean and variance and check bounds
  for (auto r = 0U; r != inSize; ++r) {
    mean += mat[r];
  }
  mean /= inSize;

  double variance = 0;
  for (auto r = 0U; r != inSize; ++r) {
    double err = mat[r] - mean;
    variance += err * err;
  }

  const double stdDev = std::sqrt(variance / (inSize - 1));
  const bool meanTest =
      (mean >= (actualMean - actualStdDev * percentError / 100)) &&
      mean <= (actualMean + actualStdDev * percentError / 100);

  const double rStdDev = stdDev / actualStdDev;
  const bool stdDevTest = rStdDev <= (1 + percentError / 100) &&
                          rStdDev >= (1 - percentError / 100);

  if (!meanTest) {
    std::cerr << "mean test failed : actual " << actualMean << " estimated ";
    std::cerr << mean << "\n";
  }
  if (!stdDevTest) {
    std::cerr << "std dev test failed : ratio " << rStdDev << "\n";
  }

  // Add further tests if needed
  bool failed = !(meanTest && stdDevTest);
  if (failed) {
    std::cerr << "Validation of Normal failed\n";
  }
  return failed;
}

template <typename T>
static bool validateTruncNormal(const std::vector<T> &mat, unsigned int inSize,
                                float actualMean, float actualStdDev,
                                float alpha, double percentError) {
  bool boundsMet = true;
  double mean = 0;
  // compute mean and variance and check bounds
  for (auto r = 0U; r != inSize; ++r) {
    if ((mat[r] < (actualMean - alpha * actualStdDev)) ||
        (mat[r] > (actualMean + alpha * actualStdDev))) {
      boundsMet = false;
      std::cerr << "bounds not met at [" << r << "] ";
      std::cerr << mat[r] << "\n";
    }
    mean += mat[r];
  }
  mean /= inSize;

  double variance = 0;
  for (auto r = 0U; r != inSize; ++r) {
    double err = mat[r] - mean;
    variance += err * err;
  }

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
  const double phi = 1 / std::sqrt(2 * M_PI) * std::exp(-alpha * alpha / 2);
  const double alphaN = alpha / std::sqrt(2.0);
  const double actualVariance =
      1 - 2 * alpha * phi / (0.5 * (std::erf(alphaN) - std::erf(-alphaN)));

  const double stdDev = std::sqrt(variance / (inSize - 1));
  const double actualTruncStdDev = std::sqrt(actualVariance) * actualStdDev;

  const bool meanTest =
      mean >= (actualMean - actualTruncStdDev * percentError / 100) &&
      mean <= (actualMean + actualTruncStdDev * percentError / 100);

  const double rStdDev = stdDev / actualTruncStdDev;
  const bool stdDevTest = rStdDev <= (1 + percentError / 100) &&
                          rStdDev >= (1 - percentError / 100);
  if (!meanTest) {
    std::cerr << "mean test failed : actual " << actualMean << " estimated ";
    std::cerr << mean << "\n";
  }

  if (!stdDevTest) {
    std::cerr << "std dev test failed : ratio " << rStdDev << "\n";
  }
  // Add further tests if needed
  bool failed = !(boundsMet && meanTest && stdDevTest);
  if (failed) {
    std::cerr << "Validation of Truncated Normal failed\n";
  }
  return failed;
}

template <typename T, bool deviceHalf = false>
static bool validateDropout(const std::vector<T> &hOut, unsigned inSize,
                            float dropoutProb, float tolerance,
                            double percentError) {
  // Check number of zeros
  std::size_t numDropout = 0;
  for (std::size_t i = 0; i != inSize; ++i) {
    if (hOut[i] == 0)
      numDropout++;
  }

  unsigned expectedDrop = inSize - dropoutProb * inSize;
  unsigned allowedError = percentError * inSize / 100;

  bool failed = (((numDropout > expectedDrop + allowedError) ||
                  (numDropout + allowedError < expectedDrop)));
  if (failed) {
    std::cerr << "Validation of Dropout failed\n";
  }
  return failed;
}

enum class TestType {
  SetSeeds,
  SetHwSeeds,
  Bernoulli,
  BernoulliInt,
  Uniform,
  UniformInt,
  LogUniform,
  LogUniformInt,
  Normal,
  TruncatedNormal,
  Dropout
};

static TestType getTestType(const std::string &testType) {
  if (testType == "SetSeeds") {
    return TestType::SetSeeds;
  } else if (testType == "SetHwSeeds") {
    return TestType::SetHwSeeds;
  } else if (testType == "Bernoulli") {
    return TestType::Bernoulli;
  } else if (testType == "BernoulliInt") {
    return TestType::BernoulliInt;
  } else if (testType == "Uniform") {
    return TestType::Uniform;
  } else if (testType == "UniformInt") {
    return TestType::UniformInt;
  } else if (testType == "LogUniform") {
    return TestType::LogUniform;
  } else if (testType == "LogUniformInt") {
    return TestType::LogUniformInt;
  } else if (testType == "Normal") {
    return TestType::Normal;
  } else if (testType == "TruncatedNormal") {
    return TestType::TruncatedNormal;
  } else if (testType == "Dropout") {
    return TestType::Dropout;
  } else {
    throw poplibs_test::poplibs_test_error("Invalid random test");
  }
}

const OptionFlags simDebugOptions{{"debug.trace", "false"}};

int main(int argc, char **argv) {
  namespace po = boost::program_options;

  float mean;
  float stdDev;
  double minVal;
  double maxVal;
  float alpha;
  float prob;
  float percentError;
  float rejectionThreshold;
  bool deviceHalf;
  bool fpChecking;
  unsigned inSize;
  std::string randTest;
  DeviceType deviceType = DeviceType::Cpu; // IpuModel;
  boost::optional<unsigned> tilesPerIPU;
  unsigned seed;
  unsigned seedModifier;
  unsigned numLoops;
  double base;

  po::options_description desc("Options");
  // clang-format off
  desc.add_options()
    ("help", "Produce help message")
    ("compile-only", "Stop after compilation; don't run the program")
    ("device-type",
     po::value<DeviceType>(&deviceType)->default_value(deviceType),
     deviceTypeHelp)
    ("tiles-per-ipu",
     po::value(&tilesPerIPU),
     "Number of tiles per IPU")
    ("seed", po::value<unsigned>(&seed)->default_value(12352345), "prng seed")
    ("seed-modifier", po::value<unsigned>(&seedModifier)->default_value(785439),
     "prng seedModifier")
    ("profile", "Output profiling report")
    ("mean", po::value<float>(&mean)->default_value(0.0),
     "Mean value. Used by Gaussian and Truncated Gaussian")
    ("std-dev", po::value<float>(&stdDev)->default_value(1.0),
     "stdDev value. Used by Gaussian and Truncated Gaussian Tests")
    ("min-val", po::value<double>(&minVal)->default_value(0.0),
     "Min Values used for uniform distribution test")
    ("max-val", po::value<double>(&maxVal)->default_value(1.0),
     "Max Values used for uniform distribution test")
    ("half-data-type", po::value<bool>(&deviceHalf)->default_value(false),
     "Half precision input/output, else float (ignored for UniformInt test)")
    ("alpha", po::value<float>(&alpha)->default_value(2.0),
     "Alpha used by the truncated normal test")
    ("prob", po::value<float>(&prob)->default_value(1.0),
     "Probability used by Bernoulli and Dropout tests")
    ("percent-error", po::value<float>(&percentError)->default_value(2.0),
     "Tolerance level")
    ("repeat", po::value<unsigned>(&numLoops)->default_value(1u),
     "Number of times to repeat test")
    ("rand-test",
     po::value<std::string>(&randTest)->default_value("None"),
     "Random Test: Uniform | UniformInt | Bernoulli| BernoulliInt | Normal "
     "| TruncatedNormal | Dropout | SetSeeds | SetHwSeeds | LogUniformInt")
    ("in-size",
     po::value<unsigned>(&inSize)->default_value(12),
     "Vector size")
    ("rejection-threshold",
     po::value<float>(&rejectionThreshold)->default_value(false),
     "Chi squared p value threshold used for log uniform tests")
    ("base",
     po::value<double>(&base)->default_value(M_E),
     "Base of logarithm for log uniform tests")
    ("fp-checking",
     po::value(&fpChecking)->default_value(true),
     "Enable hardware floating-point checks"
  );
  // clang-format on

  po::variables_map vm;
  try {
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help")) {
      std::cout << desc << "\n";
      return 1;
    }
    po::notify(vm);
  } catch (std::exception &e) {
    std::cerr << "error: " << e.what() << "\n";
    return 1;
  }

  auto testType = getTestType(randTest);

  auto dev = [&]() -> TestDevice {
    if (isIpuModel(deviceType)) {
      // When running on the IPU model we apply global exchange constraints,
      // which is why we create the device from the model here and not using
      // the normal createTestDevice factory function.
      IPUModel ipuModel(deviceTypeToIPUName(deviceType));
      if (tilesPerIPU)
        ipuModel.tilesPerIPU = *tilesPerIPU;
      addGlobalExchangeConstraints(ipuModel);
      setGlobalSyncLatency(ipuModel);
      return ipuModel.createDevice();
    } else {
      if (tilesPerIPU)
        return createTestDevice(deviceType, 1, *tilesPerIPU);
      else
        return createTestDeviceFullSize(deviceType);
    }
  }();

  const auto &target = dev.getTarget();
  Graph graph(target);
  poprand::addCodelets(graph);
  popops::addCodelets(graph);

  auto randProg = Sequence();
  auto checkSeedSeq = Sequence();

  const bool defaultSeed = vm["seed"].defaulted();

  uint32_t hSeed[2];
  hSeed[0] = seed;
  hSeed[1] = ~seed;

  Type dType = (randTest == "UniformInt" || randTest == "LogUniformInt")
                   ? poplar::INT
                   : (deviceHalf ? poplar::HALF : poplar::FLOAT);

  auto tSeed = graph.addVariable(poplar::UNSIGNED_INT, {2}, "tSeed");
  graph.setTileMapping(tSeed, 0);

  // We pad the dropout input with trailing NaNs. Add extra padding if there is
  // an odd half, so that the host copy is a multiple of 4 bytes
  unsigned padSize = 0;
  if (deviceHalf)
    padSize += inSize % 2;
  auto paddedInSize = inSize + padSize;
  auto paddedReference = graph.addVariable(dType, {paddedInSize}, "ref");
  auto reference = paddedReference.slice({0, inSize});
  mapTensorLinearly(graph, paddedReference);

  poprand::setSeed(graph, tSeed, seedModifier, randProg, "setSeed");

  // handle setseed, SetHwSeed tests as they are different from the others
  if (testType == TestType::SetSeeds || testType == TestType::SetHwSeeds) {
    auto seedsWrite = graph.addVariable(
        poplar::UNSIGNED_INT,
        {target.getNumTiles(), target.getNumWorkerContexts(), 4}, "hwSeeds");
    mapTensorLinearly(graph, seedsWrite);
    std::vector<uint32_t> hSeedsWrite(seedsWrite.numElements());
    for (unsigned i = 0; i < hSeedsWrite.size(); i++) {
      hSeedsWrite[i] = 200 * i + 1000;
    }
    if (testType == TestType::SetHwSeeds) {
      poplar::setHwSeeds(graph, seedsWrite, randProg, "setHwSeeds");
    }

    auto seedsRead = poplar::getHwSeeds(graph, randProg);
    std::vector<uint32_t> hostSeedsRead(seedsRead.numElements());
    graph.createHostWrite("seed", tSeed);
    graph.createHostWrite("seedsWrite", seedsWrite);
    graph.createHostRead("seedsRead", seedsRead);
    Engine eng(graph, randProg, options);
    dev.bind([&](const Device &d) {
      eng.load(d);
      eng.writeTensor("seed", hSeed);
      eng.writeTensor("seedsWrite", &hSeedsWrite[0]);
      eng.run();
      eng.readTensor("seedsRead", hostSeedsRead.data(),
                     hostSeedsRead.data() + hostSeedsRead.size());
    });
    std::set<std::vector<unsigned>> unique_seeds;
    assert(seedsRead.rank() == 3);
    assert(seedsRead.numElements() == hostSeedsRead.size());
    for (unsigned t = 0; t != seedsRead.dim(0); ++t) {
      for (unsigned w = 0; w != seedsRead.dim(1); ++w) {
        std::vector<unsigned> workerSeed(seedsRead.dim(2));
        for (unsigned s = 0; s != seedsRead.dim(2); ++s) {
          auto wSeed =
              hostSeedsRead[t * seedsRead.dim(1) * seedsRead.dim(2) +
                            w * seedsRead.dim(2) + seedsRead.dim(2) - 1 - s];
          workerSeed[s] = wSeed;
        }
        unique_seeds.insert(workerSeed);
      }
    }
    if (isSimulator(deviceType) || deviceType == DeviceType::Hw) {
      if (testType == TestType::SetHwSeeds) {
        return !(std::equal(hostSeedsRead.begin(), hostSeedsRead.end(),
                            hSeedsWrite.begin()));
      } else {
        return !(unique_seeds.size() == seedsRead.dim(0) * seedsRead.dim(1));
      }
    }
    return 1;
  }

  if (fpChecking) {
    if (deviceType == DeviceType::Cpu)
      std::cerr << "Warning: fp-checking not supported on CPU target\n";
    poplar::FloatingPointBehaviour checkAll(true, true, true, false, true);
    poplar::setFloatingPointBehaviour(graph, randProg, checkAll,
                                      "/initialValue");
  }
  graph.createHostWrite("tSeed", tSeed);

  auto *seedToUseInTest = defaultSeed ? nullptr : &tSeed;

  std::vector<float> flpRandOut(paddedInSize);
  std::vector<int> intRandOut(paddedInSize);

  // validate returns false on success
  auto validate = [&]() {
    if (randTest == "Normal") {
      return validateNormal<float>(flpRandOut, inSize, mean, stdDev,
                                   percentError);
    } else if (randTest == "TruncatedNormal") {
      return validateTruncNormal<float>(flpRandOut, inSize, mean, stdDev, alpha,
                                        percentError);
    } else if ((randTest == "Uniform") || (randTest == "UniformInt")) {
      if (dType == poplar::INT) {
        return validateUniform(intRandOut, inSize, minVal, maxVal,
                               percentError);
      }
      return validateUniform(flpRandOut, inSize, minVal, maxVal, percentError);
    } else if ((randTest == "LogUniform") || (randTest == "LogUniformInt")) {
      if (dType == poplar::INT) {
        return validateLogUniform<int>(intRandOut, inSize, minVal, maxVal,
                                       rejectionThreshold, base, percentError);
      }
      return validateLogUniform<float>(flpRandOut, inSize, minVal, maxVal,
                                       rejectionThreshold, base, percentError);
    } else if ((randTest == "Bernoulli") or (randTest == "BernoulliInt")) {
      if (dType == poplar::INT) {
        return validateBernoulli<int>(intRandOut, inSize, prob, percentError);
      }
      return validateBernoulli<float>(flpRandOut, inSize, prob, percentError);
    } else if (randTest == "Dropout") {
      return validateDropout<float>(flpRandOut, inSize, prob,
                                    deviceHalf ? HALF_REL_TOL : FLOAT_REL_TOL,
                                    percentError);
    }
    return false;
  };
  Tensor out;
  if (testType == TestType::Dropout) {
    std::vector<float> flpInput(paddedInSize);

    for (unsigned idx = 0; idx != inSize; ++idx) {
      flpInput[idx] = 1.0;
    }
    for (unsigned idx = inSize; idx != paddedInSize; ++idx) {
      flpInput[idx] = std::numeric_limits<float>::signaling_NaN();
    }

    auto paddedIn = graph.addVariable(dType, {paddedInSize}, "paddedIn");
    auto in = paddedIn.slice({0, inSize});
    mapTensorLinearly(graph, paddedIn);

    graph.createHostWrite("paddedIn", paddedIn, true);

    auto seedsReadBefore = poplar::getHwSeeds(graph, randProg);

    out = poprand::dropout(graph, seedToUseInTest, seedModifier, in, reference,
                           prob, 1.0 / prob, randProg);
    auto seedsReadAfter = poplar::getHwSeeds(graph, randProg);

    graph.createHostRead("out", out);
    graph.createHostRead("seedsReadBefore", seedsReadBefore);
    graph.createHostRead("seedsReadAfter", seedsReadAfter);

    std::vector<uint32_t> hostSeedsReadBefore(seedsReadBefore.numElements());
    std::vector<uint32_t> hostSeedsReadAfter(seedsReadAfter.numElements());

    Engine engine(graph, randProg);

    if (vm.count("compile-only"))
      return 0;

    if (deviceHalf) {
      dev.bind([&](const Device &d) {
        engine.load(d);
        engine.writeTensor("tSeed", hSeed);
        convertAndWriteTensor<float, true>(target, engine, "paddedIn", flpInput,
                                           paddedInSize);
        engine.run();
        readAndConvertTensor<float, true>(graph.getTarget(), engine, "out",
                                          flpRandOut, inSize);
        engine.readTensor("seedsReadBefore", hostSeedsReadBefore.data(),
                          hostSeedsReadBefore.data() +
                              hostSeedsReadBefore.size());
        engine.readTensor("seedsReadAfter", hostSeedsReadAfter.data(),
                          hostSeedsReadAfter.data() +
                              hostSeedsReadAfter.size());
      });
    } else {

      dev.bind([&](const Device &d) {
        engine.load(d);
        engine.writeTensor("tSeed", hSeed);
        convertAndWriteTensor<float, false>(target, engine, "paddedIn",
                                            flpInput, paddedInSize);
        engine.run();
        readAndConvertTensor<float, false>(graph.getTarget(), engine, "out",
                                           flpRandOut, inSize);
        engine.readTensor("seedsReadBefore", hostSeedsReadBefore.data(),
                          hostSeedsReadBefore.data() +
                              hostSeedsReadBefore.size());
        engine.readTensor("seedsReadAfter", hostSeedsReadAfter.data(),
                          hostSeedsReadAfter.data() +
                              hostSeedsReadAfter.size());
      });
    }

    if (seedToUseInTest) {
      if (!std::equal(hostSeedsReadBefore.begin(), hostSeedsReadBefore.end(),
                      hostSeedsReadAfter.begin())) {
        std::cerr << "hw seeds read before and after do not match\n";
        return 1;
      }
    } else if (deviceType != DeviceType::Cpu && !isIpuModel(deviceType)) {
      if (std::equal(hostSeedsReadBefore.begin(), hostSeedsReadBefore.end(),
                     hostSeedsReadAfter.begin())) {
        // there is always some work done and hw seeds should change
        std::cerr << "hw seeds read before and after match when they "
                     "should not\n";
        return 1;
      }
    }
  } else {
    auto seedsReadBefore = poplar::getHwSeeds(graph, randProg);

    if (testType == TestType::Normal) {
      out = poprand::normal(graph, seedToUseInTest, seedModifier, reference,
                            dType, mean, stdDev, randProg);
    } else if (testType == TestType::TruncatedNormal) {
      out = poprand::truncatedNormal(graph, seedToUseInTest, seedModifier,
                                     reference, dType, mean, stdDev, alpha,
                                     randProg);
    } else if (testType == TestType::Uniform ||
               testType == TestType::UniformInt) {
      out = poprand::uniform(graph, seedToUseInTest, seedModifier, reference,
                             dType, minVal, maxVal, randProg);
    } else if (testType == TestType::LogUniformInt ||
               testType == TestType::LogUniform) {
      out = poprand::logUniform(graph, seedToUseInTest, seedModifier, reference,
                                dType, minVal, maxVal, randProg, base);
    } else if (testType == TestType::Bernoulli ||
               testType == TestType::BernoulliInt) {
      out = poprand::bernoulli(graph, seedToUseInTest, seedModifier, reference,
                               dType, prob, randProg);
    }
    auto seedsReadAfter = poplar::getHwSeeds(graph, randProg);
    std::vector<uint32_t> hostSeedsReadBefore(seedsReadBefore.numElements());
    std::vector<uint32_t> hostSeedsReadAfter(seedsReadAfter.numElements());

    graph.createHostRead("out", out);
    graph.createHostRead("seedsReadBefore", seedsReadBefore);
    graph.createHostRead("seedsReadAfter", seedsReadAfter);

    Engine engine(graph, randProg);

    // We can't check pre and post seeds for LogUniform since it involves a
    // cast which changes the RNG state.
    bool checkSeed =
        testType != TestType::LogUniformInt && testType != TestType::LogUniform;

    dev.bind([&](const Device &d) {
      engine.load(d);
      for (unsigned i = 0; i != numLoops; ++i) {
        engine.writeTensor("tSeed", hSeed);
        engine.run();
        if (dType == poplar::INT) {
          readAndConvertTensor<int, false>(graph.getTarget(), engine, "out",
                                           intRandOut, inSize);
        } else if (deviceHalf) {
          readAndConvertTensor<float, true>(graph.getTarget(), engine, "out",
                                            flpRandOut, inSize);
        } else {
          readAndConvertTensor<float, false>(graph.getTarget(), engine, "out",
                                             flpRandOut, inSize);
        }
        engine.readTensor("seedsReadBefore", hostSeedsReadBefore.data(),
                          hostSeedsReadBefore.data() +
                              hostSeedsReadBefore.size());
        engine.readTensor("seedsReadAfter", hostSeedsReadAfter.data(),
                          hostSeedsReadAfter.data() +
                              hostSeedsReadAfter.size());
        if (checkSeed) {
          if (seedToUseInTest) {
            if (!std::equal(hostSeedsReadBefore.begin(),
                            hostSeedsReadBefore.end(),
                            hostSeedsReadAfter.begin())) {
              std::cerr << " hw seeds read before and after do not match \n";
              std::exit(1);
            }
          } else if (deviceType != DeviceType::Cpu && !isIpuModel(deviceType)) {
            if (std::equal(hostSeedsReadBefore.begin(),
                           hostSeedsReadBefore.end(),
                           hostSeedsReadAfter.begin())) {
              // there is always some work done and hw seeds should change
              std::cerr << "hw seeds read before and after match when they "
                           "should not\n";
              std::exit(1);
            }
          }
        }
        auto invalid = validate();
        if (numLoops > 1) {
          std::cerr << "Test " << i << "/" << numLoops << ": "
                    << (invalid ? "Fail" : "Pass") << "\n";
        }
        if (invalid)
          std::exit(1);
        hSeed[0]++;
        if (hSeed[0] == 0)
          hSeed[1]++;
      }
    });
  }
}


// Boost.Geometry

// Copyright (c) 2017 Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_OVERLAY_RANGE_IN_GEOMETRY_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_OVERLAY_RANGE_IN_GEOMETRY_HPP

#include <boost/geometry/algorithms/covered_by.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/tags.hpp>
#include <boost/geometry/iterators/point_iterator.hpp>

#include <boost/range.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace overlay
{

template
<
    typename Geometry,
    typename Tag = typename geometry::tag<Geometry>::type
>
struct points_range
{
    typedef geometry::point_iterator<Geometry const> iterator_type;

    explicit points_range(Geometry const& geometry)
        : m_geometry(geometry)
    {}

    iterator_type begin() const
    {
        return geometry::points_begin(m_geometry);
    }

    iterator_type end() const
    {
        return geometry::points_end(m_geometry);
    }

    Geometry const& m_geometry;
};
// Specialized because point_iterator doesn't support boxes
template <typename Box>
struct points_range<Box, box_tag>
{
    typedef typename geometry::point_type<Box>::type point_type;
    typedef const point_type * iterator_type;

    explicit points_range(Box const& box)
    {
        detail::assign_box_corners(box,
            m_corners[0], m_corners[1], m_corners[2], m_corners[3]);
    }

    iterator_type begin() const
    {
        return m_corners;
    }

    iterator_type end() const
    {
        return m_corners + 4;
    }

    point_type m_corners[4];
};

template
<
    typename Geometry,
    typename Tag = typename geometry::tag<Geometry>::type
>
struct point_in_geometry_helper
{
    template <typename Point, typename Strategy>
    static inline int apply(Point const& point, Geometry const& geometry,
                            Strategy const& strategy)
    {
        return detail::within::point_in_geometry(point, geometry, strategy);
    }
};
// Specialized because point_in_geometry doesn't support Boxes
template <typename Box>
struct point_in_geometry_helper<Box, box_tag>
{
    template <typename Point, typename Strategy>
    static inline int apply(Point const& point, Box const& box,
                            Strategy const&)
    {
        return geometry::covered_by(point, box) ? 1 : -1;
    }
};

// This function returns
// when it finds a point of geometry1 inside or outside geometry2
template <typename Geometry1, typename Geometry2, typename Strategy>
static inline int range_in_geometry(Geometry1 const& geometry1,
                                    Geometry2 const& geometry2,
                                    Strategy const& strategy,
                                    bool skip_first = false)
{
    int result = 0;
    points_range<Geometry1> points(geometry1);
    typedef typename points_range<Geometry1>::iterator_type iterator_type;
    iterator_type const end = points.end();
    iterator_type it = points.begin();
    if (it == end)
    {
        return result;
    }
    else if (skip_first)
    {
        ++it;
    }

    typename Strategy::template point_in_geometry_strategy
        <
            Geometry1, Geometry2
        >::type const in_strategy
        = strategy.template get_point_in_geometry_strategy<Geometry1, Geometry2>();

    for ( ; it != end; ++it)
    {
        result = point_in_geometry_helper<Geometry2>::apply(*it, geometry2, in_strategy);
        if (result != 0)
        {
            return result;
        }
    }
    // all points contained entirely by the boundary
    return result;
}

// This function returns if first_point1 is inside or outside geometry2 or
// when it finds a point of geometry1 inside or outside geometry2
template <typename Point1, typename Geometry1, typename Geometry2, typename Strategy>
inline int range_in_geometry(Point1 const& first_point1,
                             Geometry1 const& geometry1,
                             Geometry2 const& geometry2,
                             Strategy const& strategy)
{
    // check a point on border of geometry1 first
    int result = point_in_geometry_helper<Geometry2>::apply(first_point1, geometry2,
                    strategy.template get_point_in_geometry_strategy<Point1, Geometry2>());
    if (result == 0)
    {
        // if a point is on boundary of geometry2
        // check points of geometry1 until point inside/outside is found
        // NOTE: skip first point because it should be already tested above
        result = range_in_geometry(geometry1, geometry2, strategy, true);
    }
    return result;
}

}} // namespace detail::overlay
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_OVERLAY_RANGE_IN_GEOMETRY_HPP


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#pragma once

#include <armnn/Tensor.hpp>

#include <boost/assert.hpp>

#include <DataLayoutIndexed.hpp>

namespace armnn
{

// Utility class providing access to raw tensor memory based on indices along each dimension.
template <typename DataType>
class TensorBufferArrayView
{
public:
    TensorBufferArrayView(const TensorShape& shape, DataType* data,
                          armnnUtils::DataLayoutIndexed dataLayout = DataLayout::NCHW)
        : m_Shape(shape)
        , m_Data(data)
        , m_DataLayout(dataLayout)
    {
        BOOST_ASSERT(m_Shape.GetNumDimensions() == 4);
    }

    DataType& Get(unsigned int b, unsigned int c, unsigned int h, unsigned int w) const
    {
        return m_Data[m_DataLayout.GetIndex(m_Shape, b, c, h, w)];
    }

private:
    const TensorShape             m_Shape;
    DataType*                     m_Data;
    armnnUtils::DataLayoutIndexed m_DataLayout;
};

} //namespace armnn


#ifndef LM_INTERPOLATE_TUNE_DERIVATIVES_H
#define LM_INTERPOLATE_TUNE_DERIVATIVES_H

#include "lm/interpolate/tune_matrix.hh"

#include <Eigen/Core>
#include <cmath>

namespace lm { namespace interpolate {

class Instances;

// Given tuning instances and model weights, computes the objective function (log probability), gradient, and Hessian.
// Returns log probability / number of instances.
Accum Derivatives(Instances &instances /* Doesn't modify but ReadExtensions is lazy */, const Vector &weights, Vector &gradient, Matrix &hessian);

}} // namespaces

#endif // LM_INTERPOLATE_TUNE_DERIVATIVES_H


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

#include <complex>
#include <boost/preprocessor/cat.hpp>
#include <nt2/linalg/details/blas/blas3.hpp>
#include <nt2/linalg/details/utility/f77_wrapper.hpp>

namespace nt2 { namespace details
{
#define NT2_SM(T, PX)                                                           \
inline void trsm( const char *side, const char *uplo, const char *transa        \
                , const long int *m, const long int *n                          \
                , const T *al, const T *a, const long int *lda                  \
                , T *b, const long int *ldb                                     \
                )                                                               \
{                                                                               \
  NT2_F77NAME(BOOST_PP_CAT(PREFIX,trsm))(side,uplo,transa,m,n,al,a,lda,b,ldb);  \
}                                                                               \
/**/

  // INTERNAL ONLY
  // all sm call: trsm for data types float, double and related complex
  NT2_SM(double, d)
  NT2_SM(float,  s)
  NT2_SM(std::complex<double>, z)
  NT2_SM(std::complex<float>, c)

#undef NT2_SM
} }

#endif


#include "messages/sphere_locs.pb.h"
#include "messages/sphere_tracts.pb.h"
#include "utility/options.hpp"
#include "utility/load_protobuf.hpp"
#include "utility/protobuf_zip_ostream.hpp"
#include "utility/units.hpp"
#include <boost/iostreams/filtering_stream.hpp>
#include <boost/iostreams/filter/zlib.hpp>
#include <fmt/ostream.h>

using namespace jhmi;

int main(int argc, char* argv[]) {
  auto opts = options<spherefiles_option>{};
  if (!opts.parse(argc, argv))
    return 1;

  for (auto&& file : opts.spherefiles()) {
    auto protofile = load_protobuf<jhmi_message::SphereTracts>(file);
    jhmi_message::SphereLocs sl;
    int num_spheres = 0;
    auto sphere_diameter = protofile.sphere_diameter() * meters;
    for (auto&& tract : protofile.tracts()) {
      auto start = dbl3{tract.sx(), tract.sy(), tract.sz()} * meters;
      auto end   = dbl3{tract.ex(), tract.ey(), tract.ez()} * meters;
      auto sstep = sphere_diameter * (start - end) / distance(end - start);
      for (int i = 0; i < tract.num_spheres(); ++i) {
        auto sloc = end + double(i) * sstep;
        auto l = sl.add_locations();
        l->set_x(sloc.x.value());
        l->set_y(sloc.y.value());
        l->set_z(sloc.z.value());
        l->set_vessel_id(tract.vessel_id());
        ++num_spheres;
      }
    }
    auto new_file = file;
    new_file.replace_extension(".1.pbz");
    protobuf_zip_ostream output_stream{new_file};
    if (!sl.SerializeToZeroCopyStream(output_stream.get()))
      throw std::runtime_error("Failed to write spherefiles.");
    fmt::print("{} spheres written from {} to {}\n", num_spheres, file, new_file);
  }

  return 0;
}


#include <iostream>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include "crosslink.hpp"
#include "crosslinkPair.hpp"
#include "crosslinkTriple.hpp"

#include "crosslinkPair2.hpp"
#include "crosslinkTriple2.hpp"

//#include <random>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>

#include <boost/random/uniform_real.hpp>

#include "rngmit.hpp"

using namespace std;
namespace ublas = boost::numeric::ublas;

class Simulation {
public:
	Simulation(unsigned int, double, double, double, double, int, string, bool);

	void integrate(unsigned int);

	void sampleCoordinates();
	void sampleEnergy();
	void sampleTriples();
	void samplePairs();
	void sampleRR();
	void sampleNetwork();
	void sampleFilLength(vector<double> contLength);
	void sampleAngles(vector<double> theta);
	void sampleClDist(vector<double> clDist);

	double epot2;
	double epot3;
	double msd;

	//####### properties read in from Henry's network files ######//
	int NATOMS; // number of crosslinks
	int FEC,rp,rpp; // ????
	unsigned long int rIA[55];
	double FXX,FYY,FZZ,FXY,FXZ,FYX,FYZ,FZX,FZY; // matrix elements defining box shape
	double BENDING_PF,StretchEps; // ????
	double L, Lc, Lp; // L: length of (quadratic) box, Lc: contour length of polymers, Lp: persistence length
	ublas::vector<double> posX,posY,posZ; // particle positions

	ublas::matrix<int> NN,GB,WBX,WBY,WBZ;; // connectivity table: NN: indices of particles, GB: ghost bonds, WBX/Y/Z: gives (virtual) in which connected particle is
	ublas::matrix<double> RR; // contour length for connections in connectivity table
	//################################################//

	//double k2; // longitudinal spring constant of WLC
	//double Leq; // equilibrium length of WLC

	//double k3; // force constant for three particle force

	double stress; // total stress of the system
	double stress2,stress3; // stress from pair/triple interaction

	double gStorage, gLoss; // storage and loss modulus

	double eTotAveraged; // energy averaged over nAv time steps
	int nAv;

	int nCross;

	string outFolder;
	string outFolderNW;

	string inFile;

	bool CONTINUE;

private:

	void createFilament_(int filId, std::vector<int> fil);
	void initialize_();
	void initStreams_();

	void resetForce_();
	void force_();
	void force2_(CrosslinkPair2 &clPair);
	void force3_(CrosslinkTriple2 &clTriple);
	void euler_();
	void applyPeriodicBC_();

	double gaussian_random_number_();
	ublas::vector<double> gaussian_random_vector_();
	ublas::vector<double> gaussian_random_vector2_();

	void mean_square_displacement();

	void calcPullProperties();

	void loadNetwork(string file);

	///!!! NEW !!!
	std::vector<ublas::matrix<int>> createConTabSO(int i);
	void createCrosslinks();
	void createPairList();
	void createTripleList();
	///!!!

	void shear_();

	void correctRR_(int i);

	void compareForces_();

	std::vector<Crosslink> particles_;
	std::vector<CrosslinkPair> particlePairs_;
	std::vector<CrosslinkTriple> particleTriples_;

	///!!! NEW !!!
	std::vector<CrosslinkPair2> particlePairs2_;
	std::vector<CrosslinkTriple2> particleTriples2_;
	///!!!

	int nParticles_;
	double t_;
	double dt_;
	ofstream coordinatesStream_;
	ofstream energyStream_;
	ofstream energyAveragedStream_;
	ofstream pairStream_;
	ofstream tripleStream_;
	ofstream rrStream_;
	ofstream networkStream_;

	ofstream debugStream;

	ofstream filLengthStream_;
	ofstream angleStream_;
	ofstream clDistStream_;

	//typedef std::vector<Crosslink> particlevector;

	int seed_;

	boost::random::mt19937 rng_;
	boost::random::normal_distribution<> rndm_;
	boost::variate_generator< boost::random::mt19937, boost::random::normal_distribution<> >
                    dice_;
    //boost::random::uniform_real_distribution<> rndmU_;
    //boost::variate_generator< boost::random::mt19937, boost::random::uniform_real_distribution<> > diceU_;
	//mt19937 generator_;

	double T_;
	double gamma1_,gamma2_;

	double t_startShear_;

	double relGamma_; // gamma_crosslink/gamma_filament

	double gamma0_; // shear amplitude
	double omega_; // shear frequency

	double cut_; //maximum filament length between 2 crosslinks

};


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/function/definition/nearbyint.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/copysign.hpp>
#include <boost/simd/function/is_even.hpp>
#include <boost/simd/function/is_odd.hpp>
#include <boost/simd/function/nearbyint.hpp>
#include <boost/simd/function/tofloat.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename A0)
                          , bd::cpu_
                          , bs::tag::nearbyint_
                          , bd::scalar_< bd::int64_<A0> >
                          , bd::scalar_< bd::int64_<A0> >
                          )
  {
    A0 operator() ( bd::functor<bs::tag::nearbyint_> const&
                  , A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      using ui_t = bd::as_integer_t<A0, unsigned>;
      if (!a0) return  Zero<A0>();
      if(a1)
      {
        ui_t aa0 = bs::abs(a0);
        ui_t aa1 = bs::abs(a1);
        ui_t q = aa0/aa1;
        ui_t rx2 = 2*(aa0-q*aa1);
        if (rx2 >= aa1)
        {
          if ((rx2 == aa1) && is_even(q)) --q;
          ++q;
        }
        return copysign(A0(q), a0^a1);
      }
      else
        return ((a0>0) ? Valmax<A0>() : Valmin<A0>());
    }
  };

  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename A0)
                          , bd::cpu_
                          , bs::tag::nearbyint_
                          , bd::scalar_< bd::signed_<A0> >
                          , bd::scalar_< bd::signed_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( bd::functor<bs::tag::nearbyint_> const&
                                    , A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      if(a1)
        return static_cast<A0 >(nearbyint(static_cast<double>(a0)/static_cast<double>(a1)));
      else
      {
        return (a0) ? ((a0>0) ? Valmax<A0>() : Valmin<A0>()) : Zero<A0>();
      }
    }
  };

  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename A0)
                          , bd::cpu_
                          , bs::tag::nearbyint_
                          , bd::scalar_< bd::unsigned_<A0> >
                          , bd::scalar_< bd::unsigned_<A0> >
                          )
  {
    A0 operator() ( bd::functor<bs::tag::nearbyint_> const&
                  , A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      if(a1)
      {
        A0 q = a0/a1;
        A0 rx2 = Two<A0>()*(a0-q*a1);
        if (rx2 >= a1)
        {
          if ((rx2 == a1) && is_even(q)) --q;
          ++q;
        }
        return q;
      }
      else
        return (a0) ? Valmax<A0>() : Zero<A0>();
    }
  };

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable: 4723) // potential divide by 0
#endif

  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename A0)
                          , bd::cpu_
                          , bs::tag::nearbyint_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( bd::functor<bs::tag::nearbyint_> const&
                                    , A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return nearbyint(a0/a1);
    }
  };
} } }

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

#endif


//
// Copyright (c) 2015-2019 CNRS INRIA
// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#ifndef __pinocchio_joint_prismatic_unaligned_hpp__
#define __pinocchio_joint_prismatic_unaligned_hpp__

#include "pinocchio/macros.hpp"
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/joint/joint-translation.hpp"
#include "pinocchio/multibody/constraint.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/math/matrix.hpp"

namespace pinocchio
{

  template<typename Scalar, int Options=0> struct MotionPrismaticUnalignedTpl;
  typedef MotionPrismaticUnalignedTpl<double> MotionPrismaticUnaligned;

  template<typename Scalar, int Options>
  struct SE3GroupAction< MotionPrismaticUnalignedTpl<Scalar,Options> >
  {
    typedef MotionTpl<Scalar,Options> ReturnType;
  };

  template<typename Scalar, int Options, typename MotionDerived>
  struct MotionAlgebraAction< MotionPrismaticUnalignedTpl<Scalar,Options>, MotionDerived>
  {
    typedef MotionTpl<Scalar,Options> ReturnType;
  };

  template<typename _Scalar, int _Options>
  struct traits< MotionPrismaticUnalignedTpl<_Scalar,_Options> >
  {
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
    typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
    typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
    typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
    typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
    typedef Vector3 AngularType;
    typedef Vector3 LinearType;
    typedef const Vector3 ConstAngularType;
    typedef const Vector3 ConstLinearType;
    typedef Matrix6 ActionMatrixType;
    typedef MotionTpl<Scalar,Options> MotionPlain;
    typedef MotionPlain PlainReturnType;
    enum {
      LINEAR = 0,
      ANGULAR = 3
    };
  }; // traits MotionPrismaticUnalignedTpl

  template<typename _Scalar, int _Options>
  struct MotionPrismaticUnalignedTpl
  : MotionBase < MotionPrismaticUnalignedTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    MOTION_TYPEDEF_TPL(MotionPrismaticUnalignedTpl);

    MotionPrismaticUnalignedTpl() {}

    template<typename Vector3Like, typename S2>
    MotionPrismaticUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis,
                                const S2 & v)
    : m_axis(axis), m_v(v)
    { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); }

    inline PlainReturnType plain() const
    {
      return PlainReturnType(m_axis*m_v,
                             PlainReturnType::Vector3::Zero());
    }

    template<typename OtherScalar>
    MotionPrismaticUnalignedTpl __mult__(const OtherScalar & alpha) const
    {
      return MotionPrismaticUnalignedTpl(m_axis,alpha*m_v);
    }

    template<typename Derived>
    void addTo(MotionDense<Derived> & other) const
    {
      other.linear() += m_axis * m_v;
    }

    template<typename Derived>
    void setTo(MotionDense<Derived> & other) const
    {
      other.linear().noalias() = m_axis*m_v;
      other.angular().setZero();
    }

    template<typename S2, int O2, typename D2>
    void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
    {
      v.linear().noalias() = m_v * (m.rotation() * m_axis); // TODO: check efficiency
      v.angular().setZero();
    }

    template<typename S2, int O2>
    MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
    {
      MotionPlain res;
      se3Action_impl(m,res);
      return res;
    }

    template<typename S2, int O2, typename D2>
    void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
    {
      // Linear
      v.linear().noalias() = m_v * (m.rotation().transpose() * m_axis);

      // Angular
      v.angular().setZero();
    }

    template<typename S2, int O2>
    MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
    {
      MotionPlain res;
      se3ActionInverse_impl(m,res);
      return res;
    }

    template<typename M1, typename M2>
    void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
    {
      // Linear
      mout.linear().noalias() = v.angular().cross(m_axis);
      mout.linear() *= m_v;

      // Angular
      mout.angular().setZero();
    }

    template<typename M1>
    MotionPlain motionAction(const MotionDense<M1> & v) const
    {
      MotionPlain res;
      motionAction(v,res);
      return res;
    }

    bool isEqual_impl(const MotionPrismaticUnalignedTpl & other) const
    {
      return m_axis == other.m_axis && m_v == other.m_v;
    }

    const Scalar & linearRate() const { return m_v; }
    Scalar & linearRate() { return m_v; }

    const Vector3 & axis() const { return m_axis; }
    Vector3 & axis() { return m_axis; }

  protected:

    Vector3 m_axis;
    Scalar m_v;
  }; // struct MotionPrismaticUnalignedTpl

  template<typename Scalar, int Options, typename MotionDerived>
  inline typename MotionDerived::MotionPlain
  operator+(const MotionPrismaticUnalignedTpl<Scalar,Options> & m1, const MotionDense<MotionDerived> & m2)
  {
    typedef typename MotionDerived::MotionPlain ReturnType;
    return ReturnType(m1.linearRate() * m1.axis() + m2.linear(), m2.angular());
  }

  template<typename MotionDerived, typename S2, int O2>
  inline typename MotionDerived::MotionPlain
  operator^(const MotionDense<MotionDerived> & m1,
            const MotionPrismaticUnalignedTpl<S2,O2> & m2)
  {
    return m2.motionAction(m1);
  }

  template<typename Scalar, int Options> struct ConstraintPrismaticUnalignedTpl;

  template<typename _Scalar, int _Options>
  struct traits< ConstraintPrismaticUnalignedTpl<_Scalar,_Options> >
  {
    typedef _Scalar Scalar;
    enum { Options = _Options };
    enum {
      LINEAR = 0,
      ANGULAR = 3
    };
    typedef MotionPrismaticUnalignedTpl<Scalar,Options> JointMotion;
    typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
    typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
    typedef DenseBase MatrixReturnType;
    typedef const DenseBase ConstMatrixReturnType;

    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
  }; // traits ConstraintPrismaticUnalignedTpl

  template<typename Scalar, int Options>
  struct SE3GroupAction< ConstraintPrismaticUnalignedTpl<Scalar,Options> >
  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };

  template<typename Scalar, int Options, typename MotionDerived>
  struct MotionAlgebraAction< ConstraintPrismaticUnalignedTpl<Scalar,Options>,MotionDerived >
  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };

  template<typename Scalar, int Options, typename ForceDerived>
  struct ConstraintForceOp< ConstraintPrismaticUnalignedTpl<Scalar,Options>, ForceDerived>
  {
    typedef typename traits< ConstraintPrismaticUnalignedTpl<Scalar,Options> >::Vector3 Vector3;
    typedef Eigen::Matrix<typename PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(Vector3,typename ForceDense<ForceDerived>::ConstAngularType),1,1,Options> ReturnType;
  };

  template<typename Scalar, int Options, typename ForceSet>
  struct ConstraintForceSetOp< ConstraintPrismaticUnalignedTpl<Scalar,Options>, ForceSet>
  {
    typedef typename traits< ConstraintPrismaticUnalignedTpl<Scalar,Options> >::Vector3 Vector3;
    typedef typename MatrixMatrixProduct<Eigen::Transpose<const Vector3>,
    typename Eigen::MatrixBase<const ForceSet>::template NRowsBlockXpr<3>::Type
    >::type ReturnType;
  };

  template<typename _Scalar, int _Options>
  struct ConstraintPrismaticUnalignedTpl
  : ConstraintBase< ConstraintPrismaticUnalignedTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintPrismaticUnalignedTpl)

    enum { NV = 1 };

    typedef typename traits<ConstraintPrismaticUnalignedTpl>::Vector3 Vector3;

    ConstraintPrismaticUnalignedTpl() {}

    template<typename Vector3Like>
    ConstraintPrismaticUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
    : m_axis(axis)
    { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); }

    template<typename Vector1Like>
    JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like,1);
      return JointMotion(m_axis,v[0]);
    }

    template<typename S1, int O1>
    typename SE3GroupAction<ConstraintPrismaticUnalignedTpl>::ReturnType
    se3Action(const SE3Tpl<S1,O1> & m) const
    {
      typename SE3GroupAction<ConstraintPrismaticUnalignedTpl>::ReturnType res;
      MotionRef<DenseBase> v(res);
      v.linear().noalias() = m.rotation()*m_axis;
      v.angular().setZero();
      return res;
    }

    template<typename S1, int O1>
    typename SE3GroupAction<ConstraintPrismaticUnalignedTpl>::ReturnType
    se3ActionInverse(const SE3Tpl<S1,O1> & m) const
    {
      typename SE3GroupAction<ConstraintPrismaticUnalignedTpl>::ReturnType res;
      MotionRef<DenseBase> v(res);
      v.linear().noalias() = m.rotation().transpose()*m_axis;
      v.angular().setZero();
      return res;
    }

    int nv_impl() const { return NV; }

    struct TransposeConst
    {
      const ConstraintPrismaticUnalignedTpl & ref;
      TransposeConst(const ConstraintPrismaticUnalignedTpl & ref) : ref(ref) {}

      template<typename ForceDerived>
      typename ConstraintForceOp<ConstraintPrismaticUnalignedTpl,ForceDerived>::ReturnType
      operator* (const ForceDense<ForceDerived> & f) const
      {
        typedef typename ConstraintForceOp<ConstraintPrismaticUnalignedTpl,ForceDerived>::ReturnType ReturnType;
        ReturnType res;
        res[0] = ref.axis().dot(f.linear());
        return res;
      }

      /* [CRBA]  MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
      template<typename ForceSet>
      typename ConstraintForceSetOp<ConstraintPrismaticUnalignedTpl,ForceSet>::ReturnType
      operator*(const Eigen::MatrixBase<ForceSet> & F)
      {
        EIGEN_STATIC_ASSERT(ForceSet::RowsAtCompileTime==6,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
        /* Return ax.T * F[1:3,:] */
        return ref.axis().transpose() * F.template middleRows<3>(LINEAR);
      }

    };

    TransposeConst transpose() const { return TransposeConst(*this); }

    /* CRBA joint operators
     *   - ForceSet::Block = ForceSet
     *   - ForceSet operator* (Inertia Y,Constraint S)
     *   - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
     *   - SE3::act(ForceSet::Block)
     */
    DenseBase matrix_impl() const
    {
      DenseBase S;
      S.template segment<3>(LINEAR) = m_axis;
      S.template segment<3>(ANGULAR).setZero();
      return S;
    }

    template<typename MotionDerived>
    DenseBase motionAction(const MotionDense<MotionDerived> & v) const
    {
      DenseBase res;
      res.template segment<3>(LINEAR).noalias() = v.angular().cross(m_axis);
      res.template segment<3>(ANGULAR).setZero();

      return res;
    }

    const Vector3 & axis() const { return m_axis; }
    Vector3 & axis() { return m_axis; }

    bool isEqual(const ConstraintPrismaticUnalignedTpl & other) const
    {
      return m_axis == other.m_axis;
    }

  protected:

    Vector3 m_axis;

  }; // struct ConstraintPrismaticUnalignedTpl

  template<typename S1, int O1,typename S2, int O2>
  struct MultiplicationOp<InertiaTpl<S1,O1>, ConstraintPrismaticUnalignedTpl<S2,O2> >
  {
    typedef Eigen::Matrix<S2,6,1,O2> ReturnType;
  };

  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
  namespace impl
  {
    template<typename S1, int O1, typename S2, int O2>
    struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintPrismaticUnalignedTpl<S2,O2> >
    {
      typedef InertiaTpl<S1,O1> Inertia;
      typedef ConstraintPrismaticUnalignedTpl<S2,O2> Constraint;
      typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;

      static inline ReturnType run(const Inertia & Y,
                                   const Constraint & cpu)
      {
        ReturnType res;
        /* YS = [ m -mcx ; mcx I-mcxcx ] [ 0 ; w ] = [ mcxw ; Iw -mcxcxw ] */
        const S1 & m                             = Y.mass();
        const typename Inertia::Vector3 & c      = Y.lever();

        res.template segment<3>(Constraint::LINEAR).noalias() = m*cpu.axis();
        res.template segment<3>(Constraint::ANGULAR).noalias() = c.cross(res.template segment<3>(Constraint::LINEAR));

        return res;
      }
    };
  } // namespace impl

  template<typename M6Like, typename Scalar, int Options>
  struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintPrismaticUnalignedTpl<Scalar,Options> >
  {
    typedef typename SizeDepType<3>::ColsReturn<M6Like>::ConstType M6LikeCols;
    typedef typename Eigen::internal::remove_const<M6LikeCols>::type M6LikeColsNonConst;

    typedef ConstraintPrismaticUnalignedTpl<Scalar,Options> Constraint;
    typedef typename Constraint::Vector3 Vector3;
    typedef const typename MatrixMatrixProduct<M6LikeColsNonConst,Vector3>::type ReturnType;
  };

  /* [ABA] operator* (Inertia Y,Constraint S) */
  namespace impl
  {
    template<typename M6Like, typename Scalar, int Options>
    struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintPrismaticUnalignedTpl<Scalar,Options> >
    {
      typedef ConstraintPrismaticUnalignedTpl<Scalar,Options> Constraint;
      typedef typename MultiplicationOp<Eigen::MatrixBase<M6Like>,Constraint>::ReturnType ReturnType;
      static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y,
                                   const Constraint & cru)
      {
        EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
        return Y.derived().template middleCols<3>(Constraint::LINEAR) * cru.axis();
      }
    };
  } // namespace impl

  template<typename Scalar, int Options> struct JointPrismaticUnalignedTpl;

  template<typename _Scalar, int _Options>
  struct traits< JointPrismaticUnalignedTpl<_Scalar,_Options> >
  {
    enum {
      NQ = 1,
      NV = 1
    };
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef JointDataPrismaticUnalignedTpl<Scalar,Options> JointDataDerived;
    typedef JointModelPrismaticUnalignedTpl<Scalar,Options> JointModelDerived;
    typedef ConstraintPrismaticUnalignedTpl<Scalar,Options> Constraint_t;
    typedef TransformTranslationTpl<Scalar,Options> Transformation_t;
    typedef MotionPrismaticUnalignedTpl<Scalar,Options> Motion_t;
    typedef MotionZeroTpl<Scalar,Options> Bias_t;

    // [ABA]
    typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
    typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
    typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;

    PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE

    typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
    typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
  };

  template<typename Scalar, int Options>
  struct traits< JointDataPrismaticUnalignedTpl<Scalar,Options> >
  { typedef JointPrismaticUnalignedTpl<Scalar,Options> JointDerived; };

  template<typename _Scalar, int _Options>
  struct JointDataPrismaticUnalignedTpl
  : public JointDataBase< JointDataPrismaticUnalignedTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef JointPrismaticUnalignedTpl<_Scalar,_Options> JointDerived;
    PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
    PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR

    Transformation_t M;
    Constraint_t S;
    Motion_t v;
    Bias_t c;

    // [ABA] specific data
    U_t U;
    D_t Dinv;
    UD_t UDinv;

    JointDataPrismaticUnalignedTpl()
    : M(Transformation_t::Vector3::Zero())
    , S(Constraint_t::Vector3::Zero())
    , v(Constraint_t::Vector3::Zero(),(Scalar)0)
    , U(U_t::Zero())
    , Dinv(D_t::Zero())
    , UDinv(UD_t::Zero())
    {}

    template<typename Vector3Like>
    JointDataPrismaticUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
    : M()
    , S(axis)
    , v(axis,(Scalar)NAN)
    , U(), Dinv(), UDinv()
    {}

    static std::string classname() { return std::string("JointDataPrismaticUnaligned"); }
    std::string shortname() const { return classname(); }

  }; // struct JointDataPrismaticUnalignedTpl

  template<typename Scalar, int Options>
  struct traits< JointModelPrismaticUnalignedTpl<Scalar,Options> >
  { typedef JointPrismaticUnalignedTpl<Scalar,Options> JointDerived; };

  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelPrismaticUnalignedTpl);
  template<typename _Scalar, int _Options>
  struct JointModelPrismaticUnalignedTpl
  : public JointModelBase< JointModelPrismaticUnalignedTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef JointPrismaticUnalignedTpl<_Scalar,_Options> JointDerived;
    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);

    typedef JointModelBase<JointModelPrismaticUnalignedTpl> Base;
    using Base::id;
    using Base::idx_q;
    using Base::idx_v;
    using Base::setIndexes;

    typedef Eigen::Matrix<Scalar,3,1,_Options> Vector3;

    JointModelPrismaticUnalignedTpl() {}
    JointModelPrismaticUnalignedTpl(const Scalar & x,
                                    const Scalar & y,
                                    const Scalar & z)
    : axis(x,y,z)
    {
      axis.normalize();
      assert(isUnitary(axis) && "Translation axis is not unitary");
    }

    template<typename Vector3Like>
    JointModelPrismaticUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
    : axis(axis)
    {
      EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
      assert(isUnitary(axis) && "Translation axis is not unitary");
    }

    JointDataDerived createData() const { return JointDataDerived(axis); }

    using Base::isEqual;
    bool isEqual(const JointModelPrismaticUnalignedTpl & other) const
    {
      return Base::isEqual(other) && axis == other.axis;
    }

    template<typename ConfigVector>
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs) const
    {
      typedef typename ConfigVector::Scalar Scalar;
      const Scalar & q = qs[idx_q()];

      data.M.translation().noalias() = axis * q;
    }

    template<typename ConfigVector, typename TangentVector>
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs,
              const typename Eigen::MatrixBase<TangentVector> & vs) const
    {
      calc(data,qs.derived());

      typedef typename TangentVector::Scalar S2;
      const S2 & v = vs[idx_v()];
      data.v.linearRate() = v;
    }

    template<typename Matrix6Like>
    void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
    {
      data.U.noalias() = I.template block<6,3> (0,Inertia::LINEAR) * axis;
      data.Dinv[0] = Scalar(1)/axis.dot(data.U.template segment <3> (Inertia::LINEAR));
      data.UDinv.noalias() = data.U * data.Dinv;

      if (update_I)
        PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
    }

    static std::string classname() { return std::string("JointModelPrismaticUnaligned"); }
    std::string shortname() const { return classname(); }

    /// \returns An expression of *this with the Scalar type casted to NewScalar.
    template<typename NewScalar>
    JointModelPrismaticUnalignedTpl<NewScalar,Options> cast() const
    {
      typedef JointModelPrismaticUnalignedTpl<NewScalar,Options> ReturnType;
      ReturnType res(axis.template cast<NewScalar>());
      res.setIndexes(id(),idx_q(),idx_v());
      return res;
    }

    // data

    ///
    /// \brief 3d main axis of the joint.
    ///
    Vector3 axis;
  }; // struct JointModelPrismaticUnalignedTpl

} //namespace pinocchio

#include <boost/type_traits.hpp>

namespace boost
{
  template<typename Scalar, int Options>
  struct has_nothrow_constructor< ::pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options>
  struct has_nothrow_copy< ::pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options>
  struct has_nothrow_constructor< ::pinocchio::JointDataPrismaticUnalignedTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options>
  struct has_nothrow_copy< ::pinocchio::JointDataPrismaticUnalignedTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};
}

#endif // ifndef __pinocchio_joint_prismatic_unaligned_hpp__