/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2016 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

/**
 * \file
 * \author Martin Weiser, modified by Felix Lehmann
 */

#ifndef CONJUGATION_HH
#define CONJUGATION_HH

#include <cassert>
#include <memory>

#include <boost/timer/timer.hpp>

#include "dune/istl/bcrsmatrix.hh"
#include "dune/istl/matrixindexset.hh"

#include "linalg/localMatrices.hh"
#include "linalg/threadedMatrix.hh"

#include "utilities/timing.hh"

namespace Kaskade
{
  /**
   * \brief Creates the sparsity pattern of \f$ P^T A P\f$.
   *
   * Out now: onlyLowerTriangle. If parameter is set true, we will only touch the lower triangle of A
   * and only create the lower triangle of the resulting matrix P^T A P
   */
  template <class Scalar, class Entry>
  std::unique_ptr<Dune::MatrixIndexSet> conjugationPattern(Dune::BCRSMatrix<Dune::FieldMatrix<Scalar,1,1> > const& P,
							   Dune::BCRSMatrix<Entry> const& A ,
							   bool onlyLowerTriangle = false)
      {
    assert(A.N()==A.M());
    assert(A.N()==P.N());

    typedef Dune::BCRSMatrix<Entry> MatA;
    typedef Dune::BCRSMatrix<Dune::FieldMatrix<Scalar,1,1> > MatP;

    // If C = P^T A P, we have that C_{ij} = \sum_{k,l} P_{ki} P_{lj} A_{kl}. Hence, the entry A_{kl} contributes to
    // all C_{ij} for which there are nonzero entries P_{ki} and P_{lj} in the rows k and l of P. Thus we can simply
    // run through all nonzeros A_{kl} of A, look up the column indices i,j of rows k and l of P, and flag C_{ij}
    // as nonzero.

    std::unique_ptr<Dune::MatrixIndexSet> nzC(new Dune::MatrixIndexSet(P.M(),P.M()));

    if(onlyLowerTriangle == false)
    {
      // Step through all entries of A
      for (int k=0; k<A.N(); ++k)
        for (typename MatA::ConstColIterator ca=A[k].begin(); ca!=A[k].end(); ++ca)
        {
          int const l = ca.index();
          // Step through all entries of rows k and l of P and add entry
          for (typename MatP::ConstColIterator cpk=P[k].begin(); cpk!=P[k].end(); ++cpk)
            for (typename MatP::ConstColIterator cpl=P[l].begin(); cpl!=P[l].end(); ++cpl)
              nzC->add(cpk.index(),cpl.index());
        }
    }
    else
    {
      // Step through all entries of A
      for (int k=0; k<A.N(); ++k)
        for (typename MatA::ConstColIterator ca=A[k].begin(); ca!=A[k].end(); ++ca)
        {
          int const l = ca.index();
          // Step through all entries of rows k and l of P and add entry
          for (typename MatP::ConstColIterator cpk=P[k].begin(); cpk!=P[k].end(); ++cpk)
            for (typename MatP::ConstColIterator cpl=P[l].begin(); cpl!=P[l].end(); ++cpl)
            {
              if( cpk.index() >= cpl.index() )
                nzC->add(cpk.index(),cpl.index());
              else
                nzC->add(cpl.index(),cpk.index());
            }
        }
    }

    return nzC;

    // An alternative way of computing the sparsity pattern would be to use that the nonzero entries j in column i
    // of C are exactly those for which there is k with (nonzero P_{jk} and there is l with (nonzero P_{il} and A_{lk})).
    // Hence we can obtain the column index set J directly by the following steps:
    // (i) find all l with P_{li} nonzero -> L  [requires to access columns of P - compute the transpose patterns once]
    // (ii) find all k with A_{lk} nonzero for some l in L -> K  [probably sorting K and removing doubled entries would be a good idea here]
    // (iii) find all j with P_{kj} nonzero for some k in K -> J
    // Compared to the above implementation this would have the following (dis)advantages
    // + easy to do in parallel (since write operations are separated)
    // + fewer scattered write accesses to memory
    // - more complex implementation
    // - requires the transpose pattern of P
      }

  /**
   * \brief Creates the conjugation product \f$ P^T A P\f$.
   *
   * Note that for typical multigrid Galerkin projections the memory access patterns of the conjugation are quite
   * bad for performance. Consider assembling the projected matrix directly on the coarser discretization.
   *
   * \param onlyLowerTriangle. If true, we will only touch the lower triangle of A
   * and only create the lower triangle of the resulting matrix P^T A P
   */
  template <class IndexP, class EntryP, class IndexA, class EntryA>
  NumaBCRSMatrix<EntryA,IndexA>
  conjugation(NumaBCRSMatrix<EntryP,IndexP> const& P, NumaBCRSMatrix<EntryA,IndexA> const& A, bool onlyLowerTriangle = false)
  {
    assert(A.N()==A.M());
    assert(A.N()==P.N());

    Timings& timer = Timings::instance();

    // First create the sparsity pattern
    timer.start("conjugation pattern");
    NumaCRSPatternCreator<IndexA> creator(P.M(),P.M(),onlyLowerTriangle);

    // If C = P^T A P, we have that C_{ij} = \sum_{k,l} P_{ki} P_{lj} A_{kl}. Hence, the entry A_{kl} contributes to
    // all C_{ij} for which there are nonzero entries P_{ki} and P_{lj} in the rows k and l of P. Thus we can simply
    // run through all nonzeros A_{kl} of A, look up the column indices i,j of rows k and l of P, and flag C_{ij}
    // as nonzero.

    { // just a new scope
      // helper routine for extracting all column indices of a row
      auto getColumnIndices = [] (auto const& row, std::vector<IndexP>& ci)
      {
        ci.clear();
        for (auto i=row.begin(); i!=row.end(); ++i)       // indices i for which Pki != 0
          ci.push_back(i.index());
      };
      std::vector<IndexP> is, js;

      // Step through all entries of A
      for (IndexA k=0; k<A.N(); ++k)
      {
        getColumnIndices(P[k],is);                          // indices i for which Pki != 0

        auto row = A[k];
        for (auto ca=row.begin(); ca!=row.end(); ++ca)
        {
          IndexA const l = ca.index();
          getColumnIndices(P[l],js);                        // indices j for which Plj != 0

          // add all combinations i,j
          creator.addElements(std::begin(is),std::end(is),std::begin(js),std::end(js));
          if (onlyLowerTriangle && k>l)                     // subdiagonal entry (k,l) of A -> entry (l,k) must be treated implicitly:
            // add all combinations (j,i)
            creator.addElements(std::begin(js),std::end(js),std::begin(is),std::end(is));
        }
      }
    }
    timer.stop("conjugation pattern");

    // An alternative way of computing the sparsity pattern would be to use that the nonzero entries j in column i
    // of C are exactly those for which there is k with (nonzero P_{jk} and there is l with (nonzero P_{il} and A_{lk})).
    // Hence we can obtain the column index set J directly by the following steps:
    // (i) find all l with P_{li} nonzero -> L  [requires to access columns of P - compute the transpose patterns once]
    // (ii) find all k with A_{lk} nonzero for some l in L -> K  [probably sorting K and removing doubled entries would be a good idea here]
    // (iii) find all j with P_{kj} nonzero for some k in K -> J
    // Compared to the above implementation this would have the following (dis)advantages
    // + easy to do in parallel (since write operations are separated)
    // + fewer scattered write accesses to memory
    // - more complex implementation
    // - requires the transpose pattern of P

    // Create the sparse matrix.
    timer.start("matrix creation");
    NumaBCRSMatrix<EntryA,IndexA> pap(creator);
    timer.stop("matrix creation");

    // Fill the sparse matrix PAP. This is done as before by stepping through all Akl entries and scatter
    // Pki*Plj*Akl into PAPij.
    //
    // An alternative way of computing P^TAP would be a gather operation with inverted loop order:
    // Cij = sum_kl Pki*Plj*Akl. This requires P^T for efficient determination of required kl indices.
    // While the transpose construction is efficient, the gather implementation ist not (tested 2016-01-17),
    // presumably because A is larger than PAP and the scattered accesses have a worse locality.
    // Sequential performance was more than 10-fold slower than the scatter implementation below, so
    // we stick to the scatter.
    //
    // A second alternative is to create a triplet matrix first. This appears to be a factor 3 slower in
    // sequential implementation, and incurs a high memory footprint as several entries are duplicate.

    auto getEntryValues = [] (auto const& row, std::vector<EntryP>& vi)
    {
      vi.clear();
      for (auto i=row.begin(); i!=row.end(); ++i)       // indices i for which Pki != 0
        vi.push_back(*i);
    };

    auto getColumnIndices = [] (auto const& row, auto& ci)     // computes (global,local) column index pairs of row k of P
    {
      ci.clear();
      int idx = 0;
      for (auto i=row.begin(); i!=row.end(); ++i, ++idx)       // indices i for which Pki != 0
        ci.push_back(std::make_pair(i.index(),idx));
    };

    timer.start("conjugation scatter");
    parallelFor([&](size_t block, size_t nBlocks)
    {
      size_t rowStart = uniformWeightRangeStart(block,nBlocks,A.N());
      size_t rowEnd   = uniformWeightRangeStart(block+1,nBlocks,A.N());
      std::vector<EntryP> pki, plj;
      using SortedIndices = std::vector<std::pair<IndexP,int>>;
      SortedIndices is, js;
      LocalMatrices<EntryA,false,SortedIndices,SortedIndices> localMatrices(pap);
      for (IndexA k=rowStart; k<rowEnd; ++k)
      {
        auto Pk = P[k];
        getColumnIndices(Pk,is);                          // indices i for which Pki != 0
        getEntryValues(Pk,pki);

        auto row = A[k];
        for (auto ca=row.begin(); ca!=row.end(); ++ca)
        {
          IndexA const l = ca.index();                    // column index of Akl
          auto Pl = P[l];
          getColumnIndices(Pl,js);                        // indices j for which Plj != 0
          getEntryValues(Pl,plj);

          // For each entry Akl of A we have to create one local matrix.
          localMatrices.push_back(is,js);

          auto Akl = *ca;
          for (int i=0; i<is.size(); ++i)                 // just scatter Akl to all affected PAP entries
          {
            auto Pki_Akl = transpose(pki[i]) * Akl;
            for (int j=0; j<plj.size(); ++j)
              localMatrices.back()(i,j) = Pki_Akl * plj[j];
          }

          if (onlyLowerTriangle && k>l)
          {
            // treat Alk entry
            abort();
          }
        }
      }
    },8*NumaThreadPool::instance().cpus());

    timer.stop("conjugation scatter");

    return pap;
  }

  /**
   * \brief Computes the triple sparse matrix product \f$ C = C + P^T A P \f$.
   *
   * \param C has to have a sparsity pattern that is a superset of the sparsity pattern of \f$ P^T A P \f$.
   *
   */
  template <class Scalar, class Entry>
  void conjugation(Dune::BCRSMatrix<Entry>& C,
		   Dune::BCRSMatrix<Dune::FieldMatrix<Scalar,1,1> > const& P,
		   Dune::BCRSMatrix<Entry> const& A,
		   bool onlyLowerTriangle = false )
  {
    assert(A.N()==A.M());
    assert(A.N()==P.N());
    assert(C.N()==P.M());
    assert(C.M()==P.M());

    typedef Dune::BCRSMatrix<Entry> MatA;
    typedef Dune::BCRSMatrix<Dune::FieldMatrix<Scalar,1,1> > MatP;

    if(onlyLowerTriangle == false )
    {
      // Step through all entries of A
      for (int k=0; k<A.N(); ++k)
        for (typename MatA::ConstColIterator ca=A[k].begin(); ca!=A[k].end(); ++ca)
        {
          int const l = ca.index();
          // Step through all entries of rows k and l of P and add entry
          for (typename MatP::ConstColIterator cpk=P[k].begin(); cpk!=P[k].end(); ++cpk)
            for (typename MatP::ConstColIterator cpl=P[l].begin(); cpl!=P[l].end(); ++cpl)
              C[cpk.index()][cpl.index()].axpy((*cpl) * (*cpk),(*ca));
        }
    }
    else
    {
      // Step through all entries of A
      for (int k=0; k<A.N(); ++k)
        for (typename MatA::ConstColIterator ca=A[k].begin(); ca!=A[k].end(); ++ca)
        {
          int const l = ca.index();
          for (typename MatP::ConstColIterator cpk=P[k].begin(); cpk!=P[k].end(); ++cpk)
            for (typename MatP::ConstColIterator cpl=P[l].begin(); cpl!=P[l].end(); ++cpl)
            {
              if( cpk.index() >= cpl.index() )
                C[cpk.index()][cpl.index()].axpy((*cpl) * (*cpk), (*ca) );
            }
          if(k>l)
          {
            for (typename MatP::ConstColIterator cpl=P[l].begin(); cpl!=P[l].end(); ++cpl)
              for (typename MatP::ConstColIterator cpk=P[k].begin(); cpk!=P[k].end(); ++cpk)
                if( cpl.index() >= cpk.index() )
                  C[cpl.index()][cpk.index()].axpy((*cpl) * (*cpk) , (*ca) );
          }
        }
    }
  }
}
#endif


//=======================================================================
// Copyright (c) 2013 Piotr Smulewicz
//
// 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)
//=======================================================================
/**
 * @file scheduling_jobs_on_identical_parallel_machines_long_test.cpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2013-09-06
 */

#include "test_utils/logger.hpp"
#include "test_utils/scheduling.hpp"
#include "test_utils/test_result_check.hpp"

#include "paal/greedy/scheduling_jobs_on_identical_parallel_machines/scheduling_jobs_on_identical_parallel_machines.hpp"

#include <boost/test/unit_test.hpp>

#include <numeric>
#include <vector>

const long long MAX_TIME = 1000000000;
const long MIN_MACHINES = 10;
const long MAX_MACHINES = 1000000;
const long STEP_MACHINES = 10;
const double MIN_JOBS_ON_MACHINE_START = 1.0;
const double MIN_JOBS_ON_MACHINE_END = 5.0;
const double MIN_JOBS_ON_MACHINE_STEP = 0.33;
const long SEED = 42;
using Time = long long;

BOOST_AUTO_TEST_CASE(scheduling_jobs_on_identical_parallel_machines_long) {
    std::srand(SEED);
    for (int number_of_machines = MIN_MACHINES;
         number_of_machines <= MAX_MACHINES;
         number_of_machines *= STEP_MACHINES) {
        for (double min_jobs_on_machine = MIN_JOBS_ON_MACHINE_START;
             min_jobs_on_machine < MIN_JOBS_ON_MACHINE_END;
             min_jobs_on_machine += MIN_JOBS_ON_MACHINE_STEP) {
            LOGLN("machines: " << number_of_machines);

            std::vector<int> machines(number_of_machines);
            std::vector<Time> jobs = generate_job_loads(
                machines, min_jobs_on_machine, MAX_TIME,
                paal::utils::return_one_functor());
            LOGLN("jobs: " << jobs.size());

            std::vector<std::pair<int, decltype(jobs)::iterator>> result;

            paal::greedy::scheduling_jobs_on_identical_parallel_machines(
                number_of_machines, jobs.begin(), jobs.end(),
                back_inserter(result), paal::utils::identity_functor());
            check_jobs(result, jobs);
            std::vector<Time> sum_of_machine;
            sum_of_machine.resize(number_of_machines);
            for (auto job_machine_pair : result) {
                sum_of_machine[job_machine_pair.first] +=
                    *(job_machine_pair.second);
            }

            Time maximum_load =
                *std::max_element(sum_of_machine.begin(), sum_of_machine.end());

            Time sum_all_loads = std::accumulate(sum_of_machine.begin(),
                                                 sum_of_machine.end(), 0.0);
            // print result
            check_result(double(maximum_load),
                         double(sum_all_loads) / number_of_machines, 4.0 / 3.0);
        }
    }
}


//
//  main.cpp
//  Task
//
//  Created by Elizabeth Lorelei on 15.11.2019.
//  Copyright © 2019 Yelyzaveta Losieva. All rights reserved.
//

#include <iostream>
#include <Eigen/Dense>
#include <Eigen/LU>
#include <array>
#include <vector>
#include <string>
#include <cstdio>
#include <algorithm>
#include <fstream>

using namespace std;
using namespace Eigen;

Matrix3d calcHomography(const array<Vector2d,4>& oldPoints,
                        const array<Vector2d,4>& newPoints);
vector <unsigned char> readBMP(string filename);

int main()
{
    auto bmp = readBMP("/Users/elizabethlorelei/Downloads/Girl.bmp");
    Vector2d X1{0,1};
    Vector2d X2{1,1};
    Vector2d X3{1,0};
    Vector2d X4{0,0};
    Vector2d U1{0,1};
    Vector2d U2{2,1};
    Vector2d U3{2,0};
    Vector2d U4{0,0};
    Matrix3d homography{calcHomography({X1, X2, X3, X4}, {U1, U2, U3, U4})};
    cout << homography << endl;
}

Matrix3d calcHomography(const array<Vector2d,4>& oldPoints,
                        const array<Vector2d,4>& newPoints)
{
    Matrix<double,9,9> A{Matrix<double,9,9>::Zero()};
    for (size_t i{0}; i<4; ++i)
    {
        auto X = oldPoints[i];
        auto U = newPoints[i];

        A(2*i,0) = -X[0];
        A(2*i,1) = -X[1];
        A(2*i,2) = -1;
        A(2*i,6) =  X[0]*U[0];
        A(2*i,7) =  X[1]*U[0];
        A(2*i,8) =  U[0];

        A(2*i+1,3) =  -X[0];
        A(2*i+1,4) =  -X[1];
        A(2*i+1,5) =  -1;
        A(2*i+1,6) =  X[0]*U[1];
        A(2*i+1,7) =  X[1]*U[1];
        A(2*i+1,8) =  U[1];
    }
    A(8,8) = 1;
    cout << A << endl;
    Matrix<double,9,1> b{Matrix<double,9,1>::Zero()};
    b(8) = 1;
    Matrix<double,9,1> flattenRes {A.colPivHouseholderQr().solve(b)};
    cout << endl << flattenRes << endl << endl;
    Matrix3d res;
    for (size_t i{0}; i<9; ++i)
    {
        res(i/3, i%3) = flattenRes[i];
    }
    return res;
}

vector<unsigned char> readBMP(string filename)
{
    int i;
    ifstream inputFilestream{filename, ios_base::binary};
    char info[54];
    inputFilestream.get(info,54); // read the 54-byte header

    // extract image height and width from header
    int width = *(int*)&info[18];
    int height = *(int*)&info[22];

    int size = 3 * width * height;
    char* data = new char[size]; // allocate 3 bytes per pixel
    inputFilestream.get(data,size); // read the rest of the data at once
    inputFilestream.close();

    for(i = 0; i < size; i += 3)
    {
            char tmp = data[i];
            data[i] = data[i+2];
            data[i+2] = tmp;
    }
    vector<unsigned char> res(size);
    copy(data, data + size, res.begin());
    return res;
}


/*
    This file is part of Nori, a simple educational ray tracer

    Copyright (c) 2012 by Wenzel Jakob and Steve Marschner.

    Nori is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License Version 3
    as published by the Free Software Foundation.

    Nori 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 General Public License for more details.

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

#include <nori/camera.h>
#include <nori/rfilter.h>
#include <Eigen/Geometry>

NORI_NAMESPACE_BEGIN

/**
 * \brief Perspective camera with depth of field
 *
 * This class implements a simple perspective camera model. By default,
 * it uses an infinitesimally small aperture, creating an infinite depth
 * of field. Use the <tt>apertureRadius</tt> and <tt>focusDistance</tt>
 * parameters to change this behavior.
 */
class PerspectiveCamera : public Camera {
public:
	PerspectiveCamera(const PropertyList &propList) {
		/* Width and height in pixels. Default: 720p */
		m_outputSize.x() = propList.getInteger("width", 1280);
		m_outputSize.y() = propList.getInteger("height", 720);
		m_invOutputSize = m_outputSize.cast<float>().cwiseInverse();

		/* Specifies an optional camera-to-world transformation. Default: none */
		m_cameraToWorld = propList.getTransform("toWorld", Transform());

		/* Horizontal field of view in degrees */
		m_fov = propList.getFloat("fov", 30.0f);

		/* Denotes the radios of the camera in scene units.
		   Default: 0, i.e. no depth of field */
		m_apertureRadius = propList.getFloat("apertureRadius", 0.0f);

		/* Near and far clipping planes in world-space units */
		m_nearClip = propList.getFloat("nearClip", 1e-4f);
		m_farClip = propList.getFloat("farClip", 1e4f);

		/* Denotes the world-space distance from the camera's aperture
		   to the focal plane */
		m_focusDistance = propList.getFloat("focusDistance", m_farClip);

		m_rfilter = NULL;
	}

	void activate() {
		float aspect = m_outputSize.x() / (float) m_outputSize.y();

		/* Project vectors in camera space onto a plane at z=1:
		 *
		 *  xProj = cot * x / z
		 *  yProj = cot * y / z
		 *  zProj = (far * (z - near)) / (z * (far-near))
		 *  The cotangent factor ensures that the field of view is
		 *  mapped to the interval [-1, 1].
		 */
		float recip = 1.0f / (m_farClip - m_nearClip),
		      cot = 1.0f / std::tan(degToRad(m_fov / 2.0f));

		Eigen::Matrix4f perspective;
		perspective <<
			cot, 0,   0,   0,
			0, cot,   0,   0,
			0,   0,   m_farClip * recip, -m_nearClip * m_farClip * recip,
			0,   0,   1,   0;

		/**
		 * Translation and scaling to shift the clip coordinates into the
		 * range from zero to one. Also takes the aspect ratio into account.
		 */
		m_sampleToCamera = Transform(
			Eigen::DiagonalMatrix<float, 3>(Vector3f(0.5f, -0.5f * aspect, 1.0f)) *
			Eigen::Translation<float, 3>(1.0f, -1.0f/aspect, 0.0f) * perspective).inverse();

		/* If no reconstruction filter was assigned, instantiate a Gaussian filter */
		if (!m_rfilter)
			m_rfilter = static_cast<ReconstructionFilter *>(
				NoriObjectFactory::createInstance("gaussian", PropertyList()));
	}

	Color3f sampleRay(Ray3f &ray,
			const Point2f &samplePosition,
			const Point2f &apertureSample) const {
		Point2f tmp = squareToUniformDiskConcentric(apertureSample)
			* m_apertureRadius;

		/* Compute the corresponding position on the
		   near plane (in local camera space) */
		Point3f nearP = m_sampleToCamera * Point3f(
			samplePosition.x() * m_invOutputSize.x(),
			samplePosition.y() * m_invOutputSize.y(), 0.0f);

		Point3f apertureP(tmp.x(), tmp.y(), 0.0f);

		/* Sampled position on the focal plane */
		Point3f focusP = nearP * (m_focusDistance / nearP.z());

		/* Aperture position */
		/* Turn these into a normalized ray direction, and
		   adjust the ray interval accordingly */
		Vector3f d = (focusP - apertureP).normalized();
		float invZ = 1.0f / d.z();

		ray.o = m_cameraToWorld * apertureP;
		ray.d = m_cameraToWorld * d;
		ray.mint = m_nearClip * invZ;
		ray.maxt = m_farClip * invZ;
		ray.update();

		return Color3f(1.0f);
	}

	void addChild(NoriObject *obj) {
		switch (obj->getClassType()) {
			case EReconstructionFilter:
				if (m_rfilter)
					throw NoriException("Camera: tried to register multiple reconstruction filters!");
				m_rfilter = static_cast<ReconstructionFilter *>(obj);
				break;

			default:
				throw NoriException(QString("Camera::addChild(<%1>) is not supported!").arg(
					classTypeName(obj->getClassType())));
		}
	}

        /// Return fov as parameter
        virtual QString getParameters() const {
                return QString("%1").arg(m_fov);
        }

        virtual Transform getTransform() const {
                return m_cameraToWorld;
        }

	/// Return a human-readable summary
	QString toString() const {
		return QString(
			"PerspectiveCamera[\n"
			"  cameraToWorld = %1,\n"
			"  outputSize = %2,\n"
			"  fov = %3,\n"
			"  apertureRadius = %4,\n"
			"  focusDistance = %5,\n"
			"  clip = [%6, %7],\n"
			"  rfilter = %8\n"
			"]")
		.arg(indent(m_cameraToWorld.toString(), 18))
		.arg(m_outputSize.toString())
		.arg(m_fov)
		.arg(m_apertureRadius)
		.arg(m_focusDistance)
		.arg(m_nearClip)
		.arg(m_farClip)
		.arg(indent(m_rfilter->toString()));
	}
private:
	Vector2f m_invOutputSize;
	Transform m_sampleToCamera;
	Transform m_cameraToWorld;
	float m_fov;
	float m_apertureRadius;
	float m_focusDistance;
	float m_nearClip;
	float m_farClip;
};

NORI_REGISTER_CLASS(PerspectiveCamera, "perspective");
NORI_NAMESPACE_END


//
// Copyright (c) 2015-2019 CNRS INRIA
//

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/utils/timer.hpp"
#include "pinocchio/parsers/sample-models.hpp"

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE ( test_com )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);
  pinocchio::Data data(model);

  VectorXd q = VectorXd::Ones(model.nq);
  q.middleRows<4> (3).normalize();
  VectorXd v = VectorXd::Ones(model.nv);
  VectorXd a = VectorXd::Ones(model.nv);

  crba(model,data,q);

	/* Test COM against CRBA*/
  Vector3d com = centerOfMass(model,data,q);
  BOOST_CHECK(data.com[0].isApprox(getComFromCrba(model,data), 1e-12));

	/* Test COM against Jcom (both use different way to compute the COM). */
  com = centerOfMass(model,data,q);
  jacobianCenterOfMass(model,data,q);
  BOOST_CHECK(com.isApprox(data.com[0], 1e-12));

	/* Test COM against Jcom (both use different way to compute the COM). */
  centerOfMass(model,data,q,v,a);
  BOOST_CHECK(com.isApprox(data.com[0], 1e-12));

  /* Test vCoM against nle algorithm without gravity field */
  a.setZero();
  model.gravity.setZero();
  centerOfMass(model,data,q,v,a);
  nonLinearEffects(model, data, q, v);

  pinocchio::SE3::Vector3 acom_from_nle (data.nle.head <3> ()/data.mass[0]);
  BOOST_CHECK((data.liMi[1].rotation() * acom_from_nle).isApprox(data.acom[0], 1e-12));

	/* Test Jcom against CRBA  */
  Eigen::MatrixXd Jcom = jacobianCenterOfMass(model,data,q);
  BOOST_CHECK(data.Jcom.isApprox(getJacobianComFromCrba(model,data), 1e-12));

  /* Test CoM velocity againt jacobianCenterOfMass */
  BOOST_CHECK((Jcom * v).isApprox(data.vcom[0], 1e-12));

  centerOfMass(model,data,q,v);
  /* Test CoM velocity againt jacobianCenterOfMass */
  BOOST_CHECK((Jcom * v).isApprox(data.vcom[0], 1e-12));

//  std::cout << "com = [ " << data.com[0].transpose() << " ];" << std::endl;
//  std::cout << "mass = [ " << data.mass[0] << " ];" << std::endl;
//  std::cout << "Jcom = [ " << data.Jcom << " ];" << std::endl;
//  std::cout << "M3 = [ " << data.M.topRows<3>() << " ];" << std::endl;
}

BOOST_AUTO_TEST_CASE ( test_mass )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);

  double mass = computeTotalMass(model);

  BOOST_CHECK(mass == mass); // checking it is not NaN

  double mass_check = 0.0;
  for(size_t i=1; i<(size_t)(model.njoints);++i)
    mass_check += model.inertias[i].mass();

  BOOST_CHECK_CLOSE(mass, mass_check, 1e-12);

  pinocchio::Data data1(model);

  double mass_data = computeTotalMass(model,data1);

  BOOST_CHECK(mass_data == mass_data); // checking it is not NaN
  BOOST_CHECK_CLOSE(mass, mass_data, 1e-12);
  BOOST_CHECK_CLOSE(data1.mass[0], mass_data, 1e-12);

  pinocchio::Data data2(model);
  VectorXd q = VectorXd::Ones(model.nq);
  q.middleRows<4> (3).normalize();
  centerOfMass(model,data2,q);

  BOOST_CHECK_CLOSE(data2.mass[0], mass, 1e-12);
}

BOOST_AUTO_TEST_CASE ( test_subtree_masses )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);

  pinocchio::Data data1(model);

  computeSubtreeMasses(model,data1);

  pinocchio::Data data2(model);
  VectorXd q = VectorXd::Ones(model.nq);
  q.middleRows<4> (3).normalize();
  centerOfMass(model,data2,q);

  for(size_t i=0; i<(size_t)(model.njoints);++i)
  {
    BOOST_CHECK_CLOSE(data1.mass[i], data2.mass[i], 1e-12);
  }
}

//BOOST_AUTO_TEST_CASE ( test_timings )
//{
//  using namespace Eigen;
//  using namespace pinocchio;
//
//  pinocchio::Model model;
//  pinocchio::buildModels::humanoidRandom(model);
//  pinocchio::Data data(model);
//
//  long flag = BOOST_BINARY(1111);
//  PinocchioTicToc timer(PinocchioTicToc::US);
//  #ifdef NDEBUG
//    #ifdef _INTENSE_TESTING_
//      const size_t NBT = 1000*1000;
//    #else
//      const size_t NBT = 10;
//    #endif
//  #else
//    const size_t NBT = 1;
//    std::cout << "(the time score in debug mode is not relevant)  " ;
//  #endif
//
//  bool verbose = flag & (flag-1) ; // True is two or more binaries of the flag are 1.
//  if(verbose) std::cout <<"--" << std::endl;
//  Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
//
//  if( flag >> 0 & 1 )
//  {
//    timer.tic();
//    SMOOTH(NBT)
//    {
//      centerOfMass(model,data,q);
//    }
//    if(verbose) std::cout << "COM =\t";
//    timer.toc(std::cout,NBT);
//  }
//
//  if( flag >> 1 & 1 )
//  {
//    timer.tic();
//    SMOOTH(NBT)
//    {
//      centerOfMass(model,data,q,false);
//    }
//    if(verbose) std::cout << "Without sub-tree =\t";
//    timer.toc(std::cout,NBT);
//  }
//
//  if( flag >> 2 & 1 )
//  {
//    timer.tic();
//    SMOOTH(NBT)
//    {
//      jacobianCenterOfMass(model,data,q);
//    }
//    if(verbose) std::cout << "Jcom =\t";
//    timer.toc(std::cout,NBT);
//  }
//}

BOOST_AUTO_TEST_CASE(test_subtree_com_jacobian)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);
  Data data(model);

  model.upperPositionLimit.head<3>().fill(1000);
  model.lowerPositionLimit.head<3>() = -model.upperPositionLimit.head<3>();
  VectorXd q = pinocchio::randomConfiguration(model);
  VectorXd v = VectorXd::Random(model.nv);

  Data data_ref(model);
  jacobianCenterOfMass(model,data_ref,q,true);

  centerOfMass(model, data, q, v);
  Data::Matrix3x Jcom(3,model.nv); Jcom.setZero();
  jacobianSubtreeCenterOfMass(model, data, 0, Jcom);

  BOOST_CHECK(Jcom.isApprox(data_ref.Jcom));

  centerOfMass(model, data_ref, q, v, true);
  computeJointJacobians(model, data_ref, q);
  Data::Matrix3x Jcom_extracted(3,model.nv), Jcom_fd(3,model.nv);
  Data data_extracted(model), data_fd(model);
  const double eps = 1e-8;
  jacobianCenterOfMass(model,data_extracted,q);

  // Get subtree jacobian and check that it is consistent with the com velocity
  for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; joint_id++)
  {
    SE3::Vector3 subtreeComVelocityInWorld_ref = data_ref.oMi[joint_id].rotation() * data_ref.vcom[joint_id];
    Jcom.setZero();
    data.J.setZero();
    jacobianSubtreeCenterOfMass(model, data, joint_id, Jcom);

    BOOST_CHECK(data.J.middleCols(model.joints[joint_id].idx_v(),data.nvSubtree[joint_id]).isApprox(data_ref.J.middleCols(model.joints[joint_id].idx_v(),data.nvSubtree[joint_id])));
    SE3::Vector3 subtreeComVelocityInWorld = Jcom * v;

    Jcom_extracted.setZero();
    getJacobianSubtreeCenterOfMass(model,data_extracted,joint_id,Jcom_extracted);

    // Check with finite differences
    Eigen::VectorXd v_plus(model.nv); v_plus.setZero();
    centerOfMass(model,data_fd,q);
    const SE3::Vector3 com = data_fd.oMi[joint_id].act(data_fd.com[joint_id]);
    Jcom_fd.setZero();
    for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
    {
      v_plus[k] = eps;
      Eigen::VectorXd q_plus = integrate(model,q,v_plus);
      centerOfMass(model,data_fd,q_plus);
      const SE3::Vector3 com_plus = data_fd.oMi[joint_id].act(data_fd.com[joint_id]);
      Jcom_fd.col(k) = (com_plus - com)/eps;
      v_plus[k] = 0.;
    }

//    Eigen::VectorXd q_plus = integrate(model,q,v*eps);
//    centerOfMass(model,data_fd,q_plus);
//    const SE3::Vector3 com_plus = data_fd.oMi[joint_id].act(data_fd.com[joint_id]);
//
//    const SE3::Vector3 vcom_subtree_fd = (com_plus - com)/eps;

    BOOST_CHECK(Jcom.isApprox(Jcom_fd,sqrt(eps)));
    BOOST_CHECK(Jcom_extracted.isApprox(Jcom_fd,sqrt(eps)));
    BOOST_CHECK(Jcom_extracted.isApprox(Jcom));

    BOOST_CHECK(std::fabs(data.mass[joint_id] - data_ref.mass[joint_id]) <= 1e-12);
    BOOST_CHECK(data.com[joint_id].isApprox(data_ref.oMi[joint_id].act(data_ref.com[joint_id])));
    BOOST_CHECK(subtreeComVelocityInWorld.isApprox(subtreeComVelocityInWorld_ref));
  }
}

BOOST_AUTO_TEST_SUITE_END ()


#pragma once

#include "PolynomialBasisGen.hh"
#include "RBFKernel.hh"
#include "FiniteDifferentiator.hh"
#include <Eigen/Dense>
#include <utility>
#include <vector>

namespace kt84 {

template <int _DimIn, int _DimOut, class _RBFKernel_Core, int _DegreePolynomial>
struct MLS
    : public FiniteDifferentiator<MLS<_DimIn, _DimOut, _RBFKernel_Core, _DegreePolynomial>, _DimIn, _DimOut>
{
    enum {
        DimIn  = _DimIn,
        DimOut = _DimOut,
        DegreePolynomial = _DegreePolynomial,
    };

    typedef Eigen::Matrix<double, DimIn , 1> Point;
    typedef Eigen::Matrix<double, DimOut, 1> Value;
    typedef Eigen::Matrix<double, DimOut, DimIn> Gradient;
    typedef PolynomialBasisGen<DimIn, DegreePolynomial> PolynomialBasisGen;
    typedef RBFKernel_Bivariate<DimIn, _RBFKernel_Core> Kernel;
    typedef std::pair<Point, Value> Constraint;

    static_assert(Kernel::Decaying, "MLS requires its kernel to be decaying!");

    std::vector<Constraint> constraints;
    Kernel kernel;

    void clear_constraints() {
        constraints.clear();
    }
    void add_constraint(const Point& point, const Value& value) {
        constraints.push_back(Constraint(point, value));
    }
    Value operator()(const Point& point) const {
        const int P = PolynomialBasisGen::DimOut;
        Eigen::Matrix<double, P, P> A;
        Eigen::Matrix<double, P, DimOut> b;
        A.setZero();
        b.setZero();
        for (size_t i = 0; i < constraints.size(); ++i) {
            const Point& point_i = constraints[i].first;
            const Value& value_i = constraints[i].second;
            double w_i = kernel(point, point_i);
            if (w_i == std::numeric_limits<double>::infinity())
                return value_i;
            w_i = w_i * w_i;
            PolynomialBasisGen::Basis basis_i = PolynomialBasisGen::basis(point_i);
            A += w_i * basis_i * basis_i.transpose();
            b += w_i * basis_i * value_i.transpose();
        }
        Eigen::Matrix<double, P, DimOut> c = A.colPivHouseholderQr().solve(b);
        return c.transpose() * PolynomialBasisGen::basis(point);
    }
};

// template specialization for 0-degree polynomial due to Eigen's inability to handle 1x1 matrix (which is scalar) coherently
template <int _DimIn, int _DimOut, class _RBFKernel_Core>
struct MLS<_DimIn, _DimOut, _RBFKernel_Core, 0>
    : public FiniteDifferentiator<MLS<_DimIn, _DimOut, _RBFKernel_Core, 0>, _DimIn, _DimOut>
{
    enum {
        DimIn  = _DimIn,
        DimOut = _DimOut,
        DegreePolynomial = 0,
    };

    typedef Eigen::Matrix<double, DimIn , 1> Point;
    typedef Eigen::Matrix<double, DimOut, 1> Value;
    typedef Eigen::Matrix<double, DimOut, DimIn> Gradient;
    typedef PolynomialBasisGen<DimIn, 0> PolynomialBasisGen;
    typedef RBFKernel_Bivariate<DimIn, _RBFKernel_Core> Kernel;
    typedef std::pair<Point, Value> Constraint;

    static_assert(Kernel::Decaying, "MLS requires its kernel to be decaying!");

    std::vector<Constraint> constraints;
    Kernel kernel;

    void clear_constraints() {
        constraints.clear();
    }
    void add_constraint(const Point& point, const Value& value) {
        constraints.push_back(Constraint(point, value));
    }
    Value operator()(const Point& point) const {
        Eigen::Matrix<double, 1, DimOut> b;
        double A = 0;
        b.setZero();
        for (size_t i = 0; i < constraints.size(); ++i) {
            const Point& point_i = constraints[i].first;
            const Value& value_i = constraints[i].second;
            double w_i = kernel(point, point_i);
            if (w_i == std::numeric_limits<double>::infinity())
                return value_i;
            w_i = w_i * w_i;
            PolynomialBasisGen::Basis basis_i = PolynomialBasisGen::basis(point_i);
            A += w_i * basis_i * basis_i.transpose();
            b += w_i * basis_i * value_i.transpose();
        }
        Eigen::Matrix<double, 1, DimOut> c = b / A;
        return c.transpose() * PolynomialBasisGen::basis(point);
    }
};

}


#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/multiprecision/cpp_int.hpp>
namespace mp = boost::multiprecision;
typedef mp::number<mp::cpp_dec_float<0>> cdouble;
typedef mp::cpp_int cint;


/*
Copyright 2017-2018 Lars Pastewka, Andreas Greiner

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.

--------
| OPT1 |
--------

This is an implementation of the D2Q9 Lattice Boltzmann lattice in the simple
relaxation time approximation. The code reports the amplitude of a decaying
shear wave which can be used to measure viscosity.

The present implementation contains was optimized with respect to opt1.
The optimization is identical to the opt1 Python code.

This code requires pybind11, Eigen and boost. Compile with
c++ -O3 -Wall -std=c++11 -I/usr/local/Cellar/pybind11/2.2.3/include -I/usr/local/Cellar/eigen/3.3.5/include/eigen3 shear_wave_opt0.cpp -o shear_wave_opt0
*/

#include <boost/range/combine.hpp>
#include <iostream>
#include <vector>

#include <Eigen/Dense>

class D2Q9 {
public:
    using Density_t = double;
    using Velocity_t = double;
    using Probability_t = Eigen::Matrix<double, 9, 1>;

    using ChannelVelocity_t = Eigen::Matrix<int, 2, 9>;
    using Weight_t = Eigen::Array<double, 9, 1>;

    using DensityField_t = std::vector<Density_t>;
    using VelocityField_t = std::vector<Velocity_t>;
    using ProbabilityField_t = std::vector<Probability_t>;

    const ChannelVelocity_t c_ci;

    static constexpr double w_0 = 4./9;
    static constexpr double w_1234 = 1./9;
    static constexpr double w_5678 = 1./36;

    D2Q9(int nx, int ny, double omega):
        c_ci((ChannelVelocity_t() << 0,  1,  0, -1,  0,  1, -1, -1,  1,
                                     0,  0,  1,  0, -1,  1,  1, -1, -1).finished()) {
        this->nx = nx;
        this->ny = ny;
        this->omega = omega;
    }

    Probability_t equilibrium(double rho, Velocity_t ux, Velocity_t uy) {
        double cu5 = ux + uy;
        double cu6 = -ux + uy;
        double cu7 = -ux - uy;
        double cu8 = ux - uy;
        double uu = ux*ux + uy*uy;
        return (Probability_t() <<
            w_0*rho*(1 - 3./2*uu),
            w_1234*rho*(1 + 3*ux + 9./2*ux*ux - 3./2*uu),
            w_1234*rho*(1 + 3*uy + 9./2*uy*uy - 3./2*uu),
            w_1234*rho*(1 - 3*ux + 9./2*ux*ux - 3./2*uu),
            w_1234*rho*(1 - 3*uy + 9./2*uy*uy - 3./2*uu),
            w_5678*rho*(1 + 3*cu5 + 9./2*cu5*cu5 - 3./2*uu),
            w_5678*rho*(1 + 3*cu6 + 9./2*cu6*cu6 - 3./2*uu),
            w_5678*rho*(1 + 3*cu7 + 9./2*cu7*cu7 - 3./2*uu),
            w_5678*rho*(1 + 3*cu8 + 9./2*cu8*cu8 - 3./2*uu)).finished();
    }

    void equilibrium(DensityField_t &rho_kl,
                     VelocityField_t &ux_kl, VelocityField_t &uy_kl,
                     ProbabilityField_t &f_kli) {
        for (auto && tup: boost::combine(rho_kl, ux_kl, uy_kl, f_kli)) {
            auto && rho = tup.get<0>();
            auto && ux = tup.get<1>();
            auto && uy = tup.get<2>();
            auto && f_i = tup.get<3>();
            f_i = this->equilibrium(rho, ux, uy);
        }
    }

    void equilibrium(double rho, Velocity_t &ux, Velocity_t &uy,
                     ProbabilityField_t &f_kli) {
        for (auto && f_i: f_kli) {
            f_i = this->equilibrium(rho, ux, uy);
        }
    }

    void collide(ProbabilityField_t &f_kli) {
        for (auto && f_i: f_kli) {
            double rho = f_i.sum();
            Velocity_t ux = (f_i(1) - f_i(3) + f_i(5) - f_i(6) - f_i(7) + f_i(8))/rho;
            Velocity_t uy = (f_i(2) - f_i(4) + f_i(5) + f_i(6) - f_i(7) - f_i(8))/rho;
            f_i += this->omega*(equilibrium(rho, ux, uy) - f_i);
        }
    }

    void stream(ProbabilityField_t &f_kli) {
        ProbabilityField_t g_kli(f_kli.size());
        auto f_i = f_kli.begin();
        for (int l = 0; l < this->ny; ++l) {
            for (int k = 0; k < this->nx; ++k, ++f_i) {
                for (int i = 0; i < 9; i++) {
                    int k1 = k + this->c_ci(0, i);
                    while (k1 < 0) k1 += this->nx;
                    while (k1 >= this->nx) k1 -= this->nx;
                    int l1 = l + this->c_ci(1, i);
                    while (l1 < 0) l1 += this->ny;
                    while (l1 >= this->ny) l1 -= this->ny;
                    g_kli[l1*this->nx + k1](i) = (*f_i)(i);
                }
            }
        }
        f_kli = g_kli;
    }
private:
    int nx, ny;
    double omega;
};

int main(int argc, char *argv[])
{
    int nx = 300;
    int ny = 300;
    int nsteps = 1000;
    double omega = 0.3;
    D2Q9 lb(nx, ny, omega);
    D2Q9::ProbabilityField_t f_kli(nx*ny);

    auto f_i = f_kli.begin();
    for (int l = 0; l < ny; ++l) {
        for (int k = 0; k < nx; ++k, ++f_i) {
            D2Q9::Velocity_t uy = std::sin(2*M_PI/nx*k);
            *f_i = lb.equilibrium(1.0, 0.0, uy);
        }
    }

    for (int n = 0; n < nsteps; ++n) {
        lb.stream(f_kli);
        lb.collide(f_kli);

        double accum = 0.0;
        for (int k = 0; k < nx; ++k)
            accum += (lb.c_ci.cast<double>()*f_kli[ny/2*nx+k])(1)*std::sin(2*M_PI/nx*k);
        std::cout << accum*2/nx << std::endl;
    }
}


// Flatten.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"

#include <time.h>
#include <iostream>
#include <vector>
#include <fstream>
#include <optional>
#include <string>
#include <algorithm>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry> // For Quaternion

#include "FLAE.h"
#include "GARotorEstimator.h"
#include "ArunSVD.h"
#include "Horn.h"
#include "GAValkenburg.h"
#include "FA3R.h"
#include "Davenport.h"
#include "Quest.h"
#include "QuaternionDirect.h"

using namespace std;
using namespace Eigen;
using namespace std::placeholders;

Vector3d getRandomVector()
{
	auto e0 = (double)rand() / (double)RAND_MAX;
	auto e1 = (double)rand() / (double)RAND_MAX;
	auto e2 = (double)rand() / (double)RAND_MAX;

	auto v = Vector3d(e0, e1, e2);
	return v.normalized();
}

double getRandom()
{
	return (double)rand() / (double)RAND_MAX;
}

double WahbaError(const vector<Vector3d>& P, const vector<Vector3d>& Q, const Quaterniond& R)
{
	const size_t N = P.size();
	double error = 0;
	for (size_t i = 0; i < N; ++i)
	{
		error += (R._transformVector(P[i]) - Q[i]).squaredNorm();
	}
	return error;
}

double WahbaError(const vector<Vector3d>& P, const vector<Vector3d>& Q, const Matrix3d& R)
{
	const size_t N = P.size();
	double error = 0;
	for (size_t i = 0; i < N; ++i)
	{
		error += (R*P[i] - Q[i]).squaredNorm();
	}
	return error;
}

template<typename ResultType>
double benchmark(
	const vector<Vector3d>& P,
	const vector<Vector3d>& Q,
	const vector<double>& weights,
	function<ResultType(const vector<Vector3d>&, const vector<Vector3d>&, const vector<double>&)> estimator)
{
	ResultType result;
	double total = 0;
	for (int i = 0; i < 1000000; ++i) {
		clock_t time1 = clock();
		result = estimator(P, Q, weights);
		clock_t time2 = clock();
		total += time2 - time1;
	}
	return total;
}

class log_stream {
public:
	std::ofstream log;

	log_stream(const string& filename) : log(filename)
	{
		log.precision(15);
		std::cout.precision(15);
	}

	log_stream& operator << (ostream& (*pfun)(ostream&)) {
		pfun(log);
		pfun(std::cout);
		return *this;
	}
};

template <typename T>
log_stream& operator << (log_stream& st, T val) {
	st.log << val;
	std::cout << val;
	return st;
};

enum class DataPlanes { NONE, X, Y, Z };
class CommandLineParams {
public:
	string filename;
	DataPlanes dataPlane;
	size_t N;
	bool noise;
	optional<double> norm;
	optional<Vector3d> axis;
	optional<double> angle;

	CommandLineParams(int argc, char* argv[]) {
		set_defaults();
		for(size_t i = 1 ; i < argc ; ++i) {
			string param(argv[i]);
			string key, value;
			int pos = param.find('=', 0);
			if(pos > 0) {
				key = param.substr(0, pos);
				std::transform(key.begin(), key.end(), key.begin(), ::tolower);
				value = param.substr(pos+1, param.length()-pos-1);
				std::transform(value.begin(), value.end(), value.begin(), ::tolower);
			} else {
				key = param;
			}
			if(key == "--name") {
				filename = value;
			} else if(key == "--plane") {
				if(value=="x") dataPlane = DataPlanes::X;
				if(value=="y") dataPlane = DataPlanes::Y;
				if(value=="z") dataPlane = DataPlanes::Z;
			} else if(key == "--n") {
				 N = std::stoul(value);
			} else if(key == "--noise") {
				noise = value == "true";
			} else if(key == "--norm") {
				norm = std::stod(value);
			} else if(key == "--axis") {
				int prev = 0;
				pos = value.find(',', 0);
				double x = std::stod(value.substr(prev, pos));
				prev = pos;
				pos = value.find(',', pos+1);
				double y = std::stod(value.substr(prev+1, pos-prev-1));
				prev = pos;
				double z = std::stod(value.substr(prev+1, value.length()-prev-1));
				axis = Vector3d(x, y, z);
			} else if(key == "--angle") {
				angle = std::stod(value);
			}
		}
	}

	void set_defaults() {
		filename = "out.txt";
		dataPlane = DataPlanes::NONE;
		N = 1000;
		noise = true;
	}
};

void show_usage(log_stream& log, CommandLineParams& params)
{
	log << "Params: " << std::endl;
	log << "      "
		<< " --name=" << params.filename
		<< " --plane=" << (params.dataPlane == DataPlanes::X ? "x" : params.dataPlane == DataPlanes::Y ? "y" : params.dataPlane == DataPlanes::Z ? "z" : "[x|y|z]")
		<< " --n=" << params.N
		<< " --noise=" << (params.noise ? "true" : "false");
	if(params.norm.has_value())
		log << " --norm=" << params.norm.value_or(1);
	else
		log << " --norm=[double]";
	if(params.axis.has_value()) {
		auto axis = params.axis.value_or(Vector3d(0,0,1));
		log << " --axis=" << axis.x() << "," << axis.y() << "," << axis.z();
	}
	else
		log << " --axis=[double,double,double]";
	if(params.angle.has_value())
		log << " --angle=" << params.angle.value_or(EIGEN_PI);
	else
		log << " --angle=[double]";
	log << std::endl << std::endl;
}

int main(int argc, char* argv[])
{
	srand(static_cast<unsigned>(time(NULL)));

	CommandLineParams params(argc, argv);

	log_stream log(params.filename);

	show_usage(log, params);

	vector<Vector3d> pointsOriginal;
	vector<Vector3d> pointsTransformed;
	vector<double> weights;
	const size_t N = params.N;

	double totalNorms = 0;
	// Initialize the lines
	for(size_t i = 0 ; i < N ; ++i)
	{
		double norm = params.norm.value_or((rand() % 1000) + 1.0); // 1 to 1000 continuous
		Eigen::Vector3d v = getRandomVector() * norm;
		switch(params.dataPlane) {
			case DataPlanes::X: v[0] = 0; break;
			case DataPlanes::Y: v[1] = 0; break;
			case DataPlanes::Z: v[2] = 0; break;
			case DataPlanes::NONE:
			default: break;
		}
		pointsOriginal.push_back(v);
		weights.push_back(norm);
		totalNorms += norm;
	}
	for(size_t i = 0 ; i < N ; ++i)
	{
		weights[i] /= totalNorms;
	}

	Eigen::Vector3d axis = params.axis.value_or(getRandomVector());
	auto angle = params.angle.value_or(4 * EIGEN_PI * getRandom());
	Quaterniond Q;
	Q = AngleAxisd(angle, axis);

	for(size_t i = 0 ; i < N ; ++i)
	{
		if(params.noise)
			pointsTransformed.push_back((Q * AngleAxisd(2*EIGEN_PI * getRandom(), getRandomVector()))._transformVector(pointsOriginal[i]));
		else
			pointsTransformed.push_back(Q._transformVector(pointsOriginal[i]));
	}

	Quaterniond QI;
	QI.setIdentity();

	Quaterniond flaeQ = Flae(pointsOriginal, pointsTransformed, weights);
	Quaterniond flaeSymbolicQ = FlaeSymbolic(pointsOriginal, pointsTransformed, weights);
	Quaterniond flaeNewtonQ = FlaeNewton(pointsOriginal, pointsTransformed, weights);
	Matrix3d FA3EDoubleM = FA3R_double(pointsOriginal, pointsTransformed, weights);
	Matrix3d FA3EIntM = FA3R_int(pointsOriginal, pointsTransformed, weights);
	Quaterniond  GAQ = GARotorEstimator(pointsOriginal, pointsTransformed, weights);
	Quaterniond  LAQ = LARotorEstimator(pointsOriginal, pointsTransformed, weights);
	Matrix3d  svdM = SVDMcAdams(pointsOriginal, pointsTransformed, weights);
	Matrix3d  svdE = SVDEigen(pointsOriginal, pointsTransformed, weights);
	Quaterniond hornQ = Horn(pointsOriginal, pointsTransformed, weights);
	Quaterniond GAValkenburgQ = GAValkenburg(pointsOriginal, pointsTransformed, weights);
	Quaterniond davenportQ = Davenport(pointsOriginal, pointsTransformed, weights);
	Quaterniond questQ = Quest(pointsOriginal, pointsTransformed, weights);
	Matrix3d foamQ = Foam(pointsOriginal, pointsTransformed, weights);
	Quaterniond qDirect = QuaternionDirect(pointsOriginal, pointsTransformed, weights);

	double errorGroundTruth = WahbaError(pointsOriginal, pointsTransformed, Q);
	double errorFlae = WahbaError(pointsOriginal, pointsTransformed, flaeQ);
	double errorFlaeSymbolic = WahbaError(pointsOriginal, pointsTransformed, flaeSymbolicQ);
	double errorFlaeNewton = WahbaError(pointsOriginal, pointsTransformed, flaeNewtonQ);
	double errorFA3RDouble = WahbaError(pointsOriginal, pointsTransformed, FA3EDoubleM);
	double errorFA3RInt = WahbaError(pointsOriginal, pointsTransformed, FA3EIntM);
	double errorGA = WahbaError(pointsOriginal, pointsTransformed, GAQ);
	double errorLA = WahbaError(pointsOriginal, pointsTransformed, LAQ);
	double errorSVD = WahbaError(pointsOriginal, pointsTransformed, svdM);
	double errorSVDE = WahbaError(pointsOriginal, pointsTransformed, svdE);
	double errorHorn = WahbaError(pointsOriginal, pointsTransformed, hornQ);
	double errorGAValkenburg = WahbaError(pointsOriginal, pointsTransformed, GAValkenburgQ);
	double errorDavenport = WahbaError(pointsOriginal, pointsTransformed, davenportQ);
	double errorQuest = WahbaError(pointsOriginal, pointsTransformed, questQ);
	double errorFoam = WahbaError(pointsOriginal, pointsTransformed, foamQ);
	double errorQDirect = WahbaError(pointsOriginal, pointsTransformed, qDirect);

	log << "Axis                                      : " << axis.x() << ", " << axis.y() << ", " << axis.z() << std::endl;
	log << "Angle                                     : " << angle << std::endl;

	log << "Comparisons: " << std::endl << std::endl;
	//log << "Ground Truth error                        " << errorGroundTruth << endl;
	log << "FLAE error                                " << errorFlae << endl;
	log << "FLAE Symbolic error                       " << errorFlaeSymbolic << endl;
	log << "FLAE Newton error                         " << errorFlaeNewton << endl;
	log << "FA3R Double error                         " << errorFA3RDouble << endl;
	log << "FA3R Int error                            " << errorFA3RInt << endl;
	log << "GA Rotor Estimator error                  " << errorGA << endl;
	log << "LA Rotor Estimator error                  " << errorLA << endl;
	log << "Davenport error                           " << errorDavenport << endl;
	log << "Quest error                               " << errorQuest << endl;
	log << "Foam error                                " << errorFoam << endl;
	log << "GA Valkenburg error                       " << errorGAValkenburg << endl;
	log << "SVD McAdams error                         " << errorSVD << endl;
	log << "SVD error                                 " << errorSVDE << endl;
	log << "Horn error                                " << errorHorn << endl;
	log << "Quaternion Direct error                   " << errorQDirect << endl;

	double total;
	total = benchmark<Quaterniond>(pointsOriginal, pointsTransformed, weights, Flae);
	log << "Exec time FLAE:                           "<< total / double(CLOCKS_PER_SEC) << " sec." << endl;

	total = benchmark<Quaterniond>(pointsOriginal, pointsTransformed, weights, FlaeSymbolic);
	log << "Exec time FLAE Symbolic:                  " << total / double(CLOCKS_PER_SEC) << " sec." << endl;

	total = benchmark<Quaterniond>(pointsOriginal, pointsTransformed, weights, FlaeNewton);
	log << "Exec time FLAE Newton:                    " << total / double(CLOCKS_PER_SEC) << " sec." << endl;

	total = benchmark<Matrix3d>(pointsOriginal, pointsTransformed, weights, FA3R_double);
	log << "Exec time FA3R Double:                    " << total / double(CLOCKS_PER_SEC) << " sec." << endl;

	total = benchmark<Matrix3d>(pointsOriginal, pointsTransformed, weights, FA3R_int);
	log << "Exec time FA3R Int:                       " << total / double(CLOCKS_PER_SEC) << " sec." << endl;

	total = benchmark<Quaterniond>(pointsOriginal, pointsTransformed, weights, LARotorEstimator);
	log << "Exec time LARotorEstimator:               " << total / double(CLOCKS_PER_SEC)  << " sec." << endl;

	total = benchmark<Quaterniond>(pointsOriginal, pointsTransformed, weights, Davenport);
	log << "Exec time Davenport Q-Method:             " << total / double(CLOCKS_PER_SEC) << " sec." << endl;

	total = benchmark<Quaterniond>(pointsOriginal, pointsTransformed, weights, Quest);
	log << "Exec time QUEST:                          " << total / double(CLOCKS_PER_SEC) << " sec." << endl;

	total = benchmark<Matrix3d>(pointsOriginal, pointsTransformed, weights, Foam);
	log << "Exec time FOAM:                           " << total / double(CLOCKS_PER_SEC) << " sec." << endl;

	total = benchmark<Quaterniond>(pointsOriginal, pointsTransformed, weights, GAValkenburg);
	log << "Exec time GA Valkenburg:                  " << total / double(CLOCKS_PER_SEC) << " sec." << endl;

	total = benchmark<Matrix3d>(pointsOriginal, pointsTransformed, weights, SVDMcAdams);
	log << "Exec time SVD McAdams:                    " << total / double(CLOCKS_PER_SEC) << " sec." << endl;

	total = benchmark<Matrix3d>(pointsOriginal, pointsTransformed, weights, SVDEigen);
	log << "Exec time SVD:                            " << total / double(CLOCKS_PER_SEC) << " sec." << endl;

	total = benchmark<Quaterniond>(pointsOriginal, pointsTransformed, weights, Horn);
	log << "Exec time Horn:                           " << total / double(CLOCKS_PER_SEC) << " sec." << endl;

	log << "FINISHED..." << endl;
	return 0;
}


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    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.
 *      - Neither the name of the Delft University of Technology 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.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      120530    M.I. Ganeff       Code created.
 *      121004    M.I. Ganeff       Input parameter types and variable-naming updated.
 *      121018    M.I. Ganeff       Added computeSphereOfInfluence().
 *      121123    D. Dirkx          Added computeSphereOfInfluence() function taking mass ratios;
 *                                  updated implementation of computeSphereOfInfluence() taking
 *                                  masses.
 *      130225    D. Dirkx          Added isOrbitRetrograde(...) functions taking inclination and
 *                                  Kepler vector.
 *      130227    D. Dirkx          Set isRetrograde at initialization to 0.
 *      130301    R.C.A. Boon       Minor textual changes, changed mathematics::PI to
 *      130305    R.C.A. Boon       Replaced Eigen::VectorXd by basic_mathematics::Vector6d
 *                                  mathematical_constants::PI.
 *      131212    S. Billemont      Fixed pass-by-reference error in isOrbitRetrograde()-function.
 *
 *    References
 *      Montebruck O, Gill E. Satellite Orbits, Corrected Third Printing, Springer, 2005.
 *      Bate R. Fundamentals of Astrodynamics, Courier Dover Publications, 1971.
 *
 *    Notes
 *
 */

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

#include "Tudat/Astrodynamics/BasicAstrodynamics/missionGeometry.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/stateVectorIndices.h"

namespace tudat
{

namespace mission_geometry
{

//! Compute whether an orbit is retrograde based on inclination.
bool isOrbitRetrograde( const double inclination )
{
    bool isRetrograde = false;

    // Check which range inclination is in and return value accordingly.
    if ( inclination < 0.0 || inclination > mathematical_constants::PI )
    {
        throw std::runtime_error(
                    "The inclination is in the wrong range when determining retrogradeness" );
    }
    else if ( inclination <= mathematical_constants::PI / 2.0 )
    {
        isRetrograde = false;
    }
    else if ( inclination > mathematical_constants::PI / 2.0 )
    {
        isRetrograde = true;
    }

    return isRetrograde;
}

//! Compute whether an orbit is retrograde based on Keplerian state.
bool isOrbitRetrograde( const basic_mathematics::Vector6d& keplerElements )
{
    // Get inclination from vector and call overloaded function.
    return isOrbitRetrograde(
                keplerElements(
                    orbital_element_conversions::inclinationIndex ) );
}

//! Compute the shadow function.
double computeShadowFunction( const Eigen::Vector3d& occultedBodyPosition,
                              const double occultedBodyRadius,
                              const Eigen::Vector3d& occultingBodyPosition,
                              const double occultingBodyRadius,
                              const Eigen::Vector3d& satellitePosition )
{
    // Calculate coordinates of the spacecraft with respect to the occulting body.
    const Eigen::Vector3d satellitePositionRelativeToOccultingBody = satellitePosition
            - occultingBodyPosition;

    // Calculate apparent radius of occulted body.
    const double occultedBodyApparentRadius
            = std::asin( occultedBodyRadius
                         / ( occultedBodyPosition - satellitePosition ).norm( ) );

    // Calculate apparent radius of occulting body.
    const double occultingBodyApparentRadius =
            std::asin( occultingBodyRadius / satellitePositionRelativeToOccultingBody.norm( ) );

    // Calculate apparent separation of the center of both bodies.
    const double apparentSeparationPartOne = -satellitePositionRelativeToOccultingBody.transpose( )
            * ( occultedBodyPosition - satellitePosition );
    const double apparentSeparationPartTwo = satellitePositionRelativeToOccultingBody.norm( )
            * ( occultedBodyPosition - satellitePosition ).norm( );
    const double apparentSeparation = std::acos( apparentSeparationPartOne
                                                 / apparentSeparationPartTwo );

    // Set initial value for the shadow function.
    double shadowFunction = 1.0;

    // Check if partial occultation takes place
    if ( std::fabs( occultedBodyApparentRadius - occultingBodyApparentRadius ) < apparentSeparation
         && apparentSeparation < occultedBodyApparentRadius + occultingBodyApparentRadius )
    {
        // Pre-compute values for optimal computations.
        const double apparentSeparationSquared = apparentSeparation * apparentSeparation;
        const double occultedBodyApparentRadiusSquared = occultedBodyApparentRadius
                * occultedBodyApparentRadius;
        const double occultingBodyApparentRadiusSquared = occultingBodyApparentRadius
                * occultingBodyApparentRadius;

        // Partial occultation takes place, calculate the occulted area.
        const double occultedAreaPartOne
                = ( apparentSeparationSquared + occultedBodyApparentRadiusSquared
                    - occultingBodyApparentRadiusSquared ) / ( 2.0 * apparentSeparation );
        const double occultedAreaPartTwo = std::sqrt( occultedBodyApparentRadiusSquared
                                                      - occultedAreaPartOne * occultedAreaPartOne );
        const double occultedArea = occultedBodyApparentRadiusSquared
                * std::acos( occultedAreaPartOne / occultedBodyApparentRadius )
                + occultingBodyApparentRadiusSquared
                * std::acos( ( apparentSeparation - occultedAreaPartOne )
                             / occultingBodyApparentRadius )
                - apparentSeparation * occultedAreaPartTwo;
        shadowFunction = 1.0 - occultedArea / ( mathematical_constants::PI *
                                                occultedBodyApparentRadiusSquared );
    }

    else
    {
        // Full or no occultation takes place.
        // Check for type of occultation.
        if ( apparentSeparation < occultingBodyApparentRadius - occultedBodyApparentRadius &&
             occultedBodyApparentRadius < occultingBodyApparentRadius )
        {
            // Total occultation.
            shadowFunction = 0.0;
        }

        else if ( apparentSeparation < occultedBodyApparentRadius - occultingBodyApparentRadius &&
                  occultedBodyApparentRadius > occultingBodyApparentRadius )
        {
            // Maximum partial occultation.
            shadowFunction = 0.0;
        }

        else if ( occultedBodyApparentRadius + occultingBodyApparentRadius <= apparentSeparation )
        {
            // No occultation
            shadowFunction = 1.0;
        }
    }

    // Return the shadow function
    return shadowFunction;
}

double computeSphereOfInfluence( const double distanceToCentralBody,
                                 const double ratioOfOrbitingToCentralBodyMass )
{
    // Return the radius of the sphere of influence.
    return distanceToCentralBody * std::pow( ratioOfOrbitingToCentralBodyMass, 0.4 );
}

//! Compute the sphere of influence.
double computeSphereOfInfluence( const double distanceToCentralBody,
                                 const double massOrbitingBody,
                                 const double massCentralBody )
{
    // Return the radius of the sphere of influence.
    return computeSphereOfInfluence( distanceToCentralBody, massOrbitingBody / massCentralBody );
}

} // namespace mission_geometry

} // namespace tudat


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestRadixSort
#include <boost/test/unit_test.hpp>

#include <boost/compute/system.hpp>
#include <boost/compute/algorithm/is_sorted.hpp>
#include <boost/compute/algorithm/detail/radix_sort.hpp>
#include <boost/compute/container/vector.hpp>

#include "check_macros.hpp"
#include "context_setup.hpp"

namespace bc = boost::compute;

BOOST_AUTO_TEST_CASE(sort_char_vector)
{
    using boost::compute::char_;

    char_ data[] = { 'c', 'a', '0', '7', 'B', 'F', '\0', '$' };
    boost::compute::vector<char_> vector(data, data + 8, queue);
    BOOST_CHECK_EQUAL(vector.size(), size_t(8));
    BOOST_CHECK(boost::compute::is_sorted(vector.begin(), vector.end(), queue) == false);

    boost::compute::detail::radix_sort(vector.begin(), vector.end(), queue);
    BOOST_CHECK(boost::compute::is_sorted(vector.begin(), vector.end(), queue) == true);
    CHECK_RANGE_EQUAL(char_, 8, vector, ('\0', '$', '0', '7', 'B', 'F', 'a', 'c'));
}

BOOST_AUTO_TEST_CASE(sort_uchar_vector)
{
    using boost::compute::uchar_;

    uchar_ data[] = { 0x12, 0x00, 0xFF, 0xB4, 0x80, 0x32, 0x64, 0xA2 };
    boost::compute::vector<uchar_> vector(data, data + 8, queue);
    BOOST_CHECK_EQUAL(vector.size(), size_t(8));
    BOOST_CHECK(boost::compute::is_sorted(vector.begin(), vector.end(), queue) == false);

    boost::compute::detail::radix_sort(vector.begin(), vector.end(), queue);
    BOOST_CHECK(boost::compute::is_sorted(vector.begin(), vector.end(), queue) == true);
    CHECK_RANGE_EQUAL(uchar_, 8, vector, (0x00, 0x12, 0x32, 0x64, 0x80, 0xA2, 0xB4, 0xFF));
}

BOOST_AUTO_TEST_CASE(sort_short_vector)
{
    using boost::compute::short_;

    short_ data[] = { -4, 152, -94, 963, 31002, -456, 0, -2113 };
    boost::compute::vector<short_> vector(data, data + 8, queue);
    BOOST_CHECK_EQUAL(vector.size(), size_t(8));
    BOOST_CHECK(boost::compute::is_sorted(vector.begin(), vector.end(), queue) == false);

    boost::compute::detail::radix_sort(vector.begin(), vector.end(), queue);
    BOOST_CHECK(boost::compute::is_sorted(vector.begin(), vector.end(), queue) == true);
    CHECK_RANGE_EQUAL(short_, 8, vector, (-2113, -456, -94, -4, 0, 152, 963, 31002));
}

BOOST_AUTO_TEST_CASE(sort_ushort_vector)
{
    using boost::compute::ushort_;

    ushort_ data[] = { 4, 152, 94, 963, 63202, 34560, 0, 2113 };
    boost::compute::vector<ushort_> vector(data, data + 8, queue);
    BOOST_CHECK_EQUAL(vector.size(), size_t(8));
    BOOST_CHECK(boost::compute::is_sorted(vector.begin(), vector.end(), queue) == false);

    boost::compute::detail::radix_sort(vector.begin(), vector.end(), queue);
    BOOST_CHECK(boost::compute::is_sorted(vector.begin(), vector.end(), queue) == true);
    CHECK_RANGE_EQUAL(ushort_, 8, vector, (0, 4, 94, 152, 963, 2113, 34560, 63202));
}

BOOST_AUTO_TEST_CASE(sort_int_vector)
{
    int data[] = { -4, 152, -5000, 963, 75321, -456, 0, 1112 };
    boost::compute::vector<int> vector(data, data + 8, queue);
    BOOST_CHECK_EQUAL(vector.size(), size_t(8));
    BOOST_CHECK(boost::compute::is_sorted(vector.begin(), vector.end(), queue) == false);

    boost::compute::detail::radix_sort(vector.begin(), vector.end(), queue);
    BOOST_CHECK(boost::compute::is_sorted(vector.begin(), vector.end(), queue) == true);
    CHECK_RANGE_EQUAL(int, 8, vector, (-5000, -456, -4, 0, 152, 963, 1112, 75321));
}

BOOST_AUTO_TEST_CASE(sort_uint_vector)
{
    using boost::compute::uint_;

    uint_ data[] = { 500, 1988, 123456, 562, 0, 4000000, 9852, 102030 };
    boost::compute::vector<uint_> vector(data, data + 8, queue);
    BOOST_CHECK_EQUAL(vector.size(), size_t(8));
    BOOST_CHECK(boost::compute::is_sorted(vector.begin(), vector.end(), queue) == false);

    boost::compute::detail::radix_sort(vector.begin(), vector.end(), queue);
    BOOST_CHECK(boost::compute::is_sorted(vector.begin(), vector.end(), queue) == true);
    CHECK_RANGE_EQUAL(uint_, 8, vector, (0, 500, 562, 1988, 9852, 102030, 123456, 4000000));
}

BOOST_AUTO_TEST_CASE(sort_long_vector)
{
    using boost::compute::long_;

    long_ data[] = { 500, 1988, 123456, 562, 0, 4000000, 9852, 102030 };
    boost::compute::vector<long_> vector(data, data + 8, queue);
    BOOST_CHECK_EQUAL(vector.size(), size_t(8));
    BOOST_CHECK(boost::compute::is_sorted(vector.begin(), vector.end(), queue) == false);

    boost::compute::detail::radix_sort(vector.begin(), vector.end(), queue);
    BOOST_CHECK(boost::compute::is_sorted(vector.begin(), vector.end(), queue) == true);
    CHECK_RANGE_EQUAL(long_, 8, vector, (0, 500, 562, 1988, 9852, 102030, 123456, 4000000));
}

BOOST_AUTO_TEST_CASE(sort_ulong_vector)
{
    using boost::compute::ulong_;

    ulong_ data[] = { 500, 1988, 123456, 562, 0, 4000000, 9852, 102030 };
    boost::compute::vector<ulong_> vector(data, data + 8, queue);
    BOOST_CHECK_EQUAL(vector.size(), size_t(8));
    BOOST_CHECK(boost::compute::is_sorted(vector.begin(), vector.end(), queue) == false);

    boost::compute::detail::radix_sort(vector.begin(), vector.end(), queue);
    BOOST_CHECK(boost::compute::is_sorted(vector.begin(), vector.end(), queue) == true);
    CHECK_RANGE_EQUAL(ulong_, 8, vector, (0, 500, 562, 1988, 9852, 102030, 123456, 4000000));
}

BOOST_AUTO_TEST_CASE(sort_float_vector)
{
    float data[] = { -6023.0f, 152.5f, -63.0f, 1234567.0f, 11.2f,
                     -5000.1f, 0.0f, 14.0f, -8.25f, -0.0f };
    boost::compute::vector<float> vector(data, data + 10, queue);
    BOOST_CHECK_EQUAL(vector.size(), size_t(10));
    BOOST_CHECK(boost::compute::is_sorted(vector.begin(), vector.end(), queue) == false);

    boost::compute::detail::radix_sort(vector.begin(), vector.end(), queue);
    BOOST_CHECK(boost::compute::is_sorted(vector.begin(), vector.end(), queue) == true);
    CHECK_RANGE_EQUAL(
        float, 10, vector,
        (-6023.0f, -5000.1f, -63.0f, -8.25f, -0.0f, 0.0f, 11.2f, 14.0f, 152.5f, 1234567.0f)
    );

    // copy data, sort, and check again (to check program caching)
    boost::compute::copy(data, data + 10, vector.begin(), queue);
    boost::compute::detail::radix_sort(vector.begin(), vector.end(), queue);
    BOOST_CHECK(boost::compute::is_sorted(vector.begin(), vector.end(), queue) == true);
    CHECK_RANGE_EQUAL(
        float, 10, vector,
        (-6023.0f, -5000.1f, -63.0f, -8.25f, -0.0f, 0.0f, 11.2f, 14.0f, 152.5f, 1234567.0f)
    );
}

BOOST_AUTO_TEST_CASE(sort_double_vector)
{
    if(!device.supports_extension("cl_khr_fp64")){
        std::cout << "skipping test: device does not support double" << std::endl;
        return;
    }

    double data[] = { -6023.0, 152.5, -63.0, 1234567.0, 11.2,
                     -5000.1, 0.0, 14.0, -8.25, -0.0 };
    boost::compute::vector<double> vector(data, data + 10, queue);
    BOOST_CHECK_EQUAL(vector.size(), size_t(10));
    BOOST_CHECK(boost::compute::is_sorted(vector.begin(), vector.end(), queue) == false);

    boost::compute::detail::radix_sort(vector.begin(), vector.end(), queue);
    BOOST_CHECK(boost::compute::is_sorted(vector.begin(), vector.end(), queue) == true);
    CHECK_RANGE_EQUAL(
        double, 10, vector,
        (-6023.0, -5000.1, -63.0, -8.25, -0.0, 0.0, 11.2, 14.0, 152.5, 1234567.0)
    );
}

BOOST_AUTO_TEST_SUITE_END()