#include <iostream>
#include <NTL/ZZ.h>
#include <memory>

#include <boost/program_options.hpp>

#include "Lenstra.h"
#include "Options.h"
#include "EdwardsModel.h"
#include "WeierstrassModel.h"

namespace po = boost::program_options;

int main(int argc, char **argv) {
    std::shared_ptr<Options> options = std::make_shared<Options>();

    po::options_description desc("OPTIONS");
    po::variables_map vm;
    desc.add_options()
            ("help,h", "produce help message")
            ("weierstrass_model,w", po::bool_switch(&options->weierstrass), "set Weierstrass model")
            ("edwards_model,e", po::bool_switch(&options->edwards), "set Edwards model")
            ("timer,t", po::bool_switch(&options->timer), "time measurement")
            ("parallel,p", po::bool_switch(&options->parallel), "start parallel")
            ("bound,b", po::value<NTL::ZZ>(options->bound.get()), "Maximal bound for iterations (Default square root of composite number)")
            ("composite-number,n", po::value<NTL::ZZ>(options->composite_number.get())->required(), "Positive integer bigger than 1 to factorize");
    try {
        po::store(po::parse_command_line(argc, argv, desc), vm);
        if (vm.count("help")) {
            std::cout << argv[0] << " [OPTIONS] --composite-number/-n COMPOSITE NUMBER\n";
            std::cout << desc << "\n";
            return 1;
        }
        po::notify(vm);
    } catch (std::exception &exception) {
        std::cout << exception.what() << "\n";
        return 1;
    }

    if (options->weierstrass && options->edwards) {
        std::cerr << "Only one model can be specified!\n";
        return 2;
    }

    options->weierstrass = !options->edwards;

    if (*options->composite_number < 2) {
        std::cerr << "Composite number must be positive integer bigger than 1!\n";
        return 3;
    }

    std::cout << "Factorizing number: " << *options->composite_number << '\n';
    std::cout << "Using model: " << (options->edwards ? "Edwards" : "Weierstrass") << '\n';
    std::cout << "Using timer: " << (options->timer ? "yes" : "no") << '\n';
    double start_time = 0.0, end_time;
    std::shared_ptr<AbstractModel> model;
    if (options->weierstrass) {
        model = std::make_shared<WeierstrassModel>(options);
    } else {
        model = std::make_shared<EdwardsModel>(options);
    }
    Lenstra ecm(options, model);
    if (options->timer) {
        start_time = NTL::GetTime();
    }
    NTL::ZZ factor;
    if (!options->parallel) {
        factor = ecm.factorize();
    } else {
        factor = ecm.factorize_parallel(argc, argv);
    }

    end_time = NTL::GetTime();
    if (options->timer && !options->parallel) {
        std::cout << "time = " << end_time - start_time << " s\n";
    }

    if (factor != 0)
        std::cout << "Factor = " << factor << "\n";

    return 0;
}


// Copyright 2014 BVLC and contributors.

#include <algorithm>
#include <vector>
#include <cmath>
#include <stdio.h>

#include "google/protobuf/descriptor.h"
#include "google/protobuf/descriptor.h"
#include "caffe/layer.hpp"
#include "caffe/layers/flow_warp_layer.hpp"
#include "caffe/util/rng.hpp"
#include "caffe/util/math_functions.hpp"

#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

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

#include <iostream>
#include <fstream>
#include <omp.h>

using std::max;

namespace caffe {

template <typename Dtype>
void FlowWarpLayer<Dtype>::LayerSetUp(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top) {
}

template <typename Dtype>
void FlowWarpLayer<Dtype>::Reshape(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top) {
  CHECK_EQ(bottom.size(), 2) << "FlowWarpLayer takes two input blobs: image and flow.";
  CHECK_EQ(top.size(), 1) << "FlowWarpLayer outputs one blob.";

  const int num = bottom[0]->num();
  const int channels = bottom[0]->channels();
  const int height = bottom[0]->height();
  const int width = bottom[0]->width();

  CHECK_EQ(num, bottom[1]->num()) << "Num of the inputs should be the same";
  CHECK_EQ(2, bottom[1]->channels()) << "Flow should have 2 channels: x-flow and y-flow";
  CHECK_EQ(width, bottom[1]->width()) << "Width of the inputs should be the same";
  CHECK_EQ(height, bottom[1]->height()) << "Height of the inputs should be the same";

  top[0]->Reshape(num, channels, height, width);
  transposed_image_.Reshape(num, height, width, channels);
}

#define min(a,b) ((a<b)?(a):(b))
#define max(a,b) ((a>b)?(a):(b))

template <typename Dtype>
void FlowWarpLayer<Dtype>::Forward_cpu(const vector<Blob<Dtype>*>& bottom,
    const vector<Blob<Dtype>*>& top)
{
    int width = top[0]->width();
    int height = top[0]->height();
    int channels = top[0]->channels();
    int num = top[0]->num();
    const int wh_size = width * height;
    const int whc_size = width * height * channels;

    Dtype* warped_data = top[0]->mutable_cpu_data(); // dest
    const Dtype* image_data = bottom[0]->cpu_data(); // source image
    const Dtype* flow_data = bottom[1]->cpu_data(); // source flow

    Dtype fillValue = this->layer_param().flow_warp_param().fill_value() == FlowWarpParameter_FillParameter_ZERO ? 0 : NAN;

    for(int n=0; n<num; n++)
    {
        int off = whc_size * n;
        for(int x=0; x<width; x++)
            for(int y=0; y<height; y++)
            {
                float fx = flow_data[2*wh_size*n + y*width + x];
                float fy = flow_data[2*wh_size*n + wh_size + y*width + x];

                float x2 = float(x) + fx;
                float y2 = float(y) + fy;

                if(x2>=0 && y2>=0 && x2<width && y2<height)
                {
                    int ix2_L = int(x2);
                    int iy2_T = int(y2);
                    int ix2_R = min(ix2_L+1, width-1);
                    int iy2_B = min(iy2_T+1, height-1);

                    float alpha=x2-ix2_L;
                    float beta=y2-iy2_T;

                    for(int c=0; c<channels; c++)
                    {
                        float TL = image_data[off + c*wh_size + iy2_T*width + ix2_L];
                        float TR = image_data[off + c*wh_size + iy2_T*width + ix2_R];
                        float BL = image_data[off + c*wh_size + iy2_B*width + ix2_L];
                        float BR = image_data[off + c*wh_size + iy2_B*width + ix2_R];

                        warped_data[off + c*wh_size + y*width + x] =
                            (1-alpha)*(1-beta)*TL +
                            alpha*(1-beta)*TR +
                            (1-alpha)*beta*BL +
                            alpha*beta*BR;
                    }
                }
                else
                {
                    for(int c=0; c<channels; c++)
                        warped_data[off + c*wh_size + y*width + x] = fillValue;
                }
            }
    }
}

template <typename Dtype>
void FlowWarpLayer<Dtype>::Backward_cpu(const vector<Blob<Dtype>*>& top,
      const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& bottom)
{
    int width = top[0]->width();
    int height = top[0]->height();
    int channels = top[0]->channels();
    int num = top[0]->num();
    const int wh_size = width * height;
    const int whc_size = width * height * channels;

    const Dtype* warped_data = top[0]->cpu_data(); // dest
    const Dtype* warped_diff = top[0]->cpu_diff(); // dest
    const Dtype* image_data = bottom[0]->cpu_data(); // source image
    Dtype* image_diff = bottom[0]->mutable_cpu_diff(); // source image
    const Dtype* flow_data = bottom[1]->cpu_data(); // source flow
    Dtype* flow_diff = bottom[1]->mutable_cpu_diff(); // source flow

    for(int i=0; i<num*whc_size; i++)
        image_diff[i] = 0 ;

    for(int n=0; n<num; n++)
    {
        int off = whc_size * n;
        for(int x=0; x<width; x++)
            for(int y=0; y<height; y++)
            {
                float fx = flow_data[2*wh_size*n + y*width + x];
                float fy = flow_data[2*wh_size*n + wh_size + y*width + x];

                float x2 = float(x) + fx;
                float y2 = float(y) + fy;

                if(x2>=0 && y2>=0 && x2<width && y2<height)
                {
                    int ix2_L = int(x2);
                    int iy2_T = int(y2);
                    int ix2_R = min(ix2_L+1, width-1);
                    int iy2_B = min(iy2_T+1, height-1);

                    float alpha=x2-ix2_L;
                    float beta=y2-iy2_T;
                    for(int c=0; c<channels; c++)
                    {
                        float warped_diff_value = warped_diff[off + c*wh_size + y*width + x];
                        image_diff[off + c*wh_size + iy2_T*width + ix2_L] += warped_diff_value * (1-alpha)*(1-beta);
                        image_diff[off + c*wh_size + iy2_T*width + ix2_R] += warped_diff_value * alpha*(1-beta);
                        image_diff[off + c*wh_size + iy2_B*width + ix2_L] += warped_diff_value * (1-alpha)*beta;
                        image_diff[off + c*wh_size + iy2_B*width + ix2_R] += warped_diff_value * alpha*beta;
                    }

                    float gamma = iy2_B - y2;
                    float bot_diff = 0;
                    for(int c=0; c<channels; c++)
                    {
                        float temp = 0;
                        temp += gamma *     (image_data[off + c*wh_size + iy2_T*width + ix2_R] - image_data[off + c*wh_size + iy2_T*width + ix2_L]);
                        temp += (1-gamma) * (image_data[off + c*wh_size + iy2_B*width + ix2_R] - image_data[off + c*wh_size + iy2_B*width + ix2_L]);

                        bot_diff += warped_diff[off + c*wh_size + y*width + x] * temp;
                    }
                    flow_diff[2*wh_size*n + y*width + x] = bot_diff;

                    gamma = ix2_R - x2;
                    bot_diff = 0;
                    for(int c=0; c<channels; c++)
                    {
                        float temp = 0;
                        temp += gamma *     (image_data[off + c*wh_size + iy2_B*width + ix2_L] - image_data[off + c*wh_size + iy2_T*width + ix2_L]);
                        temp += (1-gamma) * (image_data[off + c*wh_size + iy2_B*width + ix2_R] - image_data[off + c*wh_size + iy2_T*width + ix2_R]);

                        bot_diff += warped_diff[off + c*wh_size + y*width + x] * temp;
                    }
                    flow_diff[2*wh_size*n + wh_size + y*width + x] = bot_diff;
                }
            }
    }

    if(!propagate_down[0]) caffe_memset(bottom[0]->count()*sizeof(Dtype), 0, image_diff);
    if(!propagate_down[1]) caffe_memset(bottom[1]->count()*sizeof(Dtype), 0, flow_diff);

//    {
//        printf("cpu flow u:\n");
//        for(int y=0; y<height; y++)
//        {
//            for(int x=0; x<width; x++)
//            {
//                printf("%f ", bottom[1]->data_at(0, 0, y, x));
//            }
//            printf("\n");
//        }
//        printf("cpu flow v:\n");
//        for(int y=0; y<height; y++)
//        {
//            for(int x=0; x<width; x++)
//            {
//                printf("%f ", bottom[1]->data_at(0, 1, y, x));
//            }
//            printf("\n");
//        }
//        printf("cpu image:\n");
//        for(int y=0; y<height; y++)
//        {
//            for(int x=0; x<width; x++)
//            {
//                printf("%f ", bottom[0]->data_at(0, 0, y, x));
//            }
//            printf("\n");
//        }
//        printf("cpu flow diff u:\n");
//        for(int y=0; y<height; y++)
//        {
//            for(int x=0; x<width; x++)
//            {
//                printf("%f ", bottom[1]->diff_at(0, 0, y, x));
//            }
//            printf("\n");
//        }
//        printf("cpu flow diff v:\n");
//        for(int y=0; y<height; y++)
//        {
//            for(int x=0; x<width; x++)
//            {
//                printf("%f ", bottom[1]->diff_at(0, 1, y, x));
//            }
//            printf("\n");
//        }
//        printf("cpu image diff:\n");
//        for(int y=0; y<height; y++)
//        {
//            for(int x=0; x<width; x++)
//            {
//                printf("%f ", bottom[0]->diff_at(0, 0, y, x));
//            }
//            printf("\n");
//        }
//    }
}

INSTANTIATE_CLASS(FlowWarpLayer);
REGISTER_LAYER_CLASS(FlowWarp);

}  // namespace caffe

//                    }
//                }
//            }

//        for(int x=0; x<width; x++)
//            for(int y=0; y<height; y++)
//            {
//                float fx = flow_data[off + y*width + x];
//                float fy = flow_data[off + wh_size + y*width + x];

//                float x2 = float(x) + fx;
//                float y2 = float(y) + fy;

//                if(x2>=0 && y2>=0 && x2<width && y2<height)
//                {
//                    int ix2_L = int(x2);
//                    int iy2_T = int(y2);
//                    int ix2_R = min(ix2_L+1, width-1);
//                    int iy2_B = min(iy2_T+1, height-1);


//
// Copyright (c) 2009, Markus Rickert
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//

#include <algorithm>
#include <stack>
#include <boost/graph/graphviz.hpp>
#include <rl/math/Quaternion.h>
#include <rl/math/Rotation.h>
#include <rl/math/Spatial.h>
#include <rl/math/Unit.h>

#include "Exception.h"
#include "Dynamic.h"
#include "Prismatic.h"
#include "Revolute.h"
#include "World.h"

namespace rl
{
	namespace mdl
	{
		Dynamic::Dynamic() :
			Kinematic(),
			G(),
			invM(),
			invMx(),
			M(),
			V()
		{
		}

		Dynamic::~Dynamic()
		{
		}

		void
		Dynamic::calculateCentrifugalCoriolis()
		{
			this->calculateCentrifugalCoriolis(this->V);
		}

		void
		Dynamic::calculateCentrifugalCoriolis(::rl::math::Vector& V)
		{
			::rl::math::Vector3 g = this->getWorldGravity();

			::rl::math::Vector tmp = ::rl::math::Vector::Zero(this->getDof());

			this->setAcceleration(tmp);
			this->setWorldGravity(::rl::math::Vector3::Zero());

			this->inverseDynamics();
			V = this->getTorque();

			this->setWorldGravity(g);
		}

		void
		Dynamic::calculateGravity()
		{
			this->calculateGravity(this->G);
		}

		void
		Dynamic::calculateGravity(::rl::math::Vector& G)
		{
			::rl::math::Vector tmp = ::rl::math::Vector::Zero(this->getDof());

			this->setVelocity(tmp);
			this->setAcceleration(tmp);

			this->inverseDynamics();
			G = this->getTorque();
		}

		void
		Dynamic::calculateMassMatrix()
		{
			this->calculateMassMatrix(this->M);
		}

		void
		Dynamic::calculateMassMatrix(::rl::math::Matrix& M)
		{
			::rl::math::Vector3 g = this->getWorldGravity();

			::rl::math::Vector tmp = ::rl::math::Vector::Zero(this->getDof());

			this->setVelocity(tmp);
			this->setWorldGravity(::rl::math::Vector3::Zero());

			for (::std::size_t i = 0; i < this->getDof(); ++i)
			{
				for (::std::size_t j = 0; j < this->getDof(); ++j)
				{
					tmp(j) = i == j ? 1 : 0;
				}

				this->setAcceleration(tmp);
				this->inverseDynamics();

				M.col(i) = this->getTorque();
			}

			this->setWorldGravity(g);
		}

		void
		Dynamic::calculateMassMatrixInverse()
		{
			this->calculateMassMatrixInverse(this->invM);
		}

		void
		Dynamic::calculateMassMatrixInverse(::rl::math::Matrix& invM)
		{
			::rl::math::Vector3 g = this->getWorldGravity();

			::rl::math::Vector tmp = ::rl::math::Vector::Zero(this->getDof());

			this->setVelocity(tmp);
			this->setWorldGravity(::rl::math::Vector3::Zero());

			for (::std::size_t i = 0; i < this->getDof(); ++i)
			{
				for (::std::size_t j = 0; j < this->getDof(); ++j)
				{
					tmp(j) = i == j ? 1 : 0;
				}

				this->setTorque(tmp);
				this->forwardDynamics();

				invM.col(i) = this->getAcceleration();
			}

			this->setWorldGravity(g);
		}

		void
		Dynamic::calculateOperationalMassMatrixInverse()
		{
			this->calculateOperationalMassMatrixInverse(this->J, this->invM, this->invMx);
		}

		void
		Dynamic::calculateOperationalMassMatrixInverse(const ::rl::math::Matrix& J, const ::rl::math::Matrix& invM, ::rl::math::Matrix& invMx) const
		{
			invMx = J * invM * J.transpose();
		}

		void
		Dynamic::forwardDynamics()
		{
			for (::std::vector<Element*>::iterator i = this->elements.begin(); i != this->elements.end(); ++i)
			{
				(*i)->forwardDynamics1();
			}

			for (::std::vector<Element*>::reverse_iterator i = this->elements.rbegin(); i != this->elements.rend(); ++i)
			{
				(*i)->forwardDynamics2();
			}

			for (::std::vector<Element*>::iterator i = this->elements.begin(); i != this->elements.end(); ++i)
			{
				(*i)->forwardDynamics3();
			}
		}

		const ::rl::math::Vector&
		Dynamic::getCentrifugalCoriolis() const
		{
			return this->V;
		}

		const ::rl::math::Vector&
		Dynamic::getGravity() const
		{
			return this->G;
		}

		const ::rl::math::Matrix&
		Dynamic::getMassMatrixInverse() const
		{
			return this->invM;
		}

		const ::rl::math::Matrix&
		Dynamic::getMassMatrix() const
		{
			return this->M;
		}

		const ::rl::math::Matrix&
		Dynamic::getOperationalMassMatrixInverse() const
		{
			return this->invMx;
		}

		void
		Dynamic::inverseDynamics()
		{
			for (::std::vector<Element*>::iterator i = this->elements.begin(); i != this->elements.end(); ++i)
			{
				(*i)->inverseDynamics1();
			}

			for (::std::vector<Element*>::reverse_iterator i = this->elements.rbegin(); i != this->elements.rend(); ++i)
			{
				(*i)->inverseDynamics2();
			}
		}

		void
		Dynamic::inverseForce()
		{
			for (::std::vector<Element*>::reverse_iterator i = this->elements.rbegin(); i != this->elements.rend(); ++i)
			{
				(*i)->inverseForce();
			}
		}

		void
		Dynamic::update()
		{
			Kinematic::update();

			this->M = ::rl::math::Matrix::Identity(this->getDof(), this->getDof());
			this->V = ::rl::math::Vector::Zero(this->getDof());
			this->G = ::rl::math::Vector::Zero(this->getDof());
			this->invM = ::rl::math::Matrix::Identity(this->getDof(), this->getDof());
			this->invMx = ::rl::math::Matrix::Identity(6 * this->getOperationalDof(), 6 * this->getOperationalDof());
		}
	}
}


// Copyright (c) 2016, Monea Research Labs
//
// Author: Shen Noether <shen.noether@gmx.com>
//
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without modification, are
// permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice, this list of
//    conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright notice, this list
//    of conditions and the following disclaimer in the documentation and/or other
//    materials provided with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its contributors may be
//    used to endorse or promote products derived from this software without specific
//    prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
// THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
// THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <boost/lexical_cast.hpp>
#include "misc_log_ex.h"
#include "rctOps.h"
using namespace crypto;
using namespace std;

#undef MONEA_DEFAULT_LOG_CATEGORY
#define MONEA_DEFAULT_LOG_CATEGORY "ringct"

#define CHECK_AND_ASSERT_THROW_MES_L1(expr, message) {if(!(expr)) {MWARNING(message); throw std::runtime_error(message);}}

namespace rct {

    //Various key initialization functions

    //initializes a key matrix;
    //first parameter is rows,
    //second is columns
    keyM keyMInit(size_t rows, size_t cols) {
        keyM rv(cols);
        size_t i = 0;
        for (i = 0 ; i < cols ; i++) {
            rv[i] = keyV(rows);
        }
        return rv;
    }

    //Various key generation functions

    //generates a random scalar which can be used as a secret key or mask
    void skGen(key &sk) {
        sk = crypto::rand<key>();
        sc_reduce32(sk.bytes);
    }

    //generates a random scalar which can be used as a secret key or mask
    key skGen() {
        key sk = crypto::rand<key>();
        sc_reduce32(sk.bytes);
        return sk;
    }

    //Generates a vector of secret key
    //Mainly used in testing
    keyV skvGen(size_t rows ) {
        CHECK_AND_ASSERT_THROW_MES(rows > 0, "0 keys requested");
        keyV rv(rows);
        size_t i = 0;
        crypto::rand(rows * sizeof(key), (uint8_t*)&rv[0]);
        for (i = 0 ; i < rows ; i++) {
            sc_reduce32(rv[i].bytes);
        }
        return rv;
    }

    //generates a random curve point (for testing)
    key  pkGen() {
        key sk = skGen();
        key pk = scalarmultBase(sk);
        return pk;
    }

    //generates a random secret and corresponding public key
    void skpkGen(key &sk, key &pk) {
        skGen(sk);
        scalarmultBase(pk, sk);
    }

    //generates a random secret and corresponding public key
    tuple<key, key>  skpkGen() {
        key sk = skGen();
        key pk = scalarmultBase(sk);
        return make_tuple(sk, pk);
    }

    //generates C =aG + bH from b, a is given..
    void genC(key & C, const key & a, xmr_amount amount) {
        key bH = scalarmultH(d2h(amount));
        addKeys1(C, a, bH);
    }

    //generates a <secret , public> / Pedersen commitment to the amount
    tuple<ctkey, ctkey> ctskpkGen(xmr_amount amount) {
        ctkey sk, pk;
        skpkGen(sk.dest, pk.dest);
        skpkGen(sk.mask, pk.mask);
        key am = d2h(amount);
        key bH = scalarmultH(am);
        addKeys(pk.mask, pk.mask, bH);
        return make_tuple(sk, pk);
    }

    //generates a <secret , public> / Pedersen commitment but takes bH as input
    tuple<ctkey, ctkey> ctskpkGen(const key &bH) {
        ctkey sk, pk;
        skpkGen(sk.dest, pk.dest);
        skpkGen(sk.mask, pk.mask);
        addKeys(pk.mask, pk.mask, bH);
        return make_tuple(sk, pk);
    }

    key zeroCommit(xmr_amount amount) {
        key mask = identity();
        mask = scalarmultBase(mask);
        key am = d2h(amount);
        key bH = scalarmultH(am);
        addKeys(mask, mask, bH);
        return mask;
    }

    key commit(xmr_amount amount, const key &mask) {
        key c = scalarmultBase(mask);
        key am = d2h(amount);
        key bH = scalarmultH(am);
        addKeys(c, c, bH);
        return c;
    }

    //generates a random uint long long (for testing)
    xmr_amount randXmrAmount(xmr_amount upperlimit) {
        return h2d(skGen()) % (upperlimit);
    }

    //Scalar multiplications of curve points

    //does a * G where a is a scalar and G is the curve basepoint
    void scalarmultBase(key &aG,const key &a) {
        ge_p3 point;
        sc_reduce32copy(aG.bytes, a.bytes); //do this beforehand!
        ge_scalarmult_base(&point, aG.bytes);
        ge_p3_tobytes(aG.bytes, &point);
    }

    //does a * G where a is a scalar and G is the curve basepoint
    key scalarmultBase(const key & a) {
        ge_p3 point;
        key aG;
        sc_reduce32copy(aG.bytes, a.bytes); //do this beforehand
        ge_scalarmult_base(&point, aG.bytes);
        ge_p3_tobytes(aG.bytes, &point);
        return aG;
    }

    //does a * P where a is a scalar and P is an arbitrary point
    void scalarmultKey(key & aP, const key &P, const key &a) {
        ge_p3 A;
        ge_p2 R;
        CHECK_AND_ASSERT_THROW_MES_L1(ge_frombytes_vartime(&A, P.bytes) == 0, "ge_frombytes_vartime failed at "+boost::lexical_cast<std::string>(__LINE__));
        ge_scalarmult(&R, a.bytes, &A);
        ge_tobytes(aP.bytes, &R);
    }

    //does a * P where a is a scalar and P is an arbitrary point
    key scalarmultKey(const key & P, const key & a) {
        ge_p3 A;
        ge_p2 R;
        CHECK_AND_ASSERT_THROW_MES_L1(ge_frombytes_vartime(&A, P.bytes) == 0, "ge_frombytes_vartime failed at "+boost::lexical_cast<std::string>(__LINE__));
        ge_scalarmult(&R, a.bytes, &A);
        key aP;
        ge_tobytes(aP.bytes, &R);
        return aP;
    }

    //Computes aH where H= toPoint(cn_fast_hash(G)), G the basepoint
    key scalarmultH(const key & a) {
        ge_p3 A;
        ge_p2 R;
        CHECK_AND_ASSERT_THROW_MES_L1(ge_frombytes_vartime(&A, H.bytes) == 0, "ge_frombytes_vartime failed at "+boost::lexical_cast<std::string>(__LINE__));
        ge_scalarmult(&R, a.bytes, &A);
        key aP;
        ge_tobytes(aP.bytes, &R);
        return aP;
    }

    //Curve addition / subtractions

    //for curve points: AB = A + B
    void addKeys(key &AB, const key &A, const key &B) {
        ge_p3 B2, A2;
        CHECK_AND_ASSERT_THROW_MES_L1(ge_frombytes_vartime(&B2, B.bytes) == 0, "ge_frombytes_vartime failed at "+boost::lexical_cast<std::string>(__LINE__));
        CHECK_AND_ASSERT_THROW_MES_L1(ge_frombytes_vartime(&A2, A.bytes) == 0, "ge_frombytes_vartime failed at "+boost::lexical_cast<std::string>(__LINE__));
        ge_cached tmp2;
        ge_p3_to_cached(&tmp2, &B2);
        ge_p1p1 tmp3;
        ge_add(&tmp3, &A2, &tmp2);
        ge_p1p1_to_p3(&A2, &tmp3);
        ge_p3_tobytes(AB.bytes, &A2);
    }

    rct::key addKeys(const key &A, const key &B) {
      key k;
      addKeys(k, A, B);
      return k;
    }

    //addKeys1
    //aGB = aG + B where a is a scalar, G is the basepoint, and B is a point
    void addKeys1(key &aGB, const key &a, const key & B) {
        key aG = scalarmultBase(a);
        addKeys(aGB, aG, B);
    }

    //addKeys2
    //aGbB = aG + bB where a, b are scalars, G is the basepoint and B is a point
    void addKeys2(key &aGbB, const key &a, const key &b, const key & B) {
        ge_p2 rv;
        ge_p3 B2;
        CHECK_AND_ASSERT_THROW_MES_L1(ge_frombytes_vartime(&B2, B.bytes) == 0, "ge_frombytes_vartime failed at "+boost::lexical_cast<std::string>(__LINE__));
        ge_double_scalarmult_base_vartime(&rv, b.bytes, &B2, a.bytes);
        ge_tobytes(aGbB.bytes, &rv);
    }

    //Does some precomputation to make addKeys3 more efficient
    // input B a curve point and output a ge_dsmp which has precomputation applied
    void precomp(ge_dsmp rv, const key & B) {
        ge_p3 B2;
        CHECK_AND_ASSERT_THROW_MES_L1(ge_frombytes_vartime(&B2, B.bytes) == 0, "ge_frombytes_vartime failed at "+boost::lexical_cast<std::string>(__LINE__));
        ge_dsm_precomp(rv, &B2);
    }

    //addKeys3
    //aAbB = a*A + b*B where a, b are scalars, A, B are curve points
    //B must be input after applying "precomp"
    void addKeys3(key &aAbB, const key &a, const key &A, const key &b, const ge_dsmp B) {
        ge_p2 rv;
        ge_p3 A2;
        CHECK_AND_ASSERT_THROW_MES_L1(ge_frombytes_vartime(&A2, A.bytes) == 0, "ge_frombytes_vartime failed at "+boost::lexical_cast<std::string>(__LINE__));
        ge_double_scalarmult_precomp_vartime(&rv, a.bytes, &A2, b.bytes, B);
        ge_tobytes(aAbB.bytes, &rv);
    }

    //addKeys3
    //aAbB = a*A + b*B where a, b are scalars, A, B are curve points
    //A and B must be input after applying "precomp"
    void addKeys3(key &aAbB, const key &a, const ge_dsmp A, const key &b, const ge_dsmp B) {
        ge_p2 rv;
        ge_double_scalarmult_precomp_vartime2(&rv, a.bytes, A, b.bytes, B);
        ge_tobytes(aAbB.bytes, &rv);
    }

    //subtract Keys (subtracts curve points)
    //AB = A - B where A, B are curve points
    void subKeys(key & AB, const key &A, const key &B) {
        ge_p3 B2, A2;
        CHECK_AND_ASSERT_THROW_MES_L1(ge_frombytes_vartime(&B2, B.bytes) == 0, "ge_frombytes_vartime failed at "+boost::lexical_cast<std::string>(__LINE__));
        CHECK_AND_ASSERT_THROW_MES_L1(ge_frombytes_vartime(&A2, A.bytes) == 0, "ge_frombytes_vartime failed at "+boost::lexical_cast<std::string>(__LINE__));
        ge_cached tmp2;
        ge_p3_to_cached(&tmp2, &B2);
        ge_p1p1 tmp3;
        ge_sub(&tmp3, &A2, &tmp2);
        ge_p1p1_to_p3(&A2, &tmp3);
        ge_p3_tobytes(AB.bytes, &A2);
    }

    //checks if A, B are equal in terms of bytes (may say no if one is a non-reduced scalar)
    //without doing curve operations
    bool equalKeys(const key & a, const key & b) {
        bool rv = true;
        for (int i = 0; i < 32; ++i) {
          if (a.bytes[i] != b.bytes[i]) {
            rv = false;
          }
        }
        return rv;
    }

    //Hashing - cn_fast_hash
    //be careful these are also in crypto namespace
    //cn_fast_hash for arbitrary multiples of 32 bytes
    void cn_fast_hash(key &hash, const void * data, const std::size_t l) {
        keccak((const uint8_t *)data, l, hash.bytes, 32);
    }

    void hash_to_scalar(key &hash, const void * data, const std::size_t l) {
        cn_fast_hash(hash, data, l);
        sc_reduce32(hash.bytes);
    }

    //cn_fast_hash for a 32 byte key
    void cn_fast_hash(key & hash, const key & in) {
        keccak((const uint8_t *)in.bytes, 32, hash.bytes, 32);
    }

    void hash_to_scalar(key & hash, const key & in) {
        cn_fast_hash(hash, in);
        sc_reduce32(hash.bytes);
    }

    //cn_fast_hash for a 32 byte key
    key cn_fast_hash(const key & in) {
        key hash;
        keccak((const uint8_t *)in.bytes, 32, hash.bytes, 32);
        return hash;
    }

     key hash_to_scalar(const key & in) {
        key hash = cn_fast_hash(in);
        sc_reduce32(hash.bytes);
        return hash;
     }

    //cn_fast_hash for a 128 byte unsigned char
    key cn_fast_hash128(const void * in) {
        key hash;
        keccak((const uint8_t *)in, 128, hash.bytes, 32);
        return hash;
    }

    key hash_to_scalar128(const void * in) {
        key hash = cn_fast_hash128(in);
        sc_reduce32(hash.bytes);
        return hash;
    }

    //cn_fast_hash for multisig purpose
    //This takes the outputs and commitments
    //and hashes them into a 32 byte sized key
    key cn_fast_hash(const ctkeyV &PC) {
        if (PC.empty()) return rct::hash2rct(crypto::cn_fast_hash("", 0));
        key rv;
        cn_fast_hash(rv, &PC[0], 64*PC.size());
        return rv;
    }

    key hash_to_scalar(const ctkeyV &PC) {
        key rv = cn_fast_hash(PC);
        sc_reduce32(rv.bytes);
        return rv;
    }

   //cn_fast_hash for a key-vector of arbitrary length
   //this is useful since you take a number of keys
   //put them in the key vector and it concatenates them
   //and then hashes them
   key cn_fast_hash(const keyV &keys) {
       if (keys.empty()) return rct::hash2rct(crypto::cn_fast_hash("", 0));
       key rv;
       cn_fast_hash(rv, &keys[0], keys.size() * sizeof(keys[0]));
       //dp(rv);
       return rv;
   }

   key hash_to_scalar(const keyV &keys) {
       key rv = cn_fast_hash(keys);
       sc_reduce32(rv.bytes);
       return rv;
   }

   key cn_fast_hash(const key64 keys) {
      key rv;
      cn_fast_hash(rv, &keys[0], 64 * sizeof(keys[0]));
      //dp(rv);
      return rv;
   }

   key hash_to_scalar(const key64 keys) {
       key rv = cn_fast_hash(keys);
       sc_reduce32(rv.bytes);
       return rv;
   }

    key hashToPointSimple(const key & hh) {
        key pointk;
        ge_p1p1 point2;
        ge_p2 point;
        ge_p3 res;
        key h = cn_fast_hash(hh);
        CHECK_AND_ASSERT_THROW_MES_L1(ge_frombytes_vartime(&res, h.bytes) == 0, "ge_frombytes_vartime failed at "+boost::lexical_cast<std::string>(__LINE__));
        ge_p3_to_p2(&point, &res);
        ge_mul8(&point2, &point);
        ge_p1p1_to_p3(&res, &point2);
        ge_p3_tobytes(pointk.bytes, &res);
        return pointk;
    }

    key hashToPoint(const key & hh) {
        key pointk;
        ge_p2 point;
        ge_p1p1 point2;
        ge_p3 res;
        key h = cn_fast_hash(hh);
        ge_fromfe_frombytes_vartime(&point, h.bytes);
        ge_mul8(&point2, &point);
        ge_p1p1_to_p3(&res, &point2);
        ge_p3_tobytes(pointk.bytes, &res);
        return pointk;
    }

    void hashToPoint(key & pointk, const key & hh) {
        ge_p2 point;
        ge_p1p1 point2;
        ge_p3 res;
        key h = cn_fast_hash(hh);
        ge_fromfe_frombytes_vartime(&point, h.bytes);
        ge_mul8(&point2, &point);
        ge_p1p1_to_p3(&res, &point2);
        ge_p3_tobytes(pointk.bytes, &res);
    }

    //sums a vector of curve points (for scalars use sc_add)
    void sumKeys(key & Csum, const keyV &  Cis) {
        identity(Csum);
        size_t i = 0;
        for (i = 0; i < Cis.size(); i++) {
            addKeys(Csum, Csum, Cis[i]);
        }
    }

    //Elliptic Curve Diffie Helman: encodes and decodes the amount b and mask a
    // where C= aG + bH
    void ecdhEncode(ecdhTuple & unmasked, const key & sharedSec) {
        key sharedSec1 = hash_to_scalar(sharedSec);
        key sharedSec2 = hash_to_scalar(sharedSec1);
        //encode
        sc_add(unmasked.mask.bytes, unmasked.mask.bytes, sharedSec1.bytes);
        sc_add(unmasked.amount.bytes, unmasked.amount.bytes, sharedSec2.bytes);
    }
    void ecdhDecode(ecdhTuple & masked, const key & sharedSec) {
        key sharedSec1 = hash_to_scalar(sharedSec);
        key sharedSec2 = hash_to_scalar(sharedSec1);
        //decode
        sc_sub(masked.mask.bytes, masked.mask.bytes, sharedSec1.bytes);
        sc_sub(masked.amount.bytes, masked.amount.bytes, sharedSec2.bytes);
    }
}


//==============================================================================
//         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_TRIGONOMETRIC_FUNCTIONS_SCALAR_IMPL_TRIGO_F_TRIG_REDUCTION_HPP_INCLUDED
#define NT2_TRIGONOMETRIC_FUNCTIONS_SCALAR_IMPL_TRIGO_F_TRIG_REDUCTION_HPP_INCLUDED

#include <nt2/sdk/meta/upgrade.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/meta/upgrade.hpp>
#include <nt2/include/functions/simd/rem_pio2_medium.hpp>
#include <nt2/include/functions/simd/rem_pio2_cephes.hpp>
#include <nt2/include/functions/simd/rem_pio2_straight.hpp>
#include <nt2/include/functions/simd/rem_pio2.hpp>
#include <nt2/include/functions/toint.hpp>
#include <nt2/include/functions/abs.hpp>
#include <nt2/include/functions/round.hpp>
#include <nt2/include/functions/is_odd.hpp>
#include <nt2/include/functions/if_else.hpp>
#include <nt2/include/functions/if_else_allbits.hpp>
#include <nt2/include/functions/is_not_less.hpp>
#include <nt2/include/functions/is_not_greater.hpp>
#include <nt2/include/functions/is_greater_equal.hpp>
#include <nt2/include/functions/is_less_equal.hpp>
#include <nt2/include/functions/is_greater_equal.hpp>
#include <nt2/include/functions/is_nez.hpp>
#include <nt2/include/functions/multiplies.hpp>
#include <nt2/include/functions/is_inf.hpp>
#include <nt2/include/functions/bitwise_andnot.hpp>
#include <nt2/include/functions/is_invalid.hpp>
#include <nt2/include/functions/is_flint.hpp>
#include <nt2/include/functions/rec.hpp>
#include <nt2/include/functions/all.hpp>
#include <nt2/include/constants/false.hpp>
#include <nt2/include/constants/true.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/pi.hpp>
#include <nt2/include/constants/pio_2.hpp>
#include <nt2/include/constants/pio_4.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/two.hpp>
#include <boost/mpl/not.hpp>
#include <boost/type_traits/is_same.hpp>

namespace nt2 { namespace details { namespace internal
{
  template< class A0
            , class unit_tag
            , class style
            , class mode
            , class base_A0 = typename meta::scalar_of<A0>::type
  >
  struct trig_reduction;

  // This class exposes the public static member:
  // reduce:                to provide range reduction
  //
  // unit_tag allows to choose statically the scaling  among radian_tag, pi_tag, degree_tag
  // meaning that the cosa function will (for example) define respectively
  // x-->cos(x)          (radian_tag),
  // x-->cos(p*x)        (pi_tag)
  // x-->cos((pi/180)*x) (degree_tag)
  //
  // precision_tag allows to choose policies among accuracy and speed
  // are defined:
  //   trig_tag
  //   fast_tag
  // fast_tag doe not mean that functions are returning stupid values
  //    but that the range is very restricted.
  // accu_tag does not mean that functions are ever slow,  but that they are
  //    slower and slower with increased range, but they are speedier than
  //    standard ones except for really big_ parameters values, because they return
  //    quite accurate values even in these cases
  //
  // for each trigonometric function, xxx
  //   xxx_
  //   fast_xxx_
  // NT2 functors are provided.

  // trigonometric reduction strategies to the [-pi/4, pi/4] range.
  // these reductions are used in the normal and fast
  // trigonometric functions with different policies

  template<class A0, class mode>
  struct trig_reduction < A0, radian_tag,  tag::not_simd_type, mode, float>
  {
    typedef typename meta::as_integer<A0, signed>::type int_type;

    static inline bool isalreadyreduced(const A0&a0) { return nt2::le(a0, nt2::Pio_4<A0>()); }
    static inline bool ismedium (const A0&a0)        { return nt2::le(a0,single_constant<A0,0x43490fdb>()); }
    static inline bool issmall  (const A0&a0)        { return nt2::le(a0,single_constant<A0,0x427b53d1>()); }
    static inline bool islessthanpi_2  (const A0&a0) { return nt2::le(a0,nt2::Pio_2<A0>()); }
    static inline bool conversion_allowed(){
      typedef typename meta::upgrade<A0>::type uA0;
      return boost::mpl::not_<boost::is_same<A0,uA0> >::value;
    }

    static inline bool cot_invalid(const A0& ) { return false; }
    static inline bool tan_invalid(const A0& ) { return false; }

    static inline int_type reduce(const A0& x, A0& xr){ return inner_reduce(x, xr, mode()); }
  private:
    static inline int_type inner_reduce(const A0& x, A0& xr, const big_&)
    {
      // x is always positive here
      if (isalreadyreduced(x)) // all of x are in [0, pi/4], no reduction
      {
        xr = x;
        return Zero<int_type>();
      }
      else if (islessthanpi_2(x)) // all of x are in [0, pi/2],  straight algorithm is sufficient for 1 ulp
        return nt2::rem_pio2_straight(x, xr);
      else if (issmall(x)) // all of x are in [0, 20*pi],  cephes algorithm is sufficient for 1 ulp
        return nt2::rem_pio2_cephes(x, xr);
      else if (ismedium(x)) // all of x are in [0, 2^7*pi/2],  fdlibm medium_ way
        return nt2::rem_pio2_medium(x, xr);
      else if (conversion_allowed())  // all of x are in [0, 2^18*pi],  conversion to double is used to reduce if available
      {
        typedef typename meta::upgrade<A0>::type uA0;
        typedef trig_reduction< uA0, radian_tag,  tag::not_simd_type, mode, double> aux_reduction;
        uA0 ux = x, uxr;
        int_type n = static_cast<int_type>(aux_reduction::reduce(ux, uxr));
        xr = static_cast<A0>(uxr);
        return n;
      }
      else  // all of x are in [0, inf],  standard big_ way // too long
      {
        return nt2::rem_pio2(x, xr);
      }

    }

    static inline int_type inner_reduce(const A0& x, A0& xr, const medium_&)
    {
      // x is always positive here
      if (isalreadyreduced(x)) // all of x are in [0, pi/4], no reduction
      {
        xr = x;
        return nt2::Zero<int_type>();
      }
      else if (islessthanpi_2(x)) // all of x are in [0, pi/2],  straight algorithm is sufficient for 1 ulp
        return nt2::rem_pio2_straight(x, xr);
      else if (issmall(x)) // all of x are in [0, 20*pi],  cephes algorithm is sufficient for 1 ulp
        return nt2::rem_pio2_cephes(x, xr);
      else  // correct only if all of x are in [0, 2^7*pi/2],  fdlibm medium_ way
        return nt2::rem_pio2_medium(x, xr);
    }

    static inline int_type inner_reduce(const A0& x, A0& xr, const small_&)
    {
      // x is always positive here
      if (isalreadyreduced(x)) // all of x are in [0, pi/4], no reduction
      {
        xr = x;
        return Zero<int_type>();
      }
      else if (islessthanpi_2(x)) // all of x are in [0, pi/2],  straight algorithm is sufficient for 1 ulp
        return nt2::rem_pio2_straight(x, xr);
      else  // correct only if all of x are in [0, 20*pi],  cephes algorithm is sufficient for 1 ulp
        return nt2::rem_pio2_cephes(x, xr);
    }

    static inline int_type inner_reduce(const A0& x, A0& xr, const direct_small_&)
    {
      return nt2::rem_pio2_cephes(x, xr);
    }

    static inline int_type inner_reduce(const A0& x, A0& xr, const direct_medium_&)
    {
      return nt2::rem_pio2_medium(x, xr);
    }

    static inline int_type inner_reduce(const A0& x, A0& xr, const direct_big_&)
    {
      if (conversion_allowed()) // conversion to double is used to reduce time, if available
      {
        typedef typename meta::upgrade<A0>::type uA0;
        typedef trig_reduction< uA0, radian_tag,  tag::not_simd_type, mode, double> aux_reduction;
        uA0 ux = x, uxr;
        int_type n = aux_reduction::reduce(ux, uxr);
        xr = uxr;
        return n;
      }
      else
      {
        return nt2::rem_pio2(x, xr);
      }
    }

    static inline int_type inner_reduce(const A0& x, A0& xr, const clipped_pio4_&)
    {
      xr = nt2::if_else(isalreadyreduced(x), x, Nan<A0>());
      return nt2::Zero<int_type>();
    }

    static inline int_type inner_reduce(const A0& x, A0& xr, const clipped_small_&)
    {
      xr = nt2::if_else(issmall(x), x, nt2::Nan<A0>());
      return inner_reduce(xr, xr, small_());
    }
    static inline int_type inner_reduce(const A0& x, A0& xr, const clipped_medium_&)
    {
      xr = nt2::if_else(ismedium(x), x, Nan<A0>());
      return inner_reduce(xr, xr, medium_());
    }
  };

  template<class A0>
  struct trig_reduction<A0,degree_tag, tag::not_simd_type,big_, float>
  {
    typedef typename meta::as_integer<A0, signed>::type int_type;

    static inline bool cot_invalid(const A0& x) { return nt2::is_nez(x)&&is_flint(x/_180<A0>()); }
    static inline bool tan_invalid(const A0& x) { return nt2::is_flint((x-nt2::_90<A0>())/nt2::_180<A0>()); }

    static inline int_type reduce(const A0& x, A0& xr)
    {
      A0 xi = nt2::round(x*single_constant<A0,0x3c360b61>()); //  1.111111111111111e-02f
      A0 x2 = x - xi * nt2::_90<A0>();

      xr =  x2*single_constant<A0,0x3c8efa35>(); //0.0174532925199432957692f
      return nt2::toint(xi);
    }

    static inline int_type inner_reduce(const A0& x, A0& xr, const clipped_pio4_&)
    {
      xr = nt2::if_else_nan(isalreadyreduced(nt2::abs(x)), x);
      return nt2::Zero<int_type>();
    }
    static inline int_type inner_reduce(const A0& x, A0& xr, const clipped_small_&)
    {
      x = nt2::if_else_nan(issmall(nt2::abs(x)), x);
      return inner_reduce(x, xr, small_());
    }
    static inline int_type inner_reduce(const A0& x, A0& xr, const clipped_medium_&)
    {
      x = nt2::if_else_nan(ismedium(nt2::abs(x)), x);
      return inner_reduce(x, xr, medium_());
    }
  };

  template < class A0>
  struct trig_reduction < A0, pi_tag,  tag::not_simd_type, big_, float>
  {
    typedef typename meta::as_integer<A0, signed>::type int_type;

    static inline bool cot_invalid(const A0& x) { return nt2::is_nez(x)&&nt2::is_flint(x); }
    static inline bool tan_invalid(const A0& x) { return nt2::is_flint(x-nt2::Half<A0>()) ; }

    static inline int_type reduce(const A0& x,  A0& xr)
    {
      A0 xi = nt2::round(x*nt2::Two<A0>());
      A0 x2 = x - xi * nt2::Half<A0>();
      xr = x2*nt2::Pi<A0>();
      return nt2::toint(xi);
    }
  };
} } }

#endif


/**
 * Copyright (c) 2016  Zubax Robotics OU  <info@zubax.com>
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
 * following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following
 *    disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
 *    following disclaimer in the documentation and/or other materials provided with the distribution.
 * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote
 *    products derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
 * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#pragma once

#include <Eigen/Eigen>
#include <algorithm>
#include <utility>
#include <cassert>
#include <type_traits>
#include <cmath>
#include <cstdint>
#include <zubax_chibios/util/heapless.hpp>

namespace math
{

using Scalar = float;
using Const = const Scalar;

template <int Rows, int Cols>
using Matrix = Eigen::Matrix<Scalar, Rows, Cols>;

template <int Rows>
using DiagonalMatrix = Eigen::DiagonalMatrix<Scalar, Rows>;

template <int Size>
using Vector = Matrix<Size, 1>;

constexpr inline Scalar convertKelvinToCelsius(Const kelvin)
{
    return kelvin - 273.15F;
}

constexpr inline Scalar convertCelsiusToKelvin(Const kelvin)
{
    return kelvin + 273.15F;
}

/**
 * Constants
 */
constexpr auto Pi  = Scalar(3.141592653589793);
constexpr auto Pi2 = Scalar(6.283185307179586);

/**
 * Constrains the angle within [0, Pi*2]
 */
inline Scalar normalizeAngle(Scalar x)
{
    if (x >= Pi2)
    {
        return x - Pi2;
    }
    else if (x < 0)
    {
        return x + Pi2;
    }
    else
    {
        return x;
    }
}

/**
 * Inclusive range of the form [min, max].
 */
template <typename T = Scalar>
struct Range
{
    T min = T(0);
    T max = T(0);

    constexpr Range() { }

    constexpr Range(const T min, const T max) :
        min(min),
        max(max)
    {
        assert(min < max);
    }

    constexpr T constrain(const T value) const
    {
        return std::min(std::max(value, min), max);
    }

    constexpr bool contains(const T value) const
    {
        return (value >= min) && (value <= max);
    }

    auto toString() const
    {
        return os::heapless::concatenate("[", min, ", ", max, "]");
    }
};

/**
 * Simple moving average of arbitrary depth.
 */
template <unsigned Depth, typename T>
class SimpleMovingAverageFilter
{
    T history_[Depth] = {};
    T sum_;
    unsigned next_index_ = 0;

public:
    SimpleMovingAverageFilter() :
        sum_(T() * Depth)
    {
        std::fill_n(std::begin(history_), Depth, T());
    }

    explicit SimpleMovingAverageFilter(const T& initial_value) :
        sum_(initial_value * Depth)
    {
        std::fill_n(std::begin(history_), Depth, initial_value);
    }

    void update(const T& value)
    {
        sum_ -= history_[next_index_];
        history_[next_index_] = value;
        sum_ += history_[next_index_];

        next_index_++;
        if (next_index_ >= Depth)
        {
            next_index_ = 0;
        }
    }

    void reset(const T& initial_value)
    {
        sum_ = initial_value * Depth;
        std::fill_n(std::begin(history_), Depth, initial_value);
    }

    T getValue() const
    {
        return sum_ / Depth;
    }
};

/**
 * Copmutes average over the entire collected dataset.
 * Useful for calibration and other batch measurements, e.g. motor identification.
 */
template <typename T = double>
class CumulativeAverageComputer
{
    std::uint32_t num_samples_ = 0;
    T accumulator_{};

    using FloatingPointScalar = typename std::conditional<std::is_floating_point<T>::value, T, Scalar>::type;

public:
    CumulativeAverageComputer() :
        accumulator_()
    { }

    explicit CumulativeAverageComputer(const T& init) :
        accumulator_(init)
    { }

    void addSample(const T& x)
    {
        num_samples_++;
        accumulator_ += x;
    }

    T getAverage() const
    {
        if (num_samples_ > 0)
        {
            return T(accumulator_ / FloatingPointScalar(num_samples_));
        }
        else
        {
            assert(false);
            return T();
        }
    }

    auto getNumSamples() const { return num_samples_; }
};

inline Vector<2> sincos(Scalar x)
{
    // Normally this should be replaced with a call to sincos(), but this is not a part of C++ standard library.
    // However, the compiler should be able to replace the two separate but localized calls to
    // sin()/cos() with one sincos().
    return { std::sin(x), std::cos(x) };
}

/**
 * Implementation details, do not use directly.
 */
namespace impl_
{

template <typename Scalar, int Size>
using RowVector = Eigen::Matrix<Scalar, 1, Size, Eigen::RowMajor>;

template <typename Scalar, int Size, typename Head>
inline void fillVector(RowVector<Scalar, Size>& vector,
                       int next_index,
                       Head head)
{
    vector[next_index] = static_cast<Scalar>(head);
}

template <typename Scalar, int Size, typename Head, typename... Tail>
inline void fillVector(RowVector<Scalar, Size>& vector,
                       int next_index,
                       Head head,
                       Tail... tail)
{
    vector[next_index] = static_cast<Scalar>(head);
    fillVector(vector, next_index + 1, tail...);
}

template <typename Scalar, int Rows, int Columns>
inline void fillMatrix(Eigen::Matrix<Scalar, Rows, Columns>& matrix,
                       int next_row,
                       const impl_::RowVector<Scalar, Columns>& head)
{
    matrix.row(next_row) = head;
}

template <typename Scalar, int Rows, int Columns, typename... Tail>
inline void fillMatrix(Eigen::Matrix<Scalar, Rows, Columns>& matrix,
                       int next_row,
                       const impl_::RowVector<Scalar, Columns>& head,
                       Tail... tail)
{
    matrix.row(next_row) = head;
    fillMatrix(matrix, next_row + 1, tail...);
}

template <typename Scalar, int DiagonalSize>
inline void fillDiagonalMatrix(Eigen::DiagonalMatrix<Scalar, DiagonalSize>& matrix,
                               int next_position,
                               Scalar head)
{
    matrix.diagonal()[next_position] = static_cast<Scalar>(head);
}

template <typename Scalar, int DiagonalSize, typename... Tail>
inline void fillDiagonalMatrix(Eigen::DiagonalMatrix<Scalar, DiagonalSize>& matrix,
                               int next_position,
                               Scalar head,
                               Tail... tail)
{
    matrix.diagonal()[next_position] = static_cast<Scalar>(head);
    fillDiagonalMatrix<Scalar, DiagonalSize>(matrix, next_position + 1, tail...);
}

} // namespace impl_

/**
 * Creates a row vector of arbitrary length, represented as a static row vector Eigen::Matrix<>.
 * Scalar type can be overriden.
 * See also @ref makeMatrix().
 */
template <typename Scalar = Scalar, typename... Tail>
inline impl_::RowVector<Scalar, sizeof...(Tail)>
makeRow(Tail... tail)
{
    impl_::RowVector<Scalar, sizeof...(Tail)> vector;
    impl_::fillVector(vector, 0, tail...);
    return vector;
}

/**
 * Creates a static matrix (Eigen::Matrix<>) of arbitrary size from a set of row vectors.
 * The matrix will use default layout, which is columnn-major for Eigen.
 * Note that row vectors of unequal size trigger a compile-time error.
 * Scalar type will be deduced automatically from the first row.
 * This function is totally type safe.
 * Usage:
 *      makeMatrix(makeRow(1, 2),
 *                 makeRow(3, 4))  // Produces a 2x2 matrix
 */
template <typename Scalar, int Columns, typename... Tail>
inline Eigen::Matrix<Scalar, sizeof...(Tail) + 1, Columns>
makeMatrix(const impl_::RowVector<Scalar, Columns>& head,
           Tail... tail)
{
    Eigen::Matrix<Scalar, sizeof...(Tail) + 1, Columns> matrix;
    matrix.setZero();
    impl_::fillMatrix(matrix, 0, head, tail...);
    return matrix;
}

/**
 * A helper like @ref makeMatrix() that creates diagonal matrix.
 * Size is dereved from the argument list.
 * Scalar type can be overriden.
 */
template <typename Scalar = Scalar, typename... Diagonal>
inline Eigen::DiagonalMatrix<Scalar, sizeof...(Diagonal)>
makeDiagonalMatrix(Diagonal... diag)
{
    Eigen::DiagonalMatrix<Scalar, sizeof...(Diagonal)> matrix;
    matrix.setZero();
    impl_::fillDiagonalMatrix<Scalar, sizeof...(Diagonal)>(matrix, 0, diag...);
    return matrix;
}

/**
 * Printing helpers
 * @{
 */
enum class StringRepresentation
{
    SingleLine,
    MultiLine
};

template <typename Scalar, int Rows, int Columns>
inline auto toString(const Eigen::Matrix<Scalar, Rows, Columns>& matrix,
                     const StringRepresentation representation = StringRepresentation::SingleLine)
{
    os::heapless::String<Rows * Columns * 20> s;

    for (int row = 0; row < Rows; row++)
    {
        if (row > 0)
        {
            s.append((representation == StringRepresentation::MultiLine) ? "\n" : "; ");
        }
        for (int column = 0; column < Columns; column++)
        {
            if (column > 0)
            {
                s.append(", ");
            }
            s.append(matrix(row, column));
        }
    }

    return s;
}
/**
 * @}
 */

}


/**
 * \file
 * \author Thomas Fischer
 * \date   2010-06-21
 * \brief  Implementation of the Polygon class.
 *
 * \copyright
 * Copyright (c) 2012-2019, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 */

#include "Polygon.h"

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

#include "BaseLib/quicksort.h"

#include "AnalyticalGeometry.h"

namespace GeoLib
{
Polygon::Polygon(const Polyline &ply, bool init) :
    Polyline(ply), _aabb(ply.getPointsVec(), ply._ply_pnt_ids)
{
    if (init)
    {
        initialise();
    }
    _simple_polygon_list.push_back(this);
}

Polygon::Polygon(Polygon const& other)
    : Polyline(other), _aabb(other._aabb)
{
    _simple_polygon_list.push_back(this);
    auto sub_polygon_it(other._simple_polygon_list.begin());
    for (sub_polygon_it++;  // the first entry is the polygon itself, skip the
                            // entry
         sub_polygon_it != other._simple_polygon_list.end();
         ++sub_polygon_it)
    {
        _simple_polygon_list.emplace_back(new Polygon(*(*sub_polygon_it)));
    }
}

Polygon::~Polygon()
{
    // remove polygons from list
    for (auto& polygon : _simple_polygon_list)
    {
        // the first entry of the list can be a pointer the object itself
        if (polygon != this)
        {
            delete polygon;
        }
    }
}

bool Polygon::initialise ()
{
    if (this->isClosed()) {
        ensureCCWOrientation();
        return true;
    }
    WARN("Polygon::initialise(): base polyline is not closed.");
    return false;
}

bool Polygon::isPntInPolygon(GeoLib::Point const& pnt) const
{
    MathLib::Point3d const& min_aabb_pnt(_aabb.getMinPoint());
    MathLib::Point3d const& max_aabb_pnt(_aabb.getMaxPoint());

    if (pnt[0] < min_aabb_pnt[0] || max_aabb_pnt[0] < pnt[0] ||
        pnt[1] < min_aabb_pnt[1] || max_aabb_pnt[1] < pnt[1])
    {
        return false;
    }

    if (_simple_polygon_list.size() == 1)
    {
        std::size_t n_intersections(0);
        const std::size_t n_nodes(getNumberOfPoints() - 1);
        for (std::size_t k(0); k < n_nodes; k++)
        {
            if (((*(getPoint(k)))[1] <= pnt[1] &&
                 pnt[1] <= (*(getPoint(k + 1)))[1]) ||
                ((*(getPoint(k + 1)))[1] <= pnt[1] &&
                 pnt[1] <= (*(getPoint(k)))[1]))
            {
                switch (getEdgeType(k, pnt))
                {
                    case EdgeType::TOUCHING:
                        return true;
                    case EdgeType::CROSSING:
                        n_intersections++;
                        break;
                    case EdgeType::INESSENTIAL:
                        break;
                    default:
                        // do nothing
                        ;
                }
            }
        }
        if (n_intersections % 2 == 1)
        {
            return true;
        }
    }
    else
    {
        for (auto it(_simple_polygon_list.begin()++);
             it != _simple_polygon_list.end();
             ++it)
        {
            if ((*it)->isPntInPolygon(pnt))
            {
                return true;
            }
        }
    }
    return false;
}

bool Polygon::isPntInPolygon(double x, double y, double z) const
{
    const GeoLib::Point pnt(x,y,z);
    return isPntInPolygon(pnt);
}

std::vector<GeoLib::Point> Polygon::getAllIntersectionPoints(
        GeoLib::LineSegment const& segment) const
{
    std::vector<GeoLib::Point> intersections;
    GeoLib::Point s;
    for (auto&& seg_it : *this)
    {
        if (GeoLib::lineSegmentIntersect(seg_it, segment, s)) {
            intersections.push_back(s);
        }
    }

    return intersections;
}

bool Polygon::containsSegment(GeoLib::LineSegment const& segment) const
{
    std::vector<GeoLib::Point> s(getAllIntersectionPoints(segment));

    GeoLib::Point const& a{segment.getBeginPoint()};
    GeoLib::Point const& b{segment.getEndPoint()};
    // no intersections -> check if at least one point of segment is in polygon
    if (s.empty()) {
        return (isPntInPolygon(a));
    }

    const double tol(std::numeric_limits<float>::epsilon());

    // one intersection, intersection in line segment end point
    if (s.size() == 1) {
        const double sqr_dist_as(MathLib::sqrDist(a,s[0]));
        if (sqr_dist_as < tol) {
            return (isPntInPolygon(b));
        }

        const double sqr_dist_bs(MathLib::sqrDist(b,s[0]));
        if (sqr_dist_bs < tol) {
            return (isPntInPolygon(a));
        }
    }

    // Sorting the intersection with respect to the distance to the point a.
    // This induces a partition of the line segment into sub segments.
    std::sort(s.begin(), s.end(),
        [&a] (GeoLib::Point const& p0, GeoLib::Point const& p1) {
            return MathLib::sqrDist(a, p0) < MathLib::sqrDist(a, p1);
        }
    );

    // remove sub segments with almost zero length
    for (std::size_t k(0); k<s.size()-1; ) {
        if (MathLib::sqrDist(s[k], s[k+1]) < tol) {
            s.erase(s.begin()+k+1);
        } else {
            k++;
        }
    }

    // Check if all sub segments are within the polygon.
    if (!isPntInPolygon(GeoLib::Point(0.5 * (a[0] + s[0][0]),
                                      0.5 * (a[1] + s[0][1]),
                                      0.5 * (a[2] + s[0][2]))))
    {
        return false;
    }
    const std::size_t n_sub_segs(s.size()-1);
    for (std::size_t k(0); k<n_sub_segs; k++) {
        if (!isPntInPolygon(GeoLib::Point(0.5 * (s[k][0] + s[k + 1][0]),
                                          0.5 * (s[k][1] + s[k + 1][1]),
                                          0.5 * (s[k][2] + s[k + 1][2]))))
        {
            return false;
        }
    }
    return isPntInPolygon(GeoLib::Point(0.5 * (s[0][0] + b[0]),
                                        0.5 * (s[0][1] + b[1]),
                                        0.5 * (s[0][2] + b[2])));
}

bool Polygon::isPolylineInPolygon(const Polyline& ply) const
{
    for (auto segment : ply) {
        if (!containsSegment(segment)) {
            return false;
        }
    }
    return true;
}

bool Polygon::isPartOfPolylineInPolygon(const Polyline& ply) const
{
    const std::size_t ply_size (ply.getNumberOfPoints());
    // check points
    for (std::size_t k(0); k < ply_size; k++) {
        if (isPntInPolygon (*(ply.getPoint(k)))) {
            return true;
        }
    }

    GeoLib::Point s;
    for (auto polygon_seg : *this) {
        for (auto polyline_seg : ply) {
            if (GeoLib::lineSegmentIntersect(polyline_seg, polygon_seg, s)) {
                return true;
            }
        }
    }

    return false;
}

bool Polygon::getNextIntersectionPointPolygonLine(
    GeoLib::LineSegment const& seg, GeoLib::Point & intersection,
    std::size_t& seg_num) const
{
    if (_simple_polygon_list.size() == 1) {
        for (auto seg_it(begin()+seg_num); seg_it != end(); ++seg_it) {
            if (GeoLib::lineSegmentIntersect(*seg_it, seg, intersection)) {
                seg_num = seg_it.getSegmentNumber();
                return true;
            }
        }
    } else {
        for (auto polygon : _simple_polygon_list)
        {
            for (auto seg_it(polygon->begin()); seg_it != polygon->end();
                 ++seg_it)
            {
                if (GeoLib::lineSegmentIntersect(*seg_it, seg, intersection)) {
                    seg_num = seg_it.getSegmentNumber();
                    return true;
                }
            }
        }
    }
    return false;
}

EdgeType Polygon::getEdgeType(std::size_t k, GeoLib::Point const& pnt) const
{
    switch (getLocationOfPoint(k, pnt))
    {
    case Location::LEFT: {
        const GeoLib::Point & v (*(getPoint(k)));
        const GeoLib::Point & w (*(getPoint(k + 1)));
        if (v[1] < pnt[1] && pnt[1] <= w[1])
        {
            return EdgeType::CROSSING;
        }

        return EdgeType::INESSENTIAL;
    }
    case Location::RIGHT: {
        const GeoLib::Point & v (*(getPoint(k)));
        const GeoLib::Point & w (*(getPoint(k + 1)));
        if (w[1] < pnt[1] && pnt[1] <= v[1])
        {
            return EdgeType::CROSSING;
        }

        return EdgeType::INESSENTIAL;
    }
    case Location::BETWEEN:
    case Location::SOURCE:
    case Location::DESTINATION:
        return EdgeType::TOUCHING;
    default:
        return EdgeType::INESSENTIAL;
    }
}

void Polygon::ensureCCWOrientation ()
{
    // *** pre processing: rotate points to xy-plan
    // *** copy points to vector - last point is identical to the first
    std::size_t n_pnts (this->getNumberOfPoints() - 1);
    std::vector<GeoLib::Point*> tmp_polygon_pnts;
    for (std::size_t k(0); k < n_pnts; k++)
    {
        tmp_polygon_pnts.push_back(new GeoLib::Point(*(this->getPoint(k))));
    }

    // rotate copied points into x-y-plane
    GeoLib::rotatePointsToXY(tmp_polygon_pnts);

    for (auto& tmp_polygon_pnt : tmp_polygon_pnts)
    {
        (*tmp_polygon_pnt)[2] =
            0.0;  // should be -= d but there are numerical errors
    }

    // *** get the left most upper point
    std::size_t min_x_max_y_idx (0); // for orientation check
    for (std::size_t k(0); k < n_pnts; k++)
    {
        if ((*(tmp_polygon_pnts[k]))[0] <= (*(tmp_polygon_pnts[min_x_max_y_idx]))[0])
        {
            if ((*(tmp_polygon_pnts[k]))[0] <
                (*(tmp_polygon_pnts[min_x_max_y_idx]))[0])
            {
                min_x_max_y_idx = k;
            }
            else if ((*(tmp_polygon_pnts[k]))[1] >
                     (*(tmp_polygon_pnts[min_x_max_y_idx]))[1])
            {
                min_x_max_y_idx = k;
            }
        }
    }
    // *** determine orientation
    GeoLib::Orientation orient;
    if (0 < min_x_max_y_idx && min_x_max_y_idx < n_pnts - 2)
    {
        orient = GeoLib::getOrientation(*tmp_polygon_pnts[min_x_max_y_idx - 1],
                                        *tmp_polygon_pnts[min_x_max_y_idx],
                                        *tmp_polygon_pnts[min_x_max_y_idx + 1]);
    }
    else
    {
        if (0 == min_x_max_y_idx)
        {
            orient = GeoLib::getOrientation(*tmp_polygon_pnts[n_pnts - 1],
                                            *tmp_polygon_pnts[0],
                                            *tmp_polygon_pnts[1]);
        }
        else
        {
            orient = GeoLib::getOrientation(*tmp_polygon_pnts[n_pnts - 2],
                                            *tmp_polygon_pnts[n_pnts - 1],
                                            *tmp_polygon_pnts[0]);
        }
    }

    if (orient != GeoLib::CCW)
    {
        // switch orientation
        std::size_t tmp_n_pnts (n_pnts);
        tmp_n_pnts++; // include last point of polygon (which is identical to the first)
        for (std::size_t k(0); k < tmp_n_pnts / 2; k++)
        {
            std::swap(_ply_pnt_ids[k], _ply_pnt_ids[tmp_n_pnts - 1 - k]);
        }
    }

    for (std::size_t k(0); k < n_pnts; k++)
    {
        delete tmp_polygon_pnts[k];
    }
}

#if __GNUC__ <= 4 && (__GNUC_MINOR__ < 9)
void Polygon::splitPolygonAtIntersection(
    const std::list<Polygon*>::iterator& polygon_it)
#else
void Polygon::splitPolygonAtIntersection(
    const std::list<Polygon*>::const_iterator& polygon_it)
#endif
{
    GeoLib::Polyline::SegmentIterator seg_it0((*polygon_it)->begin());
    GeoLib::Polyline::SegmentIterator seg_it1((*polygon_it)->begin());
    GeoLib::Point intersection_pnt;
    if (!GeoLib::lineSegmentsIntersect(*polygon_it, seg_it0, seg_it1,
                                       intersection_pnt))
    {
        return;
    }

    std::size_t idx0(seg_it0.getSegmentNumber());
    std::size_t idx1(seg_it1.getSegmentNumber());
    // adding intersection point to pnt_vec
    std::size_t const intersection_pnt_id (_ply_pnts.size());
    const_cast<std::vector<Point*>&>(_ply_pnts)
        .push_back(new GeoLib::Point(intersection_pnt));

    // split Polygon
    if (idx0 > idx1)
    {
        std::swap(idx0, idx1);
    }

    GeoLib::Polyline polyline0{(*polygon_it)->getPointsVec()};
    for (std::size_t k(0); k <= idx0; k++)
    {
        polyline0.addPoint((*polygon_it)->getPointID(k));
    }
    polyline0.addPoint(intersection_pnt_id);
    for (std::size_t k(idx1 + 1); k < (*polygon_it)->getNumberOfPoints(); k++)
    {
        polyline0.addPoint((*polygon_it)->getPointID(k));
    }

    GeoLib::Polyline polyline1{(*polygon_it)->getPointsVec()};
    polyline1.addPoint(intersection_pnt_id);
    for (std::size_t k(idx0 + 1); k <= idx1; k++)
    {
        polyline1.addPoint((*polygon_it)->getPointID(k));
    }
    polyline1.addPoint(intersection_pnt_id);

    // remove the polygon except the first
    if (*polygon_it != this)
    {
        delete *polygon_it;
    }
    // erase polygon_it and add two new polylines
    auto polygon1_it = _simple_polygon_list.insert(
        _simple_polygon_list.erase(polygon_it), new GeoLib::Polygon(polyline1));
    auto polygon0_it = _simple_polygon_list.insert(
        polygon1_it, new GeoLib::Polygon(polyline0));

    splitPolygonAtIntersection(polygon0_it);
    splitPolygonAtIntersection(polygon1_it);
}

void Polygon::splitPolygonAtPoint (const std::list<GeoLib::Polygon*>::iterator& polygon_it)
{
    std::size_t const n((*polygon_it)->getNumberOfPoints() - 1);
    std::vector<std::size_t> id_vec(n);
    std::vector<std::size_t> perm(n);
    for (std::size_t k(0); k < n; k++)
    {
        id_vec[k] = (*polygon_it)->getPointID (k);
        perm[k] = k;
    }

    BaseLib::quicksort (id_vec, 0, n, perm);

    for (std::size_t k(0); k < n - 1; k++)
    {
        if (id_vec[k] == id_vec[k + 1])
        {
            std::size_t idx0 = perm[k];
            std::size_t idx1 = perm[k + 1];

            if (idx0 > idx1)
            {
                std::swap(idx0, idx1);
            }

            // create two closed polylines
            GeoLib::Polyline polyline0{*(*polygon_it)};
            for (std::size_t j(0); j <= idx0; j++)
            {
                polyline0.addPoint((*polygon_it)->getPointID(j));
            }
            for (std::size_t j(idx1 + 1);
                 j < (*polygon_it)->getNumberOfPoints();
                 j++)
            {
                polyline0.addPoint((*polygon_it)->getPointID(j));
            }

            GeoLib::Polyline polyline1{*(*polygon_it)};
            for (std::size_t j(idx0); j <= idx1; j++)
            {
                polyline1.addPoint((*polygon_it)->getPointID(j));
            }

            // remove the polygon except the first
            if (*polygon_it != this)
            {
                delete *polygon_it;
            }
            // erase polygon_it and add two new polygons
            auto polygon1_it = _simple_polygon_list.insert(
                _simple_polygon_list.erase(polygon_it), new Polygon(polyline1));
            auto polygon0_it = _simple_polygon_list.insert(
                polygon1_it, new Polygon(polyline0));

            splitPolygonAtPoint(polygon0_it);
            splitPolygonAtPoint(polygon1_it);

            return;
        }
    }
}

bool operator==(Polygon const& lhs, Polygon const& rhs)
{
    if (lhs.getNumberOfPoints() != rhs.getNumberOfPoints())
    {
        return false;
    }

    const std::size_t n(lhs.getNumberOfPoints());
    const std::size_t start_pnt(lhs.getPointID(0));

    // search start point of first polygon in second polygon
    bool nfound(true);
    std::size_t k(0);
    for (; k < n-1 && nfound; k++) {
        if (start_pnt == rhs.getPointID(k)) {
            nfound = false;
            break;
        }
    }

    // case: start point not found in second polygon
    if (nfound)
    {
        return false;
    }

    // *** determine direction
    // opposite direction
    if (k == n-2) {
        for (k=1; k<n-1; k++) {
            if (lhs.getPointID(k) != rhs.getPointID(n-1-k)) {
                return false;
            }
        }
        return true;
    }

    // same direction - start point of first polygon at arbitrary position in second polygon
    if (lhs.getPointID(1) == rhs.getPointID(k+1)) {
        std::size_t j(k+2);
        for (; j<n-1; j++) {
            if (lhs.getPointID(j-k) != rhs.getPointID(j)) {
                return false;
            }
        }
        j=0; // new start point at second polygon
        for (; j<k+1; j++) {
            if (lhs.getPointID(n-(k+2)+j+1) != rhs.getPointID(j)) {
                return false;
            }
        }
        return true;
    }
    // opposite direction with start point of first polygon at arbitrary
    // position
    // *** ATTENTION
    WARN(
        "operator==(Polygon const& lhs, Polygon const& rhs) - not tested case "
        "(implementation is probably buggy) - please contact "
        "thomas.fischer@ufz.de mentioning the problem.");
    // in second polygon
    if (lhs.getPointID(1) == rhs.getPointID(k - 1))
    {
        std::size_t j(k - 2);
        for (; j > 0; j--)
        {
            if (lhs.getPointID(k - 2 - j) != rhs.getPointID(j))
            {
                return false;
            }
        }
        // new start point at second polygon - the point n-1 of a polygon is
        // equal to the first point of the polygon (for this reason: n-2)
        j = n - 2;
        for (; j > k - 1; j--)
        {
            if (lhs.getPointID(n - 2 + j + k - 2) != rhs.getPointID(j))
            {
                return false;
            }
        }
        return true;
    }
    return false;
}

} // end namespace GeoLib


#define NDEBUG
#include "pca.hh"
#include "pcaoptions.hh"
#include <cctype>
#include <cfloat>
#include <fstream>
#include <iostream>
#include <Eigen/Eigenvalues>

#define FUDGE 10

namespace
{
  using namespace hashpca;
  using veedubparse::HashAll;
  using veedubparse::HashString;

  // http://www0.cs.ucl.ac.uk/staff/d.jones/GoodPracticeRNG.pdf

  /* Implementation of a 32-bit KISS generator which uses no multiply instructions */
  unsigned int JKISS32 ()
  {
    static uint32_t x=123456789,y=234567891,z=345678912,w=456789123,c=0;
    int32_t t;
    y ^= (y<<5); y ^= (y>>7); y ^= (y<<22);
    t = z+w+c; z = w; c = t < 0; w = t&2147483647;
    x += 1411392427;
    return x + y + w;
  }

  double urandom ()
    {
      return .0000000002328306089594001 * JKISS32() + FLT_EPSILON;
    }

  //This is faster than Box-Muller but not benchmarked against inverse cdf
  double
  gensample (double)
    {
      double u,v,x,x2;

      do
        {
          u = urandom();
          v = 1.71552776992141359294*urandom()-.85776388496070679647;
          x = v/u;
          x2 = x*x;
        }
      while(x2>6-8*u+2*u*u && (x2>2/u-2*u || x2>-4*log(u)));

      return x;
    }

  void
  orthonormalize (hashpca::MatrixXd& Y)
    {
      // // http://forum.kde.org/viewtopic.php?f=74&t=91271
      // // Unfortunately this appears memory intensive :(
      // Eigen::HouseholderQR<hashpca::MatrixXd> qr (Y);
      // Y = qr.householderQ () * hashpca::MatrixXd::Identity (Y.rows (), Y.cols ());

      Y.transposeInPlace ();

      // Gram-Schmidt has no space overhead
      for (unsigned int j = 0; j < Y.rows (); ++j)
        {
          for (unsigned int i = 0; i < j; ++i)
            Y.row (j) -= Y.row (i).dot (Y.row (j)) * Y.row (i);

          for (unsigned int i = 0; i < j; ++i)
            Y.row (j) -= Y.row (i).dot (Y.row (j)) * Y.row (i);

          Y.row (j).normalize ();
        }

      Y.transposeInPlace ();
    }

  Eigen::VectorXd
  smallsvd (const hashpca::MatrixXd& Z,
            hashpca::MatrixXd&       V)
    {
      hashpca::MatrixXd ZtZ = Z.transpose () * Z;
      Eigen::SelfAdjointEigenSolver<hashpca::MatrixXd> es (ZtZ);
      Eigen::VectorXd s = es.eigenvalues ();
      hashpca::MatrixXd Upsilon = es.eigenvectors ();

      s = s.unaryExpr ([] (double x) { return x <= 0.0 ? 0.0 : sqrt (x); });

      Eigen::VectorXd pinv = s.unaryExpr ([] (double x) { return 1.0 / (1e-6 + x); });

      V.noalias () = Z * Upsilon * pinv.asDiagonal ();

      s = s.unaryExpr ([] (double x) { return x <= 0.0 ? 0.0 : sqrt (x); });

      return s;
    }

  int
  writemodel (PcaOptions                options,
              std::ostream&             model,
              const hashpca::MatrixXd&  V,
              const Eigen::VectorXd&    s,
              const Eigen::VectorXd&    sum,
              double                    n)
    {
      std::cerr << "Writing model ... ";

      model << V.rows () << " " << V.cols () - FUDGE << " " << options.hashall << std::endl;
      model
        << (options.dashq ? static_cast<unsigned char> (options.dashq[0]) : -1)
        << " "
        << (options.dashq ? static_cast<unsigned char> (options.dashq[1]) : -1)
        << std::endl;

      Eigen::IOFormat HeavyFmt (Eigen::FullPrecision);
      model << s.reverse ().topRows (s.rows () - FUDGE).format (HeavyFmt) << std::endl;

      Eigen::VectorXd mean;

      if (options.center)
        mean = (1.0 / n) * V.transpose () * sum;
      else
        mean = Eigen::VectorXd::Zero (V.cols ());

      mean = mean.reverse ().eval ();

      // http://forum.kde.org/viewtopic.php?f=74&t=107161

      model.write ((char*) & mean (0), sizeof (double) * (mean.rows () - FUDGE));

      for (unsigned int i = 0; i < V.rows (); ++i)
        {
          Eigen::VectorXd tmp = V.row (i).reverse ();
          model.write ((char*) & tmp (0),
                       sizeof (double) * (V.cols () - FUDGE));
        }

      std::cerr << "done." << std::endl;

      return 0;
    }

  char*
  make_dashq (int a,
              int b)
    {
      static char buf[3];

      buf[0] = a;
      buf[1] = b;
      buf[2] = 0;

      return buf;
    }

  int
  readmodel (PcaOptions&        options,
             std::istream&      model,
             hashpca::MatrixXd& V,
             Eigen::VectorXd&   s,
             Eigen::VectorXd&   mean)
    {
      model >> options.hashsize >> options.rank >> options.hashall;

      int a;
      int b;

      model >> a >> b;

      if (a >= 0 && b >= 0)
        options.dashq = make_dashq (a, b);
      else
        options.dashq = 0;

      s.resize (options.rank);
      for (unsigned int i = 0; i < options.rank; ++i)
        model >> s (i);

      char newline;
      model.read (&newline, 1);

      mean.resize (options.rank);
      model.read ((char*) & mean (0), sizeof (double) * options.rank);

      Eigen::VectorXd tmp (options.rank);
      V.resize (options.hashsize, options.rank);

      for (unsigned int i = 0; i < V.rows (); ++i)
        {
          model.read ((char*) & tmp (0), sizeof (double) * options.rank);
          V.row (i) = tmp;
        }

      return 0;
    }

  int
  do_project_two (std::istream& in,
                  PcaOptions    options,
                  int           argc,
                  char*         argv[])
    {
      (void) argc;

      if (! in.good ())
        {
          std::cerr << "ERROR: input is not good" << std::endl;

          return 1;
        }

      std::ifstream model (options.model);

      if (! model.good ())
        {
          std::cerr << "ERROR: failed to open file '"
                    << options.model << "' for reading: "
                    << strerror (errno) << std::endl;
          return 1;
        }

      hashpca::MatrixXd V;
      Eigen::VectorXd s;
      Eigen::VectorXd mean;

      if (readmodel (options, model, V, s, mean))
        {
          std::cerr << "ERROR: model file is corrupt or invalid." << std::endl;
          return 1;
        }

      Eigen::VectorXd pinv = s.unaryExpr ([] (double x) { return 1.0 / (1e-6 + x); });

      if (options.dashq)
        {
          BiLinearIterator it (options.hashsize,
                               static_cast<unsigned char> (options.dashq[0]),
                               static_cast<unsigned char> (options.dashq[1]));

          return (options.hashall)
            ? computeu<HashAll> (in, std::cout, V, mean, pinv, it, options)
            : computeu<HashString> (in, std::cout, V, mean, pinv, it, options);
        }
      else
        {
          LinearIterator it (options.hashsize);

          return (options.hashall)
            ? computeu<HashAll> (in, std::cout, V, mean, pinv, it, options)
            : computeu<HashString> (in, std::cout, V, mean, pinv, it, options);
        }
    }

  int
  do_project (PcaOptions    options,
              int           argc,
              char*         argv[])
    {
      if (argc < 1)
        {
          return do_project_two (std::cin, options, argc, argv);
        }
      else
        {
          std::ifstream in (argv[0]);

          if (! in.good ())
            {
              std::cerr << "ERROR: failed to open file '"
                        << argv[0] << "' for reading: "
                        << strerror (errno) << std::endl;
              return 1;
            }

          return do_project_two (in, options, argc, argv);
        }
    }

  int
  do_pca (PcaOptions    options,
          int           argc,
          char*         argv[])
    {
      std::ifstream in (argv[0]);

      if (! in.good ())
        {
          std::cerr << "ERROR: failed to open file '"
                    << argv[0] << "' for reading: "
                    << strerror (errno) << std::endl;
          return 1;
        }

      std::ifstream in2 (argv[argc > 1 ? 1 : 0]);

      if (! in2.good ())
        {
          std::cerr << "ERROR: failed to open file '"
                    << argv[argc > 1 ? 1 : 0] << "' for reading: "
                    << strerror (errno) << std::endl;
          return 1;
        }

      std::ofstream model (options.model);

      if (! model.good ())
        {
          std::cerr << "ERROR: failed to open file '"
                    << options.model << "' for writing: "
                    << strerror (errno) << std::endl;
          return 1;
        }

      options.rank += FUDGE;

      hashpca::MatrixXd Y; Y.setZero (options.hashsize, options.rank);
      hashpca::MatrixXd Omega =
        hashpca::MatrixXd::Zero (options.hashsize, options.rank)
          .unaryExpr (std::ptr_fun (gensample));
      Eigen::VectorXd sum;

      std::cerr << "Processing examples ... ";
      std::pair<double, uint64_t> lines1;

      if (options.dashq)
        {
          BiLinearIterator it (options.hashsize,
                               static_cast<unsigned char> (options.dashq[0]),
                               static_cast<unsigned char> (options.dashq[1]));

          lines1 = (options.hashall)
            ? pca_accumulate<HashAll> (in, Y, Omega, it, options.center, sum)
            : pca_accumulate<HashString> (in, Y, Omega, it, options.center, sum);
        }
      else
        {
          LinearIterator it (options.hashsize);

          lines1 = (options.hashall)
            ? pca_accumulate<HashAll> (in, Y, Omega, it, options.center, sum)
            : pca_accumulate<HashString> (in, Y, Omega, it, options.center, sum);
        }

      std::cerr << "(" << lines1.first << ", "
                << lines1.second << ") done." << std::endl;

      std::cerr << "Orthogonalizing ... ";
      orthonormalize (Y);
      std::cerr << "done." << std::endl;

      std::cerr << "Reprocessing examples ... ";
      hashpca::MatrixXd& Z (Omega);
      Z.setZero (Z.rows (), Z.cols ());

      std::pair<double, uint64_t> lines2;

      if (options.dashq)
        {
          BiLinearIterator it (options.hashsize,
                               static_cast<unsigned char> (options.dashq[0]),
                               static_cast<unsigned char> (options.dashq[1]));

          lines2 = (options.hashall)
            ? pca_accumulate<HashAll> (in2, Z, Y, it, options.center, sum)
            : pca_accumulate<HashString> (in2, Z, Y, it, options.center, sum);
        }
      else
        {
          LinearIterator it (options.hashsize);

          lines2 = (options.hashall)
            ? pca_accumulate<HashAll> (in2, Z, Y, it, options.center, sum)
            : pca_accumulate<HashString> (in2, Z, Y, it, options.center, sum);
        }

      std::cerr << "(" << lines2.first << ", "
                << lines2.second << ") done." << std::endl;

      std::cerr << "Small svding + postprocessing ... ";
      hashpca::MatrixXd& V (Y);
      Eigen::VectorXd s = smallsvd (Z, V);
      std::cerr << "done." << std::endl;

      options.rank -= FUDGE;
      return writemodel (options, model, V, s, sum, lines2.first);
    }
}

int
main (int   argc,
      char* argv[])
{
  std::ios::sync_with_stdio (false);

  if (argc < 2)
    {
      std::cerr << help << std::endl;
      return 1;
    }

  PcaOptions options = parse_pca_options (argc, argv);

  if (! options.project && argc < 1)
    {
      std::cerr << "ERROR: did not specify input file\n" << std::endl;
      std::cerr << help << std::endl;
      return 1;
    }

  if (! options.model)
    {
      std::cerr << "ERROR: did not specify model file\n" << std::endl;
      std::cerr << help << std::endl;
      return 1;
    }

  return options.project ? do_project (options, argc, argv)
                         : do_pca (options, argc, argv);
}