#include <QtTest>
#include <Eigen/Dense>
#include <vector>
#include <iostream>
#include "permutationtree.h"
#include "multyrowarray.h"
#include "matrices.h"

//Utility class used to compare generated permutations
struct classcomp {
    //assuming that lhs and rhs have the same size
    bool operator() (const std::vector<int>& lhs, const std::vector<int>& rhs) const{
        int n = lhs.size();

        for(int i = 0; i < n; i++){
            if(lhs[i] < rhs[i])
                return true;
            else if(lhs[i] > rhs[i])
                return false;
        }
        return false;
    }
};

class TestPermutationTree : public QObject
{
    Q_OBJECT

public:
    TestPermutationTree();
    ~TestPermutationTree();

private:
    Eigen::MatrixXd X1;
    Eigen::MatrixXd X2;
    MultyRowArray a1;
    MultyRowArray a2;
    void testLAlgorithmTree(Eigen::MatrixXd& X, MultyRowArray& a);
    void testAllSignFlippingsTree(Eigen::MatrixXd& X, MultyRowArray& a);
    void testAllSignFlippingsAndPermutation(Eigen::MatrixXd& X, MultyRowArray& a);
    void testRandomSwapping(Eigen::MatrixXd& X, MultyRowArray& a, int n);
    void testRandomSignFlipping(Eigen::MatrixXd& X, MultyRowArray& a, int n);
    void computeShuffling(PermutationTree &t, std::set<std::vector<int>, classcomp> &generatedPermutations, int &currentPermutation);

private slots:
    void initTestCase();
    void allSignFlippingsAndPermutationsTest1();
    void allSignFlippingsAndPermutationsTest2();
    //Binary Flippings only need two tests since they don't care about
    //repetitions in the design matrix
    void allSignFlippingsTest1();
    void allSignFlippingsTest2();
    void lAlgorithmtest1();
    void lAlgorithmtest2();
    void lAlgorithmtest3();
    void lAlgorithmtest4();
    void cleanUpTestCase();
};

//--------------PRIVATE METHODS------------------

void TestPermutationTree::testLAlgorithmTree(Eigen::MatrixXd& X, MultyRowArray& a) {

    PermutationTree t(a);
    t.initializeThreeColsArray();
    t.initializeBinaryCounters();
    int numPermutations = t.calculatePermutations(X, true, false);
    std::cout << "Number of possible shufflings:" << numPermutations << std::endl;
    int currentPermutation = 0;
    std::set<std::vector<int>, classcomp> generatedPermutations;
    t.reverseLAlgorithm();
    while(true){
        //std::vector<int> currentPerm = t.getPermutationVector();
        std::vector<int> currentPerm = t.getSignVector();
        if(generatedPermutations.count(currentPerm)){
            std::cout << "Warning! Permutation:" << std::endl;
            for(int a: currentPerm)
                std::cout << a << " ";
            std::cout << std::endl;
            std::cout << " already generated!" << std::endl;
            //the test fails if we generate twice the same permutation
            QCOMPARE(false, true);
        }
        else
            generatedPermutations.insert(currentPerm);
        Eigen::MatrixXd P = buildShufflingMatrix(currentPerm);
        std::cout << currentPermutation << " permutation vector is: " << std::endl;
        for(int a: currentPerm)
            std::cout << a << " ";
        std::cout << std::endl;
        std::cout << currentPermutation++ << " permutation matrix is: " << std::endl;
        std::cout << P << std::endl;
        if(!t.LAlgorithm())
            break;
    }
    QCOMPARE(currentPermutation, numPermutations);

    std::cout << "Finished permutations! Restoring the tree to its original state" << std::endl;
    t.resetTreePermutationState();
    //std::vector<int> currentPerm = t.getPermutationVector();
    std::vector<int> currentPerm = t.getSignVector();
    std::cout << "Original permutation is: " << std::endl;
    for(int a: currentPerm)
        std::cout << a << " ";
    std::cout << std::endl;
    std::cout << "Finished!" << std::endl;
}

void TestPermutationTree::testAllSignFlippingsTree(Eigen::MatrixXd& X, MultyRowArray& a) {

    PermutationTree t(a);
    t.initializeThreeColsArray();
    t.initializeBinaryCounters();
    int numPermutations = t.calculatePermutations(X, false, true);
    std::cout << "Number of possible sign flipping:" << numPermutations << std::endl;
    int currentPermutation = 0;
    std::set<std::vector<int>, classcomp> generatedPermutations;
    while(true){
        //std::vector<int> currentPerm = t.getPermutationVector();
        std::vector<int> currentPerm = t.getSignVector();
        if(generatedPermutations.count(currentPerm)){
            std::cout << "Warning! Sign flipping:" << std::endl;
            for(int a: currentPerm)
                std::cout << a << " ";
            std::cout << std::endl;
            std::cout << " already generated!" << std::endl;
            //the test fails if we generate twice the same permutation
            QCOMPARE(false, true);
        }
        else
            generatedPermutations.insert(currentPerm);
        Eigen::MatrixXd P = buildShufflingMatrix(currentPerm);
        std::cout << currentPermutation << " sign flipping vector is: " << std::endl;
        for(int a: currentPerm)
            std::cout << a << " ";
        std::cout << std::endl;
        std::cout << currentPermutation++ << " sign flipping matrix is: " << std::endl;
        std::cout << P << std::endl;
        if(!t.signFlipping())
            break;
    }
    QCOMPARE(currentPermutation, numPermutations);
    std::cout << "Finished sign flippings! Restoring the tree to its original state" << std::endl;
    t.resetTreeSignState();
    std::vector<int> currentPerm = t.getSignVector();
    std::cout << "Original sign flipping is: " << std::endl;
    for(int a: currentPerm)
        std::cout << a << " ";
    std::cout << std::endl;
    std::cout << "Finished!" << std::endl;

}

void TestPermutationTree::computeShuffling(PermutationTree& t, std::set<std::vector<int>, classcomp>& generatedPermutations, int& currentPermutation)
{
    std::vector<int> currentPerm = t.getSignVector();
    if(generatedPermutations.count(currentPerm)){
        std::cout << "Warning! Shuffling:" << std::endl;
        for(int a: currentPerm)
            std::cout << a << " ";
        std::cout << std::endl;
        std::cout << " already generated!" << std::endl;
        //the test fails if we generate twice the same permutation
        QCOMPARE(false, true);
    }
    else
        generatedPermutations.insert(currentPerm);
    Eigen::MatrixXd P = buildShufflingMatrix(currentPerm);
    std::cout << currentPermutation << " shuffling is: " << std::endl;
    for(int a: currentPerm)
        std::cout << a << " ";
    std::cout << std::endl;
    std::cout << currentPermutation++ << " shuffling matrix is: " << std::endl;
    std::cout << P << std::endl;
}

void TestPermutationTree::testAllSignFlippingsAndPermutation(Eigen::MatrixXd& X, MultyRowArray &a){
    PermutationTree t(a);
    t.initializeThreeColsArray();
    t.initializeBinaryCounters();
    int numPermutations = t.calculatePermutations(X, true, true);
    std::cout << "Number of possible shufflings:" << numPermutations << std::endl;
    int currentPermutation = 0;
    std::set<std::vector<int>, classcomp> generatedPermutations;
    t.reverseLAlgorithm();
    computeShuffling(t, generatedPermutations, currentPermutation);
    while(t.signFlipping()){
        computeShuffling(t, generatedPermutations, currentPermutation);
    }
    t.resetTreeSignState();
    while(t.LAlgorithm()){
        t.resetTreeSignState();
        computeShuffling(t, generatedPermutations, currentPermutation);
        while(t.signFlipping()){
            computeShuffling(t, generatedPermutations, currentPermutation);
        }
    }
    QCOMPARE(currentPermutation, numPermutations);
    std::cout << "Finished all shufflings! Restoring the tree to its original state" << std::endl;
    t.resetTreeSignState();
    t.resetTreePermutationState();
    std::vector<int> currentPerm = t.getSignVector();
    std::cout << "Original shuffling is: " << std::endl;
    for(int a: currentPerm)
        std::cout << a << " ";
    std::cout << std::endl;
    std::cout << "Finished!" << std::endl;
}

void TestPermutationTree::testRandomSwapping(Eigen::MatrixXd& X, MultyRowArray &a, int n){
    PermutationTree t(a);
    t.initializeThreeColsArray();
    t.initializeBinaryCounters();
    int numPermutations = t.calculatePermutations(X, true, false);
    std::cout << "Number of possible permutations:" << numPermutations << std::endl;
    std::vector<int> originalPerm = t.getSignVector();
    Eigen::MatrixXd O = buildShufflingMatrix(originalPerm);
    std::cout << " Original permutation is: " << std::endl;
    for(int a: originalPerm)
        std::cout << a << " ";
    std::cout << std::endl;
    std::cout << "Original shuffling matrix is: " << std::endl;
    std::cout << O << std::endl;
    std::cout << std::endl;
    for(int currentPermutation = 1; currentPermutation <= n; currentPermutation++){
        t.randomShuffle();
        std::vector<int> currentPerm = t.getSignVector();
        Eigen::MatrixXd P = buildShufflingMatrix(currentPerm);
        std::cout << currentPermutation << " Shuffling vector is: " << std::endl;
        for(int a: currentPerm)
            std::cout << a << " ";
        std::cout << std::endl;
        std::cout << currentPermutation << " shuffling matrix is: " << std::endl;
        std::cout << P << std::endl;
    }

    std::cout << "Finished required random shufflings! Restoring the tree to its original state" << std::endl;
    t.resetTreePermutationState();
    std::vector<int> currentPerm = t.getSignVector();
    std::cout << "Original permutation is: " << std::endl;
    for(int a: currentPerm)
        std::cout << a << " ";
    std::cout << std::endl;
    std::cout << "Finished!" << std::endl;
}

void TestPermutationTree::testRandomSignFlipping(Eigen::MatrixXd& X, MultyRowArray &a, int n){
    PermutationTree t(a);
    t.initializeThreeColsArray();
    t.initializeBinaryCounters();
    int numPermutations = t.calculatePermutations(X, false, true);
    std::cout << "Number of possible sign flippings:" << numPermutations << std::endl;
    for(int currentPermutation = 1; currentPermutation <= n; currentPermutation++){
        std::vector<int> currentPerm = t.getSignVector();
        Eigen::MatrixXd P = buildShufflingMatrix(currentPerm);
        std::cout << currentPermutation << " sign flipping vector is: " << std::endl;
        for(int a: currentPerm)
            std::cout << a << " ";
        std::cout << std::endl;
        std::cout << currentPermutation << " sign flipping matrix is: " << std::endl;
        std::cout << P << std::endl;
        t.randomSignFlip();
    }

    std::cout << "Finished required random sign flippings! Restoring the tree to its original state" << std::endl;
    t.resetTreeSignState();
    std::vector<int> currentPerm = t.getSignVector();
    std::cout << "Original permutation is: " << std::endl;
    for(int a: currentPerm)
        std::cout << a << " ";
    std::cout << std::endl;
    std::cout << "Finished!" << std::endl;
}

//----------PUBLIC METHODS-----------------

TestPermutationTree::TestPermutationTree()
{
    //First matrix used for tests, no repetitions at all
    X1 = Eigen::MatrixXd(9, 2);

    X1(0,0) = 1;
    X1(0,1) = 2;
    X1(1,0) = 3;
    X1(1,1) = 4;
    X1(2,0) = 5;
    X1(2,1) = 6;
    X1(3,0) = 7;
    X1(3,1) = 8;
    X1(4,0) = 9;
    X1(4,1) = 10;

    X1(5,0) = 11;
    X1(5,1) = 12;
    X1(6,0) = 13;
    X1(6,1) = 14;
    X1(7,0) = 15;
    X1(7,1) = 16;
    X1(8,0) = 17;
    X1(8,1) = 18;

    //Second matrix used for tests
    //Here there are repetitions, specifically:
    //0 = 6
    //1 = 7
    //2 = 8
    X2 = Eigen::MatrixXd(9, 2);

    X2(0,0) = 1;
    X2(0,1) = 2;
    X2(1,0) = 3;
    X2(1,1) = 4;
    X2(2,0) = 5;
    X2(2,1) = 6;
    X2(3,0) = 7;
    X2(3,1) = 8;
    X2(4,0) = 9;
    X2(4,1) = 10;

    X2(5,0) = 11;
    X2(5,1) = 12;
    X2(6,0) = 1;
    X2(6,1) = 2;
    X2(7,0) = 3;
    X2(7,1) = 4;
    X2(8,0) = 5;
    X2(8,1) = 6;

    a1 = MultyRowArray(9,3);
    a1[0][0] = 1;
    a1[0][1] = 1;
    a1[0][2] = 1;
    a1[0][3] = 1;
    a1[0][4] = 1;
    a1[0][5] = 1;
    a1[0][6] = 1;
    a1[0][7] = 1;
    a1[0][8] = 1;

    a1[1][0] = -1;
    a1[1][1] = -1;
    a1[1][2] = -1;
    a1[1][3] = -2;
    a1[1][4] = -2;
    a1[1][5] = -2;
    a1[1][6] = -3;
    a1[1][7] = -3;
    a1[1][8] = -3;

    a2 = MultyRowArray(9,2);

    a2[0][0] = 1;
    a2[0][1] = 1;
    a2[0][2] = 1;
    a2[0][3] = 1;
    a2[0][4] = 1;
    a2[0][5] = 1;
    a2[0][6] = 1;
    a2[0][7] = 1;
    a2[0][8] = 1;
}

TestPermutationTree::~TestPermutationTree()
{

}

//-----------TEST CASES------------------

void TestPermutationTree::initTestCase(){

}

void TestPermutationTree::lAlgorithmtest1(){
    testLAlgorithmTree(X2, a1);
}

void TestPermutationTree::lAlgorithmtest2(){
    testLAlgorithmTree(X2, a2);
}

void TestPermutationTree::lAlgorithmtest3(){
    testLAlgorithmTree(X1, a1);
}

void TestPermutationTree::lAlgorithmtest4(){
    testLAlgorithmTree(X1, a2);
}

//For both EE and ISE assumptions, we use only multyRowArray1 since
//otherwise exhaustive approach won't end
void TestPermutationTree::allSignFlippingsAndPermutationsTest1(){
    testAllSignFlippingsAndPermutation(X1, a1);
}

void TestPermutationTree::allSignFlippingsAndPermutationsTest2(){
    testAllSignFlippingsAndPermutation(X2, a1);
}

void TestPermutationTree::allSignFlippingsTest1(){
    //We could also have used X2, doesn't really matter for sign flips
    testAllSignFlippingsTree(X1, a1);
}

void TestPermutationTree::allSignFlippingsTest2(){
    //We could also have used X2, doesn't really matter for sign flips
    testAllSignFlippingsTree(X1, a2);
}

void TestPermutationTree::cleanUpTestCase(){

}

QTEST_APPLESS_MAIN(TestPermutationTree)

#include "testpermutationtree.moc"


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

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_NEXTAFTER_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_NEXTAFTER_HPP_INCLUDED
#include <boost/simd/function/std.hpp>

#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/next.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/prev.hpp>
#include <boost/simd/function/sign.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <cmath>

namespace boost { namespace simd {
  namespace ext
  {
    namespace bd = boost::dispatch;
    BOOST_DISPATCH_OVERLOAD ( nextafter_
                            , (typename A0)
                            , bd::cpu_
                            , bd::scalar_< bd::floating_<A0> >
                            , bd::scalar_< bd::floating_<A0> >
                            )
    {
      BOOST_FORCEINLINE A0 operator() ( A0 x, A0 y) const BOOST_NOEXCEPT
      {
        return  (y >  x)  ? next(x) : ((y <  x) ?  prev(x) : x);
      }
    };
    BOOST_DISPATCH_OVERLOAD ( nextafter_
                            , (typename A0)
                            , bd::cpu_
                            , bd::scalar_< bd::arithmetic_<A0> >
                            , bd::scalar_< bd::arithmetic_<A0> >
                            )
    {
      BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1) const BOOST_NOEXCEPT
      {
        return a0+sign(a1-a0);
      }
    };
    BOOST_DISPATCH_OVERLOAD ( nextafter_
                            , (typename A0)
                            , bd::cpu_
                            , bd::scalar_< bd::unsigned_<A0> >
                            , bd::scalar_< bd::unsigned_<A0> >
                            )
    {
      BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1) const BOOST_NOEXCEPT
      {
        return (a1 == a0) ? a0 : (a1 > a0) ? saturated_(inc)(a0) : saturated_(dec)(a0);
      }
    };
    BOOST_DISPATCH_OVERLOAD ( nextafter_
                            , (typename A0)
                            , bd::cpu_
                            , boost::simd::std_tag
                            , bd::scalar_< bd::floating_<A0> >
                            , bd::scalar_< bd::floating_<A0> >
                            )
    {
      BOOST_FORCEINLINE A0 operator() (const std_tag &, A0  a0, A0 a1
                                      ) const BOOST_NOEXCEPT
      {
        return std::nextafter(a0, a1);
      }
    };
    BOOST_DISPATCH_OVERLOAD ( nextafter_
                            , (typename A0)
                            , bd::cpu_
                            , boost::simd::std_tag
                            , bd::scalar_< bd::integer_<A0> >
                            , bd::scalar_< bd::integer_<A0> >
                            )
    {
      BOOST_FORCEINLINE A0 operator() (const std_tag &, A0  a0, A0 a1
                                      ) const BOOST_NOEXCEPT
      {
        return bs::nextafter(a0, a1);
      }
    };
  }
} }

#endif


#include "Day04-PassportProcessing.h"

#include <AdventOfCodeCommon/DisableLibraryWarningsMacros.h>

__BEGIN_LIBRARIES_DISABLE_WARNINGS
#include <boost/algorithm/string.hpp>

#include <unordered_map>
#include <array>
#include <regex>
__END_LIBRARIES_DISABLE_WARNINGS

namespace AdventOfCode
{
namespace Year2020
{
namespace Day04
{

using PassportData = std::unordered_map<std::string, std::string>;

const std::array<std::string, 7> REQUIRED_FIELDS =
{
    "byr",
    "iyr",
    "eyr",
    "hgt",
    "hcl",
    "ecl",
    "pid",
};

bool isValidPassportData(const PassportData& passportData)
{
    for (const auto& requiredField : REQUIRED_FIELDS)
    {
        if (passportData.find(requiredField) == passportData.cend())
        {
            return false;
        }
    }

    return true;
}

bool isAllDigits(const std::string& value)
{
    return std::all_of(value.cbegin(), value.cend(), [](char c) {return std::isdigit(c); });
}

bool isNumberBetween(const std::string& value, int lowerBound, int upperBound)
{
    if (!isAllDigits(value))
    {
        return false;
    }

    int valueInt = std::stoi(value);
    return valueInt >= lowerBound && valueInt <= upperBound;
}

bool isYearBetween(const std::string& value, int lowerBound, int upperBound)
{
    if (value.size() != 4)
    {
        return false;
    }

    return isNumberBetween(value, lowerBound, upperBound);
}

bool isValidBirthYear(const std::string& value)
{
    return isYearBetween(value, 1920, 2002);
}

bool isValidIssueYear(const std::string& value)
{
    return isYearBetween(value, 2010, 2020);
}

bool isValidExpirationYear(const std::string& value)
{
    return isYearBetween(value, 2020, 2030);
}

bool isValidHeight(const std::string& value)
{
    if (value.size() < 3)
    {
        return false;
    }

    std::string unit = value.substr(value.size() - 2);
    std::string scalar = value.substr(0, value.size() - 2);
    if (unit == "cm")
    {
        return isNumberBetween(scalar, 150, 193);
    }
    else if (unit == "in")
    {
        return isNumberBetween(scalar, 59, 76);
    }
    else
    {
        return false;
    }
}

bool isValidHairColor(const std::string& value)
{
    const std::regex hexColorRegex("#[a-f0-9]{6}");
    return std::regex_match(value, hexColorRegex);
}

bool isValidEyeColor(const std::string& value)
{
    const std::regex eyeColorRegex("(amb|blu|brn|gry|grn|hzl|oth)");
    return std::regex_match(value, eyeColorRegex);
}

bool isValidPassportID(const std::string& value)
{
    const std::regex passportIDRegex("[0-9]{9}");
    return std::regex_match(value, passportIDRegex);
}

bool isValidKeyValuePair(const std::pair<std::string, std::string>& keyValuePair)
{
    const std::string& key = keyValuePair.first;
    const std::string& value = keyValuePair.second;

    if (key == "byr")
    {
        return isValidBirthYear(value);
    }
    if (key == "iyr")
    {
        return isValidIssueYear(value);
    }
    if (key == "eyr")
    {
        return isValidExpirationYear(value);
    }
    if (key == "hgt")
    {
        return isValidHeight(value);
    }
    if (key == "hcl")
    {
        return isValidHairColor(value);
    }
    if (key == "ecl")
    {
        return isValidEyeColor(value);
    }
    if (key == "pid")
    {
        return isValidPassportID(value);
    }
    if (key == "cid")
    {
        return true;
    }
    else
    {
        return false;
    }
}

bool isValidPassportDataStrict(const PassportData& passportData)
{
    if (!isValidPassportData(passportData))
    {
        return false;
    }

    for (const auto& keyValuePair : passportData)
    {
        if (!isValidKeyValuePair(keyValuePair))
        {
            return false;
        }
    }

    return true;
}

std::vector<std::string> transformBatchFileLinesToSingleLines(const std::vector<std::string>& batchFileLines)
{
    std::vector<std::string> singleLines;
    std::vector<std::string> currentLineParts;
    for (const auto& batchFileLine : batchFileLines)
    {
        if (batchFileLine == "")
        {
            singleLines.push_back(boost::algorithm::join(currentLineParts, " "));
            currentLineParts = {};
            continue;
        }

        currentLineParts.push_back(batchFileLine);
    }

    singleLines.push_back(boost::algorithm::join(currentLineParts, " "));

    return singleLines;
}

PassportData parsePassportData(const std::string& singleLine)
{
    PassportData passportData;

    std::vector<std::string> keyValuePairs;
    boost::split(keyValuePairs, singleLine, boost::is_any_of(" "));

    for (const auto& keyValuePair : keyValuePairs)
    {
        std::vector<std::string> keyValuePairTokens;
        boost::split(keyValuePairTokens, keyValuePair, boost::is_any_of(":"));

        passportData.emplace(std::move(keyValuePairTokens.at(0)), std::move(keyValuePairTokens.at(1)));
    }

    return passportData;
}

std::vector<PassportData> parseAllPassportData(const std::vector<std::string>& batchFileLines)
{
    std::vector<PassportData> allPassportData;
    std::vector<std::string> singleLines = transformBatchFileLinesToSingleLines(batchFileLines);

    for (const auto& singleLine : singleLines)
    {
        PassportData passportData = parsePassportData(singleLine);
        allPassportData.push_back(std::move(passportData));
    }

    return allPassportData;
}

int numValidPassports(const std::vector<std::string>& batchFileLines)
{
    std::vector<PassportData> allPassportData = parseAllPassportData(batchFileLines);
    return std::count_if(allPassportData.cbegin(), allPassportData.cend(), [](const auto& passportData)
                         {
                             return isValidPassportData(passportData);
                         });
}

int numValidPassportsStrict(const std::vector<std::string>& batchFileLines)
{
    std::vector<PassportData> allPassportData = parseAllPassportData(batchFileLines);
    return std::count_if(allPassportData.cbegin(), allPassportData.cend(), [](const auto& passportData)
                         {
                             return isValidPassportDataStrict(passportData);
                         });
}

}
}
}


// Copyright (c) 2016 The University of Sydney
// Copyright (c) 2021 Mission Systems Pty Ltd
// Copyright (c) 2021 Vsevolod Vlaskine

#include <array>
#include <cmath>
#include <iostream>
#include <limits>
#include <memory>
#include <random>
#include <boost/lexical_cast.hpp>
#include <comma/application/command_line_options.h>
#include <comma/application/verbose.h>
#include <comma/base/exception.h>
#include <comma/csv/stream.h>
#include <comma/string/string.h>
#include "../../math/range_bearing_elevation.h"
#include "../../visiting/eigen.h"

namespace snark { namespace points_make {

template < typename T > struct operation_t
{
    static std::string output_fields() { return comma::join( comma::csv::names< T >( false ), ',' ); }
    static std::string output_format() { return comma::csv::format::value< T >(); }
};

class random
{
    public:
        random( int seed, bool true_random )
        {
            if( true_random )
            {
                device_.reset( new std::random_device );
            }
            else
            {
                engine_.reset( std::mt19937_64( seed ) );
                distribution_.reset( std::uniform_real_distribution< double >( 0, 1 ) );
            }
        }

        random( const std::string& how ): random( how == "true" ? 0 : boost::lexical_cast< int >( how ), how == "true" ) {}

        double operator()() { return device_ ? double( ( *device_ )() ) / std::numeric_limits< unsigned int >::max() : ( *distribution_ )( *engine_ ); }

    private:
        std::unique_ptr< std::random_device > device_;
        boost::optional< std::mt19937_64 > engine_;
        boost::optional< std::uniform_real_distribution< double > > distribution_;
};

static Eigen::Vector3d tensor_product( const Eigen::Vector3d& s, const Eigen::Vector3d& t ) { return Eigen::Vector3d( s[0] * t[0], s[1] * t[1], s[2] * t[2] ); }

struct box : public operation_t< Eigen::Vector3d >
{
    static void usage()
    {
        std::cerr << "\n    box: output point cloud of rectangular box shape; surface only or filled";
        std::cerr << "\n         current limitation: extent on a given dimension will be shrank to";
        std::cerr << "\n                             multiple of resolution of that dimension";
        std::cerr << "\n        --fill";
        std::cerr << "\n        --extents,-e=<point>";
        std::cerr << "\n        --origin,o=<point>; default=0,0,0";
        std::cerr << "\n        --random; output random points";
        std::cerr << "\n        --random-seed,--seed,-s=<seed>; if present, output uniform random sample on sphere";
        std::cerr << "\n                                        instead of regular reasonably uniform sample";
        std::cerr << "\n                                        <seed>: number for repeatable pseudo-random numbers or 'true' for true random";
        std::cerr << "\n        --resolution,-r=<resolution>; <resolution>: <number> or <point>; default=1";
        std::cerr << "\n        --size=<n>; desired number of points, overrides --resolution";
        std::cerr << "\n        --width,-w=<width>; shortcut for --extents, assumes cube";
    }

    static void examples()
    {
        std::cerr << "\n    points-make box --width 100 | view-points '-;color=yellow'";
        std::cerr << "\n    points-make box --width 10 --fill --binary 3d | view-points '-;color=yellow;binary=3d'";
    }

    static const char* completion_options() { return " box --fill --extents -e --origin -o --resolution -r --random-seed --random --seed -s --width -w"; }

    static int run( const comma::command_line_options& options )
    {
        options.assert_mutually_exclusive( "--extents,-e", "--width,-w" );
        bool fill = options.exists( "--fill" );
        auto origin = comma::csv::ascii< Eigen::Vector3d >().get( options.value< std::string >( "--origin,-o", "0,0,0" ) );
        Eigen::Vector3d extents;
        if( !options.exists( "--extents,-e,--width,-w" ) ) { COMMA_THROW( comma::exception, "box: please specify either --extents or --width" ); }
        if( options.exists( "--extents,-e" ) ) { extents = comma::csv::ascii< Eigen::Vector3d >().get( options.value< std::string >( "--extents,-e" ) ); }
        else { double w = options.value< double >( "--width,-w" ); extents = Eigen::Vector3d( w, w, w ); }
        Eigen::Vector3d resolution;
        std::string s = options.value< std::string >( "--resolution,-r", "1,1,1" );
        if( comma::split( s, ',' ).size() == 1 ) { auto r = boost::lexical_cast< double >( s ); resolution = Eigen::Vector3d( r, r, r ); }
        else { resolution = comma::csv::ascii< Eigen::Vector3d >().get( s ); }
        auto size = options.optional< unsigned int >( "--size" );
        auto seed = options.optional< std::string >( "--random-seed,--seed,-s" );
        if( options.exists( "--random" ) && !seed ) { seed = "0"; } // quick and dirty
        comma::csv::output_stream< Eigen::Vector3d > ostream( std::cout, comma::csv::options( options ) );
        auto p = origin;
        auto end = origin + extents;
        if( fill )
        {
            if( seed )
            {
                if( !size ) { size = ( extents[0] * extents[1] * extents[2] ) / ( resolution[0] * resolution[1] * resolution[2] ); }
                points_make::random rd( *seed );
                for( unsigned int i = 0; i < *size; ++i ) { ostream.write( origin + tensor_product( extents, Eigen::Vector3d( rd(), rd(), rd() ) ) ); }
            }
            else
            {
                //if( size ) { double f = std::pow( extents[0] * extents[1] * extents[2] / *size, 1. / 3 ); resolution = Eigen::Vector3d( f, f, f ); }
                if( size ) { COMMA_THROW( comma::exception, "box: --fill --size: todo" ); } // the formula above does not guarantee the exact number of points
                for( p[0] = origin[0]; p[0] < end[0]; p[0] += resolution[0] )
                {
                    for( p[1] = origin[1]; p[1] < end[1]; p[1] += resolution[1] )
                    {
                        for( p[2] = origin[2]; p[2] < end[2]; p[2] += resolution[2] ) { ostream.write( p ); }
                    }
                }
            }
        }
        else
        {
            if( seed )
            {
                std::array< double, 3 > area = {{ extents[1] * extents[2], extents[0] * extents[2], extents[0] * extents[1] }};
                if( !size ) { size = 2 * ( area[0] / resolution[1] * resolution[2] + area[1] / resolution[0] * resolution[2] + area[2] / resolution[0] * resolution[1] ); }
                points_make::random rd( *seed );
                std::mt19937_64 e;
                std::uniform_real_distribution< double > r( 0, area[0] + area[1] + area[2] );
                std::uniform_int_distribution< int > l( 0, 1 );
                auto index = [&]( double s ) -> unsigned int { return s < area[0] ? 0 : s < ( area[0] + area[1] ) ? 1 : 2; };
                for( unsigned int i = 0; i < *size; ++i )
                {
                    Eigen::Vector3d v( rd(), rd(), rd() );
                    v[ index( r( e ) ) ] = l( e );
                    ostream.write( origin + tensor_product( extents, v ) );
                }
            }
            else
            {
                //if( size ) { double f = std::sqrt( ( extents[0] * extents[1] + extents[1] * extents[2] + extents[0] * extents[2] ) * 2 / * size ); resolution = Eigen::Vector3d( f, f, f ); }
                if( size ) { COMMA_THROW( comma::exception, "box: --size: todo" ); } // the formula above does not guarantee the exact number of points
                auto output_filled = [&]()
                {
                    for( p[0] = origin[0]; p[0] < end[0]; p[0] += resolution[0] )
                    {
                        for( p[1] = origin[1]; p[1] < end[1]; p[1] += resolution[1] ) { ostream.write( p ); }
                    }
                };
                auto last = p - resolution;  // quick and dirty
                auto output_unfilled = [&]()
                {
                    p[1] = origin[1];
                    for( p[0] = origin[0]; p[0] < end[0]; p[0] += resolution[0] ) { ostream.write( p ); }
                    for( p[1] = origin[1] + resolution[1]; p[1] < end[1] - resolution[1]; p[1] += resolution[0] )
                    {
                        ostream.write( Eigen::Vector3d( origin[0], p[1], p[2] ) );
                        ostream.write( Eigen::Vector3d( last[0], p[1], p[2] ) );
                    }
                    if( p[1] < end[1] ) { for( p[0] = origin[0]; p[0] < end[0]; p[0] += resolution[0] ) { ostream.write( p ); } }
                };
                output_filled();
                p[2] += resolution[2];
                for( ; p[2] < end[2] - resolution[2]; p[2] += resolution[2] ) { output_unfilled(); }
                if( p[2] < end[2] ) { output_filled(); }
            }
        }
        return 0;
    }
};

struct sphere : public operation_t< Eigen::Vector3d >
{
    static void usage()
    {
        std::cerr << "\n    sphere: output point cloud of spherical shape; surface only or filled";
        std::cerr << "\n        --fill; todo";
        std::cerr << "\n        --radius=<radius>";
        std::cerr << "\n        --center,-c=<point>; default=0,0,0";
        std::cerr << "\n        --resolution,-r=<resolution>; roughly how far points need to be from each other; default=1";
        std::cerr << "\n        --random; output random points";
        std::cerr << "\n        --random-seed,--seed,-s=<seed>; if present, output uniform random sample on sphere";
        std::cerr << "\n                                        instead of regular reasonably uniform sample";
        std::cerr << "\n                                        <seed>: number for repeatable pseudo-random numbers or 'true' for true random";
        std::cerr << "\n        --size=<n>; desired number of points, overrides resolution";
    }

    static void examples()
    {
        std::cerr << "\n    points-make sphere --radius 10 | view-points '-;color=yellow'";
        std::cerr << "\n    points-make sphere --radius 10 --size 10000 --random | view-points '-;color=yellow'";
    }

    static const char* completion_options() { return " sphere --radius --center -c --resolution -r --random-seed --random --seed -s"; }

    static int run( const comma::command_line_options& options )
    {
        options.assert_mutually_exclusive( "--resolution,-r", "--size" );
        bool fill = options.exists( "--fill" );
        auto center = comma::csv::ascii< Eigen::Vector3d >().get( options.value< std::string >( "--center,-c", "0,0,0" ) );
        double radius = options.value< double >( "--radius" );
        auto resolution = options.value( "--resolution,-r", 1.0 );
        auto size = options.optional< unsigned int >( "--size" );
        auto seed = options.optional< std::string >( "--random-seed,--seed,-s" );
        if( options.exists( "--random" ) && !seed ) { seed = "0"; } // quick and dirty
        comma::csv::output_stream< Eigen::Vector3d > ostream( std::cout, comma::csv::options( options ) );
        if( seed )
        {
            points_make::random rd( *seed );
            unsigned int n = size ? *size
                                  : fill ? ( ( radius * radius * radius * 4. / 3. * M_PI ) / ( resolution * resolution * resolution / ( 6. * std::sqrt( 2 ) ) ) ) * 4 / 20  // quick and dirty: assuming resolution much less than radius
                                         : ( ( radius * radius * 4. * M_PI ) / ( resolution * resolution * std::sqrt( 3. ) / 2. ) ) * 3. / 6.; // quick and dirty: assuming resolution much less than radius
            for( unsigned int i = 0; i < n; ++i )
            {
                double a = ( rd() * 2 - 1 ) * M_PI;
                double e = std::asin( rd() * 2 - 1 ); // todo: improve; even though it's reasonably uniform, there are local irregularities
                double r = ( fill ? std::pow( rd(), 1. / 3 ) : 1 ) * radius;
                ostream.write( snark::range_bearing_elevation( r, a, e ).to_cartesian() + center );
            }
        }
        else
        {
            if( size ) { COMMA_THROW( comma::exception, "sphere: --size for --random: todo; meanwhile use --resolution" ); }
            if( fill )
            {
                COMMA_THROW( comma::exception, "sphere: --fill" );
            }
            else
            {
                double as = std::asin( ( resolution / 2 ) / radius ) * 2;
                for( double a = 0; a < M_PI * 2; a += as ) { ostream.write( snark::range_bearing_elevation( radius, a, 0 ).to_cartesian() + center ); }
                double es = std::asin( resolution / radius * std::sqrt( 3 ) / 2 );
                double t = 0;
                for( double e = es; e < M_PI / 2; e += es )
                {
                    t = 0.5 - t; // quick and dirty
                    double s = std::asin( ( resolution / 2 ) / ( radius * std::cos( e ) ) ) * 2;
                    for( double a = s * t; a < M_PI * 2; a += s )
                    {
                        auto p = snark::range_bearing_elevation( radius, a, e ).to_cartesian();
                        ostream.write( p + center );
                        p[2] = -p[2];
                        ostream.write( p + center );
                    }
                }
            }
        }
        return 0;
    }
};

namespace test_pattern {

struct color_t
{
    boost::array< unsigned char, 4 > rgba = {{ 0, 0, 0, 255 }};
    color_t() {}
    color_t( unsigned char red, unsigned char green, unsigned char blue, unsigned char alpha = 255 ): rgba( {{ red, green, blue, alpha }} ) {}

    unsigned char red() const   { return rgba[0]; }
    unsigned char green() const { return rgba[1]; }
    unsigned char blue() const  { return rgba[2]; }
    unsigned char alpha() const { return rgba[3]; }
};

// Define our own vertex type using our colour type.
// A more flexible alternative is to use snark::graphics::qt3d::vertex_t
// but this type works better with view-points.
struct vertex_t
{
    Eigen::Vector3f position;
    color_t color;

    vertex_t(): position( 0, 0, 0 ) {}
    vertex_t( const Eigen::Vector3f& position, const color_t& color ): position( position ), color( color ) {}
};

struct cube : public operation_t< vertex_t >
{
    static const unsigned int default_num_points = 100000;
    static constexpr float default_width = 1.0f;
    static constexpr float default_thickness = 0.01f;

    static void usage()
    {
        std::cerr << "\n    test-cube";
        std::cerr << "\n        --number-of-points,--num,-n=<val>:     number of points in cube (default: " << default_num_points << ")";
        std::cerr << "\n        --width,-w=<val>:   cube width (default: " << default_width << ")";
        std::cerr << "\n        --thickness=<val>:  cube thickness (default: " << default_thickness << ")";
    }

    static void examples()
    {
        std::cerr << "\n    " << "points-make test-cube | view-points --fields x,y,z,r,g,b,a";
    }

    static const char* completion_options() { return " test-cube --num -n --width -w --thickness"; }

    static int run( const comma::command_line_options& options )
    {
        unsigned seed = options.value< unsigned int >( "--random-seed,--seed,-s", time( 0 ) );
        srand( seed );
        unsigned int num_points = options.value< unsigned int >( "--number-of-points,--num,-n", default_num_points );
        float width = options.value< float >( "--width,-w", default_width );
        float thickness = options.value< float >( "--thickness", default_thickness );

        comma::csv::options output_csv( options );
        output_csv.full_xpath = false;
        if( options.exists( "--binary,-b" )) { output_csv.format( comma::csv::format::value< vertex_t >() ); }
        comma::csv::output_stream< vertex_t > ostream( std::cout, output_csv );

        for( unsigned int i = 0; i < num_points; i++ )
        {
            Eigen::Vector3f p = make_point( width, thickness );
            vertex_t vertex( p, color_t( point_to_color( p.x(), width, thickness )
                                       , point_to_color( p.y(), width, thickness )
                                       , point_to_color( p.z(), width, thickness )
                                       , 255 ));
            ostream.write( vertex );
        }
        return 0;
    }

private:
    static unsigned char point_to_color( float p, float width, float thickness ) { return (unsigned char)( fabs( p / ( width + thickness )) * 256 ); }
    static float random( float min, float max ) { return (float)::random() / RAND_MAX * ( max - min ) + min; }
    static int random_sign() { return ::random() % 2 * 2 - 1; }

    static Eigen::Vector3f make_point( float width, float thickness )
    {
        float min = width - thickness;
        float max = width + thickness;
        float x_min = 0;
        float y_min = 0;
        float z_min = 0;
        switch( ::random() % 3 )
        {
            case 0: x_min = min; break;
            case 1: y_min = min; break;
            default: case 2: z_min = min; break;
        }
        float x = random_sign() * random( x_min, max );
        float y = random_sign() * random( y_min, max );
        float z = random_sign() * random( z_min, max );
        return Eigen::Vector3f( x, y, z );
    }
};

struct grid : public operation_t< Eigen::Vector3f >
{
    static constexpr float default_width = 6.0f;
    static constexpr float default_spacing = 1.0f;
    static constexpr float default_z_offset = 0.0f;

    static void usage()
    {
        std::cerr << "\n    test-grid";
        std::cerr << "\n        --width,-w=<val>:   grid width; default: " << default_width;
        std::cerr << "\n        --spacing,-s=<val>: grid spacing; default: " << default_spacing;
        std::cerr << "\n        --z-offset=<val>:   grid z offset; default: " << default_z_offset;
    }

    static void examples()
    {
        std::cerr << "\n    " << "points-make test-grid | view-points --shape lines";
    }

    static const char* completion_options() { return " test-grid --width -w --spacing -s --z-offset"; }

    static int run( const comma::command_line_options& options )
    {
        unsigned seed = options.value< unsigned int >( "--random-seed,--seed,-s", time( 0 ) );
        srand( seed );
        float width = options.value< float >( "--width,-w", default_width );
        float spacing = options.value< float >( "--spacing,-s", default_spacing );
        float z_offset = options.value< float >( "--z-offset", default_z_offset );

        comma::csv::options output_csv( options );
        output_csv.full_xpath = false;
        if( options.exists( "--binary,-b" )) { output_csv.format( comma::csv::format::value< Eigen::Vector3f >() ); }

        comma::csv::output_stream< Eigen::Vector3f > ostream( std::cout, output_csv );

        int sign = 1;
        float offset = width / 2.0f;
        for( float y = -offset; y <= offset; y += spacing )
        {
            ostream.write( Eigen::Vector3f( -offset * sign, y, z_offset ));
            ostream.write( Eigen::Vector3f(  offset * sign, y, z_offset ));
            sign *= -1;
        }
        sign = 1;
        for( float x = -offset; x <= offset; x += spacing )
        {
            ostream.write( Eigen::Vector3f( x,  offset * sign, z_offset ));
            ostream.write( Eigen::Vector3f( x, -offset * sign, z_offset ));
            sign *= -1;
        }
        return 0;
    }
};

} } // namespace test_pattern {

} // namespace snark { namespace points_make {

namespace comma { namespace visiting {

template <> struct traits< snark::points_make::test_pattern::color_t >
{
    template < typename Key, class Visitor >
    static void visit( Key, snark::points_make::test_pattern::color_t& p, Visitor& v )
    {
        v.apply( "r", p.rgba[0] );
        v.apply( "g", p.rgba[1] );
        v.apply( "b", p.rgba[2] );
        v.apply( "a", p.rgba[3] );
    }

    template < typename Key, class Visitor >
    static void visit( Key, const snark::points_make::test_pattern::color_t& p, Visitor& v )
    {
        v.apply( "r", p.red() );
        v.apply( "g", p.green() );
        v.apply( "b", p.blue() );
        v.apply( "a", p.alpha() );
    }
};

template <> struct traits< snark::points_make::test_pattern::vertex_t >
{
    template < typename Key, class Visitor >
    static void visit( Key, snark::points_make::test_pattern::vertex_t& p, Visitor& v )
    {
        v.apply( "position", p.position );
        v.apply( "color", p.color );
    }

    template < typename Key, class Visitor >
    static void visit( Key, const snark::points_make::test_pattern::vertex_t& p, Visitor& v )
    {
        v.apply( "position", p.position );
        v.apply( "color", p.color );
    }
};

} } // namespace comma { namespace visiting {

template < typename Operation > static int run( const comma::command_line_options& options )
{
    if( options.exists( "--output-fields" ) ) { std::cout << Operation::output_fields() << std::endl; return 0; }
    if( options.exists( "--output-format" ) ) { std::cout << Operation::output_format() << std::endl; return 0; }
    return Operation::run( options );
}

static void bash_completion( unsigned const ac, char const * const * av )
{
    std::cout << "--help -h --output-fields --output-format --binary -b --random-seed --seed -s"
              << snark::points_make::test_pattern::cube::completion_options()
              << snark::points_make::test_pattern::grid::completion_options()
              << std::endl;
    exit( 0 );
}

static void usage( bool verbose )
{
    std::cerr << "\noutput point clouds";
    std::cerr << "\n";
    std::cerr << "\nusage: " << "points-make <operation> [<options>]";
    std::cerr << "\n";
    std::cerr << "\n<operation>";
    std::cerr << "\n    cube: todo: document...";
    std::cerr << "\n    grid: todo: document...";
    std::cerr << "\n";
    std::cerr << "\n<options>";
    std::cerr << "\n    --help,-h:       show this help, --help --verbose for more help";
    std::cerr << "\n    --output-fields: show output fields and exit";
    std::cerr << "\n    --output-format: show binary output format and exit";
    std::cerr << "\n";
    std::cerr << "\ntest-* options";
    std::cerr << "\n    --binary,-b:     output in binary (default is ascii)";
    std::cerr << "\n    --random-seed,--seed,-s=<seed>: seed for random generator (see srand)";
    std::cerr << "\n                                    <seed>: seed for random generator; default: current time";
    std::cerr << "\n";
    std::cerr << "\ncsv options\n";
    std::cerr << comma::csv::options::usage( verbose );
    std::cerr << "\n";
    snark::points_make::box::usage();
    std::cerr << "\n";
    snark::points_make::sphere::usage();
    std::cerr << "\n";
    snark::points_make::test_pattern::cube::usage();
    std::cerr << "\n";
    snark::points_make::test_pattern::grid::usage();
    std::cerr << "\n";
    std::cerr << "\nexamples";
    if( verbose )
    {
        snark::points_make::box::examples();
        snark::points_make::sphere::examples();
        snark::points_make::test_pattern::cube::examples();
        snark::points_make::test_pattern::grid::examples();
    }
    else
    {
        std::cerr << "\n    run points-make --help --verbose to see examples...";
    }
    std::cerr << std::endl;
    std::cerr << std::endl;
    exit( 0 );
}

int main( int argc, char** argv )
{
    try
    {
        comma::command_line_options options( argc, argv, usage );
        if( options.exists( "--bash-completion" ) ) { bash_completion( argc, argv ); }
        std::vector< std::string > unnamed = options.unnamed( "--help,-h,--fill,--flush,--random,--verbose,-v", "--.*" );
        if( unnamed.empty() ) { std::cerr << "points-make: please specify operation" << std::endl; return 1; }
        std::string operation = unnamed[0];
        if( operation == "box" ) { return run< snark::points_make::box >( options ); }
        if( operation == "sphere" ) { return run< snark::points_make::sphere >( options ); }
        if( operation == "test-cube" ) { return run< snark::points_make::test_pattern::cube >( options ); }
        else if( operation == "test-grid" ) { return run< snark::points_make::test_pattern::grid >( options ); }
        std::cerr << "points-make: expected operation, got: '" << operation << "'" << std::endl;
    }
    catch( std::exception& ex ) { std::cerr << "points-make: " << ex.what() << std::endl;  }
    catch( ... ) { std::cerr << "points-make: unknown exception" << std::endl; }
    return 1;
}


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

  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-predicates
    Function object implementing is_ltz capabilities

    Returns @ref True or @ref False according x is less than @ref Zero or not.

    @par Semantic:

    @code
    auto r = is_ltz(x);
    @endcode

    is similar to:

    @code
    auto r = x < 0;
    @endcode

  **/
  as_logical_t<Value> is_ltz(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/is_ltz.hpp>
#include <boost/simd/function/simd/is_ltz.hpp>

#endif


#pragma once
#ifndef CANNON_RAY_FILM_H
#define CANNON_RAY_FILM_H

/*!
 * \file cannon/ray/film.hpp
 * \brief File containing Film and FilmTile class definitions.
 */

#include <memory>
#include <mutex>
#include <vector>

#include <Eigen/Dense>

#include <cannon/ray/write_ppm.hpp>
#include <cannon/utils/class_forward.hpp>

using namespace Eigen;

namespace cannon {
  namespace ray {

    CANNON_CLASS_FORWARD(Filter);

    /*!
     * \brief Struct representing a single tile in the image film. Accumulates
     * contributions from image samples.
     */
    struct FilmPixel {
      Vector3d color_sum_ = Vector3d::Zero(); //!< Sum of color samples for this pixel
      double filter_weight_sum_ = 0.0; //!< Sum of filter weights for this pixel
    };

    /*!
     * \brief Struct representing a tile of pixels in the film which can be
     * independently processed.
     */
    struct FilmTile {

      /*!
       * \brief Add a sample to this film tile.
       *
       * \param p_film Point on film that sample hits.
       * \param color Color of sample
       */
      void add_sample(const Vector2d& p_film, const Vector3d& color);

      /*!
       * \brief Get pixel in this tile at location (i, j)
       *
       * \param i Horizontal coordinate of pixel
       * \param j Vertical coordinate of pixel
       *
       * \returns Reference to pixel.
       */
      FilmPixel& get_pixel(int i, int j);

      unsigned int origin_x_, origin_y_; //!< Origin of this tile
      unsigned int extent_x_, extent_y_; //!< Extent of this tile
      Vector2d filter_radius_; //!< Filter radius for film
      Vector2d inv_filter_radius_; //!< 1 / filter_radius
      const double *filter_table_; //!< Filter table for film
      int filter_table_width_;
      std::vector<FilmPixel> pixels_; //!< Pixels in this tile
    };

    /*!
     * \brief Class representing Film to which an image is rendered.
     */
    class Film {
      public:

        /*!
         * \brief Constructor.
         */
        Film(unsigned int width, unsigned int height, unsigned int tile_size,
             std::unique_ptr<Filter> filter);

        /*!
         * \brief Get tile (i, j) of this film.
         */
        std::unique_ptr<FilmTile> get_film_tile(int i, int j) const;

        /*!
         * \brief Merge input film tile into the final image.
         *
         * \param tile The tile to merge.
         */
        void merge_film_tile(std::unique_ptr<FilmTile> tile);

        /*!
         * \brief Write this film to the input file.
         *
         * \param filename The filename to write this film to in PPM format.
         */
        void write_image(const std::string& filename);

        /*!
         * \brief Write this film to the input data pointer.
         *
         * \param data Array in which to write data as RGB triples. Should have
         * size = 3 * width_ * height_.
         */
        void write_image(float *data);

      public:
        unsigned int width_, height_; //!< Width and height of film
        unsigned int tile_size_; //!< Size of each film tile
        std::mutex mut_; //!< Mutex controlling image data writing/reading
        std::vector<FilmPixel> pixels_; //!< Rendered image data
        std::unique_ptr<Filter> filter_; //!< Image reconstruction filter

        static constexpr int filter_table_width_ = 16; //!< Size of cached filter table
        double filter_table_[filter_table_width_ * filter_table_width_]; //!< Cached filter table

    };

  } // namespace ray
} // namespace cannon

#endif /* ifndef CANNON_RAY_FILM_H */


#include <boost/mpl/math/is_even.hpp>


#define BOOST_TEST_MODULE TEST_PARAMS

#include <cstdio>
#include <iostream>
#include <fstream>
#include <string>
#include <stdexcept>

#include <boost/test/unit_test.hpp>

#include <dynet/dynet.h>
#include <dynet/expr.h>
#include <dynet/model.h>
#include <dynet/param-init.h>
#include <dynet/lstm.h>
#include <dynet/gru.h>
#include <dynet/treelstm.h>
#include <dynet/io.h>

#include "test.h"

using namespace dynet;
using namespace std;

struct ParamsTest {
    ParamsTest() {
        // initialize if necessary
        if (default_device == nullptr) {
            for (auto x : {"ParamsTest", "--dynet-mem", "512"}) {
                av.push_back(strdup(x));
            }
            ADD_EXTRA_ARGUMENTS(av)
            char **argv = &av[0];
            int argc = av.size();
            dynet::initialize(argc, argv);
        }
        gain = 2.0;
        epsilon = 1e-6;
        d = dynet::Dim({10, 10});
    }
    ~ParamsTest() {
        for (auto x : av) free(x);
    }

    float gain, epsilon;
    dynet::Dim d;
    std::vector<char*> av;
};

class testParameterCollection {
 public:
  testParameterCollection(dynet::ParameterCollection &model) {
    lookup_param = model.add_lookup_parameters(1000, {128});
    affine_params = model.add_subcollection("affine");
    W_x = affine_params.add_parameters({40, 30});
    b_x = affine_params.add_parameters({40});
  }
  std::string get_affine_model_name() { return affine_params.get_fullname(); }
  dynet::ParameterCollection get_affine_model() const { return affine_params; }
 private:
  dynet::LookupParameter lookup_param;
  dynet::Parameter W_x, b_x;
  dynet::ParameterCollection affine_params;
}; // class testParameterCollection

// define the test suite
BOOST_FIXTURE_TEST_SUITE(params_test, ParamsTest);

BOOST_AUTO_TEST_CASE( init_saxe ) {
    dynet::ParameterCollection mod;
    // Random orthogonal matrix scaled by gain
    dynet::Parameter saxe_p = mod.add_parameters({10, 10}, ParameterInitSaxe(gain));
    // gain^2 * identity matrix
    dynet::Parameter identity_p = mod.add_parameters({10, 10}, ParameterInitIdentity());
    // Initialize graph
    dynet::ComputationGraph cg;
    dynet::Expression saxe = dynet::parameter(cg, saxe_p);
    dynet::Expression identity = dynet::parameter(cg, identity_p);
    // check that the matrix is indeed orthogonal
    dynet::Expression diff_expr_left = dynet::squared_norm(dynet::transpose(saxe) * saxe - (gain * gain) * identity);
    dynet::Expression diff_expr_right = dynet::squared_norm(saxe * dynet::transpose(saxe) - (gain * gain) * identity);
    float diff = dynet::as_scalar(cg.forward((diff_expr_left + diff_expr_right) / 2.0));
    // Leave a margin of error of epsilon=10^-6
    BOOST_CHECK_LT(diff, epsilon);
}

BOOST_AUTO_TEST_CASE ( test_parameter_collection ) {
  dynet::ParameterCollection model;
  dynet::Parameter a = model.add_parameters({10});
  dynet::Parameter b1 = model.add_parameters({1,2}, "b");
  dynet::Parameter b2 = model.add_parameters({1,2}, "b");
  dynet::ParameterCollection submodel = model.add_subcollection("foo");
  dynet::Parameter c = submodel.add_parameters({10});
  dynet::Parameter d = submodel.add_parameters({1, 2}, "d");
  dynet::Parameter b3 = submodel.add_parameters({1, 2}, "b");
  DYNET_CHECK_EQUAL(model.get_fullname(), "/");
  DYNET_CHECK_EQUAL(a.get_fullname(), "/_0");
  DYNET_CHECK_EQUAL(b1.get_fullname(), "/b");
  DYNET_CHECK_EQUAL(b2.get_fullname(), "/b_1");
  DYNET_CHECK_EQUAL(submodel.get_fullname(), "/foo/");
  DYNET_CHECK_EQUAL(c.get_fullname(), "/foo/_0");
  DYNET_CHECK_EQUAL(d.get_fullname(), "/foo/d");
  DYNET_CHECK_EQUAL(b3.get_fullname(), "/foo/b");
}

BOOST_AUTO_TEST_CASE ( test_parameter_class ) {
  auto save_parameters_lambda = [] (const std::string & fname, dynet::ParameterCollection & m) -> size_t {
    auto params = m.get_parameter_storages();
    auto lookup_params = m.get_lookup_parameter_storages();
    for (auto & param: params) {
      std::cout << param->name << " saved in file " << fname << std::endl;
    }
    for (auto & lookup_param: lookup_params) {
      std::cout << lookup_param->name << " saved in file " << fname << std::endl;
    }
    return params.size() + lookup_params.size();
  };
  auto save_parameters_lambda2 = [] (const std::string & fname, dynet::Parameter & p) -> std::string {
    std::cout << p.get_storage().name << " saved in file " << fname << std::endl;
    return p.get_storage().name;
  };
  auto save_parameters_lambda3 = [] (const std::string & fname,
                                     std::shared_ptr<dynet::ParameterStorage>p) ->std::string {
    std::cout << p->name << " saved in file " << fname << std::endl;
    return p->name;
  };
  ParameterCollection collec;
  testParameterCollection spec(collec);
  std::string affine_id_for_posterity = spec.get_affine_model_name();
  DYNET_CHECK_EQUAL(affine_id_for_posterity, "/affine/");
  DYNET_CHECK_EQUAL(save_parameters_lambda("model_file.txt", collec), 3);
  auto affine_model = spec.get_affine_model();
  DYNET_CHECK_EQUAL(save_parameters_lambda("affine_file.txt", affine_model), 2);
  auto submodel = collec.add_subcollection("affine");
  auto p = submodel.add_parameters({10});
  std::cout << p.get_fullname() << std::endl;
  DYNET_CHECK_EQUAL(save_parameters_lambda2("tuning_parameter_file.txt", p), "/affine_1/_0");
  DYNET_CHECK_EQUAL(save_parameters_lambda3("tuning_parameter_file.txt",
                                            affine_model.get_parameter_storage("/affine/_1")), "/affine/_1");
}

BOOST_AUTO_TEST_CASE ( test_parametercollection_with_builder ) {
  dynet::ParameterCollection collec;
  auto gru_builder = dynet::GRUBuilder(3, 10, 2, collec);
  DYNET_CHECK_EQUAL(gru_builder.get_parameter_collection().size(), 9 * 3);
  dynet::ParameterCollection collec2;
  auto bi_treelstm_builder = BidirectionalTreeLSTMBuilder(3, 10, 2, collec2);
  DYNET_CHECK_EQUAL(bi_treelstm_builder.get_parameter_collection().size(), 3 * 3 * 2);
}

BOOST_AUTO_TEST_CASE( scale ) {
    dynet::ParameterCollection mod;
    // Create parameter
    dynet::Parameter w_p = mod.add_parameters({1}, ParameterInitConst(1));
    // Initial value
    float init_value= as_scalar(*(w_p.values()));
    // Rescale
    w_p.scale(0.3);
    // Value after rescaling
    float end_value=as_scalar(*(w_p.values()));
    // Check with a margin of error
    BOOST_CHECK_CLOSE(init_value * 0.3, end_value, 0.001);
}

BOOST_AUTO_TEST_CASE( scale_grad ) {
    dynet::ParameterCollection mod;
    // Create parameter
    dynet::Parameter w_p = mod.add_parameters({1}, ParameterInitConst(1));
    // Run forward/backward
    dynet::ComputationGraph cg;
    dynet::Expression x = dynet::input(cg,1.f);
    dynet::Expression w = dynet::parameter(cg, w_p);
    dynet::Expression y = x * w;
    cg.forward(y);
    cg.backward(y);
    // Rescale gradient
    w_p.scale_gradient(0.5);
    // Value after rescaling
    float rescaled_grad=as_scalar(w_p.get_storage().g);
    // Check with a margin of error
    BOOST_CHECK_CLOSE(0.5, rescaled_grad, 0.001);
}

BOOST_AUTO_TEST_CASE( set_value ) {
    dynet::ParameterCollection mod;
    // Create parameter
    dynet::Parameter w_p = mod.add_parameters({3}, ParameterInitConst(1));
    // Initial parameter = {1., 1., 1.}
    std::vector<float> value_to_set = {1.f, 2.f, 3.f};
    // Set value
    w_p.set_value(value_to_set);
    // New value
    std::vector<float> parameter_after = as_vector(w_p.get_storage().values);
    for(unsigned i=0; i<3; i++){
      BOOST_CHECK_EQUAL(value_to_set[i], parameter_after[i]);
    }
}

BOOST_AUTO_TEST_SUITE_END()


#include "../interface/BTagWeight.h"
#include "../interface/GlobalVariables.h"

#include <functional>
#include <numeric>
#include <boost/scoped_ptr.hpp>
#include <boost/array.hpp>

using namespace std;
namespace BAT {

double BjetWeight(const JetCollection jets, const int systematic) {
	boost::scoped_ptr<BTagWeight> btagWeight(new BTagWeight());

	double weight = btagWeight->weight( jets, systematic );
	return weight;
}

BTagWeight::BTagWeight() :
				minNumberOfTags_(0), //
				maxNumberOfTags_(0) {
}

double BTagWeight::weight(const JetCollection jets, const int systematic) const {
	float bTaggedMCJet = 1.0;
	float nonBTaggedMCJet = 1.0;
	float bTaggedDataJet = 1.0;
	float nonBTaggedDataJet = 1.0;

	for (unsigned int index = 0; index < jets.size(); ++index) {
		// Info on this jet
		const JetPointer jet(jets.at(index));

		double jetPt = jet->pt();
		if ( jetPt < 25 ) continue;

		// If the pt of the jet is outside the pt range of the SFs,
		// use the pt at the upper/lower edge and double the uncertainty.
		bool ptOutOfRange = false;
		if ( jetPt <= 30 || jet->pt() >= 670 ) {
			ptOutOfRange = true;
		}

		const unsigned int partonFlavour = abs( jet->partonFlavour() );
		const bool isBTagged = jet->isBJet();

		// Get scale factor for this jet
		const double sf = jet->getBTagSF( 0 );
		double sf_up = jet->getBTagSF( 1 );
		double sf_down = jet->getBTagSF( -1 );

		if ( ptOutOfRange ) {
			sf_up = sf + 2 * ( sf_up - sf );
			sf_down = sf - 2 * ( sf - sf_down );

			if ( sf_up < 0 ) sf_up = 0;
			if ( sf_down < 0 ) sf_down = 0;
		}

		// Get efficiency for this jet
		const double eff = getEfficiency( partonFlavour, jet );
		double sfToUse = sf;
		if ( systematic == 1 ) {
			sfToUse = sf_up;
		}
		else if ( systematic == -1 ) {
			sfToUse = sf_down;
		}

		if ( isBTagged ) {
			bTaggedMCJet *= eff;
			if ( eff*sfToUse > 1 ) {
				bTaggedDataJet *= 1;
			}
			else if ( eff*sfToUse < 0 ) {
				bTaggedDataJet *= 0;
			}
			else {
				bTaggedDataJet *= eff*sfToUse;
			}
		}
		else {
			nonBTaggedMCJet *= ( 1 - eff );

			if ( eff*sfToUse > 1 ) {
				nonBTaggedDataJet *= 0;
			}
			else if ( eff*sfToUse < 0 ) {
				nonBTaggedDataJet *= 1;
			}
			else {
				bTaggedDataJet *= ( 1 - eff*sfToUse );
			}
		}
		// if ( nonBTaggedMCJet < 0 || nonBTaggedDataJet < 0 ) {
		// 	cout << nonBTaggedMCJet << " " << nonBTaggedDataJet << endl;
		// 	cout << eff << " " << sfToUse << endl;
		// }
	}

	double bTagWeight = (nonBTaggedDataJet * bTaggedDataJet) / (nonBTaggedMCJet * bTaggedMCJet);

	return bTagWeight;
}

double BTagWeight::getEfficiency( const unsigned int partonFlavour, const JetPointer jet ) const {
	if ( partonFlavour == 5) { //b-quark
		return getBEfficiency( jet );
	}
	else if ( partonFlavour == 4) { //c-quark
		return getCEfficiency( jet );
	}
	else if ( partonFlavour == 3 || partonFlavour == 2 || partonFlavour == 1 || partonFlavour == 0) { // u/d/s/unclassified-quark
		return getUDSEfficiency ( jet );
	}
	else if ( partonFlavour == 21) { //gluon
		return getGEfficiency( jet );
	}
	else return 0.;
}

float BTagWeight::getBEfficiency(const JetPointer jet) const {

	const double jetPt = jet->pt();
	const double jetEta = jet->eta();
	// std::cout << "Jet Pt : " << jetPt << ", Jet Eta : " << jetEta << std::endl;
	int binNumber = Globals::bQuarkJet->FindBin( jetPt , jetEta );
	float BTagEff = Globals::bQuarkJet->GetBinContent( binNumber );
	// std::cout << "B-quark Jet, B Tag Efficiency : " << BTagEff << std::endl;
	return BTagEff;
}

double BTagWeight::getCEfficiency(const JetPointer jet) const {

	const double jetPt = jet->pt();
	const double jetEta = jet->eta();
	// std::cout << "Jet Pt : " << jetPt << ", Jet Eta : " << jetEta << std::endl;
	int binNumber = Globals::cQuarkJet->FindBin( jetPt , jetEta );
	float BTagEff = Globals::cQuarkJet->GetBinContent( binNumber );
	// std::cout << "C-quark Jet, B Tag Efficiency : " << BTagEff << std::endl;
	return BTagEff;
}

double BTagWeight::getUDSEfficiency(const JetPointer jet) const {

	const double jetPt = jet->pt();
	const double jetEta = jet->eta();
	// std::cout << "Jet Pt : " << jetPt << ", Jet Eta : " << jetEta << std::endl;
	int binNumber = Globals::udsQuarkJet->FindBin( jetPt , jetEta );
	float BTagEff = Globals::udsQuarkJet->GetBinContent( binNumber );
	// std::cout << "UDS-quark Jet, B Tag Efficiency : " << BTagEff << std::endl;
	return BTagEff;
}

double BTagWeight::getGEfficiency(const JetPointer jet) const {

	const double jetPt = jet->pt();
	const double jetEta = jet->eta();
	// std::cout << "Jet Pt : " << jetPt << ", Jet Eta : " << jetEta << std::endl;
	int binNumber = Globals::gluonJet->FindBin( jetPt , jetEta );
	float BTagEff = Globals::gluonJet->GetBinContent( binNumber );
	// std::cout << "Gluon Jet, B Tag Efficiency : " << BTagEff << std::endl;
	return BTagEff;
}

void BTagWeight::setNumberOfBtags(unsigned int min, unsigned int max) {
	minNumberOfTags_ = min;
	maxNumberOfTags_ = max;
}

}


/***********************************************************************************************************************
 *
 * Copyright (c)
 * 2015, ABB Schweiz AG
 * 2021, JOiiNT LAB, Fondazione Istituto Italiano di Tecnologia, Intellimech Consorzio per la Meccatronica.
 * 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 ABB 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.
 *
 ***********************************************************************************************************************
 *
 * Authors: Gianluca Lentini, Ugo Alberto Simioni
 * Date:18/01/2022
 * Version 1.0
 * Description: this package provides a ROS node that communicates with the controller using Robot Web Services 2.0, original code can be retrieved at https://github.com/ros-industrial/abb_libegm
 *
 ***********************************************************************************************************************
 */

#define _USE_MATH_DEFINES

#include <cmath>

#include <boost/math/quaternion.hpp>

#include "abb_libegm/egm_common_auxiliary.h"

namespace abb
{
namespace egm
{
/***********************************************************************************************************************
 * Math functions
 */

double saturate(const double value, const double lower, const double upper)
{
  return (value < lower ? lower : (value > upper ? upper : value));
}

void multiply(wrapper::Joints* p_j, const double factor)
{
  if (p_j)
  {
    for (int i = 0; i < p_j->values_size(); ++i)
    {
      p_j->set_values(i, p_j->values(i)*factor);
    }
  }
}

void multiply(wrapper::Cartesian* p_c, const double factor)
{
  if (p_c)
  {
    p_c->set_x(p_c->x()*factor);
    p_c->set_y(p_c->y()*factor);
    p_c->set_z(p_c->z()*factor);
  }
}

void multiply(wrapper::Euler* p_e, const double factor)
{
  if (p_e)
  {
    p_e->set_x(p_e->x()*factor);
    p_e->set_y(p_e->y()*factor);
    p_e->set_z(p_e->z()*factor);
  }
}

void multiply(wrapper::Quaternion* p_q, const double factor)
{
  if (p_q)
  {
    p_q->set_u0(p_q->u0()*factor);
    p_q->set_u1(p_q->u1()*factor);
    p_q->set_u2(p_q->u2()*factor);
    p_q->set_u3(p_q->u3()*factor);
  }
}

wrapper::Quaternion multiply(const wrapper::Quaternion& q1, const wrapper::Quaternion& q2)
{
  wrapper::Quaternion result;

  result.set_u0(q1.u0()*q2.u0() - q1.u1()*q2.u1() - q1.u2()*q2.u2() - q1.u3()*q2.u3());
  result.set_u1(q1.u0()*q2.u1() + q1.u1()*q2.u0() + q1.u2()*q2.u3() - q1.u3()*q2.u2());
  result.set_u2(q1.u0()*q2.u2() + q1.u2()*q2.u0() + q1.u3()*q2.u1() - q1.u1()*q2.u3());
  result.set_u3(q1.u0()*q2.u3() + q1.u3()*q2.u0() + q1.u1()*q2.u2() - q1.u2()*q2.u1());

  return result;
}

double dotProduct(const wrapper::Quaternion& q1, const wrapper::Quaternion& q2)
{
  return q1.u0()*q2.u0() + q1.u1()*q2.u1() + q1.u2()*q2.u2() + q1.u3()*q2.u3();
}

double euclideanNorm(const wrapper::Quaternion& q)
{
  return std::sqrt(dotProduct(q, q));
}

void normalize(wrapper::Quaternion* p_q)
{
  if (p_q)
  {
    double norm = euclideanNorm(*p_q);
    if (norm != 0.0)
    {
      p_q->set_u0(p_q->u0() / norm);
      p_q->set_u1(p_q->u1() / norm);
      p_q->set_u2(p_q->u2() / norm);
      p_q->set_u3(p_q->u3() / norm);
    }
  }
}

void convert(wrapper::Quaternion* p_q, const wrapper::Euler& e)
{
  if (p_q)
  {
    double z = e.z() * Constants::Conversion::DEG_TO_RAD;
    double y = e.y() * Constants::Conversion::DEG_TO_RAD;
    double x = e.x() * Constants::Conversion::DEG_TO_RAD;

    // Convert ZYX Euler angles to a rotation matrix.
    // See for example https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles for the equations.

    double cx = std::cos(0.5*x);
    double sx = std::sin(0.5*x);
    double cy = std::cos(0.5*y);
    double sy = std::sin(0.5*y);
    double cz = std::cos(0.5*z);
    double sz = std::sin(0.5*z);

    p_q->set_u0(sx*sy*sz + cx*cy*cz);
    p_q->set_u1(-cx*sy*sz + sx*cy*cz);
    p_q->set_u2(sx*cy*sz + cx*sy*cz);
    p_q->set_u3(cx*cy*sz - sx*sy*cz);

    normalize(p_q);
  }
}

void convert(wrapper::Euler* p_e, const wrapper::Quaternion& q)
{
  if(p_e && euclideanNorm(q) != 0.0)
  {
    // Convert a quaternion to ZYX Euler angles.
    // See for example https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles for the equations.
    //
    // Handle singularities.
    // See for example http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm
    // for indications of how to derive the equations.

    double y = 0.0;
    double z = 0.0;
    double x = 0.0;

    double u0 = q.u0();
    double u1 = q.u1();
    double u2 = q.u2();
    double u3 = q.u3();

    const double SINGULARITY_THRESHOLD = 0.000001;
    double singularity_check = u0*u2 - u1*u3;

    // Check for singularity (i.e. when y is close to +- 90 degrees).
    // This occur when the argument of y = sin(2*(u0*u2 - u1*u3)) is close to 1.0 -> u0*u2 - u1*u3 = 0.5.
    if (std::abs(singularity_check - 0.5) <= SINGULARITY_THRESHOLD)
    {
      // Y is close to 90 degrees.
      x = 2.0*std::atan2(u1, u0);
      y = M_PI_2;
    }
    else if (std::abs(singularity_check + 0.5) <= SINGULARITY_THRESHOLD)
    {
      // Y is close to -90 degrees.
      x = 2.0*std::atan2(u1, u0);
      y = -M_PI_2;
    }
    else
    {
      x = std::atan2(2.0*(u0*u1 + u2*u3), 1.0 - 2.0*(u1*u1 + u2*u2));
      y = std::asin(2.0*(u0*u2 - u1*u3));
      z = std::atan2(2.0*(u0*u3 + u1*u2), 1.0 - 2.0*(u2*u2 + u3*u3));
    }

    p_e->set_x(x*Constants::Conversion::RAD_TO_DEG);
    p_e->set_y(y*Constants::Conversion::RAD_TO_DEG);
    p_e->set_z(z*Constants::Conversion::RAD_TO_DEG);
  }
}

void convert(wrapper::Quaternion* p_dq, const wrapper::Quaternion& previous_q, const wrapper::Euler& av)
{
  if (p_dq)
  {
    wrapper::Quaternion temp;
    temp.set_u0(0.0);
    temp.set_u1(av.x()*Constants::Conversion::DEG_TO_RAD);
    temp.set_u2(av.y()*Constants::Conversion::DEG_TO_RAD);
    temp.set_u3(av.z()*Constants::Conversion::DEG_TO_RAD);

    p_dq->CopyFrom(multiply(temp, previous_q));
    multiply(p_dq, 0.5);
  }
}

/***********************************************************************************************************************
 * Estimation functions
 */

bool estimateVelocities(wrapper::Joints* p_estimate,
                        const wrapper::Joints& current,
                        const wrapper::Joints& previous,
                        const double sample_time)
{
  bool success = false;
  double delta_speed = 0.0;

  if (p_estimate && sample_time > 0.0)
  {
    p_estimate->Clear();

    for (int i = 0; i < current.values_size() && i < previous.values_size(); ++i)
    {
      delta_speed = current.values(i) - previous.values(i);
      p_estimate->add_values(delta_speed / sample_time);
    }
    success = true;
  }

  return success;
}

bool estimateVelocities(wrapper::Euler* p_estimate,
                        const wrapper::Quaternion& current,
                        const wrapper::Quaternion& previous,
                        const double sample_time)
{
  bool success = false;

  if (p_estimate && sample_time > 0.0)
  {
    // Estimate the angular velocity.
    // See for example https://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions for equations.
    // Note: Only valid for orientations, for the same object, at two points close in time.
    // Also assumes constant angular velocity between the points.
    boost::math::quaternion<double> q1(previous.u0(), previous.u1(),
                                       previous.u2(), previous.u3());

    boost::math::quaternion<double> q2(current.u0(), current.u1(),
                                       current.u2(), current.u3());

    boost::math::quaternion<double> estimation = ((2.0*(q2 - q1) / sample_time)*boost::math::conj(q1));

    p_estimate->set_x(estimation.R_component_2()*Constants::Conversion::RAD_TO_DEG);
    p_estimate->set_y(estimation.R_component_3()*Constants::Conversion::RAD_TO_DEG);
    p_estimate->set_z(estimation.R_component_4()*Constants::Conversion::RAD_TO_DEG);

    success = true;
  }

  return success;
}

bool estimateVelocities(wrapper::CartesianVelocity* p_estimate,
                        const wrapper::CartesianPose& current,
                        const wrapper::CartesianPose& previous,
                        const double sample_time)
{
  bool success = false;

  if (p_estimate && sample_time > 0.0)
  {
    // Estimate the linear velocity.
    p_estimate->mutable_linear()->set_x((current.position().x() - previous.position().x()) / sample_time);
    p_estimate->mutable_linear()->set_y((current.position().y() - previous.position().y()) / sample_time);
    p_estimate->mutable_linear()->set_z((current.position().z() - previous.position().z()) / sample_time);

    // Estimate the angular velocity.
    success = estimateVelocities(p_estimate->mutable_angular(),
                                 current.quaternion(),
                                 previous.quaternion(),
                                 sample_time);
  }

  return success;
}

/***********************************************************************************************************************
 * Find functions
 */

double findMaxDifference(const wrapper::Joints& j1, const wrapper::Joints& j2)
{
  double max_difference = 0.0;

  for (int i = 0; i < j1.values_size() && i < j2.values_size(); ++i)
  {
    max_difference = std::max(max_difference, std::abs(j1.values(i) - j2.values(i)));
  }

  return max_difference;
}

double findMaxDifference(const wrapper::Cartesian& c1, const wrapper::Cartesian& c2)
{
  double max_difference = std::abs(c1.x() - c2.x());
  max_difference = std::max(max_difference, std::abs(c1.y() - c2.y()));
  max_difference = std::max(max_difference, std::abs(c1.z() - c2.z()));

  return max_difference;
}

double findMaxDifference(const wrapper::Euler& e1, const wrapper::Euler& e2)
{
  double max_difference = std::abs(e1.x() - e2.x());
  max_difference = std::max(max_difference, std::abs(e1.y() - e2.y()));
  max_difference = std::max(max_difference, std::abs(e1.z() - e2.z()));

  return max_difference;
}

/***********************************************************************************************************************
 * Copy functions
 */

void copyPresent(wrapper::Joints* p_target, const wrapper::Joints& source)
{
  if (p_target)
  {
    for (int i = 0; i < source.values_size() && i < p_target->values_size(); ++i)
    {
      p_target->set_values(i, source.values(i));
    }
  }
}

void copyPresent(wrapper::JointSpace* p_target, const wrapper::JointSpace& source)
{
  if (p_target)
  {
    if (source.has_position())
    {
      copyPresent(p_target->mutable_position(), source.position());
    }

    if (source.has_velocity())
    {
      copyPresent(p_target->mutable_velocity(), source.velocity());
    }
  }
}

void copyPresent(wrapper::Cartesian* p_target, const wrapper::Cartesian& source)
{
  if (p_target)
  {
    if (source.has_x())
    {
      p_target->set_x(source.x());
    }

    if (source.has_y())
    {
      p_target->set_y(source.y());
    }

    if (source.has_z())
    {
      p_target->set_z(source.z());
    }
  }
}

void copyPresent(wrapper::Euler* p_target, const wrapper::Euler& source)
{
  if (p_target)
  {
    if (source.has_x())
    {
      p_target->set_x(source.x());
    }

    if (source.has_y())
    {
      p_target->set_y(source.y());
    }

    if (source.has_z())
    {
      p_target->set_z(source.z());
    }
  }
}

void copyPresent(wrapper::Quaternion* p_target, const wrapper::Quaternion& source)
{
  if (p_target)
  {
    if (source.has_u0())
    {
      p_target->set_u0(source.u0());
    }

    if (source.has_u1())
    {
      p_target->set_u1(source.u1());
    }

    if (source.has_u2())
    {
      p_target->set_u2(source.u2());
    }

    if (source.has_u3())
    {
      p_target->set_u3(source.u3());
    }

    normalize(p_target);
  }
}

void copyPresent(wrapper::CartesianPose* p_target, const wrapper::CartesianPose& source)
{
  if (p_target)
  {
    if (source.has_position())
    {
      copyPresent(p_target->mutable_position(), source.position());
    }

    if (source.has_euler())
    {
      copyPresent(p_target->mutable_euler(), source.euler());
      convert(p_target->mutable_quaternion(), p_target->euler());
    }
    else if (source.has_quaternion())
    {
      copyPresent(p_target->mutable_quaternion(), source.quaternion());
      normalize(p_target->mutable_quaternion());
      convert(p_target->mutable_euler(), p_target->quaternion());
    }
  }
}

void copyPresent(wrapper::CartesianVelocity* p_target, const wrapper::CartesianVelocity& source)
{
  if (p_target)
  {
    if (source.has_linear())
    {
      copyPresent(p_target->mutable_linear(), source.linear());
    }

    if (source.has_angular())
    {
      copyPresent(p_target->mutable_angular(), source.angular());
    }
  }
}

void copyPresent(wrapper::CartesianSpace* p_target, const wrapper::CartesianSpace& source)
{
  if (p_target)
  {
    if (source.has_pose())
    {
      copyPresent(p_target->mutable_pose(), source.pose());
    }

    if (source.has_velocity())
    {
      copyPresent(p_target->mutable_velocity(), source.velocity());
    }
  }
}

void copyPresent(wrapper::Robot* p_target, const wrapper::Robot& source)
{
  if (p_target)
  {
    if (source.has_joints())
    {
      copyPresent(p_target->mutable_joints(), source.joints());
    }

    if (source.has_cartesian())
    {
      copyPresent(p_target->mutable_cartesian(), source.cartesian());
    }
  }
}

void copyPresent(wrapper::External* p_target, const wrapper::External& source)
{
  if (p_target)
  {
    if (source.has_joints())
    {
      copyPresent(p_target->mutable_joints(), source.joints());
    }
  }
}

void copyPresent(wrapper::Output* p_target, const wrapper::Output& source)
{
  if (p_target)
  {
    if (source.has_robot())
    {
      copyPresent(p_target->mutable_robot(), source.robot());
    }

    if (source.has_external())
    {
      copyPresent(p_target->mutable_external(), source.external());
    }
  }
}

/***********************************************************************************************************************
 * Parse functions
 */

bool parse(wrapper::Header* p_target, const EgmHeader& source)
{
  bool success = false;

  if (p_target && source.has_seqno() && source.has_tm() && source.has_mtype())
  {
    p_target->set_sequence_number(source.seqno());
    p_target->set_time_stamp(source.tm());

    switch (source.mtype())
    {
      case EgmHeader_MessageType_MSGTYPE_DATA:
      {
        p_target->set_message_type(wrapper::Header_MessageType_DATA);
        success = true;
      }
      break;

      case EgmHeader_MessageType_MSGTYPE_UNDEFINED:
      case EgmHeader_MessageType_MSGTYPE_COMMAND:
      case EgmHeader_MessageType_MSGTYPE_CORRECTION:
      case EgmHeader_MessageType_MSGTYPE_PATH_CORRECTION:
      default:
      {
        p_target->set_message_type(wrapper::Header_MessageType_UNDEFINED);
      }
    }
  }

  return success;
}

bool parse(wrapper::Status* p_target, const EgmRobot& source)
{
  bool success = false;

  if (p_target &&
      source.has_motorstate() && source.motorstate().has_state() &&
      source.has_mcistate() && source.mcistate().has_state() &&
      source.has_rapidexecstate() && source.rapidexecstate().has_state() &&
      source.has_mciconvergencemet())
  {
    switch (source.motorstate().state())
    {
      case EgmMotorState_MotorStateType_MOTORS_UNDEFINED:
      {
        p_target->set_motor_state(wrapper::Status_MotorState_MOTORS_UNDEFINED);
      }
      break;

      case EgmMotorState_MotorStateType_MOTORS_ON:
      {
        p_target->set_motor_state(wrapper::Status_MotorState_MOTORS_ON);
      }
      break;

      case EgmMotorState_MotorStateType_MOTORS_OFF:
      {
        p_target->set_motor_state(wrapper::Status_MotorState_MOTORS_OFF);
      }
      break;

      default:
      {
        p_target->set_motor_state(wrapper::Status_MotorState_MOTORS_UNDEFINED);
      }
    }

    switch (source.mcistate().state())
    {
      case EgmMCIState_MCIStateType_MCI_UNDEFINED:
      {
        p_target->set_egm_state(wrapper::Status_EGMState_EGM_UNDEFINED);
      }
      break;

      case EgmMCIState_MCIStateType_MCI_ERROR:
      {
        p_target->set_egm_state(wrapper::Status_EGMState_EGM_ERROR);
      }
      break;

      case EgmMCIState_MCIStateType_MCI_STOPPED:
      {
        p_target->set_egm_state(wrapper::Status_EGMState_EGM_STOPPED);
      }
      break;

      case EgmMCIState_MCIStateType_MCI_RUNNING:
      {
        p_target->set_egm_state(wrapper::Status_EGMState_EGM_RUNNING);
      }
      break;

      default:
      {
        p_target->set_egm_state(wrapper::Status_EGMState_EGM_UNDEFINED);
      }
    }

    switch (source.rapidexecstate().state())
    {
      case EgmRapidCtrlExecState_RapidCtrlExecStateType_RAPID_UNDEFINED:
      {
        p_target->set_rapid_execution_state(wrapper::Status_RAPIDExecutionState_RAPID_UNDEFINED);
      }
      break;

      case EgmRapidCtrlExecState_RapidCtrlExecStateType_RAPID_STOPPED:
      {
        p_target->set_rapid_execution_state(wrapper::Status_RAPIDExecutionState_RAPID_STOPPED);
      }
      break;

      case EgmRapidCtrlExecState_RapidCtrlExecStateType_RAPID_RUNNING:
      {
        p_target->set_rapid_execution_state(wrapper::Status_RAPIDExecutionState_RAPID_RUNNING);
      }
      break;

      default:
      {
        p_target->set_rapid_execution_state(wrapper::Status_RAPIDExecutionState_RAPID_UNDEFINED);
      }
    }

    p_target->set_egm_convergence_met(source.mciconvergencemet());

    success = true;
  }

  return success;
}

bool parse(wrapper::Clock* p_target, const EgmClock& source)
{
  bool success = true;

  if (p_target && source.has_sec() && source.has_usec())
  {
    p_target->set_sec(source.sec());
    p_target->set_usec(source.usec());
  }
  else
  {
    success = false;
  }

  return success;
}

bool parse(wrapper::Joints* p_target_robot,
           wrapper::Joints* p_target_external,
           const EgmJoints& source_robot,
           const EgmJoints& source_external,
           const RobotAxes axes)
{
  bool success = false;

  if (p_target_robot && p_target_external)
  {
    p_target_robot->Clear();
    p_target_external->Clear();

    switch (axes)
    {
      case Six:
      {
        if (source_robot.joints_size() == Constants::RobotController::DEFAULT_NUMBER_OF_ROBOT_JOINTS)
        {
          for (int i = 0; i < source_robot.joints_size(); ++i)
          {
            p_target_robot->add_values(source_robot.joints(i));
          }

          for (int i = 0; i < source_external.joints_size(); ++i)
          {
            p_target_external->add_values(source_external.joints(i));
          }

          success = true;
        }
      }
      break;

      case Seven:
      {
        // If using a seven axes robot (e.g. IRB14000): Map to special case.
        if (source_robot.joints_size() == Constants::RobotController::DEFAULT_NUMBER_OF_ROBOT_JOINTS &&
            source_external.joints_size() >= 1)
        {
          p_target_robot->add_values(source_robot.joints(0));
          p_target_robot->add_values(source_robot.joints(1));
          p_target_robot->add_values(source_external.joints(0));
          p_target_robot->add_values(source_robot.joints(2));
          p_target_robot->add_values(source_robot.joints(3));
          p_target_robot->add_values(source_robot.joints(4));
          p_target_robot->add_values(source_robot.joints(5));

          for (int i = 1; i < source_external.joints_size(); ++i)
          {
            p_target_external->add_values(source_external.joints(i));
          }

          success = true;
        }
      }
      break;
    }
  }

  return success;
}

bool parse(wrapper::CartesianPose* p_target, const EgmPose& source)
{
  bool success = true;

  if (p_target)
  {
    p_target->Clear();

    if (source.has_pos() && source.pos().has_x() && source.pos().has_y() && source.pos().has_z())
    {
      p_target->mutable_position()->set_x(source.pos().x());
      p_target->mutable_position()->set_y(source.pos().y());
      p_target->mutable_position()->set_z(source.pos().z());
    }
    else
    {
      success = false;
    }

    if (success &&
        source.has_orient() &&
        source.orient().has_u0() &&
        source.orient().has_u1() &&
        source.orient().has_u2() &&
        source.orient().has_u3())
    {
      p_target->mutable_quaternion()->set_u0(source.orient().u0());
      p_target->mutable_quaternion()->set_u1(source.orient().u1());
      p_target->mutable_quaternion()->set_u2(source.orient().u2());
      p_target->mutable_quaternion()->set_u3(source.orient().u3());
    }
    else
    {
      success = false;
    }

    if (success)
    {
      if (source.has_euler() && source.euler().has_x() && source.euler().has_y() && source.euler().has_z())
      {
        p_target->mutable_euler()->set_x(source.euler().x());
        p_target->mutable_euler()->set_y(source.euler().y());
        p_target->mutable_euler()->set_z(source.euler().z());
      }
      else
      {
        convert(p_target->mutable_euler(), p_target->quaternion());
      }
    }
  }

  return success;
}

bool parse(wrapper::Feedback* p_target, const EgmFeedBack& source, const RobotAxes axes)
{
  bool success = false;

  if (p_target)
  {
    success = parse(p_target->mutable_robot()->mutable_joints()->mutable_position(),
                    p_target->mutable_external()->mutable_joints()->mutable_position(),
                    source.joints(), source.externaljoints(), axes);

    if (success)
    {
      success = parse(p_target->mutable_robot()->mutable_cartesian()->mutable_pose(), source.cartesian());

      if (success)
      {
        success = parse(p_target->mutable_time(), source.time());
      }
    }
  }

  return success;
}

bool parse(wrapper::Planned* p_target, const EgmPlanned& source, const RobotAxes axes)
{
  bool success = false;

  if (p_target)
  {
    success = parse(p_target->mutable_robot()->mutable_joints()->mutable_position(),
                    p_target->mutable_external()->mutable_joints()->mutable_position(),
                    source.joints(), source.externaljoints(), axes);

    if (success)
    {
      success = parse(p_target->mutable_robot()->mutable_cartesian()->mutable_pose(), source.cartesian());

      if (success)
      {
        success = parse(p_target->mutable_time(), source.time());
      }
    }
  }

  return success;
}

/***********************************************************************************************************************
 * Reset functions
 */

void reset(wrapper::Joints* p_joints, const unsigned int number_of_joints)
{
  if (p_joints)
  {
    p_joints->Clear();

    for (unsigned int i = 0; i < number_of_joints; ++i)
    {
      p_joints->add_values(0.0);
    }
  }
}

void reset(wrapper::Cartesian* p_cartesian)
{
  if (p_cartesian)
  {
    p_cartesian->set_x(0.0);
    p_cartesian->set_y(0.0);
    p_cartesian->set_z(0.0);
  }
}

void reset(wrapper::Euler* p_euler)
{
  if (p_euler)
  {
    p_euler->set_x(0.0);
    p_euler->set_y(0.0);
    p_euler->set_z(0.0);
  }
}

} // end namespace egm
} // end namespace abb