//
// Copyright (c) 2016 - 2017 Mesh Consultants Inc.
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.
//

#include "Geomlib_Rhodo.h"

#include <Eigen/Core>

#include <Urho3D/Core/Variant.h>
#include <Urho3D/Math/Vector3.h>

Geomlib::Rhodo::Rhodo(Rhodo& originalRhodo, int jumpIndex)
{
	Eigen::RowVector3i unitlessCoordinates = originalRhodo.ComputeUnitlessCoordinates()
		+ RhodoEigen::jumpsToNeighbors[jumpIndex];
	family_ = ComputeFamilyFromUnitlessCoordinates(unitlessCoordinates);
	xDiamJumps_ = (unitlessCoordinates(0) - RhodoEigen::iGeneratingRhodos[family_](0)) / 4;
	yDiamJumps_ = (unitlessCoordinates(1) - RhodoEigen::iGeneratingRhodos[family_](1)) / 4;
	zDiamJumps_ = (unitlessCoordinates(2) - RhodoEigen::iGeneratingRhodos[family_](2)) / 4;
}

Geomlib::Rhodo::Rhodo(int xUnitless, int yUnitless, int zUnitless)
{
	Eigen::RowVector3i unitlessCoords(xUnitless, yUnitless, zUnitless);
	family_ = ComputeFamilyFromUnitlessCoordinates(unitlessCoords);
	Eigen::RowVector3i generatingCenter = RhodoEigen::iGeneratingRhodos[family_];
	xDiamJumps_ = (unitlessCoords[0] - generatingCenter[0]) / 4;
	yDiamJumps_ = (unitlessCoords[1] - generatingCenter[1]) / 4;
	zDiamJumps_ = (unitlessCoords[2] - generatingCenter[2]) / 4;
}

Eigen::RowVector3f Geomlib::Rhodo::ComputeCoordinates(float s) const
{
	return s * (RhodoEigen::fGeneratingRhodos[family_] +
		(4 * (float)xDiamJumps_) * Eigen::RowVector3f(1.0f, 0.0f, 0.0f) +
		(4 * (float)yDiamJumps_) * Eigen::RowVector3f(0.0f, 1.0f, 0.0f) +
		(4 * (float)zDiamJumps_) * Eigen::RowVector3f(0.0f, 0.0f, 1.0f));
}

Eigen::RowVector3i Geomlib::Rhodo::ComputeUnitlessCoordinates() const
{
	return RhodoEigen::iGeneratingRhodos[family_] +
		(4 * xDiamJumps_) * Eigen::RowVector3i(1, 0, 0) +
		(4 * yDiamJumps_) * Eigen::RowVector3i(0, 1, 0) +
		(4 * zDiamJumps_) * Eigen::RowVector3i(0, 0, 1);
}

Urho3D::VariantVector Geomlib::Rhodo::ComputeVertices(float scale) const
{
	Urho3D::VariantVector tmp;

	Eigen::RowVector3i uCenter = ComputeUnitlessCoordinates();

	for (unsigned i = 0; i < 14; ++i) {
		Eigen::RowVector3i vert = uCenter + RhodoEigen::rhodoVertexCoords[i];
		Urho3D::Vector3 baseCoords(scale * vert(0), scale * vert(1), scale * vert(2));
		tmp.Push(baseCoords);
	}

	return tmp;
}

Urho3D::VariantVector Geomlib::Rhodo::ComputeRhombicPolylines(float scale) const
{
	Urho3D::VariantVector tmp;

	Eigen::RowVector3i uCenter = ComputeUnitlessCoordinates();
	Eigen::RowVector3f fCenter = scale * Eigen::RowVector3f((float)uCenter(0), (float)uCenter(1), (float)uCenter(2));

	// Loop across the 12 rhombic faces of the rhombic dodecahedron
	for (unsigned i = 0; i < 12; ++i) {
		// Get rhombic face i
		const Eigen::RowVector3f* rhombus = RhodoEigen::neighborRhombi[i];
		Urho3D::VariantMap rhombusPolyLine;
		Urho3D::VariantVector verts;
		// Loop over the 4 vertices of this rhombus
		for (unsigned j = 0; j < 4; ++j) {
			Eigen::RowVector3f v = scale * rhombus[j] + fCenter;

			Urho3D::Vector3 vert(v(0), v(1), v(2));
			verts.Push(vert);
		}
		// Close the rhombus polyline by adding the initial vertex again at the end
		Eigen::RowVector3f v = scale * rhombus[0] + fCenter;
		Urho3D::Vector3 vert(v(0), v(1), v(2));
		verts.Push(vert);

		rhombusPolyLine["vertices"] = verts;
		tmp.Push(rhombusPolyLine);
	}

	return tmp;
}

// hackish, depends on definition of generatingCenterDisplacements
int Geomlib::ComputeFamilyFromUnitlessCoordinates(const Eigen::RowVector3i& unitlessCoordinates)
{
	int a = unitlessCoordinates[0] % 4;
	int b = unitlessCoordinates[1] % 4;
	if (a == 0) {
		if (b == 0) {
			return 0;
		}
		else {
			return 1;
		}
	}
	else {
		if (b == 0) {
			return 2;
		}
		else {
			return 3;
		}
	}
}


/* Copyright (c) 2014, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/features/normal_3d.h>

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

namespace po = boost::program_options;
using std::cout;
using std::endl;

#include <jsCore/timer.hpp>

void ComputePcBoundaries(const pcl::PointCloud<pcl::PointXYZ>&
    pc, Eigen::Vector3f& min, Eigen::Vector3f& max) {
  const Eigen::Map<const Eigen::MatrixXf,1,Eigen::OuterStride<> > x = pc.getMatrixXfMap(1, 4, 0); // this works for PointXYZRGBNormal
  const Eigen::Map<const Eigen::MatrixXf,1,Eigen::OuterStride<> > y = pc.getMatrixXfMap(1, 4, 1); // this works for PointXYZRGBNormal
  const Eigen::Map<const Eigen::MatrixXf,1,Eigen::OuterStride<> > z = pc.getMatrixXfMap(1, 4, 2); // this works for PointXYZRGBNormal
  min << x.minCoeff(), y.minCoeff(), z.minCoeff();
  max << x.maxCoeff(), y.maxCoeff(), z.maxCoeff();
}

int main (int argc, char** argv)
{
  // Declare the supported options.
  po::options_description desc("Allowed options");
  desc.add_options()
    ("help,h", "produce help message")
    ("input,i", po::value<string>(),"path to input")
    ("output,o", po::value<string>(),"path to output")
    ("scale,s", po::value<float>(),"scale for normal extraction search radius")
    ("noiseStd,n", po::value<float>(),"std of isotropic gaussian noise to be added")
    ("outlierRatio,r", po::value<float>(),"ratio of outliers to be achieved for the output pc")
    ("bbFactor,f", po::value<float>(),"bounding box scaling factor for outlier sampling")
    ;

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

  if (vm.count("help")) {
    cout << desc << "\n";
    return 1;
  }

  float bbFactor = 2.;
  float noiseStd = 0.;
  float outlier = 0.;
  float scale = 0.1;
  string inputPath = "./file.ply";
  string outputPath = "./out.ply";
  if(vm.count("input")) inputPath = vm["input"].as<string>();
  if(vm.count("output")) outputPath = vm["output"].as<string>();
  if(vm.count("scale")) scale = vm["scale"].as<float>();
  if(vm.count("noiseStd")) noiseStd = vm["noiseStd"].as<float>();
  if(vm.count("outlierRatio")) outlier = vm["outlierRatio"].as<float>();
  if(vm.count("bbFactor")) bbFactor = vm["bbFactor"].as<float>();

  cout<< " extracting from "<<inputPath<<endl;
  cout<< " scale for normal estimation radius: "<<scale<<endl;
  cout<< " noise stdandard deviation: "<<noiseStd<<endl;
  cout<< " final outlier ration : "<<outlier<<endl;
  cout<< " output to "<<outputPath<<endl;

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PLYReader reader;
  if (reader.read(inputPath, *cloud))
    std::cout << "error reading " << inputPath << std::endl;
  else
    std::cout << "loaded pc from " << inputPath << ": " << cloud->width << "x"
      << cloud->height << std::endl;

  int w = cloud->width;
  int h = cloud->height;
  cout<<w << "x"<<h<<endl;

  boost::random::mt19937 gen;
  if (noiseStd > 0.) {
    std::cout << " adding noise with std " << noiseStd << std::endl;
    boost::random::normal_distribution<float> normal(0.0,noiseStd);
    for(uint32_t i=0; i< cloud->size(); ++i) {
      cloud->at(i).x += normal(gen);
      cloud->at(i).y += normal(gen);
      cloud->at(i).z += normal(gen);
    }
  }
  if (outlier > 0.) {
    uint32_t nOutl = outlier/(1.-outlier)*cloud->size();
    std::cout << " adding " << nOutl << " outliers to achieve "
      << outlier << " ratio" << std::endl;
    Eigen::Vector3f min, max;
    ComputePcBoundaries(*cloud, min, max);
    std::cout << "bounding box corners " << min.transpose()
      << " -> " << max.transpose() << std::endl;
    std::cout << "inflating bounding box by a factor of " << bbFactor << std::endl;
    Eigen::Vector3f delta = bbFactor*(max - min);
    min -= 0.5*delta;
    max += 0.5*delta;
    std::cout << "bounding box corners " << min.transpose()
      << " -> " << max.transpose() << std::endl;
    boost::random::uniform_01<float> unif;
    for(uint32_t i=0; i< nOutl; ++i) {
      cloud->push_back(pcl::PointXYZ(unif(gen)*(max(0)-min(0))+min(0),
            unif(gen)*(max(1)-min(1))+min(1),
            unif(gen)*(max(2)-min(2))+min(2)));
    }
  }

  jsc::Timer t0;
  pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
//  ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT); // 31ms
  //ne.setNormalEstimationMethod (ne.AVERAGE_DEPTH_CHANGE); // 23ms
  //ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX); // 47ms
//  ne.setMaxDepthChangeFactor(0.02f);
//  ne.setNormalSmoothingSize(10.0f);
  ne.setInputCloud(cloud);

  // Create an empty kdtree representation, and pass it to the normal estimation object.
  // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
  ne.setSearchMethod (tree);
  ne.setRadiusSearch(scale);

  ne.compute(*normals);
  t0.toc();
  cout<<t0<<endl;

  cout<<normals->width<<" "<<normals->height<<endl;

  pcl::PointCloud<pcl::PointXYZRGBNormal> ptNormals(normals->width,
      normals->height);
  for(uint32_t i=0; i< ptNormals.size(); ++i) {
    ptNormals.at(i).x = cloud->points[i].x;
    ptNormals.at(i).y = cloud->points[i].y;
    ptNormals.at(i).z = cloud->points[i].z;
    if (normals->points[i].normal_x == normals->points[i].normal_x
        && normals->points[i].normal_y == normals->points[i].normal_y
        && normals->points[i].normal_z == normals->points[i].normal_z) {
      ptNormals.at(i).normal_x = normals->points[i].normal_x;
      ptNormals.at(i).normal_y = normals->points[i].normal_y;
      ptNormals.at(i).normal_z = normals->points[i].normal_z;
    } else {
      ptNormals.at(i).normal_x = 0;
      ptNormals.at(i).normal_y = 0;
      ptNormals.at(i).normal_z = 1;
    }
  }

  pcl::PLYWriter writer;
  writer.write(outputPath, ptNormals, false, false);

//  return 0;

  // visualize normals
  pcl::visualization::PCLVisualizer viewer("PCL Viewer");
  viewer.setBackgroundColor (0.3, 0.3, 0.3);
//  viewer.addPointCloud(cloud,"cloud");
//  viewer.addPointCloudNormals<pcl::PointXYZ,pcl::Normal>(cloud, normals,10,0.05,"normals");
  viewer.addPointCloudNormals<pcl::PointXYZ,pcl::Normal>(cloud, normals,1,0.001,"normals");

  while (!viewer.wasStopped ())
  {
    viewer.spinOnce ();
  }

  return 0;
}


#define BOOST_AUTO_TEST_MAIN spaces_test
#include <string>
#include <boost/test/unit_test.hpp>
#include "prx/utilities/spaces/space.hpp"

BOOST_AUTO_TEST_CASE( spaces_test )
{
    double x,y,theta;
    x = y = theta = 1;
    std::vector<double*> address_1 = {&x, &y, &theta};
    prx::space_t space_1("EER", address_1, "space_1");

    prx::space_point_t pt_sp1 = space_1.make_point();

    space_1.copy_to_point(pt_sp1);

    BOOST_CHECK((*pt_sp1)[0] == pt_sp1 -> at(0));
    BOOST_CHECK((*pt_sp1)[1] == pt_sp1 -> at(1));
    BOOST_CHECK((*pt_sp1)[2] == pt_sp1 -> at(2));
    BOOST_CHECK(pt_sp1 -> get_dim() == space_1.get_dimension());

    // Checking add
    pt_sp1 -> add(pt_sp1);
    BOOST_CHECK((*pt_sp1)[0] == 2);
    BOOST_CHECK((*pt_sp1)[1] == 2);
    BOOST_CHECK((*pt_sp1)[2] == 2);

    // Checking multiply
    pt_sp1 -> multiply(2);
    BOOST_CHECK((*pt_sp1)[0] == 4);
    BOOST_CHECK((*pt_sp1)[1] == 4);
    BOOST_CHECK((*pt_sp1)[2] == 4);

    // Checking add multiply
    pt_sp1 -> add_multiply(2, pt_sp1);
    BOOST_CHECK((*pt_sp1)[0] == 12);
    BOOST_CHECK((*pt_sp1)[1] == 12);
    BOOST_CHECK((*pt_sp1)[2] == 12);

    prx::space_point_t pt_sp2 = space_1.clone_point(pt_sp1);
    BOOST_CHECK(space_1.equal_points(pt_sp1, pt_sp2));
    (*pt_sp1)[0] = 10;
    (*pt_sp1)[1] = 20;
    (*pt_sp1)[2] = 3;
    space_1.copy_from_point(pt_sp1);
    space_1.copy_to_point(pt_sp2);
    BOOST_CHECK(space_1.equal_points(pt_sp1, pt_sp2));
    (*pt_sp1)[0] = -10;
    (*pt_sp1)[1] = -20;
    (*pt_sp1)[2] = -3;
    space_1.copy_point(pt_sp2, pt_sp1);
    BOOST_CHECK(space_1.equal_points(pt_sp1, pt_sp2));

    space_1.copy_point_from_vector(pt_sp2, {2,5,1});
    std::vector<double> v;
    space_1.copy_vector_from_point(v, pt_sp2);
    BOOST_CHECK(v[0] == 2 && v[1] == 5 && v[2] == 1);

    (*pt_sp1)[0] = (*pt_sp2)[0] = -100;
    (*pt_sp1)[1] = (*pt_sp2)[1] = -200;
    (*pt_sp1)[2] = (*pt_sp2)[2] = 0;
    BOOST_CHECK(!space_1.satisfies_bounds(pt_sp1));
    space_1.copy_from_point(pt_sp2);
    space_1.set_bounds({-10, -10, -3}, {10, 10, 3});
    space_1.enforce_bounds(pt_sp1);
    BOOST_CHECK(space_1.satisfies_bounds(pt_sp1));
    space_1.enforce_bounds();
    space_1.copy_to_point(pt_sp2);
    BOOST_CHECK(space_1.satisfies_bounds(pt_sp2));
    space_1.sample(pt_sp1);
    BOOST_CHECK(space_1.satisfies_bounds(pt_sp1));

    // integrate test not necessary: testing integration in other test by its own
    // TODO: add interpolate test

    (*pt_sp1)[0] = 0;
    (*pt_sp1)[1] = -1;
    (*pt_sp1)[2] = 2;
    (*pt_sp2)[0] = 1;
    (*pt_sp2)[1] = -2;
    (*pt_sp2)[2] = 3;
    BOOST_CHECK(prx::space_t::l1_norm(pt_sp1) == 3);
    BOOST_CHECK(prx::space_t::l1_norm(pt_sp1, pt_sp2) == 3);
    BOOST_CHECK(prx::space_t::l2_norm(pt_sp1) == std::sqrt(1+4));
    BOOST_CHECK(prx::space_t::l2_norm(pt_sp1, pt_sp2) == std::sqrt(1+1+1));
    (*pt_sp1)[1] = 1;
    (*pt_sp2)[1] = 2;

    BOOST_CHECK(prx::space_t::lp_norm(pt_sp2,1) == 6);
    BOOST_CHECK(prx::space_t::lp_norm(pt_sp2,2) == std::sqrt(14));
    BOOST_CHECK(prx::space_t::lp_norm(pt_sp2,3) == std::pow(6, 2.0/3.0));
    BOOST_CHECK(prx::space_t::lp_norm(pt_sp2,4) == std::pow(2, 1.0/4) * std::sqrt(7));

    (*pt_sp1)[0] = 0;
    (*pt_sp1)[1] = 1;
    (*pt_sp1)[2] = 2;
    (*pt_sp2)[0] = 1;
    (*pt_sp2)[1] = 3;
    (*pt_sp2)[2] = 5;
    BOOST_CHECK(prx::space_t::lp_norm(pt_sp2, pt_sp1,1) == 6);
    BOOST_CHECK(prx::space_t::lp_norm(pt_sp2, pt_sp1,2) == std::sqrt(14));
    BOOST_CHECK(prx::space_t::lp_norm(pt_sp2, pt_sp1,3) == std::pow(6, 2.0/3.0));
    BOOST_CHECK(prx::space_t::lp_norm(pt_sp2, pt_sp1,4) == std::pow(2, 1.0/4) * std::sqrt(7));

    BOOST_CHECK(prx::space_t::euclidean_2d(pt_sp2, pt_sp1) == std::sqrt(1+4));

}


//          Copyright Carl Philipp Reh 2009 - 2016.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#ifndef FCPPT_MATH_MATRIX_HAS_DIM_HPP_INCLUDED
#define FCPPT_MATH_MATRIX_HAS_DIM_HPP_INCLUDED

#include <fcppt/math/size_type.hpp>
#include <fcppt/math/detail/dim_matches.hpp>
#include <fcppt/math/matrix/object_fwd.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/mpl/and.hpp>
#include <fcppt/config/external_end.hpp>

namespace fcppt
{
namespace math
{
namespace matrix
{

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

/**
\brief Metafunction to check if a static matrix has the specified dimensions

\ingroup fcpptmathmatrix

\tparam Matrix A fcppt::math::matrix::object type

\tparam R The static row count

\tparam C The static column count
*/
template<
	typename Matrix,
	fcppt::math::size_type R,
	fcppt::math::size_type C
>
struct has_dim;

/// \cond FCPPT_DOXYGEN_DEBUG
template<
	typename T,
	fcppt::math::size_type R,
	fcppt::math::size_type C,
	typename S,
	fcppt::math::size_type DR,
	fcppt::math::size_type DC
>
struct has_dim<
	fcppt::math::matrix::object<
		T,
		R,
		C,
		S
	>,
	DR,
	DC
>
:
boost::mpl::and_<
	fcppt::math::detail::dim_matches<
		DR,
		R
	>,
	fcppt::math::detail::dim_matches<
		DC,
		C
	>
>
{
};
/// \endcond

FCPPT_PP_POP_WARNING

}
}
}

#endif


#pragma once

#include <iostream>
#include <math.h>

#include <Eigen/Dense>
#include <FreeImage.h>

using std::cin;
using std::cout;
using std::endl;

using Eigen::Vector3f;

typedef Vector3f Point3f;
typedef Vector3f Color3f;

float square(float a);


#define _CRT_SECURE_NO_WARNINGS
#include <boost/algorithm/string.hpp>
#include <iomanip>
#include <iostream>
#include <fstream>
#include <vector>
#include <array>
#include <cmath>
using namespace std;

bool CompareFV(const vector<string>& inFiles) {
	vector<unique_ptr<ifstream>> ifsList;
	for (const auto& path : inFiles)
		ifsList.emplace_back(new ifstream(path, ios_base::binary));
	if (!all_of(ifsList.begin(), ifsList.end(), bind(&ifstream::good, placeholders::_1))) {
		cerr << "エラー: 入力ファイルの読み込みに失敗: " << strerror(errno) << endl;
		return false;
	}

	int64_t notEqualCount = 0;
	int absMax = 0;
	double absSum = 0, absSq = 0;

	vector<int16_t> values(inFiles.size());

	size_t elementCount = 0;
	for (; ; elementCount++) {
		for (size_t i = 0; i < inFiles.size(); i++)
			ifsList[i]->read((char*)&values[i], sizeof values[i]);
		if (all_of(ifsList.begin(), ifsList.end(), bind(&ifstream::eof, placeholders::_1)))
			break;
		if (any_of(ifsList.begin(), ifsList.end(), bind(&ifstream::eof, placeholders::_1))) {
			cerr << "エラー: 入力ファイルのサイズ不一致" << endl;
			return false;
		}
		if (!all_of(ifsList.begin(), ifsList.end(), bind(&ifstream::good, placeholders::_1))) {
			cerr << "エラー: 読み込み失敗: " << strerror(errno) << endl;
			return false;
		}

		assert(values.size() == 2);

		if (values[0] != values[1])
		{
			notEqualCount++;
			auto d = abs((int)values[0] - (int)values[1]);
			if (absMax < d)
				absMax = d;
			absSum += d;
			absSq += d * d;
		}
	}

	cout << fixed;
	cout << "入力ファイル:" << endl;
	for (auto& path : inFiles)
		cout << "    " << path << endl;
	cout << "要素数:             " << elementCount << endl;
	cout << "不一致要素数:       " << notEqualCount
		<< " (" << setprecision(1) << (notEqualCount * 100.0) / elementCount << "%)" << endl;
	cout << "差の絶対値の最大:   " << absMax << endl;
	cout << "差の絶対値の平均:   " << setprecision(1) << (absSum / notEqualCount) << endl;
	cout << "差の二乗平均平方根: " << setprecision(1) << sqrt(absSq / notEqualCount) << endl;
	cout << endl;
	return true;
}

void Usage() {
	cerr << "usage: CompareFV InFile1 InFile2" << endl;
	cerr << endl;
	cerr << "2つの重みファイル(Bonanzaのfv.binのような2バイト符号付整数値が" << endl;
	cerr << "連続で記録されたバイナリファイル)の差の統計(差の絶対値の平均など)を算出します。" << endl;
	cerr << endl;
	cerr << "    InFile1    比較元のファイルパス" << endl;
	cerr << "    InFile2    比較先のファイルパス" << endl;
	cerr << endl;
	cerr << "  例：CompareFV FV1.bin FV2.bin > CompareFV.log" << endl;
	cerr << "    ⇒ FV1.binとFV2.binの差の統計を、" << endl;
	cerr << "       CompareFV.logに出力。" << endl;
}

int main(int argc, char* argv[]) {
	vector<string> files;
	bool hasInWeights = false;
	{
		vector<string> args(argv + 1, argv + argc);
		for (size_t i = 0; i < args.size(); i++) {
			files.push_back(args[i]);
		}
		if (files.size() != 2) {
			cerr << "不正なオプション: 入力ファイル数が2以外" << endl;
			Usage();
			return 1;
		}
	}
	// 処理
	return CompareFV(files) ? 0 : 2;
}


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

#ifndef __pinocchio_math_sincos_hpp__
#define __pinocchio_math_sincos_hpp__

#include <boost/type_traits.hpp>
#include <cmath>

namespace pinocchio
{
  // Forward declaration
  template<typename S1, typename S2 = S1, typename S3 = S1, bool value = boost::is_floating_point<S1>::value & boost::is_floating_point<S2>::value & boost::is_floating_point<S3>::value> struct SINCOSAlgo;

  ///
  /// \brief Computes sin/cos values of a given input scalar.
  ///
  /// \tparam Scalar Type of the input/output variables
  ///
  /// \param[in] a The input scalar from which we evalute the sin and cos.
  /// \param[inout] sa Variable containing the sin of a.
  /// \param[inout] ca Variable containing the cos of a.
  ///
  template<typename S1, typename S2, typename S3>
  void SINCOS(const S1 & a, S2 * sa, S3 * ca)
  {
    SINCOSAlgo<S1,S2,S3>::run(a,sa,ca);
  }

  /// \brief Generic evaluation of sin/cos functions.
  template<typename S1, typename S2, typename S3, bool>
  struct SINCOSAlgo
  {
    static void run(const S2 & a, S2 * sa, S3 * ca)
    {
      (*sa) = std::sin(a); (*ca) = std::cos(a);
    }
  };

  /// \brief Specific evaluation of sin/cos for double type.
  template<>
  struct SINCOSAlgo<double>
  {
    static void run(const double & a, double * sa, double * ca)
    {
#ifdef __linux__
      sincos(a,sa,ca);
#elif __APPLE__
      __sincos(a,sa,ca);
#else // if sincos specialization does not exist
      (*sa) = std::sin(a); (*ca) = std::cos(a);
#endif
    }
  };

  /// \brief Specific evaluation of sin/cos for float type.
  template<>
  struct SINCOSAlgo<float>
  {
    static void run(const float & a, float * sa, float * ca)
    {
#ifdef __linux__
      sincosf(a,sa,ca);
#elif __APPLE__
      __sincosf(a,sa,ca);
#else // if sincosf specialization does not exist
      (*sa) = std::sin(a); (*ca) = std::cos(a);
#endif
    }
  };

  /// \brief Specific evaluation of sin/cos for long double.
  template<>
  struct SINCOSAlgo<long double>
  {
    static void run(const long double & a, long double * sa, long double * ca)
    {
#ifdef __linux__
      sincosl(a,sa,ca);
#else // if sincosl specialization does not exist
      (*sa) = std::sin(a); (*ca) = std::cos(a);
#endif
    }
  };

  /// \brief Implementation for overloaded scalar types (e.g. Automatic Differentiation).
  template<typename S1, typename S2, typename S3>
  struct SINCOSAlgo<S1, S2, S3, false >
  {
    static void run(const S1 & a, S2 * sa, S3 * ca)
    {
      (*sa) = sin(a); (*ca) = cos(a);
    }
  };

}

#endif //#ifndef __pinocchio_math_sincos_hpp__


// Copyright András Vukics 2006–2020. Distributed under the Boost Software License, Version 1.0. (See accompanying file LICENSE.txt)
#include "Particle_.h"

#include "ParsParticle.h"
#include "ParticleInitialCondition.h"

#include "LazyDensityOperatorFFT.h"
#include "TridiagonalHamiltonian.h"

#include <boost/math/special_functions/hermite.hpp>

#include "TMP_Tools.h"

#include <boost/assign/list_of.hpp>
#include <boost/bind.hpp>
#include <utility>

using namespace std;
using namespace mathutils;
using namespace cpputils;
using namespace fft;

////////
//
// Exact
//
////////

namespace {

const particle::Exact::Diagonal fExpFill(const particle::Spatial& space, double omrec)
{
  return particle::Exact::Diagonal(-DCOMP_I*omrec*blitz::sqr(space.getK()));
}

}

particle::Exact::Exact(const Spatial& space, double omrec)
  : FreeExact(space.getDimension()), details::Storage(space,omrec), factorExponents_(fExpFill(space,omrec))
{
}

void particle::Exact::updateU(structure::OneTime t) const
{
  getDiagonal()=exp(factorExponents_*t);
}

//////////////
//
// Hamiltonian
//
//////////////

namespace {

const particle::Tridiagonal expINKX(size_t dim, ptrdiff_t nK)
{
  using particle::Tridiagonal;

  typedef Tridiagonal::Diagonal Diag;

  ptrdiff_t D=dim-(nK>0 ? nK : -nK);
  if (D<0) return Tridiagonal();

  Diag temp(D); temp=1;
  if (nK>0) return Tridiagonal(Diag(),nK,temp);
  else if (nK==0) return Tridiagonal(temp);
  return Tridiagonal(Diag(),-nK,Diag(),temp);
}

const particle::Tridiagonal cosNKX(size_t dim, ptrdiff_t nK)
{
  return (expINKX(dim,nK)+expINKX(dim,-nK))/2.;
}

const particle::Tridiagonal hOverI(size_t dim, double vClass, const ModeFunction& mf)
{
  return (vClass && !isComplex(get<0>(mf))) ? vClass*(get<0>(mf)==MFT_SIN ? -1 : 1)*cosNKX(dim,get<1>(mf)<<1)/(2.*DCOMP_I) : particle::Tridiagonal();
}

const particle::Tridiagonal::Diagonal mainDiagonal(const particle::Spatial& space, double omrec)
{
  return particle::Tridiagonal::Diagonal(DCOMP_I*omrec*blitz::sqr(space.getK()));
}

}

namespace particle { // open it for the partial specializations

template<>
Hamiltonian<true >::Hamiltonian(const Spatial& space, double omrec, double vClass, const ModeFunction& mf)
  : Base(furnishWithFreqs(hOverI(space.getDimension(),vClass,mf),mainDiagonal(space,omrec))), Exact(space,omrec)
{
}

template<>
Hamiltonian<false>::Hamiltonian(const Spatial& space, double omrec, double vClass, const ModeFunction& mf)
  : Base(
         Tridiagonal(mainDiagonal(space,-omrec))
         +
         hOverI(space.getDimension(),vClass,mf)
         )
{
}

template<> template<>
Hamiltonian<false>::Hamiltonian(const Spatial& space, double omrec)
  : Base(
         Tridiagonal(mainDiagonal(space,-omrec))
         )
{
}

} // particle

///////////
//
// Averaged
//
///////////

particle::Averaged::Averaged(const Spatial& space)
  : Base("Particle",{"<P>","VAR(P)","<X>","DEV(X)"}),
    space_(space)
{
}

auto particle::Averaged::average_v(NoTime, const LazyDensityOperator& matrix) const -> const Averages
{
  int dim=space_.getDimension();

  auto averages(initializedAverages());

  const LazyDensityOperator::Ptr matrixX(quantumdata::ffTransform<tmptools::Vector<0> >(matrix,DIR_KX));

  for (int i=0; i<dim; i++) {

    double diag=matrix(i);
    averages(0)+=    space_.k(i) *diag;
    averages(1)+=sqr(space_.k(i))*diag;
  }

  for (int i=0; i<dim; i++) {
    double diag=matrixX->operator()(i);
    averages(2)+=    space_.x(i) *diag;
    averages(3)+=sqr(space_.x(i))*diag;
  }

  return averages;
}

void particle::Averaged::process_v(Averages& averages) const
{
  averages(1)-=sqr(averages(0));
  averages(3)=sqrt(averages(3)-sqr(averages(2)));
}

////////////////
//
// Highest level
//
////////////////

ParticleBase::ParticleBase(size_t fin,
                           const RealFreqs& realFreqs, const ComplexFreqs& complexFreqs)
  : Free(1<<fin,realFreqs,complexFreqs), Averaged(particle::Spatial(fin))
{
  getParsStream()<<"Particle\n";
  getSpace().header(getParsStream());
}

PumpedParticleBase::PumpedParticleBase(size_t fin, double vClass, const ModeFunction& mf,
                                       const RealFreqs& realFreqs, const ComplexFreqs& complexFreqs)
  : ParticleBase(fin,
                 boost::assign::list_of(*realFreqs.begin()).range(next(realFreqs.begin()),realFreqs.end())({"vClass",vClass,1.}),
                 complexFreqs),
    vClass_(vClass), mf_(mf)
{
  getParsStream()<<"Pump "<<mf<<endl;
}

Particle::Particle(const particle::Pars& p)
  : ParticleBase(p.fin,{RF{"omrec",p.omrec,1<<p.fin}}),
    Exact(getSpace(),p.omrec)
{
}

ParticleSch::ParticleSch(const particle::Pars& p)
  : ParticleBase(p.fin,{RF{"omrec",p.omrec,sqr(1<<p.fin)}}),
    Hamiltonian<false>(getSpace(),p.omrec)
{
  getParsStream()<<"Schroedinger picture.\n";
}

PumpedParticle::PumpedParticle(const particle::ParsPumped& p)
  : PumpedParticleBase(p.fin,p.vClass,ModeFunction(p.modePart,p.kPart),{RF{"omrec",p.omrec,1<<p.fin}}),
    Hamiltonian<true>(getSpace(),p.omrec,p.vClass,getMF())
{
}

PumpedParticleSch::PumpedParticleSch(const particle::ParsPumped& p)
  : PumpedParticleBase(p.fin,p.vClass,ModeFunction(p.modePart,p.kPart),{RF{"omrec" ,p.omrec,sqr(1<<p.fin)}}),
    Hamiltonian<false>(getSpace(),p.omrec,p.vClass,getMF())
{
  getParsStream()<<"Schroedinger picture.\n";
}

//////////
//
// Helpers
//
//////////

namespace {

using particle::Spatial;

const Spatial::Array fill(size_t fin, double d, double m)
{
  Spatial::Array res(1<<fin);
  return res=d*blitz::tensor::i-m;
}

}

particle::Spatial::Spatial(size_t fin, double deltaK)
  : fin_(fin),
    xMax_(PI/deltaK), deltaX_(2*xMax_/(1<<fin)), kMax_(PI/deltaX_), deltaK_(deltaK),
    x_(fill(fin,deltaX_,xMax_)), k_(fill(fin,deltaK_,kMax_))
{
}

void particle::Spatial::header(std::ostream& os) const
{
  os<<"Spatial Degree of Freedom finesse="<<fin_<<" xMax="<<xMax_<<" deltaX="<<deltaX_<<" kMax="<<kMax_<<" deltaK="<<deltaK_<<std::endl;
}

auto particle::expINKX(particle::Ptr particle, ptrdiff_t nK) -> const Tridiagonal
{
  size_t dim=particle->getDimension();
  Tridiagonal res(::expINKX(dim,nK));
  if (const auto exact=dynamic_cast<const particle::Exact*>(particle.get())) res.furnishWithFreqs(mainDiagonal(get<0>(*exact),get<1>(*exact)));
  return res;
}

auto particle::mfNKX(particle::Ptr particle, const ModeFunction& modeFunction) -> const Tridiagonal
{
  ModeFunctionType mf(get<0>(modeFunction));
  ptrdiff_t        nK(get<1>(modeFunction));
  switch (mf) {
  case MFT_SIN  : return sinNKX (particle,nK);
  case MFT_COS  : return cosNKX (particle,nK);
  case MFT_PLUS : return expINKX(particle,nK);
  case MFT_MINUS:                            ;
  }
  return expINKX(particle,-nK);
}

auto particle::mfComposition(particle::Ptr particle, const ModeFunction& modeFunction1, const ModeFunction& modeFunction2) -> const Tridiagonal
{
  ModeFunctionType mf1(get<0>(modeFunction1));
  ModeFunctionType mf2(get<0>(modeFunction2));
  ptrdiff_t        nK1(get<1>(modeFunction1));
  ptrdiff_t        nK2(get<1>(modeFunction2));
  ptrdiff_t        nK(abs(nK1));
  ptrdiff_t        sign1 = mf1==MFT_SIN && nK1<0 ? -1 : 1;
  ptrdiff_t        sign2 = mf2==MFT_SIN && nK2<0 ? -1 : 1;
  ptrdiff_t        sign  = sign1*sign2;

  if (abs(nK1) != abs(nK2)) throw std::runtime_error("Not a Tridiagonal");

  if (mf1==MFT_PLUS  && nK1<0) { mf1=MFT_MINUS; nK1*=-1; }
  if (mf1==MFT_MINUS && nK1<0) { mf1=MFT_PLUS;  nK1*=-1; }
  if (mf2==MFT_PLUS  && nK2<0) { mf2=MFT_MINUS; nK2*=-1; }
  if (mf2==MFT_MINUS && nK2<0) { mf2=MFT_PLUS;  nK2*=-1; }

  Tridiagonal id(quantumoperator::identity(particle->getSpace().getDimension()));

  typedef pair<ModeFunctionType,ModeFunctionType> MFPair;

  MFPair mfs(mf1,mf2);

  if (mfs == MFPair(MFT_SIN,MFT_SIN)){
    return sign * (id-cosNKX(particle,2*nK)) / 2.;
  }
  if (mfs == MFPair(MFT_COS,MFT_COS)){
    return (id+cosNKX(particle,2*nK)) / 2.;
  }
  if (mfs == MFPair(MFT_SIN,MFT_COS) || mfs == MFPair(MFT_COS,MFT_SIN)){
    return sign * sinNKX(particle,2*nK) / 2.;
  }
  if (mfs == MFPair(MFT_PLUS,MFT_PLUS) || mfs == MFPair(MFT_MINUS,MFT_MINUS)){
    return id;
  }
  if (mfs == MFPair(MFT_PLUS,MFT_MINUS)){
    return expINKX(particle,-2*nK);
  }
  if (mfs == MFPair(MFT_MINUS,MFT_PLUS)){
    return expINKX(particle,2*nK);
  }
  if (mfs == MFPair(MFT_PLUS,MFT_SIN) || mfs == MFPair(MFT_SIN,MFT_MINUS)) {
    return sign * (expINKX(particle, -2*nK)-id) * DCOMP_I/2.;
  }
  if (mfs == MFPair(MFT_PLUS,MFT_COS) || mfs == MFPair(MFT_COS,MFT_MINUS)) {
    return (id + expINKX(particle,-2*nK)) / 2.;
  }
  if (mfs == MFPair(MFT_MINUS,MFT_COS) || mfs == MFPair(MFT_COS,MFT_PLUS)) {
    return (id + expINKX(particle, 2*nK)) / 2.;
  }
  if (!(mfs == MFPair(MFT_MINUS,MFT_SIN) || mfs == MFPair(MFT_SIN,MFT_PLUS))) {
    // this should never be reached
    throw std::logic_error("In mfComposition");
  }
  return -sign * (expINKX(particle,2*nK)-id) * DCOMP_I/2.;
}

/*
const Tridiagonal mfNKX_AbsSqr(ModeFunctionType mf, size_t dim, ptrdiff_t K)
{
  return (mfcomplex(mf) ? Identity(dim) : (mf==MFT_Sin ? -1 : 1)*cosNKX(dim,2*K))/2.;
  // The other 1/2 should be taken into account separately!
}
*/

auto particle::wavePacket(const InitialCondition& init, const Spatial& space, bool kFlag) -> StateVector
{
  double
    offset1=PI*init.getX0(),
    offset2=   init.getK0();

  if (init.isInK()) {swap(offset1,offset2); offset2=-offset2;}

  const Spatial::Array array(init.isInK() ? space.getK() : space.getX());

  StateVectorLow psiLow(exp(-blitz::sqr(array-offset1)/(4*sqr(init.getSig()))+DCOMP_I*array*offset2));

  if      ( kFlag && !init.isInK()) quantumdata::ffTransform(psiLow,DIR_XK);
  else if (!kFlag &&  init.isInK()) quantumdata::ffTransform(psiLow,DIR_KX);

  StateVector res(psiLow,quantumdata::byReference); res.renorm();

  return res;

}

auto particle::wavePacket(const Pars& p, bool kFlag) -> StateVector
{
  return wavePacket(p.init,Spatial(p.fin),kFlag);
}

namespace {

const particle::InitialCondition coherent(const particle::ParsPumped& p)
{
  return particle::InitialCondition(p.init.getX0(),p.init.getK0(),pow(p.omrec/fabs(p.vClass),.25)/sqrt(2),false);
}

}

auto particle::wavePacket(const ParsPumped& p, bool kFlag) -> StateVector
{
  if (p.init.getSig()) return wavePacket(static_cast<const Pars&>(p),kFlag);
  else                 return wavePacket(coherent(p),
                                         Spatial(p.fin),
                                         kFlag);
}

auto particle::hoState(int n, const InitialCondition& init, const Spatial& space, bool kFlag) -> StateVector
{
  if (n<0) n=0;

  double kx0(PI*init.getX0());

  size_t dim=space.getDimension();

  StateVectorLow psiLow(dim);

  for (size_t j=0; j<dim; j++) {
    double temp=(space.getX()(j)-kx0)/init.getSig();
    psiLow(j)=exp(-sqr(temp)/2.)*boost::math::hermite(n,temp);
  }

  if (kFlag) quantumdata::ffTransform(psiLow,DIR_XK);

  StateVector res(psiLow,quantumdata::byReference); res.renorm();

  return res;

}

auto particle::hoState(const Pars      & p, bool kFlag) -> StateVector
{
  return hoState(p.hoInitn,p.init,Spatial(p.fin),kFlag);
}

auto particle::hoState(const ParsPumped& p, bool kFlag) -> StateVector
{
  if (p.init.getSig()) return hoState(static_cast<const Pars&>(p),kFlag);
  else                 return hoState(p.hoInitn,
                                      coherent(p),
                                      Spatial(p.fin),
                                      kFlag);
}

auto particle::init(const Pars& p) -> StateVector
{
  if (const auto pp=dynamic_cast<const ParsPumped*>(&p))
    return p.hoInitn<0 ? wavePacket(*pp) : hoState(*pp);
  else
    return p.hoInitn<0 ? wavePacket( p ) : hoState( p );
}

particle::Ptr particle::make(const Pars& p, QM_Picture qmp)
{
  return qmp==QMP_SCH ? Ptr(std::make_shared<ParticleSch>(p)) : Ptr(std::make_shared<Particle>(p));
}

particle::PtrPumped particle::makePumped(const ParsPumped& p, QM_Picture qmp)
{
  return qmp==QMP_SCH ? PtrPumped(std::make_shared<PumpedParticleSch>(p)) : PtrPumped(std::make_shared<PumpedParticle>(p));
}


#ifndef SCALESHIFTER_HPP
#define SCALESHIFTER_HPP

#include <Eigen/Dense>
#include <iostream>
#include <algorithm>
#include <OpenMesh/Core/IO/MeshIO.hh>
#include <OpenMesh/Core/Mesh/TriMesh_ArrayKernelT.hh>
#include "../global.hpp"
#include "helper_functions.hpp"
#include "NeighbourFinder.hpp"

typedef Eigen::Vector3f Vec3Float;
typedef Eigen::VectorXf VecDynFloat;
typedef Eigen::VectorXi VecDynInt;
typedef Eigen::Matrix< float, Eigen::Dynamic, 3> Vec3Mat;
typedef Eigen::Matrix< int, Eigen::Dynamic, Eigen::Dynamic> IntegerMat;
typedef Eigen::Matrix< float, Eigen::Dynamic, Eigen::Dynamic> FloatMat;
typedef Eigen::Matrix< float, Eigen::Dynamic, registration::NUM_FEATURES> FeatureMat; //matrix Mx6 of type float
typedef Eigen::Matrix< int, Eigen::Dynamic, 3> FacesMat;
typedef OpenMesh::DefaultTraits MyTraits;
typedef OpenMesh::TriMesh_ArrayKernelT<MyTraits>  TriMesh;

namespace registration{

/*
ScaleShifter class.

This class is tailored to 'shift scales' between pyramid layers of the nonrigid registration. This means it has to
transfer the features of the mesh of the previous scale to the mesh of the current scale.

This is done by finding matches between the elements of each mesh, and interpolating the features for the elements
for which no matches are found.

Because both meshes are a subsampled version of the same original mesh, matches can be efficiently found through the
original indices of each element. Therefor, the ScaleShifter expects the original indices for each mesh as input:
-inLowOriginalIndices for the original indices of the (lower sampled) mesh of the previous scale.
-inHighOriginalIndicies for the original indices of the (higher sampled) mesh of the current scale.
*/

class ScaleShifter
{
    public:

        void set_input(const FeatureMat &inLowFeatures,
                       const VecDynInt &inLowOriginalIndices,
                       const VecDynInt &inHighOriginalIndices);
        void set_output(FeatureMat &outHighFeatures);
        void update();

    protected:

    private:
        //# Inputs
        const FeatureMat * _inLowFeatures = NULL;
        const VecDynInt *_inLowOriginalIndices = NULL;
        const VecDynInt *_inHighOriginalIndices = NULL;

        //# Outputs
        FeatureMat * _outHighFeatures;

        //# User Parameters

        //# Internal Data structures
        std::vector<std::pair<int,int> > _matchingIndexPairs;
        std::vector<int> _newIndices;

        //# Internal Parameters
        size_t _numLowNodes = 0;
        size_t _numHighNodes = 0;
        size_t _numMatchingNodes = 0;
        size_t _numNewNodes = 0;

        //# Internal functions
        void _find_matching_and_new_indices();
        void _interpolate_new_nodes();
        void _copy_matching_nodes();
};

}//namespace registration

#endif // SCALESHIFTER_HPP


/*
 * Copyright 2018 James Dyer
 *
 * 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.
 *
 * This is being developed for the TANGO Project: http://tango-project.eu
 */

#include <sstream>
#include <NTL/RR.h>
#include <json/json.h>
#include "HE2NEncrypter.h"
#include "Random.h"

HE2NEncrypter::~HE2NEncrypter()
{
	delete kappamod;
};

HE2NEncrypter::HE2NEncrypter(int lambda, int eta, int nu){
	kappa = NTL::RandomPrime_ZZ(nu,20);
	generateParameters(lambda,eta);
	NTL::ZZ_p::init(modulus);
	NTL::ZZ_p ktmp = NTL::to_ZZ_p(kappa);
	kappamod = new NTL::ZZ_p(ktmp);
}

HE2NEncrypter::HE2NEncrypter(int n, int d, int rho, int rhoprime)
{
    /* Compute lower bound for kappa */
	NTL::RR rtwo = NTL::RR(2);
	NTL::RR exp1 = NTL::RR(rho*d);
	NTL::RR nplusone = NTL::RR(n+1);
	NTL::RR exp2 = NTL::RR(d);
	NTL::RR exp3 = NTL::RR(rho);
	NTL::RR kloBound = pow(rtwo,exp1)*pow(nplusone,exp2);
	NTL::ZZ kappaLowerBound = to_ZZ(kloBound);
	/* Generate kappa as prime > lower bound */
	long nu = NumBits(kappaLowerBound)+1;
	kappa = NTL::RandomPrime_ZZ(nu,20);

	/*compute lower bound for p */
	NTL::RR kappareal = to_RR(kappa);
	NTL::RR ploBound = pow(pow(rtwo,exp3)+pow(kappareal,rtwo),exp2)*pow(nplusone,exp2);
	NTL::ZZ pLowerBound = to_ZZ(ploBound);

	/* Generate p and q */
	long lambda = NumBits(pLowerBound) + 1;
	long eta = ((lambda * lambda / rho) - lambda);

	generateParameters(lambda,eta);
	NTL::ZZ_p::init(modulus);
	NTL::ZZ_p ktmp = NTL::to_ZZ_p(kappa);
	kappamod = new NTL::ZZ_p(ktmp);
};

NTL::vec_ZZ_p HE2NEncrypter::encrypt(NTL::ZZ& plaintext)
{
    NTL::ZZ_p r = NTL::to_ZZ_p(rng->nextBigInteger(q));
    NTL::ZZ_p s = NTL::to_ZZ_p(rng->nextBigInteger(kappa));
    NTL::ZZ_p t = NTL::to_ZZ_p(rng->nextBigInteger(modulus));
    NTL::ZZ_p ptext = NTL::to_ZZ_p(plaintext);
    NTL::ZZ_p c = ptext + r*(*pmod) +s*(*kappamod);
    return ONE_VECTOR*c + (*a)*t;
};

std::string HE2NEncrypter::writeSecretsToJSON()
{
	Json::Value root;
	std::ostringstream pStr,modStr,kappaStr,gammaStr;
	pStr << p;
	root["p"] = pStr.str();
	modStr << modulus;
	root["modulus"] = modStr.str();
	kappaStr << kappa;
	root["kappa"] = kappaStr.str();
	gammaStr << (*gamma);
    root["gamma"]=gammaStr.str();
	Json::FastWriter writer;
	return writer.write(root);
};


/*
M3 -- Meka Robotics Robot Components
Copyright (c) 2010 Meka Robotics
Author: edsinger@mekabot.com (Aaron Edsinger)

M3 is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

M3 is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with M3.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "m3/hardware/motor.h"
#include <math.h>
#include <cmath>
#include <Eigen/LU>
#include <stdio.h>
#include <time.h>
#include <sys/stat.h>
#include <sys/types.h>

namespace m3{

using namespace std;

void M3MotorModel::ThermalInit(string config_filename)
{

	mReal C1, C2, tau1, tau2, tau3;

	//Initialize matrices
	eA2 = VectorXf::Zero(2);
	Tprev = VectorXf::Zero(2);
	eA1 = MatrixXf::Zero(2,2);

	tau1 = thermal_time_constant_winding;
	tau2 = thermal_time_constant_motor;
	C1 = tau1/thermal_resistance_rotor_housing;
	C2 = tau2/thermal_resistance_housing_ambient;
	tau3 =  thermal_resistance_rotor_housing*C2;

	MatrixXf I=MatrixXf::Zero(2,2);
	MatrixXf ser=MatrixXf::Zero(2,2);
	MatrixXf A=MatrixXf::Zero(2,2);
	VectorXf B=VectorXf::Zero(2);

	mReal dt = 1./RT_TASK_FREQUENCY;
	I << 1,0,0,1;

//	eA1 = exp(-1/tau1/RT_TASK_FREQUENCY);
//	eA2 = -tau1*(eA1-1)*1/C1;

	A << -1/tau1, 1/tau1,
	      1/tau3, -1/tau3-1/tau2;
	B << 1/C1, 0;

	ser = I;
	eA1 = I;
	for (int i = 1;i<5;i++) {
	    ser = ser*A*dt/i;
	    eA1 = eA1 + ser;
	}
	eA2 = A.inverse()*(eA1-I)*B;

	Tprev << 0.,0.;

	if (use_old_temp_values)
	{

	    // Get Old Temps to init model
	    string temp_filename = config_filename.substr(0, config_filename.length()-4);
	    temp_filename += "_temps.yml";
	    string s(temp_filename);

        YAML::Node node;
		doc_path = m3rt::GetYamlDoc(s.c_str(),node,NULL);
		bool previous_temp_missing = doc_path.empty();
                if(previous_temp_missing)
                    M3_WARN("Previous temp missing, setting to default.\n");
	    /*try
	    {
	      GetYamlDoc(temp_filename.c_str(), node);
	    } catch(YAML::RepresentationException e) {
	      previous_temp_missing = true;
	      M3_DEBUG("b\n");
	    }
	    catch(...) {
	      previous_temp_missing = true;
	      M3_DEBUG("a\n");
	    }*/
	    previous_winding_temp=25.0;
            previous_case_temp=25.0;
            previous_temp_timestamp="";
	    if (!previous_temp_missing)
	    {
	      try
	      {

		      node["previous_winding_temp"] >> previous_winding_temp;
	      } catch(...){}

	      try
	      {
		      node["previous_case_temp"] >> previous_case_temp;

	      } catch(...){}
	      try
	      {
		      node["previous_temp_timestamp"] >> previous_temp_timestamp;

	      } catch(...){}

	      if (previous_temp_timestamp != "")
	      {
			time_t timenew;
			time(&timenew);

			struct tm timeold;
			if (strptime(previous_temp_timestamp.c_str(), "%c", &timeold) == NULL) //ERROR
			  return;
			time_t oldtime = mktime(&timeold);

			double diff = difftime(timenew, oldtime);
			if (diff > 2.0*thermal_time_constant_motor)
				return;

			Tprev << previous_winding_temp, previous_case_temp;
			int model_run_cnts = int(diff / dt);
			for (int i = 0; i < model_run_cnts; i++)
			{
				Tprev = eA1*Tprev;
			}

			/*ser = I;
			MatrixXf eA1_i = I;
			for (int i = 1;i<5;i++) {
			    ser = ser*A*diff/i;
			    eA1_i = eA1_i + ser;
			}
			Tprev << previous_winding_temp, previous_case_temp;
			Tprev = eA1_i*Tprev;
			if (Tprev[0] < 0.0)
				Tprev[0] = 0.0;
			if (Tprev[1] < 0.0)
				Tprev[1] = 0.0;*/

			/*M3_DEBUG("Init motor model:\n");
			//M3_DEBUG("eA1_i00: %f\n", eA1_i(0,0));
			//M3_DEBUG("eA1_i11: %f\n", eA1_i(1,1));
			M3_DEBUG("eA1_i00: %f\n", eA1_i(0,0));
			M3_DEBUG("eA1_i11: %f\n", eA1_i(1,1));
			M3_DEBUG("diff: %f\n", diff);
			M3_DEBUG("winding: %f\n", Tprev[0]);
			M3_DEBUG("case: %f\n", Tprev[1]);*/
	      }
	    }

	}

	// C1 = tau1/r1, C2 = tau2/r2, tau3 = r1*C2
	// A = [-1/tau1 1/tau1;1/tau3,-1/tau3-1/tau2]
	// B = [1/C1;0]
        //RT_TASK_FREQUENCY
	// eA1 = eye(2)
	// ser = eye(2)
	// for i = 1:5
	//	ser *= A*dt/i;
	//	eA1 += ser;
	// eA2 == inv(A)*(eA1-eye(2))*B
}

void M3MotorModel::ThermalShutdown(string config_filename, mReal ambient_temp)
{
	if (use_old_temp_values)
	{
		  time_t rawtime;
		  struct tm *timeinfo;
		  char buf[80];

		  time(&rawtime);
		  timeinfo = localtime(&rawtime);
		  strftime(buf,80,"%c",timeinfo);
		  string s = string(buf);

		  // Write Old Temps to file
		  string temp_filename = config_filename.substr(0, config_filename.length()-4);
		  temp_filename += "_temps.yml";

		  mReal wind_temp = GetWindingTemp() - ambient_temp;
		  if (wind_temp < 0.0)
			wind_temp = 0.1;

		  mReal case_temp = GetCaseTemp() - ambient_temp;
		  if (case_temp < 0.0)
			case_temp = 0.1;

		  YAML::Emitter out;
		  out << YAML::BeginMap;
		  out << YAML::Key << "previous_temp_timestamp";
		  out << YAML::Value << s;
		  out << YAML::Key << "previous_winding_temp";
		  out << YAML::Value << wind_temp;
		  out << YAML::Key << "previous_case_temp";
		  out << YAML::Value << case_temp;
		  out << YAML::EndMap;
		  string fileout = doc_path+temp_filename;
		  m3rt::WriteYamlDoc(fileout.c_str(), out);

	}
		  //M3_DEBUG("writing: %s\n", temp_filename.c_str() );
		  /*
		  YAML::Node config = YAML::LoadFile(config_filename);
		  config["previous_temp_timestamp"] = s;
		  config["previous_winding_temp"] = GetWindingTemp();
		  config["previous_case_temp"] = GetCaseTemp();

		  std::ofstream fout(config_filename);
		  fout << config;
		  */
}

void M3MotorModel::ReadConfig(const YAML::Node & doc, string config_filename)
{
	string t;
	doc["name"] >> name;

	try
	{
		doc["model_type"] >> t;
		if (t.compare("none")==0){model_type=NONE; return;}
		if (t.compare("model_v0")==0){model_type=MODEL_V0;}
		if (t.compare("model_v1")==0){model_type=MODEL_V1;}
		if (t.compare("model_v2")==0){model_type=MODEL_V2;}
	} catch(...)
	{
		//M3_WARN("Missing version key in config file for motor %s. Defaulting to type MODEL_V0\n",name.c_str());
		model_type=MODEL_V0;
	}

	if (model_type==MODEL_V0)
	{
		doc["winding_resistance"] >>winding_resistance;
		doc["thermal_resistance_housing_ambient"] >>thermal_resistance_housing_ambient;
		doc["thermal_resistance_rotor_housing"] >>thermal_resistance_rotor_housing;
		doc["max_winding_temp"] >>max_winding_temp;
		doc["gear_ratio"] >>gear_ratio;
		doc["thermal_time_constant_winding"] >>thermal_time_constant_winding;
		current_sensor_type=CURRENT_MEASURED; //standard for RBL and previous versions
		temp_sensor_type=TEMP_CASE; //standard for RBL and previous versions
		//The winding temp will take thermal_time_constant_winding seconds to achieve a steady-state calculated value
		winding_temp_avg.Resize(1.0/(mReal)RT_TASK_FREQUENCY, thermal_time_constant_winding);
	}
	if (model_type==MODEL_V1 || model_type==MODEL_V2)
	{
		doc["nominal_voltage"] >> nominal_voltage;
		doc["no_load_speed"] >> no_load_speed;
		doc["no_load_current"] >> no_load_current;
		doc["nominal_speed"] >> nominal_speed;
		doc["nominal_torque"] >> nominal_torque;
		doc["nominal_current"] >> nominal_current;
		doc["stall_torque"] >> stall_torque;
		doc["starting_current"] >> starting_current;
		doc["max_efficiency"] >> max_efficiency;
		doc["winding_resistance"] >> winding_resistance;
		doc["winding_inductance"] >> winding_inductance;
		doc["torque_constant"] >> torque_constant;
		doc["speed_constant"] >> speed_constant;
		doc["speed_torque_gradient"] >> speed_torque_gradient;
		doc["mechanical_time_constant"] >> mechanical_time_constant;
		doc["rotor_inertia"] >> rotor_inertia;
		doc["thermal_resistance_housing_ambient"] >> thermal_resistance_housing_ambient;
		doc["thermal_resistance_rotor_housing"] >> thermal_resistance_rotor_housing;
		doc["thermal_time_constant_winding"] >> thermal_time_constant_winding;
		doc["thermal_time_constant_motor"] >> thermal_time_constant_motor;
		doc["max_winding_temp"] >> max_winding_temp;
		doc["gear_ratio"] >> gear_ratio;
		doc["max_pwm_duty"] >> max_pwm_duty;
		safe_pwm_duty=max_pwm_duty;
		safe_pwm_slew.Reset(max_pwm_duty);
		try
		{
			doc["alpha_cu"]>>alpha_cu;
		} catch(...)
		{
			alpha_cu=.00393; //copper constant
		}
		try
		{
			doc["safe_thermal_pct"]>>safe_thermal_pct;
		} catch(...)
		{
			safe_thermal_pct=0.80;
		}
		try
		{
			doc["safe_pwm_pct"]>>safe_pwm_pct;
		} catch(...)
		{
			safe_pwm_pct=0.90;
		}
		try
		{
			doc["i_scale"]>>i_scale;
		} catch(...)
		{
			i_scale=1.0;
		}
		doc["amplifier_resistance"] >> amplifier_resistance;
		doc["current_sensor_type"]>>t;
		if (t.compare("none")==0){current_sensor_type=CURRENT_NONE;}
		if (t.compare("measured")==0){current_sensor_type=CURRENT_MEASURED;}
		if (t.compare("controlled")==0){current_sensor_type=CURRENT_CONTROLLED;}
		doc["temp_sensor_type"]>>t;
		if (t.compare("none")==0){temp_sensor_type=TEMP_NONE;}
		if (t.compare("ambient")==0){temp_sensor_type=TEMP_AMBIENT;}
		if (t.compare("case")==0){temp_sensor_type=TEMP_CASE;}
		//The winding temp will take thermal_time_constant_winding seconds to achieve a steady-state calculated value
		winding_temp_avg.Resize(1.0/(mReal)RT_TASK_FREQUENCY, thermal_time_constant_winding);
		case_temp_avg.Resize(1.0/(mReal)RT_TASK_FREQUENCY, thermal_time_constant_motor);
	}
	//1/2 sec for rms current estimates.
	//5 sec for continuous current estimates
	int window_us=0.5*1000000;
	int downsample = (window_us/RT_TASK_FREQUENCY)/100;
	curr_avg_rms.Resize(window_us, downsample);
	window_us=5.0*1000000;
	downsample = (window_us/RT_TASK_FREQUENCY)/100;
	curr_avg_cont.Resize(window_us, downsample);
	//Report back a 1s filtered temp sensor
	window_us=1.0*1000000;
	downsample = (window_us/RT_TASK_FREQUENCY)/100;
	tmp_avg.Resize(window_us,downsample);

	if (model_type==MODEL_V2)
	{
	  try
	{
		doc["use_old_temp_values"] >> use_old_temp_values;
	} catch(...)
	{
		//M3_WARN("Missing version key in config file for motor %s. Defaulting to type MODEL_V0\n",name.c_str());
		use_old_temp_values = false;
	}
	  ThermalInit(config_filename);
	}
}

void M3MotorModel::Step(mReal i, mReal pwm, mReal rpm, mReal tmp)
{
	if (model_type==NONE)
	  return;
	if (model_type==MODEL_V0)
	  StepModelV0(i, pwm, rpm, tmp);
	if (model_type==MODEL_V1)
	  StepModelV1(i, pwm, rpm, tmp);
	if (model_type==MODEL_V2)
	  StepModelV2(i, pwm, rpm, tmp);
}

//Input sensor data of current sensor, amplifier pwm, velocity, and temp sensor
void M3MotorModel::StepModelV2(mReal i, mReal pwm, mReal rpm, mReal tmp)
{
	//Voltage
	mReal duty=0;
	v_pwm = 0;
	v_cemf =0;
	power_mech=0;
	power_elec=0;
	duty=pwm/max_pwm_duty ;
	v_pwm = ABS(nominal_voltage*pwm/max_pwm_duty);
	v_cemf = ABS(rpm*gear_ratio/speed_constant);

	if (current_sensor_type==CURRENT_NONE)
	{
		//Basic motor model with back emf. Estimates current from applied duty cycle and motor velocity.
		//See paper: "Towards a dynamic actuator model for a hexapod robot"
		//I_a = (d*Vs-Ks*omega)/(Ra+d*d*Ramp)
		//duty should be -1.0 to 1.0
		//put in mA

		mReal I_ma = MAX(.00001,((MAX(0.0,v_pwm-v_cemf))/(winding_resistance+duty*duty*amplifier_resistance))*1000.0);
		mReal x=curr_avg_rms.Step(I_ma*I_ma);
		i_rms = i_scale*sqrt(ABS(x)); //min(starting_current*1000.0,sqrt(curr_avg_rms.Step(I_ma*I_ma)));
		/*if (tmp_cnt++%100==0)
			M3_INFO("i_rms %f x %f I_ma %f v_pwm %f v_cemf %f duty %f\n",i_rms,x,I_ma, v_pwm, v_cemf, duty);
		i_cont=curr_avg_cont.Step(i_rms);*/
		i = I_ma;
	}

	//Current
	//i_rms=min(starting_current*1000.0,sqrt(ABS(curr_avg_rms.Step(i*i)))); //Just filter (Avoid NAN)
	i_rms=min(starting_current*1000.0,i); // Lee: note this model does not use rms
	i_cont=curr_avg_cont.Step(i_rms);

	//Winding resistance
	mReal hot_resistance = winding_resistance*(1.0+alpha_cu*(winding_temp-25.0));

	//Power
	power_mech=(i_rms/1000.0)*v_cemf;
	power_elec=(i_rms/1000.0)*v_pwm;
	power_heat=(i_rms/1000.0)*(i_rms/1000.0)*hot_resistance;

	//Temp
	if (first_wt_step)
		tmp_avg.Reset(25.0);
	ambient_temp=tmp_avg.Step(tmp);

	Tprev = eA1*Tprev+eA2*power_heat;
	winding_temp=ambient_temp+Tprev(0);
	case_temp=ambient_temp+Tprev(1);

	first_wt_step=MAX(0,first_wt_step-1);
	StepVoltageLimit();
}

//////////////////////////////////////////////////////////////////////
//Using cold resistance
mReal  M3MotorModel::mNmToPwm(mReal mNm)
{
    mReal i_a = (mNm/torque_constant)/gear_ratio;

    mReal duty = (i_a*winding_resistance+v_cemf)/nominal_voltage;
    duty=CLAMP(duty,-1.0,1.0);
    mReal pwm = duty*max_pwm_duty;
    //if (tmp_cnt++%100==0)
    //  M3_INFO("mNm: %f iA: %f v_cemf: %f vreq: %f, duty: %f pwm %f\n", mNm, i_a, v_cemf, i_a*winding_resistance, duty, pwm);
    return pwm;
}

void M3MotorModel::StepVoltageLimit()
{
	//When the winding is thermal_limit_pct% of over-temp, limit the voltage to the motor
	//to a value that results in the safe y% of max continuous current
	//slew to the limit over thermal_time_constant_winding seconds
	//restore to max over 5 x thermal_time_constant_winding seconds (allow extra time for motor to cool down)
	mReal mpa=safe_pwm_pct*max_pwm_duty*nominal_torque/stall_torque;
	mReal rate_down = (max_pwm_duty-mpa)/MIN(thermal_time_constant_winding,1.0);
	mReal rate_up = (max_pwm_duty-mpa)/(5*thermal_time_constant_winding);
	if (winding_temp>safe_thermal_pct*max_winding_temp)
	{
		safe_pwm_duty=safe_pwm_slew.Step(mpa,rate_down);
	}
	else
		safe_pwm_duty=safe_pwm_slew.Step(max_pwm_duty,rate_up);
}

////////////////////////// LEGACY /////////////////////////////////////
//Keep around older (less accurate) models so thermal safety behavior of
//older robots doesn't change

//Model V1 generally has an ambient temp sensor near the motor
//and may or may not have a useful current sensor value
//If it does not have a current sensor, the current is estimated from the
//applied voltage and back-emf. The winding temp is approximated with
//an exponential filter, but the time constants are approximate
//Voltage limiting was added to this generation for additional safety.
void M3MotorModel::StepModelV1(mReal i, mReal pwm, mReal rpm, mReal tmp)
{
	/////////////// Voltage terms //////////////////////////////
	mReal duty=0;
	v_pwm = 0;
	v_cemf =0;
	power_mech=0;
	power_elec=0;
	duty=pwm/max_pwm_duty ;
	v_pwm = ABS(nominal_voltage*pwm/max_pwm_duty);
	v_cemf = ABS(rpm*gear_ratio/speed_constant);
	/////////////// Estimate current ////////////////////////////
	//Current controlled amplifier, Use the commanded value as the 'sensed' value
	if (current_sensor_type==CURRENT_CONTROLLED)
	{
		i_rms=i;
		i_cont=i;
	}
	//Current measurement available from the amplifier
	//i_cont is longer time scale than i_rms
	if (current_sensor_type==CURRENT_MEASURED)
	{
	  i_rms=min(starting_current*1000.0,sqrt(ABS(curr_avg_rms.Step(i*i)))); //Just filter (Avoid NAN)
	  i_cont=curr_avg_cont.Step(i_rms);
	}
	//No current sensing available. Estimate it based on the applied voltage and back-EMF
	if (current_sensor_type==CURRENT_NONE)
	{
		//Basic motor model with back emf. Estimates current from applied duty cycle and motor velocity.
		//See paper: "Towards a dynamic actuator model for a hexapod robot"
		//I_a = (d*Vs-Ks*omega)/(Ra+d*d*Ramp)
		//duty should be -1.0 to 1.0
		//put in mA

		mReal I_ma = MAX(.00001,((MAX(0.0,v_pwm-v_cemf))/(winding_resistance+duty*duty*amplifier_resistance))*1000.0);
		mReal x=curr_avg_rms.Step(I_ma*I_ma);
		i_rms = i_scale*sqrt(ABS(x)); //min(starting_current*1000.0,sqrt(curr_avg_rms.Step(I_ma*I_ma)));
		/*if (tmp_cnt++%100==0)
			M3_INFO("i_rms %f x %f I_ma %f v_pwm %f v_cemf %f duty %f\n",i_rms,x,I_ma, v_pwm, v_cemf, duty);
		i_cont=curr_avg_cont.Step(i_rms);*/
	}
	power_mech=(i_rms/1000.0)*v_cemf;
	power_elec=(i_rms/1000.0)*v_pwm;

	/////////////// Estimate winding temp ///////////////////////
	//Get Case/Ambient temp
	ambient_temp=25.0; //provide default in case no sensor present
	case_temp=25.0;
	winding_temp=25.0;

	if (first_wt_step)
		tmp_avg.Reset(25.0);

	//Newer hardware has ambient temp sensors
	if (temp_sensor_type==TEMP_AMBIENT)
	{
		if (first_wt_step)
		{
			tmp_avg.Reset(tmp);
		}
		ambient_temp=tmp_avg.Step(tmp);
	}

	//Older hardware has case temp sensors
	if (temp_sensor_type==TEMP_CASE)
	{
		if (first_wt_step)
			tmp_avg.Reset(tmp);
		case_temp=tmp_avg.Step(tmp);
		ambient_temp=case_temp; //w/o greater knowledge, assume that internal body temp follows motor case temp

	}

	//Estimate winding resistance (from maxon catalog)
	mReal hot_resistance = winding_resistance*(1.0+alpha_cu*(winding_temp-25.0));
	//Estimate winding temp
	power_heat=(i_rms/1000.0)*(i_rms/1000.0)*hot_resistance;
	if (temp_sensor_type==TEMP_CASE)
	{
		mReal val=(thermal_resistance_rotor_housing)*power_heat+case_temp;
		if (first_wt_step)
			winding_temp_avg.Reset(val);
		winding_temp = winding_temp_avg.Step(val);
	}

	if ((temp_sensor_type==TEMP_AMBIENT || temp_sensor_type==TEMP_NONE))
	{
		mReal val_c=ambient_temp+thermal_resistance_rotor_housing*power_heat;
		mReal val_w=(thermal_resistance_rotor_housing+thermal_resistance_housing_ambient)*power_heat+ambient_temp;
		if (first_wt_step)
		{
			winding_temp_avg.Reset(val_w);
			case_temp_avg.Reset(val_c);
		}
		case_temp = case_temp_avg.Step(val_c); //Exponential filter with thermal time constant
		winding_temp = winding_temp_avg.Step(val_w);//Exponential filter with thermal time constant
	}
	first_wt_step=MAX(0,first_wt_step-1);//Resets for first few cycles to ensure start with valid data
	StepVoltageLimit();
}

//Model V0 generally has a temp sensor on the case of the motor
//Currents are measured from sensors, although this gen. or robot
//current sensing may be innacurate.
//Voltage limiting was added to this generation for additional safety.
void M3MotorModel::StepModelV0(mReal i, mReal pwm, mReal rpm, mReal tmp)
{
	/////////////// Voltage terms //////////////////////////////
	power_mech=0;
	power_elec=0;
	/////////////// Estimate current ////////////////////////////
	//Current controlled amplifier with no feedback, Use the commanded value as the 'sensed' value
	if (current_sensor_type==CURRENT_CONTROLLED)
	{
		i_rms=i;
		i_cont=i;
	}
	//Current measurement available from the amplifier
	//i_cont is longer time scale than i_rms
	if (current_sensor_type==CURRENT_MEASURED)
	{
	  i_rms=sqrt(ABS(curr_avg_rms.Step(i*i))); //Just filter (Avoid NAN)
	  i_cont=curr_avg_cont.Step(i_rms);
	}

	/////////////// Estimate winding temp ///////////////////////
	//Get Case/Ambient temp
	ambient_temp=25.0; //provide default in case no sensor present
	case_temp=25.0;
	winding_temp=25.0;

	if (first_wt_step)
		tmp_avg.Reset(25.0);

	//Newer hardware has ambient temp sensors
	if (temp_sensor_type==TEMP_AMBIENT)
	{
		if (first_wt_step)
		{
			tmp_avg.Reset(tmp);
		}
		ambient_temp=tmp_avg.Step(tmp);
	}

	//Older hardware has case temp sensors
	if (temp_sensor_type==TEMP_CASE)
	{
		if (first_wt_step)
			tmp_avg.Reset(tmp);
		case_temp=tmp_avg.Step(tmp);
		ambient_temp=case_temp; //w/o greater knowledge, assume that internal body temp follows motor case temp

	}
	//Estimate winding resistance (from maxon catalog)
	mReal hot_resistance = winding_resistance*(1.0+alpha_cu*(winding_temp-25.0));
	//Estimate winding temp
	power_heat=(i_rms/1000.0)*(i_rms/1000.0)*hot_resistance;
	if (temp_sensor_type==TEMP_CASE)
	{
		mReal val=(thermal_resistance_rotor_housing)*power_heat+case_temp;
		if (first_wt_step)
			winding_temp_avg.Reset(val);
		winding_temp = winding_temp_avg.Step(val);
	}
	first_wt_step=MAX(0,first_wt_step-1);//Resets for first few cycles to ensure start with valid data
	StepVoltageLimit();
}

} //namespace
///////////////////////////////////////////////////////


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2000, 2001, 2002, 2003 RiskMap srl
 Copyright (C) 2003 Ferdinando Ametrano
 Copyright (C) 2007 StatPro Italia srl

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/instruments/vanillaoption.hpp>
#include <ql/instruments/impliedvolatility.hpp>
#include <ql/pricingengines/vanilla/analyticeuropeanengine.hpp>
#include <ql/pricingengines/vanilla/fdamericanengine.hpp>
#include <ql/pricingengines/vanilla/fdbermudanengine.hpp>
#include <ql/exercise.hpp>
#include <boost/scoped_ptr.hpp>

namespace QuantLib {

    VanillaOption::VanillaOption(
        const boost::shared_ptr<StrikedTypePayoff>& payoff,
        const boost::shared_ptr<Exercise>& exercise)
    : OneAssetOption(payoff, exercise) {}

    Volatility VanillaOption::impliedVolatility(
             Real targetValue,
             const boost::shared_ptr<GeneralizedBlackScholesProcess>& process,
             Real accuracy,
             Size maxEvaluations,
             Volatility minVol,
             Volatility maxVol) const {

        QL_REQUIRE(!isExpired(), "option expired");

        boost::shared_ptr<SimpleQuote> volQuote(new SimpleQuote);

        boost::shared_ptr<GeneralizedBlackScholesProcess> newProcess =
            detail::ImpliedVolatilityHelper::clone(process, volQuote);

        // engines are built-in for the time being
        boost::scoped_ptr<PricingEngine> engine;
        switch (exercise_->type()) {
          case Exercise::European:
            engine.reset(new AnalyticEuropeanEngine(newProcess));
            break;
          case Exercise::American:
            engine.reset(new FDAmericanEngine<CrankNicolson>(newProcess));
            break;
          case Exercise::Bermudan:
            engine.reset(new FDBermudanEngine<CrankNicolson>(newProcess));
            break;
          default:
            QL_FAIL("unknown exercise type");
        }

        return detail::ImpliedVolatilityHelper::calculate(*this,
                                                          *engine,
                                                          *volQuote,
                                                          targetValue,
                                                          accuracy,
                                                          maxEvaluations,
                                                          minVol, maxVol);
    }

}


//==============================================================================
//         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
//==============================================================================
/*!
 * \file
**/
#ifndef BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_REMAINDER_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_REMAINDER_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

/*!
 * \ingroup boost_simd_arithmetic
 * \defgroup boost_simd_arithmetic_remainder remainder
 *
 * \par Description
 * The function computes the remainder of dividing a0 by a1.
 * The return value is a0-n*a1, where n is the value a0/a1,
 * rounded to the nearest integer.
 * If the absolute value of a0-n*a1 is 0.5, n is chosen to be even.
 * \par
 * if one prefer: if a1 is zero returns a0, else return
 * a0-divround(a0,a1)*a1
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/remainder.hpp>
 * \endcode
 *
 * \par Alias
 * \arg drem
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class A0>
 *     meta::call<tag::remainder_(A0,A0)>::type
 *     remainder(const A0 & a0,const A0 & a1);
 * }
 * \endcode
 *
 * \param a0 the first parameter of remainder
 * \param a1 the second parameter of remainder
 *
 * \return a value of the common type of the parameters
 *
 * \par Notes
 * In SIMD mode, this function acts elementwise on the inputs vectors elements
 * \par
 *
**/

namespace boost { namespace simd { namespace tag
  {
    /*!
     * \brief Define the tag remainder_ of functor remainder
     *        in namespace boost::simd::tag for toolbox boost.simd.arithmetic
    **/
    struct remainder_ : ext::elementwise_<remainder_> { typedef ext::elementwise_<remainder_> parent; };
  }
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::remainder_, remainder, 2)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::remainder_, drem, 2)
} }

#endif

// modified by jt the 25/12/2010