// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

// #define MTL_DEBUG_DMAT_DMAT_MULT // For debugging evidently

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

template <typename Matrix>
void list_entries(const Matrix& A, int begin, int end)
{
    using namespace mtl; using namespace mtl::tag; using mtl::tag::major; using mtl::traits::range_generator;
    typedef typename range_generator<major, Matrix>::type     cur_type;
    typedef typename range_generator<nz, cur_type>::type      icur_type;
    typename mtl::traits::col<Matrix>::type                   col(A);
    typename mtl::traits::row<Matrix>::type                   row(A);
    typename mtl::traits::const_value<Matrix>::type           value(A);

    for(cur_type c= mtl::begin<major>(A), cend= mtl::end<major>(A); c != cend; ++c)
	for(icur_type kc= mtl::begin<nz>(c), kend= mtl::end<nz>(c); kc != kend; ++kc) {
	    std::cout << "A[" << row(*kc) << "][" << col(*kc) << "] = " << value(*kc) << "\n";
	    int band= col(*kc) - row(*kc);
	    if (band < begin) std::cout << "outside on the left!\n";
	    if (band >= end) std::cout << "outside on the right!\n";
	}
}

template <typename Matrix>
void check(const Matrix& A, int begin, int end)
{
    // list_entries(A, begin, end);
    typedef typename mtl::Collection<Matrix>::value_type   value_type;
    typedef typename mtl::Collection<Matrix>::size_type    size_type;

    for (size_type i= 0; i < num_rows(A); i++)
	for (size_type j= 0; j < num_cols(A); j++) {
	    long band= long(j) - long(i);
	    if (band < begin) {
		MTL_THROW_IF(A[i][j] != value_type(0), mtl::runtime_error("Value must be zero left of the bands"));
	    } else if (band >= end) {
		MTL_THROW_IF(A[i][j] != value_type(0), mtl::runtime_error("Value must be zero right of the bands"));
	    } else
		MTL_THROW_IF(A[i][j] != value_type(band + 0), mtl::runtime_error("Wrong non-zero value within the bands"));
	}
}

template <typename Matrix>
void test(Matrix& A, const char* name)
{
    using mtl::mat::inserter; using mtl::Collection;

    typedef typename Collection<Matrix>::value_type   value_type;

    A.change_dim(6, 5);
    {
	inserter<Matrix>   ins(A);
	for (unsigned i= 0; i < num_rows(A); i++)
	    for (unsigned j= 0; j < num_cols(A); j++)
		ins[i][j]= value_type(j) - value_type(i);
    }
    cout << "\n" << name << "\n";
    cout << "\n" << name << "\n" << "A =\n" << A;
    Matrix B( bands(A, 2, 4) );
    cout << "\nbands(A, 2, 4) = \n" << B;
    check(B, 2, 4);

    Matrix U( upper(A) );
    cout << "\nupper(A) = \n" << U;
    check(U, 0, 10000);

    Matrix SU( strict_upper(A) );
    cout << "\nstrict_upper(A) = \n" << SU;
    check(SU, 1, 10000);

    Matrix L( lower(A) );
    cout << "\nlower(A) = \n" << L;
    check(L, -10000, 1);

    Matrix SL( strict_lower(A) );
    cout << "\nstrict_lower(A) = \n" << SL;
    check(SL, -10000, 0);

    Matrix P( trans(A) * upper(A) );
    cout << " trans(A) * upper(A) = \n" << with_format(P, 4, 3);
    Matrix P_cmp( trans(A) * U );
    Matrix P_cmp2( trans(A) * A );
    cout << "\ntrans(A) * upper(A) = \n" << with_format(P, 4, 3);
    cout << " for comparison trans(A) * U = \n" << with_format(P_cmp, 4, 3);
    cout << " for comparison trans(A) * A = \n" << with_format(P_cmp2, 4, 3);
    MTL_THROW_IF(abs(P[1][1] - P_cmp[1][1]) > .00001, mtl::runtime_error("Multiplication wrong"));

#if 0
    // Take this out as sparse matrices have no sub-matrix; only for debugging wrong products
    Matrix A2= sub_matrix(A, 0, 3, 0, 3), U2= upper(A2);
    Matrix P2= A2 * upper(A2); //, P2_cmp= A2 * U2, A2_square= A2 * A2;
    cout << "\nA2 * upper(A2) = \n" << with_format(P2, 4, 3);
    cout << " for comparison A2 * U = \n" << with_format(P2_cmp, 4, 3);
    cout << " for comparison A2 * A2 = \n" << with_format(A2_square, 4, 3);
    MTL_THROW_IF(abs(P2[1][1] - P2_cmp[1][1]) > .00001, mtl::runtime_error("Multiplication wrong"));
#endif

#if 0
    // Would too painfully slow !

    Matrix D( diagonal(A) );
    cout << "\ndiagonal(A) = \n" << D;
    check(D, 0, 1);

    Matrix T( tri_diagonal(A) );
    cout << "\ntri_diagonal(A) = \n" << T;
    check(T, 0, 1);
#endif
}

int main(int, char**)
{
    using namespace mtl;

    dense2D<double>                                      dr;
    dense2D<double, mat::parameters<col_major> >      dc;
    morton_dense<double, recursion::morton_z_mask>       mzd;
    morton_dense<double, recursion::doppled_2_row_mask>  d2r;
    compressed2D<double>                                 cr;
    compressed2D<double, mat::parameters<col_major> > cc;

    dense2D<complex<double> >                            drc;
    compressed2D<complex<double> >                       crc;

    test(dr, "Dense row major");
    test(dc, "Dense column major");
    test(mzd, "Morton Z-order");
    test(d2r, "Hybrid 2 row-major");
    test(cr, "Compressed row major");
    test(drc, "Dense row major complex");
    test(crc, "Compressed row major complex");

    // For better readability I don't want finish with a complex
    test(cc, "Compressed column major");

    return 0;
}


#include "core/PPE/Context.hpp"
#include "core/PPE/PubKey.hpp"
#include "core/PPE/SecKey.hpp"
#include "core/PPE/EncVec.hpp"
#include "core/PPE/EncMat.hpp"

#include <NTL/mat_ZZ.h>
#include <NTL/vec_ZZ.h>
#include <NTL/vec_ZZ_p.h>
#include <NTL/mat_ZZ_p.h>

#include <iostream>
#include <fstream>

int test_EncVec() {
    std::vector<long> Ms = {2048, 4096};
    std::vector<long> Ps = {13, 23};
    std::vector<long> Rs = {1, 1};
    ppe::Context context(Ms, Ps, Rs);
    context.buildModChain(7);
    ppe::SecKey sk(context);
    sk.GenSecKey(64);
    sk.addSome1DMatrices();
    ppe::PubKey pk(sk);
    NTL::ZZ P(1);
    for (long p : Ps)
        P *= p;

    Vector vec;
    vec.SetLength(10);
    for (int i = 0; i < vec.length(); i++)
        vec[i] = NTL::to_ZZ(i * 10);

    ppe::EncVec encVec(pk);
    encVec.pack(vec);
    {
        Vector vec2;
        encVec.unpack(vec2, sk);
        if (vec != vec2)
            return -1;
    } // Test packing and unpacking

    {
        ppe::EncVec encVec2(encVec);
        encVec2.add(encVec);
        ppe::EncVec encVec3(encVec);
        encVec3.add(vec);

        Vector vec2, vec3;
        encVec2.unpack(vec2, sk);
        encVec3.unpack(vec3, sk);

        NTL::vec_ZZ_p vec_p, vec_p_2;
        NTL::ZZ_p::init(P);
        NTL::conv(vec_p, vec);
        NTL::add(vec_p_2, vec_p, vec_p);

        NTL::vec_ZZ_p computed;
        NTL::conv(computed, vec2);
        if (computed != vec_p_2)
            return -1;
        NTL::conv(computed, vec3);
        if (computed != vec_p_2)
            return -1;

        ppe::EncVec encVec4(pk);
        encVec4.add(encVec);
        encVec4.unpack(vec2, sk);
        NTL::conv(computed, vec2);
        if (computed != vec_p)
            return -1;

        ppe::EncVec encVec5(pk);
        encVec5.sub(encVec);
        encVec5.unpack(vec2, sk);
        NTL::conv(computed, vec2);
        NTL::negate(vec_p_2, vec_p);
        if (computed !=  vec_p_2)
            return -1;
    } // Test addition

    {
        ppe::EncVec encVec2(encVec);
        ppe::EncVec encVec3(encVec);
        encVec2.sub(encVec);
        encVec3.sub(vec);
        Vector vec2, vec3;
        encVec2.unpack(vec2, sk);
        encVec3.unpack(vec3, sk);
        for (auto itr = vec2.begin(); itr != vec2.end(); itr++)
            if (*itr != 0) return -1;
        for (auto itr = vec3.begin(); itr != vec3.end(); itr++)
            if (*itr != 0) return -1;
    } // Test subtraction

    return 0;
}

int test_io() {
    std::vector <long> Ms = {2048, 4096};
    std::vector <long> Ps = {13, 23};
    std::vector <long> Rs = {1, 1};
    ppe::Context context(Ms, Ps, Rs);
    context.buildModChain(7);

    ppe::SecKey sk(context);
    sk.GenSecKey(64);
    sk.addSome1DMatrices();

    ppe::PubKey pk(sk);
    Vector vec;
    vec.SetLength(7);
    for (int i = 0; i < vec.length(); i++)
        vec[i] = NTL::to_ZZ(i * 10);
    ppe::EncVec encVec(pk);
    encVec.pack(vec);

    {
        std::ofstream out("./tmp.context", std::ios::binary);
        if (!out.is_open()) { std::cerr << "can not create tmp.context" << std::endl; return -1; }
        if (!context.dump(out)) {
            out.close();
            return -1;
        }
        out.close();
    }
    {
        std::ofstream out("./tmp.sk", std::ios::binary);
        if (!out.is_open()) { std::cerr << "can not create tmp.sk" << std::endl; return -1; }
        if (!sk.dump(out)) {
            out.close();
            return -1;
        }
        out.close();
    }
    {
        std::ofstream out("./tmp.pk", std::ios::binary);
        if (!out.is_open()) { std::cerr << "can not create tmp.pk" << std::endl; return -1; }
        if (!pk.dump(out)) {
            out.close();
            return -1;
        }
        out.close();
    }

    ppe::Context context2;
    ppe::SecKey sk2;
    ppe::PubKey pk2;
    {
        std::ifstream in("./tmp.context", std::ios::binary);
        if (!context2.restore(in)) {
            in.close();
            return -1;
        }
        in.close();
    }
    {
        std::ifstream in("./tmp.sk", std::ios::binary);
        if (!sk2.restore(in, context2)) {
            in.close();
            return -1;
        }
        in.close();
    }
    {
        std::ifstream in("./tmp.pk", std::ios::binary);
        if (!pk2.restore(in, context2)) {
            in.close();
            return -1;
        }
        in.close();
    }
    {
        Vector vec2;
        ppe::EncVec encVec2(pk2);
        encVec2.pack(vec);
        encVec2.unpack(vec2, sk2);
        if (vec!=vec2) { std::cerr << "IO for context & pk & sk failed" << std::endl; return -1; }
    } // Test IO for Context, PubKey, and SecKey

    {
        ppe::EncVec encVec(pk2);
        ppe::EncMat encMat(pk2);
        NTL::vec_ZZ vec;
        NTL::mat_ZZ mat;
        vec.SetLength(10);
        for (long i = 0; i < vec.MaxLength(); i++) vec[i] = 10;

        mat.SetDims(7, 5);
        for (long r = 0; r < mat.NumRows(); r++)
            for (long c = 0; c < mat.NumCols(); c++) mat[r][c] = 10;

        encVec.pack(vec);
        encMat.pack(mat);

        std::ofstream out("./encVec.ctxt", std::ios::binary);
        if (!out.is_open()) { std::cerr << "Can not create encVec.ctxt\n"; return -1; }
        encVec.dump(out);
        out.close();

        out = std::ofstream("./encMat.ctxt", std::ios::binary);
        if (!out.is_open()) { std::cerr << "Can not create encMat.ctxt\n"; return -1; }
        encMat.dump(out);
        out.close();

        std::ifstream in("./encVec.ctxt", std::ios::binary);
        ppe::EncVec encVec2(pk2);
        encVec2.restore(in);
        NTL::vec_ZZ vec2;
        encVec2.unpack(vec2, sk2);
        in.close();
        if (vec2 != vec) { std::cerr << "IO for EncVec failed" << std::endl; }

        ppe::EncMat encMat2(pk2);
        NTL::mat_ZZ mat2;
        in = std::ifstream("./encMat.ctxt", std::ios::binary);
        encMat2.restore(in);
        encMat2.unpack(mat2, sk2);
        if (mat2 != mat) { std::cerr << "IO for EncMat failed" << std::endl; return -1; }
    }// Test IO for ppe::EncMat and ppe::EncVec
    return 0;
}

int test_EncMat() {
    std::vector<long> Ms = {512};
    std::vector<long> Ps = {8209};
    std::vector<long> Rs = {1};
    ppe::Context context(Ms, Ps, Rs);
    context.buildModChain(15);
    ppe::SecKey sk(context);
    sk.GenSecKey(64);
    sk.addSome1DMatrices();
    ppe::PubKey pk(sk);
    NTL::ZZ P(1);
    for (long p : Ps)
        P *= p;

    Matrix mat;
    Vector vec;
    mat.SetDims(7, 7);
    vec.SetLength(7);
    for (long r = 0; r < mat.NumRows(); r++) {
        for (long c = r; c < mat.NumCols(); c++) {
            mat[r][c] = NTL::to_ZZ(r * 10 + c);
            mat[c][r] = NTL::to_ZZ(r * 10 + c);
        }
        vec[r] = 1;
    }

    ppe::EncMat encMat(pk);
    encMat.pack(mat);
    ppe::EncVec encVec(pk);
    encVec.pack(vec);
    {
        Matrix mat2;
        encMat.unpack(mat2, sk);
        NTL::ZZ_p::init(P);
        NTL::mat_ZZ_p computed, groud_truth;
        NTL::conv(groud_truth, mat);
        NTL::conv(computed, mat2);
        if (groud_truth != computed) {
            std::cerr << "Test pack & unpack failed" << std::endl;
            return -1;
        }
    } // test pack & unpack
    {
        ppe::EncMat encMat2(encMat);
        encMat2.add(mat);
        Matrix mat2;
        encMat2.unpack(mat2, sk);

        NTL::ZZ_p::init(P);
        NTL::mat_ZZ_p computed, groud_truth;
        NTL::conv(groud_truth, mat);
        groud_truth *= 2;

        NTL::conv(computed, mat2);
        if (groud_truth != computed) {
            std::cerr << "Test matrix addtion (1) failed " << std::endl;
            return -1;
        }

        ppe::EncMat encMat3(encMat);
        encMat3.add(encMat);
        encMat3.unpack(mat2, sk);
        NTL::conv(computed, mat2);
        if (groud_truth != computed) {
            std::cerr << "Test matrix addtion (2) failed " << std::endl;
            return -1;
        }

        NTL::conv(groud_truth, mat);
        ppe::EncMat encMat4(pk);
        encMat4.add(mat);
        encMat4.unpack(mat2, sk);
        NTL::conv(computed, mat2);
        if (groud_truth != computed) {
            std::cerr << "Test matrix addtion (3) failed " << std::endl;
            return -1;
        }

        ppe::EncMat encMat5(pk);
        encMat5.sub(encMat);
        encMat5.unpack(mat2, sk);
        NTL::conv(computed, mat2);
        NTL::mat_ZZ_p negated;
        NTL::negate(negated, groud_truth);
        if (negated != computed) {
            std::cerr << "Test matrix addtion (4) failed " << std::endl;
            return -1;
        }
    } // test add
    {
        ppe::EncMat encMat2(encMat);
        encMat2.sub(encMat);
        Matrix mat2;
        encMat2.unpack(mat2, sk);
        for (long r = 0; r < mat2.NumRows(); r++) {
            for (long c = 0; c < mat2.NumCols(); c++) {
                if (mat2[r][c]!=0) {
                    std::cerr << "Test matrix sub (1) failed " << std::endl;
                    return -1;
                }
            }
        }

        ppe::EncMat encMat3(encMat);
        encMat3.sub(mat);
        encMat2.unpack(mat2, sk);
        for (long r = 0; r < mat2.NumRows(); r++)
            for (long c = 0; c < mat2.NumCols(); c++) {
                if (mat2[r][c] != 0) {
                    std::cerr << "Test matrix sub (2) failed " << std::endl;
                    return -1;
                }
            }
    } // test sub
    {
        ppe::EncVec ret = encMat.sym_dot(encVec);
        Vector vec2;
        ret.unpack(vec2, sk);

        NTL::ZZ_p::init(P);
        NTL::mat_ZZ_p mat_p;
        NTL::vec_ZZ_p vec_p, ret_p, computed;

        NTL::conv(mat_p, mat);
        NTL::conv(vec_p, vec);
        NTL::conv(computed, vec2);
        NTL::mul(ret_p, mat_p, vec_p);
        if (computed != ret_p) {
            std::cerr << "Test matrix-vec mult (1) failed " << std::endl;
            return -1;
        }
    } // test matrix-vector multiplication
    return 0;
}

int main() {
//    if (test_EncVec() != 0)
//        return -1;
    if (test_EncMat() != 0)
        return -1;
//    if (test_io() != 0)
//        return -1;
    return 0;
}


/*==============================================================================

  Program: 3D Slicer

  Portions (c) Copyright Brigham and Women's Hospital (BWH) All Rights Reserved.

  See COPYRIGHT.txt
  or http://www.slicer.org/copyright/copyright.txt for details.

  Unless required by applicable law or agreed to in writing, software
  distributed under the License is distributed on an "AS IS" BASIS,
  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  See the License for the specific language governing permissions and
  limitations under the License.

==============================================================================*/
#include "Spoke.h"
#include <math.h>
#include <vtkMath.h>
// Eigen includes
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
Spoke::Spoke(){}

Spoke::Spoke(double radius, double px, double py, double pz, double ux, double uy, double uz)
{
    mR = radius;
    mPx = px;
    mPy = py;
    mPz = pz;

    double r = sqrt(ux * ux + uy * uy + uz * uz);
    mUx = ux / r;
    mUy = uy / r;
    mUz = uz / r;
}

Spoke::Spoke(double *ptSkeletal, double *ptBoundary)
{
    mPx = ptSkeletal[0];
    mPy = ptSkeletal[1];
    mPz = ptSkeletal[2];

    double diffX = ptBoundary[0] - ptSkeletal[0];
    double diffY = ptBoundary[1] - ptSkeletal[1];
    double diffZ = ptBoundary[2] - ptSkeletal[2];
    mR = sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ);

    mUx = diffX / mR;
    mUy = diffY / mR;
    mUz = diffZ / mR;
}

Spoke::Spoke(const Spoke &other)
{
    double u[3], p[3], r;
    other.GetDirection(u);
    other.GetSkeletalPoint(p);
    r = other.GetRadius();

    mR = r;
    mPx = p[0];
    mPy = p[1];
    mPz = p[2];

    mUx = u[0];
    mUy = u[1];
    mUz = u[2];
}

Spoke &Spoke::operator=(const Spoke &other)
{
    double u[3], p[3], r;
    other.GetDirection(u);
    other.GetSkeletalPoint(p);
    r = other.GetRadius();

    mR = r;
    mPx = p[0];
    mPy = p[1];
    mPz = p[2];

    mUx = u[0];
    mUy = u[1];
    mUz = u[2];
    return *this;
}

void Spoke::SetRadius(double r)
{
    mR = r;
}

void Spoke::SetSkeletalPoint(double px, double py, double pz)
{
    mPx = px;
    mPy = py;
    mPz = pz;
}

void Spoke::SetDirection(double *u)
{
    vtkMath::Normalize(u);
    mUx = u[0];
    mUy = u[1];
    mUz = u[2];
}

void Spoke::GetDirection(double *output) const
{
    output[0] = this->mUx;
    output[1] = this->mUy;
    output[2] = this->mUz;

    vtkMath::Normalize(output);
}

double Spoke::GetRadius() const
{
    return mR;
}

void Spoke::GetSkeletalPoint(double *output) const
{
    output[0] = mPx;
    output[1] = mPy;
    output[2] = mPz;
}

void Spoke::Add(Spoke *another, double *output) const
{
    double eX = this->mR * this->mUx;
    double eY = this->mR * this->mUy;
    double eZ = this->mR * this->mUz;

    double aX = another->mR * another->mUx;
    double aY = another->mR * another->mUy;
    double aZ = another->mR * another->mUz;

    output[0] = eX + aX;
    output[1] = eY + aY;
    output[2] = eZ + aZ;

}

void Spoke::Diff(Spoke *another, double *output) const
{
    double eX = this->mR * this->mUx;
    double eY = this->mR * this->mUy;
    double eZ = this->mR * this->mUz;

    double aX = another->mR * another->mUx;
    double aY = another->mR * another->mUy;
    double aZ = another->mR * another->mUz;

    output[0] = eX - aX;
    output[1] = eY - aY;
    output[2] = eZ - aZ;
}

void Spoke::GetBoundaryPoint(double *output) const
{
    output[0] = mPx + mR * mUx;
    output[1] = mPy + mR * mUy;
    output[2] = mPz + mR * mUz;
}

void Spoke::SetNeighborU(const std::vector<Spoke *> &neighbors, bool isForward)
{
    mNeighborsU.clear();
    for (size_t i = 0; i < neighbors.size(); ++i) {
        mNeighborsU.push_back(neighbors[i]);
    }
    mIsForwardU = isForward;
}

void Spoke::SetNeighborV(const std::vector<Spoke *> &neighbors, bool isForward)
{
    mNeighborsV.clear();
    for (size_t i = 0; i < neighbors.size(); ++i) {
        mNeighborsV.push_back(neighbors[i]);
    }
    mIsForwardV = isForward;
}

double Spoke::GetRSradPenalty(double stepSize)
{
    // 1. compute derivatives
    double dxdu[3], dxdv[3], dSdu[3], dSdv[3], U[3], drdu, drdv; // variables that will be used

    // neighbors in each direction could be either 1 or 2
    ComputeDerivatives(mNeighborsU, mIsForwardU, stepSize, dxdu, dSdu, &drdu);
    ComputeDerivatives(mNeighborsV, mIsForwardV, stepSize, dxdv, dSdv, &drdv);
    this->GetDirection(U);

    // 2. construct rSrad Matrix
    double UTU[3][3]; // UT*U - I
    UTU[0][0] = U[0] * U[0] - 1;
    UTU[0][1] = U[0] * U[1];
    UTU[0][2] = U[0] * U[2];
    UTU[1][0] = U[1] * U[0];
    UTU[1][1] = U[1] * U[1] -1;
    UTU[1][2] = U[1] * U[2];
    UTU[2][0] = U[2] * U[0];
    UTU[2][1] = U[2] * U[1];
    UTU[2][2] = U[2] * U[2] -1;

    // Notation in Han, Qiong's dissertation
    Eigen::MatrixXd Q(2,3);
    Q(0,0) = dxdu[0] * UTU[0][0] + dxdu[1] * UTU[1][0] + dxdu[2] * UTU[2][0];
    Q(0,1) = dxdu[0] * UTU[0][1] + dxdu[1] * UTU[1][1] + dxdu[2] * UTU[2][1];
    Q(0,2) = dxdu[0] * UTU[0][2] + dxdu[1] * UTU[1][2] + dxdu[2] * UTU[2][2];

    Q(1,0) = dxdv[0] * UTU[0][0] + dxdv[1] * UTU[1][0] + dxdv[2] * UTU[2][0];
    Q(1,1) = dxdv[0] * UTU[0][1] + dxdv[1] * UTU[1][1] + dxdv[2] * UTU[2][1];
    Q(1,2) = dxdv[0] * UTU[0][2] + dxdv[1] * UTU[1][2] + dxdv[2] * UTU[2][2];

    Eigen::MatrixXd leftSide(2,3), rightSide(3, 2);
    leftSide(0,0) = dSdu[0] - drdu * U[0];
    leftSide(0,1) = dSdu[1] - drdu * U[1];
    leftSide(0,2) = dSdu[2] - drdu * U[2];

    leftSide(1,0) = dSdv[0] - drdv * U[0];
    leftSide(1,1) = dSdv[1] - drdv * U[1];
    leftSide(1,2) = dSdv[2] - drdv * U[2];

    Eigen::Matrix2d QQT, QQT_inv;
    QQT = Q * Q.transpose();
    QQT_inv = QQT.inverse();

    rightSide = Q.transpose() * QQT_inv;

    Eigen::Matrix2d rSradMat;
    rSradMat = leftSide * rightSide;
    rSradMat.transposeInPlace();
    // 3. compute rSrad penalty
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigensolver(rSradMat);
    double maxEigen = eigensolver.eigenvalues()[1];

    if(maxEigen > 1) return maxEigen-1;
    else return 0.0;
//    double detRSrad = rSradMat.determinant();
//    if(detRSrad < 0) return 0.0;
//    else if(detRSrad < 1) return 0.0;
//    else return (abs(detRSrad)-1);
}

bool Spoke::IsValid()
{
    return !(std::isnan(mR) ||
            std::isnan(mUx) || std::isnan(mUy) || std::isnan(mUz)/*
            || std::isnan(mPx) || std::isnan(mPy) || std::isnan(mPz)*/);
}

void Spoke::ComputeDerivatives(std::vector<Spoke*> neibors, bool isForward, double stepSize, // input
                                  double *dxdu, double *dSdu, double *drdu) // output
{
    if(neibors.size() == 1)
    {
        double neiborX[3], neiborR;
        neibors[0]->GetSkeletalPoint(neiborX);
        neiborR = neibors[0]->GetRadius();
        if(isForward)
        {
            dxdu[0] = neiborX[0] - mPx;
            dxdu[1] = neiborX[1] - mPy;
            dxdu[2] = neiborX[2] - mPz;
            *drdu = neiborR - mR;
            neibors[0]->Diff(this, dSdu);
        }
        else {
            dxdu[0] = mPx - neiborX[0];
            dxdu[1] = mPy - neiborX[1];
            dxdu[2] = mPz - neiborX[2];
            *drdu = mR - neiborR;
            this->Diff(neibors[0], dSdu);
        }

        dxdu[0] /= stepSize;
        dxdu[1] /= stepSize;
        dxdu[2] /= stepSize;
        *drdu /= stepSize;

        dSdu[0]  /= stepSize;
        dSdu[1]  /= stepSize;
        dSdu[2]  /= stepSize;
    }
    else if(neibors.size() == 2)
    {
        double neiborX1[3], neiborX0[3], neiborR1, neiborR0;
        neibors[1]->Diff(neibors[0], dSdu);
        neibors[1]->GetSkeletalPoint(neiborX1);
        neibors[0]->GetSkeletalPoint(neiborX0);

        neiborR1 = neibors[1]->GetRadius();
        neiborR0 = neibors[0]->GetRadius();

        *drdu = (neiborR1 - neiborR0) / stepSize / 2;
        dxdu[0] = (neiborX1[0] - neiborX0[0]) / stepSize / 2;
        dxdu[1] = (neiborX1[1] - neiborX0[1]) / stepSize / 2;
        dxdu[2] = (neiborX1[2] - neiborX0[2]) / stepSize / 2;
    }
}


// Copyright University of Warwick 2014
// 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)

// Authors:
// John Back

/*! \file LpcAlgorithm.hh
    \brief File containing the declaration of the LpcAlgorithm class
*/

/*! \class LpcAlgorithm
    \brief Class that follows the scaled data points to find the lpc

    This is a C++ implementation of the main Lpc curve algorithm in
    the R-code package LPCM written by Jochen Einbeck and Ludger Evers
    (http://cran.r-project.org/web/packages/LPCM/index.html)
*/

#ifndef LPC_ALGORITHM_HH
#define LPC_ALGORITHM_HH

#include "LACE/LpcFunctions.hh"
#include "LACE/LpcPoint.hh"

#include <Eigen/Dense>
#include <vector>

class LpcCurve;
class LpcHitCollection;
class LpcParameters;

class LpcAlgorithm {

public:

    //! Constructor
    /*!
      \param [in] thePars a pointer to the constant lpc parameters
    */
    LpcAlgorithm(const LpcParameters* thePars);

    //! Destructor
    virtual ~LpcAlgorithm();

    //! Enumerator specifying the direction of the curve
    enum LpcDirection {Both = 0, Forward = 1, Backward = 2};

    //! Find the local principal curve for the given dataset and return the result
    /*!
      \param [in] curveIndex the index integer number for the curve
      \param [in] theHits a pointer to the constant LpcHitCollection
      \param [in] direction an integer specifying if we want the forward and/or backward parts
      \returns a new pointer to the lpc curve result
    */
    LpcCurve* getCurve(int curveIndex, const LpcHitCollection* theHits,
		       int direction = LpcAlgorithm::Both);

    //! Find the local principal curve for the given dataset and return the result
    /*!
      \param [in] curveIndex the index integer number for the curve
      \param [in] theHits a pointer to the constant LpcHitCollection
      \param [in] startPoint the starting point which may be different to the centroid of theHits
      \param [in] weights the Eigen VectorXd object of the weights (of each hit) to be used
      \param [in] lastEVect the last, previous eigen vector for branching sections
      \param [in] direction an integer specifying if we want the forward and/or backward parts
      \returns a new pointer to the lpc curve result
    */
    LpcCurve* getCurve(int curveIndex, const LpcHitCollection* theHits,
		       const LpcPoint& startPoint, const Eigen::VectorXd& weights,
		       const Eigen::VectorXd& lastEVect, int direction = LpcAlgorithm::Both);

protected:

    //! Initialise
    void initialise();

    // ! Find the forward-going part of the lpc
    /*!
      \param [in] lastEVect The last, previous eigenvector for branching sections
    */
    void forward(const Eigen::VectorXd& lastEVect);

    //! Find the backward-going part of the lpc
    /*!
      \param [in] lastEVect The last, previous eigenvector for branching sections
    */
    void backward(const Eigen::VectorXd& lastEVect);

private:

    //! Private default & copy constructors and assignment operator
    LpcAlgorithm();

    //! Copy constructor
    LpcAlgorithm(const LpcAlgorithm& other);

    //! Assignment operator
    LpcAlgorithm& operator=(const LpcAlgorithm& other);

    //! Pointer to the constant lpc parameters
    const LpcParameters* thePars_;

    //! The number of dimensions for each hit
    int nDim_;

    //! The matrix of the scaled positions of all of the hits
    //! Each row = a hit, each column = x, y, z,.. co-ordinate
    Eigen::MatrixXd Xi_;

    //! The 1-row vector of the hit weights
    Eigen::VectorXd weights_;

    //! The scaled starting point for the algorithm
    Eigen::VectorXd x0_;

    //! The starting point as an LpcPoint
    LpcPoint startPoint_;

    //! The co-ordinate ranges (x,y,z,...)
    Eigen::VectorXd theRange_;

    //! The kernel width
    double h_;

    //! The step size
    double t_;

    //! The number of lpc points
    int iter2_;

    //! The number of lpc points/2
    int iter_;

    //! The angle penalisation factor
    double pen_;

    //! The ratio of eigenvalues for identifying possible branching points
    double rho0_;

    //! The boundary condition
    double boundary_;

    //! The convergence requirement
    double convergence_;

    //! The number of data points
    int N_;

    //! The local neighbourhood points ("x" or "u") with eigenvalue ratios > rho0
    std::vector<LpcPoint> highRhoPoints_;

    //! The lpc points ("m(u)" or "mu"), equivalent to the local weighted mean
    Eigen::MatrixXd lpcPoints_;

    //! The principal eigen vectors
    Eigen::MatrixXd eigenVectors_;

    //! The cosine between neighbouring principal eigenvectors
    Eigen::VectorXd cosAngles_;

    //! The values of the cumulative path length
    Eigen::VectorXd lambda_;

    //! The values of the ordered cumulative path length, starting at dlambda = 0.
    Eigen::VectorXd dlambda_;

    //! The matrix of the cumulative path length along each axis (row = point, col = x,y,z..)
    Eigen::MatrixXd lambdaAxes_;

    //! The ratio of the eigenvalues between neighbouring eigenvectors
    Eigen::VectorXd rho_;

    //! The c0 coefficients used to scale the kernel factor
    Eigen::VectorXd c0_;

    //! Object to access the functions within LpcFunctions
    LpcFunctions functions_;

};

#endif


/*
PURPOSE

* Evaluate speed of computing dense jacobians

RULES

* Input/output sizes known at compile time (benefits forward methods)
* No multithreading
* For tape methods employ all available optimization in the recording step

TESTS

* Support variable sized inputs
* No branching (if/else) allowed

OTHER BENCHMARKS (CODE)

* Adept benchmarks: https://github.com/rjhogan/Adept-2/tree/master/benchmark
* Ceres benchmarks: https://github.com/ceres-solver/ceres-solver/tree/master/internal/ceres/autodiff_benchmarks
* "A Benchmark of Selected Algorithmic Differentiation Tools on Some Problems in Computer Vision and Machine Learning"
  - Paper: https://arxiv.org/abs/1807.10129
  - Code: https://github.com/microsoft/ADBench
  - Tools: Adept, ADOL-C, Ceres
* "Automatic Differentiation of Rigid Body Dynamics for Optimal Control and Estimation"
  - Paper: https://arxiv.org/abs/1709.03799
  - Tools: CppAD, CppAD-CG

TODOS

* Add Ceres v1.14
* Add option for dynamic-sized tests

AD TOOLS NOT IN BENCHMARK

* boost::math::differentiation     (Seems incompatible with array math)
* Enzime: https://enzyme.mit.edu/  (Clang compiler only)
* Fadbad: http://www.fadbad.com    (Seems unmaintained + Sacado claims to be faster successor)
* dco/c++                          (Non-free license)

*/

#include <Eigen/Core>

#include <autodiff/common/meta.hpp>

#include <exception>
#include <iomanip>
#include <iostream>

#include "adept_tester.hpp"
#include "adolc_testers.hpp"
#include "autodiff_testers.hpp"
#include "ceres_tester.hpp"
#include "cppadcg_tester.hpp"  // must be before cppad_tester
#include "cppad_tester.hpp"
#include "numerical_tester.hpp"
#include "sacado_tester.hpp"

#include "test_utils.hpp"
#include "tests.hpp"

template<typename Tester, typename Test>
void run_speedtest()
{
  auto res = test_speed<Tester, Test>();

  std::string name = std::string(Test::name) + "<" + std::to_string(Test::N) + ">";

  if (res.calc_timeout || res.setup_timeout) {
    std::cerr <<
      std::left << std::setw(20) << Tester::name <<
      std::left << std::setw(20) << name <<
      "TIMEOUT (DETACHED)" <<
      std::endl;
    return;
  }

  // check if error occured
  if (!res.exception.empty()) {
    std::cerr <<
      std::left << std::setw(20) << Tester::name <<
      std::left << std::setw(20) << name <<
      "EXCEPTION: " << res.exception <<
      std::endl;
    return;
  }

  // compare with numerical
  if (!test_correctness<Tester, NumericalTester, Test>()) {
    std::cerr <<
      std::left << std::setw(20) << Tester::name <<
      std::left << std::setw(20) << name <<
      "CORRECTNESS ERROR" <<
      std::endl;
  }

  std::cout <<
    std::left << std::setw(20) << Tester::name <<
    std::left << std::setw(20) << name <<
    std::setprecision(2) <<
    std::right << std::setw(10) << std::scientific <<
    static_cast<double>(res.setup_time.count()) / res.setup_iter <<
    std::right << std::setw(10) << std::scientific <<
    static_cast<double>(res.calc_time.count()) / res.calc_iter <<
    std::endl;
}

template<typename ... T>
struct TypePack
{
  static constexpr std::size_t size = sizeof...(T);

  template<std::size_t Idx>
  using type = std::tuple_element_t<Idx, std::tuple<T...>>;
};

template<typename TesterPack, typename TestPack>
void run_tests()
{
  autodiff::detail::For<TestPack::size>(
    [](auto test_i) {
      autodiff::detail::For<TesterPack::size>(
        [ = ](auto tester_i) {
          using Tester = typename TesterPack::template type<tester_i>;
          using Test = typename TestPack::template type<test_i>;
          run_speedtest<Tester, Test>();
        });
    });
}

int main()
{
  using AllTests = TypePack<
    Constant<3>, Constant<10>, Constant<100>, Constant<1000>,
    ManyToOne<3>, ManyToOne<10>, ManyToOne<100>, ManyToOne<1000>,
    OneToMany<3>, OneToMany<10>, OneToMany<100>, OneToMany<1000>,
    ODE<3>, ODE<10>, ODE<30>,
    NeuralNet<3>, NeuralNet<10>, NeuralNet<100>,
    ReprojectionError<3>, ReprojectionError<10>, ReprojectionError<100>, ReprojectionError<1000>,
    Manipulator<3>, Manipulator<10>, Manipulator<100>, Manipulator<1000>,
    SE3ODE<3>, SE3ODE<10>, SE3ODE<100>, SE3ODE<1000>
  >;

  using AllTestsNoODE = TypePack<
    Constant<3>, Constant<10>, Constant<100>, Constant<1000>,
    ManyToOne<3>, ManyToOne<10>, ManyToOne<100>, ManyToOne<1000>,
    OneToMany<3>, OneToMany<10>, OneToMany<100>, OneToMany<1000>,
    NeuralNet<3>, NeuralNet<10>, NeuralNet<100>,
    ReprojectionError<3>, ReprojectionError<10>, ReprojectionError<100>, ReprojectionError<1000>,
    Manipulator<3>, Manipulator<10>, Manipulator<100>, Manipulator<1000>
  >;

  using AllTestsNoSE3 = TypePack<
    Constant<3>, Constant<10>, Constant<100>, Constant<1000>,
    ManyToOne<3>, ManyToOne<10>, ManyToOne<100>, ManyToOne<1000>,
    OneToMany<3>, OneToMany<10>, OneToMany<100>, OneToMany<1000>,
    ODE<3>, ODE<10>, ODE<30>,
    NeuralNet<3>, NeuralNet<10>, NeuralNet<100>
  >;

  // these can run all tests (but do not necessarily succeed)
  run_tests<TypePack<
      AdeptTester,
      AdolcTapelessTester,
      AutodiffFwdTester,
      CppADTester,
      CppADCGTester,
      NumericalTester,
      SacadoTester
    >,
    AllTests
  >();

  // * Ceres can't handle ODE tests because no conversion double -> Jet (required inside boost)
  // * AutodiffRev times out on ODE tests
  run_tests<
    TypePack<AutodiffRevTester, CeresTester>,
    AllTestsNoODE
  >();

  // Adolc can not handle Eigen quaterion-vector product
  run_tests<
    TypePack<AdolcTester>,
    AllTestsNoSE3
  >();

  return 0;
}


#ifndef ELEMENT_STRUCTURE_ALGORITHMS
#define ELEMENT_STRUCTURE_ALGORITHMS

#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/mtl/matrix/element.hpp>

namespace imf {

// Extracts an artificial element structure from the given sparse matrix. This
// algorithm
template< class Element_struct, class Matrix >
void greedy_extract_element_structure(Element_struct& es,  Matrix& M , std::string& output) {

/*******************************************************************************
 * PHASE 1: Generate Artificial 1x1 and kxk Elements
 ******************************************************************************/

	typedef unsigned int usint;

	// Elements
	typedef typename Matrix::value_type value_type;
	typedef typename Matrix::size_type size_type;
	typedef mtl::matrix::element<value_type> element_type;
	typedef typename element_type::index_type index_type;
	typedef typename element_type::matrix_type matrix_type;
	// typedef typename element_type::neighbor_iterator neigh_iterator;

#if 0
	// Sparse matrices
	typedef glas::compressed_sparse_structure<glas::row_orientation>			sparse_structure;
	typedef typename sparse_structure::compressed_index_array_type			compressed_index_array_type;
	typedef glas::sparse_matrix<value_type, sparse_structure> sparse_type;

	compressed_index_array_type& row_start = glas::compressed_index_array( M );
	typename sparse_structure::index_array_type& col_idx =		glas::index_array( M );
	typename sparse_type::value_array_type& values = glas::value_array(M);
#endif
	std::vector<size_type> row_start(M.ref_major());
	std::vector<size_type> col_idx(M.ref_minor());
	std::vector<value_type> values(M.data);

	const value_type ZERO = value_type(0);
	const usint nb_rows = num_rows( M );

	// Determine a "suitable" guess for the maximum size of the elements.
	double avg_nnz_row = M.nnz();
	double std_dev = 0;
	for(usint r = 0; r < nb_rows; ++r) {
		std_dev += pow(double(row_start[r+1]-row_start[r]), 2) / double(nb_rows);
	}
	int max_idx = int(avg_nnz_row + 2*std_dev) + 1;
	if( max_idx < 7 ) {
		max_idx = 7;
	}

	std::cout << "max_idx="<<max_idx<< std::endl;

 	std::cout << "nnz="<< M.nnz() << std::endl;

	std::vector<element_type*> elements;
	elements.reserve(M.nnz() / max_idx);

	std::vector<int> idx;
	idx.reserve(max_idx);

	// Generate the element matrices from the given sparse matrix.
	int seq_nbr = 0;
	for(usint row = 0; row < nb_rows; ++row) {
		for(
			usint col_off = row_start[row];
				col_off < row_start[row+1];
		) {
			std::cout<< "row="<< row << "-"<< nb_rows << "\n";
			// We have at most k different indices.
			idx.clear();
			idx.push_back(row);
			for(
				int my_size = 1;
				(my_size < max_idx) && (col_off < row_start[row+1]);
				++col_off
			) {
				if( col_idx[col_off] != row ) {
					idx.push_back( col_idx[col_off] );
					++my_size;
				}
			}
			std::sort( idx.begin(), idx.end() );

			// Construct index.
			index_type el_index( idx.size() );
			for(usint i = 0; i < idx.size(); ++i) {
				el_index(i) = idx[i];
			}

			// Copy values.
			const usint n =  size(el_index);
			matrix_type el_vals(n,n);
			el_vals = ZERO;
			bool non_zero = false;
			for(usint i = 0; i < n; ++i) {
				const int lrow = el_index(i);

				// Seek matching indices.
				usint k = row_start[lrow];
				usint j = 0;
				while( (k < row_start[lrow+1]) && (j < n) ) {
					const int diff = el_index(j) - col_idx[k];
					if(diff > 0) {
						++k;
					} else if(diff < 0) {
						++j;
					} else {
						// We found a match. Copy the value.
						el_vals( i,j ) = values[k];
						values[k] = ZERO;
						non_zero |= (el_vals(i,j) != ZERO);
						++j;
						++k;
					}
				}
			}

			// if the element matrix is non-zero, add it.
			if(non_zero) {
				element_type* el = new element_type(seq_nbr, el_index, el_vals);
				elements.push_back(el);
				++seq_nbr;
			}
		}
		// If the diagonal entry is not included in the sparsity of the matrix,
		// it is wise *not* to add an element with a single node. In this
		// manner, we will avoid the situation wherein that element is selected,
		// and the matrix is zero at this position, but where the elimination of
		// a different element (which includes the node of this row as off-
		// diagonal element) would make the matrix at this position non-zero
		// (and hence invertible).
	}

	// Remove unnecessary nodes.
	for(usint i = 0; i < elements.size(); ++i) {
		element_type& el = *(elements[i]);
		index_type& idx = el.get_indices();
		matrix_type& vals = el.get_values();

		std::vector<int> remove_nodes;
		for(int c = 0; c < el.nb_vars(); ++c) {
			bool all_zero = true;
			for(int k = 0; k < el.nb_vars(); ++k) {
				all_zero &= ( (vals(k,c) == ZERO) && (vals(c,k) == ZERO) );
			}
			if(all_zero) {
				remove_nodes.push_back( idx(c) );
			}
		}
		el.remove_nodes(remove_nodes, el);
	}

	// Generate neighbourhood information.
	std::vector<int>* node_element_map = new std::vector<int>[nb_rows];
	for( usint i = 0; i < elements.size(); ++i ) {
		element_type& el = *(elements[i]);
		index_type& idx = el.get_indices();
		for(int j = 0; j < el.nb_vars(); ++j) {
			node_element_map[ idx(j) ].push_back(el.get_id());
		}
	}
	// Construct neighbourhood set for every element.
	for( usint i = 0; i < elements.size(); ++i ) {
		element_type& el = *(elements[i]);
		index_type& idx = el.get_indices();
		std::set<int> neighs;
		for(int j = 0; j < el.nb_vars(); ++j) {
			neighs.insert(
				node_element_map[ idx(j) ].begin(),
				node_element_map[ idx(j) ].end()
			);
		}
			// An element shouldn't neighbour itself.
		neighs.erase( el.get_id() );
		for(
			std::set<int>::iterator it = neighs.begin();
			it != neighs.end();
			++it
		) {
			el.get_neighbors().push_back( elements[*it] );
		}
	}
	delete[] node_element_map;

/*******************************************************************************
 * PHASE 3: Construct Grid
 ******************************************************************************/
	const int orig_els = elements.size();
	element_type* elems = new element_type[ elements.size() ];

	// Map the location of the new elements.
	std::vector<int> seq_nbr_map( orig_els, -1 );
	for(usint i = 0; i < elements.size(); ++i) {
		seq_nbr_map[ elements[i]->get_id() ] = i;
	}

	// Construct the element set.
	for(usint i = 0; i < elements.size(); ++i) {
		// Copy the element to the grid.
		elems[i] = *(elements[i]);
		element_type& el = elems[i];
		el.get_id() = i;

		typename element_type::neighbor_collection_type& neighs =
			el.get_neighbors();
		for( usint j = 0; j < neighs.size(); ++j ) {
			// Remap the neighbour pointer to the new location.
			int offset = seq_nbr_map[neighs[j]->get_id()];
			neighs[j] = elems + offset;

			assert( offset >= 0 );
		}

		// Clear the data of the copy.
		elements[i]->clear();
	}
#if 0
#ifndef NDEBUG
#if VERBOSE_MODE > 10
	std::cout << "The elements in the algebraic grid: " << std::endl;
	for(usint i = 0; i < elements.size(); ++i) {
		std::cout << elems[i] << std::endl;
	}
#endif
#endif
#endif

	es.consume(orig_els, nb_rows, elems);
#if 0
	element_structure<value_type>* es = new element_structure<value_type>(
		orig_els, nb_rows, elems
	);
#endif
	// Delete the copied elements.
	for(usint i = 0; i < elements.size(); ++i) {
		assert( elements[i] );
		delete elements[i];
	}
	elements.clear();

	std::cout << "Generated " << orig_els << " artificial elements." <<
		std::endl;
 es.write_to_file(output);
//	return es;
}

} // end namespace imf

#endif /* ELEMENT_STRUCTURE_ALGORITHMS */


/* Copyright (C) 2012-2020 IBM Corp.
 * This program is Licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance
 * with the License. You may obtain a copy of the License at
 *   http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. See accompanying LICENSE file.
 */

/* TestPermutations.cpp - Applying plaintext permutation to encrypted vector
 */
#include <NTL/ZZ.h>

#include <helib/NumbTh.h>
#include <helib/timing.h>
#include <helib/permutations.h>
#include <helib/EncryptedArray.h>
#include <helib/ArgMap.h>
#include <helib/debugging.h>

#include "gtest/gtest.h"
#include "test_common.h"

namespace {

struct Parameters
{
  Parameters(std::vector<long> orders, std::vector<long> good, long depth) :
      depth(depth)
  {
    gens.SetLength(orders.size());
    for (std::size_t i = 0; i < orders.size(); ++i) {
      gens[i] = helib::GenDescriptor(orders[i], good[i], /*genIdx=*/i);
    }
  }

  NTL::Vec<helib::GenDescriptor> gens;
  const long depth;

  friend std::ostream& operator<<(std::ostream& os, const Parameters& params)
  {
    os << "{gens = ";
    for (long i = 0; i < params.gens.length(); ++i) {
      os << "(" << params.gens[i].order << ", " << params.gens[i].good << "), ";
    }
    return os << "depth = " << params.depth << "}";
  }
};

struct BGVParameters
{
  BGVParameters(long m, long p, long r, long bits, long depth) :
      m(m), p(p), r(r), bits(bits), depth(depth){};

  const long m;
  const long p;
  const long r;
  const long bits;
  const long depth;

  friend std::ostream& operator<<(std::ostream& os, const BGVParameters& params)
  {
    return os << "{"
              << "m = " << params.m << ", "
              << "p = " << params.p << ", "
              << "r = " << params.r << ", "
              << "bits = " << params.bits << ", "
              << "depth = " << params.depth << "}";
  }
};

struct CKKSParameters : public BGVParameters
{
  CKKSParameters(long m, long r, long bits, long depth) :
      BGVParameters(m, /*p=*/-1, r, bits, depth){};
};

class TestPermutationsGeneral : public ::testing::TestWithParam<Parameters>
{
protected:
  const NTL::Vec<helib::GenDescriptor> gens;
  const long depth;

  TestPermutationsGeneral() : gens(GetParam().gens), depth(GetParam().depth) {}
};

class TestPermutationsBGV : public ::testing::TestWithParam<BGVParameters>
{
protected:
  const long m;
  const long p;
  const long r;
  const long bits;
  const long depth;

  helib::Context context;
  helib::SecKey secretKey;
  helib::PubKey publicKey;
  const helib::EncryptedArray& ea;

  TestPermutationsBGV() :
      m(GetParam().m),
      p(GetParam().p),
      r(GetParam().r),
      bits(GetParam().bits),
      depth(GetParam().depth),
      context(m, p, r),
      secretKey((buildModChain(context, bits), context)),
      publicKey(
          (secretKey.GenSecKey(), addSome1DMatrices(secretKey), secretKey)),
      ea(*(context.ea))
  {}

  virtual void SetUp() override
  {
    helib::setupDebugGlobals(&secretKey, context.ea);
  }

  virtual void TearDown() override { helib::cleanupDebugGlobals(); }
};

class TestPermutationsCKKS : public ::testing::TestWithParam<CKKSParameters>
{
protected:
  const long m;
  const long r;
  const long bits;
  const long depth;

  helib::Context context;
  helib::SecKey secretKey;
  helib::PubKey publicKey;
  const helib::EncryptedArrayCx& ea;

  TestPermutationsCKKS() :
      m(GetParam().m),
      r(GetParam().r),
      bits(GetParam().bits),
      depth(GetParam().depth),
      context(m, /*p=*/-1, r),
      secretKey((buildModChain(context, bits), context)),
      publicKey(
          (secretKey.GenSecKey(), addSome1DMatrices(secretKey), secretKey)),
      ea(context.ea->getCx())
  {}

  virtual void SetUp() override
  {
    helib::setupDebugGlobals(&secretKey, context.ea);
  }

  virtual void TearDown() override { helib::cleanupDebugGlobals(); }
};

TEST_P(TestPermutationsBGV, ciphertextPermutations)
{
  std::vector<long> in(ea.size());
  // Arbitrary initial data
  for (std::size_t i = 0; i < in.size(); ++i) {
    in[i] = i % p;
  }

  // Set up generator-descriptors for the PAlgebra generators
  NTL::Vec<helib::GenDescriptor> gens;
  gens.SetLength(ea.dimension());
  for (long i = 0; i < ea.dimension(); ++i) {
    gens[i] = helib::GenDescriptor(/*order=*/ea.sizeOfDimension(i),
                                   /*good=*/ea.nativeDimension(i),
                                   /*genIdx=*/i);
  }

  // Get the gnerator-tree structures and the corresponding hypercube
  helib::GeneratorTrees trees;
  long cost = trees.buildOptimalTrees(gens, depth);

  if (helib_test::verbose) {
    std::cout << "trees = " << trees << "\n"
              << "cost = " << cost << std::endl;
  }

  for (long cnt = 0; cnt < 3; ++cnt) {
    // Choose a random permutation
    helib::Permut pi;
    helib::randomPerm(pi, trees.getSize());

    // Build a permutation network for pi
    helib::PermNetwork net;
    net.buildNetwork(pi, trees);

    // Make sure we have the key-switching matrices needed for this network
    addMatrices4Network(secretKey, net);

    // Apply the permutation pi to the plaintext
    std::vector<long> out1(ea.size());
    std::vector<long> out2(ea.size());
    helib::applyPermToVec(out1, in, pi); // Direct application

    // Encrypt plaintext array, then apply permutation network to the ciphertext
    helib::Ctxt ctxt(publicKey);
    ea.encrypt(ctxt, publicKey, in);
    if (helib_test::verbose) {
      std::cout << "  ** Applying permutation network to ciphertext... "
                << std::flush;
    }

    double t = NTL::GetTime();
    net.applyToCtxt(ctxt, ea); // Applying permutation network
    t = NTL::GetTime() - t;

    if (helib_test::verbose) {
      std::cout << "done in " << t << " seconds" << std::endl;
    }

    ea.decrypt(ctxt, secretKey, out2);

    EXPECT_EQ(out1, out2);
  }
}

TEST_P(TestPermutationsBGV, ciphertextPermutationsWithNewAPI)
{
  helib::PermIndepPrecomp pip(context, depth);

  if (helib_test::verbose) {
    std::cout << "depth = " << pip.getDepth() << "\n"
              << "cost = " << pip.getCost() << "\n";
  }

  helib::Permut pi;
  helib::randomPerm(pi, context.getNSlots());

  helib::PermPrecomp pp(pip, pi);
  helib::Ctxt ctxt(publicKey);
  helib::PtxtArray v(context);
  v.random();
  v.encrypt(ctxt);

  pp.apply(ctxt); // Ciphertext permutation
  pp.apply(v);    // Plaintext permutation

  helib::PtxtArray w(context);
  w.decrypt(ctxt, secretKey);

  EXPECT_EQ(w, v);
}

TEST_P(TestPermutationsCKKS, ciphertextPermutationsWithNewAPI)
{
  helib::PermIndepPrecomp pip(context, depth);

  if (helib_test::verbose) {
    std::cout << "depth = " << pip.getDepth() << "\n"
              << "cost = " << pip.getCost() << "\n";
  }

  helib::Permut pi;
  helib::randomPerm(pi, context.getNSlots());

  helib::PermPrecomp pp(pip, pi);
  helib::Ctxt ctxt(publicKey);
  helib::PtxtArray v(context);
  v.random();
  v.encrypt(ctxt, context.getNSlots()); // CKKS encryption

  pp.apply(ctxt); // Ciphertext permutation
  pp.apply(v);    // Plaintext permutation

  helib::PtxtArray w(context);
  w.decrypt(ctxt, secretKey);

  EXPECT_TRUE(w == helib::Approx(v));
}

TEST_P(TestPermutationsGeneral, testCube)
{
  helib::GeneratorTrees trees;
  long cost = trees.buildOptimalTrees(gens, depth);

  if (helib_test::verbose) {
    std::cout << "trees = " << trees << "\n"
              << "cost  = " << cost << std::endl;
  }

  NTL::Vec<long> dims;
  trees.getCubeDims(dims);
  helib::CubeSignature sig(dims);

  for (long count = 0; count < 3; ++count) {
    helib::Permut pi;
    helib::randomPerm(pi, trees.getSize());

    helib::PermNetwork net;
    net.buildNetwork(pi, trees);

    helib::HyperCube<long> cube1(sig), cube2(sig);
    for (long i = 0; i < cube1.getSize(); ++i) {
      cube1[i] = i;
    }
    helib::HyperCube<long> cube3 = cube1;
    // Direct application
    helib::applyPermToVec(cube2.getData(), cube1.getData(), pi);
    // Applying permutation network
    net.applyToCube(cube3);

    EXPECT_EQ(cube2, cube3)
        << "input = " << cube1.getData() << "\noutput1 = " << cube2.getData()
        << "\noutput2 = " << cube2.getData();
  }
}

INSTANTIATE_TEST_SUITE_P(variousParameters,
                         TestPermutationsBGV,
                         ::testing::Values(BGVParameters(/*m=*/4369,
                                                         /*p=*/2,
                                                         /*r=*/1,
                                                         /*bits=*/1000,
                                                         /*depth=*/5)));

INSTANTIATE_TEST_SUITE_P(variousParameters,
                         TestPermutationsCKKS,
                         ::testing::Values(CKKSParameters(/*m=*/8192,
                                                          /*r=*/20,
                                                          /*bits=*/1000,
                                                          /*depth=*/5)));

INSTANTIATE_TEST_SUITE_P(
    variousParameters,
    TestPermutationsGeneral,
    ::testing::Values(Parameters(/*orders=*/{3}, /*good=*/{1}, /*depth=*/1),
                      Parameters({31}, {0}, 5),
                      Parameters({2, 3}, {0, 0}, 3),
                      Parameters({2, 3}, {1, 0}, 3),
                      Parameters({6}, {1}, 3),
                      Parameters({6, 2}, {1, 0}, 5),
                      Parameters({682, 2}, {1, 0}, 11)));

} // namespace


#ifndef AFFINE_INVARIANT_FEATURES_RESULT_MATCHER
#define AFFINE_INVARIANT_FEATURES_RESULT_MATCHER

#include <cmath>
#include <vector>

#include <affine_invariant_features/parallel_tasks.hpp>
#include <affine_invariant_features/results.hpp>
#include <ros/console.h>

#include <boost/bind.hpp>
#include <boost/ref.hpp>

#include <opencv2/calib3d.hpp>
#include <opencv2/core.hpp>
#include <opencv2/features2d.hpp>
#include <opencv2/flann.hpp>

namespace affine_invariant_features {

class ResultMatcher {
public:
  ResultMatcher(const cv::Ptr< const Results > &reference) : reference_(reference) {
    CV_Assert(reference_);

    switch (reference_->normType) {
    case cv::NORM_L2:
      matcher_ = new cv::FlannBasedMatcher(new cv::flann::KDTreeIndexParams(4));
      break;
    case cv::NORM_HAMMING:
      matcher_ = new cv::FlannBasedMatcher(new cv::flann::LshIndexParams(6, 12, 1));
      break;
    }

    CV_Assert(matcher_);

    matcher_->add(reference_->descriptors);
    matcher_->train();
  }

  virtual ~ResultMatcher() {}

  const Results &getReference() const { return *reference_; }

  void match(const Results &source, cv::Matx33f &transform, std::vector< cv::DMatch > &matches,
             const double min_match_ratio = 0.) const {
    // number of matches wanted
    const int n_min_matches(std::ceil(min_match_ratio * reference_->keypoints.size()));

    // find the 1st & 2nd matches for each descriptor in the source
    std::vector< std::vector< cv::DMatch > > all_matches;
    matcher_->knnMatch(source.descriptors, all_matches, 2);

    // filter unique matches whose 1st is enough better than 2nd
    std::vector< cv::DMatch > unique_matches;
    for (std::vector< std::vector< cv::DMatch > >::const_iterator m = all_matches.begin();
         m != all_matches.end(); ++m) {
      if (m->size() < 2) {
        continue;
      }
      if ((*m)[0].distance > 0.75 * (*m)[1].distance) {
        continue;
      }
      unique_matches.push_back((*m)[0]);
    }
    if (unique_matches.size() < std::max(n_min_matches, 4)) {
      // abort if the number of unique matches is less than required.
      // 4 is the minimum requirement for cv::findHomography().
      matches.clear();
      return;
    }

    // further filter matches compatible to a registration
    std::vector< unsigned char > mask;
    {
      std::vector< cv::Point2f > source_points;
      std::vector< cv::Point2f > reference_points;
      for (std::vector< cv::DMatch >::const_iterator m = unique_matches.begin();
           m != unique_matches.end(); ++m) {
        source_points.push_back(source.keypoints[m->queryIdx].pt);
        reference_points.push_back(reference_->keypoints[m->trainIdx].pt);
      }
      try {
        transform = cv::findHomography(source_points, reference_points, cv::RANSAC, 5., mask);
      } catch (const cv::Exception & /* error */) {
        // abort if cv::findHomography() is failed. this can happen when no good transform is found.
        ROS_INFO("An exception from cv::findHomography() was properly handled. "
                 "An error message may be printed just before this message but it is still ok.");
        matches.clear();
        return;
      }
    }

    // pack the final matches
    matches.clear();
    for (std::size_t i = 0; i < unique_matches.size(); ++i) {
      if (mask[i] == 0) {
        continue;
      }
      matches.push_back(unique_matches[i]);
    }
    if (matches.size() < n_min_matches) {
      // abort if the number of matches is not enough
      matches.clear();
      return;
    }
  }

  static void parallelMatch(const std::vector< cv::Ptr< const ResultMatcher > > &matchers,
                            const Results &source, std::vector< cv::Matx33f > &transforms,
                            std::vector< std::vector< cv::DMatch > > &matches_array,
                            const std::vector< double > &min_match_ratios = std::vector< double >(),
                            const double nstripes = -1.) {
    CV_Assert(min_match_ratios.empty() || matchers.size() == min_match_ratios.size());

    // initiate output
    const int ntasks(matchers.size());
    transforms.resize(ntasks, cv::Matx33f::eye());
    matches_array.resize(ntasks);

    // populate tasks
    ParallelTasks tasks(ntasks);
    for (int i = 0; i < ntasks; ++i) {
      if (matchers[i]) {
        tasks[i] = boost::bind(&ResultMatcher::match, matchers[i].get(), boost::ref(source),
                               boost::ref(transforms[i]), boost::ref(matches_array[i]),
                               min_match_ratios.empty() ? 0. : min_match_ratios[i]);
      }
    }

    // do paralell matching
    cv::parallel_for_(cv::Range(0, ntasks), tasks, nstripes);
  }

private:
  const cv::Ptr< const Results > reference_;
  cv::Ptr< cv::DescriptorMatcher > matcher_;
};

} // namespace affine_invariant_features

#endif


/*
 * Copyright 2009-2020 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
#include <libint2/initialize.h>
#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE eris_test

// Third party includes
#include <boost/test/unit_test.hpp>

// Local VOTCA includes
#include "votca/tools/eigenio_matrixmarket.h"
#include "votca/xtp/ERIs.h"
#include "votca/xtp/orbitals.h"

using namespace votca::xtp;
using namespace std;

BOOST_AUTO_TEST_SUITE(eris_test)

BOOST_AUTO_TEST_CASE(fourcenter) {
  libint2::initialize();
  Orbitals orbitals;
  orbitals.QMAtoms().LoadFromFile(std::string(XTP_TEST_DATA_FOLDER) +
                                  "/eris/molecule.xyz");
  BasisSet basis;
  basis.Load(std::string(XTP_TEST_DATA_FOLDER) + "/eris/3-21G.xml");

  AOBasis aobasis;
  aobasis.Fill(basis, orbitals.QMAtoms());

  Eigen::MatrixXd dmat = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/eris/dmat.mm");

  ERIs eris;
  eris.Initialize_4c(aobasis);
  Eigen::MatrixXd erissmall = eris.CalculateERIs_4c(dmat, 1e-20);

  Eigen::MatrixXd eris_ref = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/eris/eris_ref.mm");

  bool eris_check = erissmall.isApprox(eris_ref, 0.00001);
  if (!eris_check) {
    std::cout << "result eri" << std::endl;
    std::cout << erissmall << std::endl;
    std::cout << "ref eri" << std::endl;
    std::cout << eris_ref << std::endl;
    std::cout << " quotient" << std::endl;
    std::cout << erissmall.cwiseQuotient(eris_ref);
  }
  BOOST_CHECK_EQUAL(eris_check, 1);

  std::array<Eigen::MatrixXd, 2> both = eris.CalculateERIs_EXX_4c(dmat, 1e-20);
  const Eigen::MatrixXd& exx_small = both[1];
  Eigen::MatrixXd exx_ref = -votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/eris/exx_ref.mm");

  Eigen::MatrixXd sum = both[0] + both[1];
  Eigen::MatrixXd sum_ref = exx_ref + eris_ref;
  bool sum_check = sum.isApprox(sum_ref, 1e-5);
  BOOST_CHECK_EQUAL(sum_check, 1);
  if (!sum_check) {
    std::cout << "result sum" << std::endl;
    std::cout << sum << std::endl;
    std::cout << "ref sum" << std::endl;
    std::cout << sum_ref << std::endl;
  }

  bool exxs_check = exx_small.isApprox(exx_ref, 0.00001);
  if (!eris_check) {
    std::cout << "result exx" << std::endl;
    std::cout << exx_small << std::endl;
    std::cout << "ref exx" << std::endl;
    std::cout << exx_ref << std::endl;
    std::cout << "quotient" << std::endl;
    std::cout << exx_small.cwiseQuotient(exx_ref);
  }
  BOOST_CHECK_EQUAL(exxs_check, 1);

  libint2::finalize();
}

BOOST_AUTO_TEST_CASE(threecenter) {
  libint2::initialize();
  Orbitals orbitals;
  orbitals.QMAtoms().LoadFromFile(std::string(XTP_TEST_DATA_FOLDER) +
                                  "/eris/molecule.xyz");
  BasisSet basis;
  basis.Load(std::string(XTP_TEST_DATA_FOLDER) + "/eris/3-21G.xml");

  AOBasis aobasis;
  aobasis.Fill(basis, orbitals.QMAtoms());

  Eigen::MatrixXd dmat = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/eris/dmat2.mm");

  Eigen::MatrixXd mos = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/eris/mos.mm");

  ERIs eris;
  eris.Initialize(aobasis, aobasis);
  Eigen::MatrixXd exx_dmat =
      eris.CalculateERIs_EXX_3c(Eigen::MatrixXd::Zero(0, 0), dmat)[1];
  Eigen::MatrixXd exx_mo =
      eris.CalculateERIs_EXX_3c(mos.block(0, 0, 17, 4), dmat)[1];

  bool compare_exx = exx_mo.isApprox(exx_dmat, 1e-4);
  BOOST_CHECK_EQUAL(compare_exx, true);

  Eigen::MatrixXd exx_ref = -votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/eris/exx_ref2.mm");

  bool compare_exx_ref = exx_ref.isApprox(exx_mo, 1e-5);
  if (!compare_exx_ref) {
    std::cout << "result exx" << std::endl;
    std::cout << exx_mo << std::endl;
    std::cout << "ref exx" << std::endl;
    std::cout << exx_ref << std::endl;
  }
  BOOST_CHECK_EQUAL(compare_exx_ref, true);

  Eigen::MatrixXd eri = eris.CalculateERIs_3c(dmat);

  Eigen::MatrixXd eris_ref = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/eris/eris_ref2.mm");

  bool compare_eris = eris_ref.isApprox(eri, 1e-5);
  if (!compare_eris) {
    std::cout << "result eris" << std::endl;
    std::cout << eri << std::endl;
    std::cout << "ref eris" << std::endl;
    std::cout << eris_ref << std::endl;
  }
  BOOST_CHECK_EQUAL(compare_eris, true);
}

BOOST_AUTO_TEST_SUITE_END()


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

#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/beta.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/lexical_cast.hpp>
#include <fstream>
#include <map>
#include <boost/math/tools/test_data.hpp>
#include <boost/random.hpp>
#include "mp_t.hpp"

using namespace boost::math::tools;
using namespace boost::math;
using namespace std;

#include <libs/math/test/table_type.hpp>

#define T double
#define SC_(x) static_cast<double>(x)
#include <libs/math/test/ibeta_int_data.ipp>

int main(int, char* [])
{
   std::cout << "Enter name of test data file [default=ibeta_derivative_data.ipp]";
   std::string line;
   std::getline(std::cin, line);
   boost::algorithm::trim(line);
   if(line == "")
      line = "ibeta_derivative_data.ipp";
   std::ofstream ofs(line.c_str());
   ofs << std::scientific << std::setprecision(40);

   ofs <<
      "//  (C) Copyright John Maddock 2006.\n"
      "//  Use, modification and distribution are subject to the\n"
      "//  Boost Software License, Version 1.0. (See accompanying file\n"
      "//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)\n"
      "\n\n"
      "static const boost::array<boost::array<typename table_type<T>::type, 4>, " << ibeta_int_data.size() << "> ibeta_derivative_small_data = { {\n";

   for(unsigned i = 0; i < ibeta_int_data.size(); ++i)
   {
      mp_t a(ibeta_int_data[i][0]);
      mp_t b(ibeta_int_data[i][1]);
      mp_t x(ibeta_int_data[i][2]);

      std::cout << a << std::endl;
      std::cout << b << std::endl;
      std::cout << x << std::endl;

      mp_t bet = exp(boost::math::lgamma(a) + boost::math::lgamma(b) - boost::math::lgamma(a + b));
      mp_t d = pow(1 - x, b - 1) * pow(x, a - 1) / bet;

      ofs << "{{ SC_(" << a << "), SC_(" << b << "), SC_(" << x << "), SC_(" << d << ") }}\n";
   }

   ofs << "}};\n\n";

   return 0;
}


/*******************************************************************************
* Copyright 2016 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
*     http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/

/* Authors: Hye-Jong KIM*/

#ifndef OMMANAGER_HPP_
#define OMMANAGER_HPP_

#include "OMDebug.hpp"

#include <unistd.h>
#include <WString.h>
#include <Eigen.h>
#include <vector>

using namespace std;

typedef struct
{
  Eigen::Vector3f position;
  Eigen::Matrix3f orientation;
} Pose;

typedef struct
{
  float angle;
  float angular_velocity;
  float angular_acceleration;
} State;

typedef struct
{
  int8_t joint_number;
  Eigen::Vector3f relative_position;
  Eigen::Matrix3f relative_orientation;
} InnerJoint;

typedef struct
{
  float mass;
  Eigen::Vector3f relative_center_position;
  float moment;
} Inertia;

class Joint
{
 private:
  int8_t dxl_id_;
  Eigen::Vector3f axis_;
  State joint_state_;
  Pose joint_pose_;

 public:
  /////////////////func///////////////////
  Joint():
  dxl_id_(-1),
  {
    joint_state_.angle = 0.0;
    joint_state_.angular_velocity = 0.0;
    joint_state_.angular_acceleration = 0.0;
    joint_pose_.position = Eigen::Vector3f::Zero();
    joint_pose_.orientation = Eigen::Matrix3f::Identity(3,3);
    axis_ = Eigen::Vector3f::Zero();
  }

  ~Joint(){}

  void init(int8_t dxl_id, Eigen::Vector3f axis)
  {
    dxl_id_ = dxl_id;
    axis_ = axis;
  }

  int8_t getId()
  {
    return dxl_id_;
  }
  Eigen::Vector3f getAxis()
  {
    return axis_;
  }

  void setAngle(float angle)
  {
    joint_state_.angle = angle;
  }

  void setAngularVelocity(float angular_velocity)
  {
    joint_state_.angular_velocity = angular_velocity;
  }

  void setAngularAcceleration(float angular_acceleration)
  {
    joint_state_.angular_acceleration = angular_acceleration;
  }

  void setJointState(State joint_state)
  {
    joint_state_.angle = joint_state.angle;
    joint_state_.angular_velocity = joint_state.angular_velocity;
    joint_state_.angular_acceleration = joint_state.angular_acceleration;
  }

  float getAngle()
  {
    return joint_state_.angle_;
  }

  float getAngularVelocity()
  {
    return joint_state_.angular_velocity;
  }

  float getAngularAcceleration()
  {
    return joint_state_.angular_acceleration;
  }

  State getJointState()
  {
    State temp;
    temp.angle = joint_state_.angle;
    temp.angular_velocity = joint_state_.angular_velocity;
    temp.angular_acceleration = joint_state_.angular_acceleration;
    return temp;
  }

  void setPosition(Eigen::Vector3f position)
  {
    joint_pose_.position = position;
  }

  void setOrientation(Eigen::Matrix3f orientation)
  {
    joint_pose_.orientation = orientation;
  }

  void setPose(Pose joint_pose)
  {
    joint_pose_.position = joint_pose.position;
    joint_pose_.orientation = joint_pose.orientation;
  }

  Eigen::Vector3f getPosition()
  {
    return joint_pose_.position;
  }

  Eigen::Matrix3f getOrientation()
  {
    return joint_pose_.orientation;
  }

  Pose getPose()
  {
    Pose temp;
    temp.position = joint_pose_.position;
    temp.orientation = joint_pose_.orientation;
    return temp;
  }
  ////////////////////////////////////////
};

class Link
{
 private:
  int8_t inner_joint_size_;
  Inertia inertia_;
  vector<InnerJoint> inner_joint_;

  Eigen::Vector3f center_position_;
 public:
  /////////////////func///////////////////
  Link():
  inner_joint_size_(1),
  {
    inertia_.mass = 0;
    inertia_.center_position = Eigen::Vector3f::Zero();
    inertia_.moment = 0;
  }

  ~Link(){}

  void init(int8_t inner_joint_size)
  {
    inner_joint_size_ = inner_joint_size;
    inner_joint_.resize(inner_joint_size_);
  }

  void setInnerJoint(int8_t joint_number, Eigen::Vector3f relative_position, Eigen::Matrix3f relative_orientation)
  {
    int8_t i;
    if(findJoint(joint_number) >= 0)
    {
      inner_joint_.at(findJoint(joint_number)).joint_number = joint_number;
      inner_joint_.at(findJoint(joint_number)).relative_position = relative_position;
      inner_joint_.at(findJoint(joint_number)).relative_orientation = relative_orientation;
      return 0;
    }
    else
    {
      for(i=0; i>inner_joint_size_;i++)
      {
        if(findJoint(i) == -1)
        {
          inner_joint_.at(findJoint(i)).joint_number = joint_number;
          inner_joint_.at(findJoint(i)).relative_position = relative_position;
          inner_joint_.at(findJoint(i)).relative_orientation = relative_orientation;
          return 0;
        }
      }
    }
  }

  int8_t getInnerJointSize()
  {
    return inner_joint_size_;
  }

  InnerJoint getInnerJointInformation(int8_t joint_number)
  {
    InnerJoint temp;
    temp.joint_number = inner_joint_.at(findJoint(joint_number)).joint_number;
    temp.relative_position = inner_joint_.at(findJoint(joint_number)).relative_position;
    temp.relative_orientation = inner_joint_.at(findJoint(joint_number)).relative_orientation;
    return temp;
  }

  Eigen::Vector3f getRelativeJointPosition(int8_t to, int8_t from)
  {
    Eigen::Vector3f temp;
    temp = inner_joint_.at(FindJoint(to)).relative_position - inner_joint_.at(FindJoint(from)).relative_position;
    return temp;
  }

  Eigen::Matrix3f getRelativeJointOrientation(int8_t to, int8_t from)
  {
    Eigen::Matrix3f temp;
    temp = inner_joint_.at(FindJoint(from)).relative_orientation.transpose() * inner_joint_.at(FindJoint(to)).relative_orientation;
    return temp;
  }

  Eigen::Vector3f getRelativeJointPosition(int8_t joint_number)
  {
    Eigen::Vector3f temp;
    temp = inner_joint_.at(FindJoint(joint_number)).relative_position;
    return temp;
  }

  Eigen::Matrix3f getRelativeJointOrientation(int8_t joint_number)
  {
    Eigen::Matrix3f temp;
    temp = inner_joint_.at(FindJoint(joint_number)).relative_orientation;
    return temp;
  }

  int8_t findJoint(int8_t joint_number)
  {
    int8_t i;
    for(i=0; i < inner_joint_size_; i++)
    {
      if(inner_joint_.at(i).joint_number == joint_number)
      {
        return i;
      }
    }
    return -1;
  }

  void setCenterPosition(Eigen::Vector3f center_position)
  {
    center_position_ = center_position;
  }

    Eigen::Vector3f getCenterPosition()
  {
    return center_position_;
  }

  void setInertia(Inertia inertia)
  {
    inertia_ = inertia;
  }

  void setMass(float mass)
  {
    inertia_.mass = mass;
  }

  void setRelativeCenterPosition(Eigen::Vector3f relative_center_position)
  {
    inertia_.center_position =center_position;
  }

  void setInertiaMoment(float inertia_moment)
  {
    inertia_.moment = inertia_moment;
  }

  Inertia getInertia()
  {
    return inertia_;
  }

  float getMass()
  {
    return inertia_.mass;
  }

  Eigen::Vector3f getRelativeCenterPosition()
  {
    return inertia_.center_position;
  }

  Eigen::Vector3f getRelativeCenterPosition(int8_t from)
  {
    Eigen::Vector3f temp;
    temp = inertia_.center_position - inner_joint_.at(FineJoint(from)).relative_position;
    return temp;
  }

  float getInertiaMoment()
  {
    return inertia_.moment;
  }

  ////////////////////////////////////////
};

class Base: public Link
{
 private:
  Pose relative_base_pose_;

  Pose base_pose_;
 public:
  /////////////////func///////////////////
  Base():
  {
    base_pose_.position = Eigen::Vector3f::Zero();
    base_pose_.orientation = Eigen::Matrix3f::Identity(3,3);
    relative_base_pose_.position = Eigen::Vector3f::Zero();
    relative_base_pose_.orientation = Eigen::Matrix3f::Identity(3,3);
  }
    ~Base(){}

  void setRelativeBasePostion(Eigen::Vector3f relative_base_position)
  {
    relative_base_pose_.position = relative_base_position;
  }

  void setRelativeBaseOrientation(Eigen::Matrix3f relative_base_orientation)
  {
    relative_base_pose_.orientation = relative_base_orientation;
  }

  void setRelativeBasePose(Pose relative_base_pose)
  {
    relative_base_pose_.position = relative_base_pose.position;
    relative_base_pose_.orientation = relative_base_pose.orientation;
  }

  void setPosition(Eigen::Vector3f base_position)
  {
    base_pose_.position = base_position;
  }

  void setOrientation(Eigen::Matrix3f base_orientation)
  {
    base_pose_.orientation = base_orientation;
  }

  void setPose(Pose base_pose)
  {
    base_pose_.position = base_pose.position;
    base_pose_.orientation = base_pose.orientation;
  }

  Eigen::Vector3f getPosition()
  {
    return base_pose_.position;
  }

  Eigen::Matrix3f getOrientation()
  {
    return base_pose_.orientation;
  }

  Pose getPose()
  {
    Pose temp;
    temp.position = base_pose_.position;
    temp.orientation = base_pose_.orientation;
    return temp;
  }

  Eigen::Vector3f getRelativeBaseJointPosition(int8_t to)
  {
    Eigen::Vector3f temp;
    temp = getRelativeJointPosition(to) - relative_base_pose_.position;
    return temp;
  }

  Eigen::Matrix3f getRelativeBaseJointOrientation(int8_t to)
  {
    Eigen::Matrix3f temp;
    temp = relative_base_pose_.orientation.transpose() * getRelativeJointorientation(to);
    return temp;
  }
    ////////////////////////////////////////
};

class Tool: public Link
{
 private:
  int8_t tool_type_;
  Pose relative_tool_pose_;

  Pose tool_pose_;
 public:
  /////////////////func///////////////////
  Tool():
  tool_type_(0)
  {
    tool_pose_.position = Eigen::Vector3f::Zero();
    tool_pose_.orientation = Eigen::Matrix3f::Identity(3,3);
    relatvie_tool_pose_.position = Eigen::Vector3f::Zero();
    relatvie_tool_pose_.orientation = Eigen::Matrix3f::Identity(3,3);
  }

  ~Tool(){}

  void setToolType(int8_t tool_type)
  {
    tool_type_ = tool_type;
  }

  int8_t getToolType()
  {
    return tool_type_;
  }

  void setRelativeToolPosition(Eigen::Vector3f relative_tool_position)
  {
    relatvie_tool_pose_.position = relatvie_tool_position;
  }

  void setRelativeToolOrientation(Eigen::Matrix3f relatvie_tool_orientation)
  {
    relatvie_tool_pose_.orientation = relatvie_tool_orientation;
  }

  void setRelativeToolPose(Pose relative_tool_pose)
  {
    relatvie_tool_pose_.position = relatvie_tool_pose.position;
    relatvie_tool_pose_.orientation = relatvie_tool_pose.orientation;
  }

  void setPosition(Eigen::Vector3f tool_position)
  {
    tool_pose_.position = tool_position;
  }

  void setOrientation(Eigen::Matrix3f tool_orientation)
  {
    tool_pose_.orientation = tool_orientation;
  }

  void setPose(Pose tool_pose)
  {
    tool_pose_.position = tool_pose.position;
    tool_pose_.orientation = tool_pose.orientation;
  }

  Eigen::Vector3f getPosition()
  {
    return tool_pose_.position;
  }

  Eigen::Matrix3f getOrientation()
  {
    return tool_pose_.orientation;
  }

  Pose getPose()
  {
    Pose temp;
    temp.position = tool_pose_.position;
    temp.orientation = tool_pose_.orientation;
    return temp;
  }

  Eigen::Vector3f getRelativeToolPosition(int8_t from)
  {
    Eigen::Vector3f temp;
    temp = relative_tool_pose_.position - getRelativeJointPosition(from);
    return temp;
  }

  Eigen::Matrix3f getRelativeToolOrientation(int8_t from)
  {
    Eigen::Matrix3f temp;
    temp = getRelativeJointorientation(from).transpose() * relative_tool_pose_.orientation;
    return temp;
  }
  ////////////////////////////////////////
};

template <int8_t TYPE, int8_t JOINT_SIZE, int8_t LINK_SIZE, int8_t TOOL_SIZE>
class Manipulator
{
 private:
  inte8_t type_ = TYPE;
  int8_t dof_;
  int8_t joint_size_ = JOINT_SIZE;
  int8_t link_size_ = LINK_SIZE;
  int8_t tool_size_ = TOOL_SIZE;

 public:
  Base base_;
  Joint joint_[JOINT_SIZE];
  Link link_[LINK_SIZE];
  Tool tool_[TOOL_SIZE];

  /////////////////func///////////////////
  Manipulator(int8_t dof):
  name("UnknownManipulator")
  {
    dof_ = dof;
  }
  ~Manipulator(){}

  void setDOF(int8_t dof);
  {
    dof_=dof;
  }

  int8_t getDOF()
  {
    return dof_;
  }

  int8_t getJointSize()
  {
    return joint_size_;
  }

  int8_t getLinkSize()
  {
    return link_size_;
  }

  int8_t getToolSize()
  {
    return tool_size_;
  }

////////////////////////////////////////
};

#endif // OMMANAGER_HPP_