#include <Eigen/Core>
#include <fstream>
#include <sstream>
#include "json/json.h"
#include <aruco/aruco.h>
#include <aruco/cvdrawingutils.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include <opencv/cv.h>
#include <opencv2/highgui/highgui_c.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/core/eigen.hpp>

void rotateXAxis(cv::Mat &rotation) {
    cv::Mat R(3, 3, CV_32F);
    cv::Rodrigues(rotation, R);
    // create a rotation matrix for x axis
    cv::Mat RX = cv::Mat::eye(3, 3, CV_32F);
    float angleRad = M_PI / 2;
    RX.at< float >(1, 1) = cos(angleRad);
    RX.at< float >(1, 2) = -sin(angleRad);
    RX.at< float >(2, 1) = sin(angleRad);
    RX.at< float >(2, 2) = cos(angleRad);
    // now multiply
    R = R * RX;
    // finally, the the rodrigues back
    cv::Rodrigues(R, rotation);
}

double calculateExtrinsics(aruco::Marker & m, float markerSizeMeters, cv::Mat camMatrix, cv::Mat distCoeff, bool setYPerpendicular,cv::Mat &Rvec,cv::Mat &Tvec)
     {
    if (!m.isValid())
        return -1;
    if (markerSizeMeters <= 0)
        return -1;
    if (camMatrix.rows == 0 || camMatrix.cols == 0)
        return -1;

    double halfSize = markerSizeMeters / 2.;
    cv::Mat ObjPoints(4, 3, CV_32FC1);
    ObjPoints.at< float >(1, 0) = -halfSize;
    ObjPoints.at< float >(1, 1) = halfSize;
    ObjPoints.at< float >(1, 2) = 0;
    ObjPoints.at< float >(2, 0) = halfSize;
    ObjPoints.at< float >(2, 1) = halfSize;
    ObjPoints.at< float >(2, 2) = 0;
    ObjPoints.at< float >(3, 0) = halfSize;
    ObjPoints.at< float >(3, 1) = -halfSize;
    ObjPoints.at< float >(3, 2) = 0;
    ObjPoints.at< float >(0, 0) = -halfSize;
    ObjPoints.at< float >(0, 1) = -halfSize;
    ObjPoints.at< float >(0, 2) = 0;

    cv::Mat ImagePoints(4, 1, CV_32FC2);
    cv::Mat ximagePoints(4, 1, CV_32FC2);

    // Set image points from the marker
    for (int c = 0; c < 4; c++) {
        ImagePoints.at< cv::Point2f >(c, 0) = m[c];
    }

    cv::Mat raux, taux;
    bool x = cv::solvePnP(ObjPoints, ImagePoints, camMatrix, distCoeff, raux, taux);
    if(!x)
        return -2;
    std::cout << "solved\n";
    raux.convertTo(Rvec, CV_32F);
    taux.convertTo(Tvec, CV_32F);
    // rotate the X axis so that Y is perpendicular to the marker plane
    cv::projectPoints(ObjPoints,raux,taux,camMatrix,distCoeff,ximagePoints);
    std::cout << " op " << ImagePoints.rows << " " << ImagePoints.cols << " c " << ImagePoints.channels() << " t " << ImagePoints.type() << std::endl;
    std::cout << " op " << ximagePoints.rows << " " << ximagePoints.cols << " c " << ximagePoints.channels() << " t" << ximagePoints.type() << std::endl;
    std::cout << "reproject " << ImagePoints-ximagePoints << std::endl;
    //double r = 0;
    //for(int i = 0; i < 4; i++)
    //    r += cv::norm(ImagePoints.at<cv::Point2f>(i,0)-ximagePoints.at<cv::Point2f>(i,0));
    double r = cv::norm(ImagePoints-ximagePoints);
    if (setYPerpendicular)
        rotateXAxis(Rvec);
    // and now reprojection error
    return r;
}

Json::Value mat2json(cv::Mat m)
{
    Json::Value r;
    for(int i = 0; i < m.rows; i++)
    {
        Json::Value q;
      for(int j = 0; j < m.cols; j++)
            q[j] = m.at<double>(i,j);
        r[i] = q;
    }
    return r;
}

Json::Value vec2json(double * p, int n)
{
    Json::Value r;
    for(int i = 0; i < n; i++)
        r[i] = p[i];
    return r;
}

Json::Value mat2json1(Eigen::Matrix4d m)
{
    Json::Value r;
    double *v_ptr = m.data();
    for (int i = 0; i < 16; ++i)
        r.append(Json::Value(v_ptr[i]));
    return r;
}

Json::Value mat2json(const Eigen::Matrix4d & m)
{
    Json::Value r;
    for(int i = 0; i < m.rows(); i++)
    {
        Json::Value q;
      for(int j = 0; j < m.cols(); j++)
            q[j] = m(i,j);
        r[i] = q;
    }
    return r;
}

void help(){
        std::cerr << "Arguments: [-xy] calibrationfile size imagefile alreadyundist [-|outfilename]\n\t-x mirror image along x\n\t-y mirror image along y\n\t-xy mirror image along xy\n"";
        exit(1);
}

int main(int argc, char const *argv[])
{
	// first is camera as OpenCV
	// second is file
	// output is array of markers
	bool y_axis_perpendicular = false;
    int mirror = 0;
    bool show = false;

    if(argc == 1)
        help();

    for(; argv[1][0] == '-'; argc--, argv++)
    {
        if(strcmp(argv[1],"-xy") == 0)
            mirror = 3;
        ele if(strcmp(argv[1],"-x") == 0)
            mirror |= 1;
        else if(strcmp(argv[1],"-y") == 0)
            mirror |= 2;
        else if(strcmp(argv[1],"-s") == 0)
            show = true;
    }

	if(argc < 5)
	{
        help();
		return -1;
	}

    std::string saveframe;
    if(argc > 5)
    {
        if(strcmp(argv[5],"-") == 0)
            show = true;
        else
            saveframe = argv[5];
    }

	cv::Mat camera_matrix,dist_coeffs;
	cv::FileStorage camera_calibration_file(argv[1], cv::FileStorage::READ);
    if (!camera_calibration_file.isOpened())
    {
    	std::cerr << "wrong calibration file" << std::endl;
    	return -2;
    }
	camera_calibration_file["rgb_intrinsics"] >> camera_matrix;
	camera_calibration_file["rgb_distortion"] >> dist_coeffs;
	if(!camera_matrix.rows || !dist_coeffs.rows)
	{
		std::cerr << "expected rgb_intrinsics or rgb_distortion in file" << std::endl;
		return -1;
	}

	float marker_size = atof(argv[2]);

    if(atoi(argv[4]) != 0)
    {
        // setZero
        for(int i = 0; i < dist_coeffs.cols; i++)
            dist_coeffs.at<float>(i) = 0;
    }

    std::cout << "K:"<<camera_matrix << std::endl;
    std::cout << "dist:"<<dist_coeffs << std::endl;
    std::cout << "marker_size:"<<marker_size << std::endl;

    cv::VideoCapture cap(argv[3]);
    if(!cap.isOpened())
    {
        std::cerr << "wrong image/video file " << argv[3] << std::endl;
        return -3;
    }

    std::vector<aruco::Marker> markers;
    aruco::MarkerDetector marker_detector;
    marker_detector.setMinMaxSize(0.01,0.1);

    cv::Mat frame;
    bool singleframe = cap.get(CV_CAP_PROP_FRAME_COUNT) == 1;

    std::ofstream onf((std::string(argv[3]) +".json").c_str());

    for(;;)
    {
        cap >> frame;
        if(frame.rows == 0)
        {
            break;
        }
        if(mirror != 0)
        {
            cv::Mat dst;
            cv::flip(frame,dst,mirror == 1 ? 0 : mirror == 2 ? 1 : -1);
            frame = dst; // ref
        }
        aruco::CameraParameters cp(camera_matrix,dist_coeffs, cv::Size(frame.cols,frame.rows));

        marker_detector.detect(
        	frame,
        	markers,
        	cp,
            marker_size,
            y_axis_perpendicular);
        double pmat[16];
        cp.glGetProjectionMatrix(cp.CamSize,cp.CamSize,pmat,0.1,100,false);

        // TODO YML output for frames
        std::string xin = argv[3];
        std::unique_ptr<cv::FileStorage> recreate;
        if(singleframe)
            recreate = std::unique_ptr<cv::FileStorage>(new cv::FileStorage(xin+".yml", cv::FileStorage::WRITE));

        Json::Value jmarkers(Json::arrayValue);
        int found = 0;

        std::cout << "frame\n";

    	for (auto marker : markers)
        {
            if (!marker.isValid())
            {
                continue;
            }
            found++;
            cv::Mat Rvec,Tvec;
            auto e = calculateExtrinsics(marker,marker_size,camera_matrix,dist_coeffs,y_axis_perpendicular,Rvec,Tvec);

            marker.draw(frame, cv::Scalar(0,0,255), 2);
            aruco::CvDrawingUtils::draw3dAxis(frame, marker, cp);

            Eigen::Matrix4d marker_pose = Eigen::Matrix4d::Identity();
            marker_pose.block<3, 1>(0, 3) = Eigen::Vector3d(Tvec.at<float>(0),
                                                  Tvec.at<float>(1),
                                                  Tvec.at<float>(2));
            cv::Mat marker_rot;
            cv::Rodrigues(Rvec, marker_rot);
            marker_pose(0, 0) = marker_rot.at<float>(0, 0);
            marker_pose(0, 1) = marker_rot.at<float>(0, 1);
            marker_pose(0, 2) = marker_rot.at<float>(0, 2);
            marker_pose(1, 0) = marker_rot.at<float>(1, 0);
            marker_pose(1, 1) = marker_rot.at<float>(1, 1);
            marker_pose(1, 2) = marker_rot.at<float>(1, 2);
            marker_pose(2, 0) = marker_rot.at<float>(2, 0);
            marker_pose(2, 1) = marker_rot.at<float>(2, 1);
            marker_pose(2, 2) = marker_rot.at<float>(2, 2);

            //void eigen2cv(const Eigen::Matrix<_Tp, _rows, _cols, _options, _maxRows, _maxCols>& src, Mat& dst)
            cv::Mat marker_posecv(4,4,CV_32F);
            //marker_rot.copyTo(marker_posecv(cv::Rect(0,0,3,3)));
            //Tvec.copyTo(marker_posecv(cv::Rect(0,3,3,1)));
            cv::eigen2cv(marker_pose,marker_posecv);

            //cv::Mat cvT(4,4,CV_32FC1);
            //Eigen::Map<Matrix4f> eigenT( cvT.data() );

            //std::cout << "marker mid:" << marker.id << " error:" << e << "\n\tTvec:" << Tvec << "\n\tRvec:" << Rvec << std::endl;

    	    double mat[16];
            marker.glGetModelViewMatrix(mat);
            cv::Point2f center = marker.getCenter();

            Json::Value jmarker;
            jmarker["id"] = marker.id;
            jmarker["center"][0] = center.x;
            jmarker["center"][1] = center.y;
            jmarker["error"] = e;
            jmarker["areapx"] = marker.getArea();
            jmarker["areau"] = marker.getArea()/(cp.CamSize.width*cp.CamSize.height);
            jmarker["Tvec"][0] = Tvec.at<float>(0,0);
            jmarker["Tvec"][1] = Tvec.at<float>(1,0);
            jmarker["Tvec"][2] = Tvec.at<float>(2,0);
            jmarker["Rvec"][0] = Rvec.at<float>(0,0);
            jmarker["Rvec"][1] = Rvec.at<float>(1,0);
            jmarker["Rvec"][2] = Rvec.at<float>(2,0);
            jmarker["pose"] = mat2json(marker_pose);
            jmarker["glmodelview"] = vec2json(mat,16);
            for(int q = 0; q < marker.size(); q++)
            {
                jmarker["points"][q][0] = marker[q].x;
                jmarker["points"][q][1] = marker[q].y;
            }
            jmarker["points"][(int)marker.size()][0] = center.x;
            jmarker["points"][(int)marker.size()][1] = center.y;
            // and then the center
            jmarkers.append(jmarker);
            std::cout <<" mid:" << marker.id << " error:" << e << " area:" << marker.getArea() << std::endl;

            //markerpose
            //markerid
            //markersize
            //mode
            if(recreate)
            {
                *recreate << ((std::ostringstream() << "markerid" << found ).str().c_str()) << marker.id;
                *recreate << ((std::ostringstream() << "markersize" << found  ).str().c_str()) << marker_size;
                *recreate << ((std::ostringstream() << "markerpose" << found  ).str().c_str()) << marker_posecv;
                *recreate << ((std::ostringstream() << "mode" << found ).str().c_str()) << 3;
                *recreate << ((std::ostringstream() << "corners" << found ).str().c_str()) << marker;
            }
        }

        Json::Value jroot;
        jroot["markers"] = jmarkers;
        jroot["glprojection"] = vec2json(pmat,16);
        jroot["K"] = mat2json(camera_matrix);
        jroot["dist"] = mat2json(dist_coeffs);
        jroot["markersize"] = marker_size;
        jroot["yaxisup"] = y_axis_perpendicular;
        jroot["imagesize"][0] = cp.CamSize.width;
        jroot["imagesize"][1] = cp.CamSize.height;

        // multiple frames == multiple JSON messages, one per line
        onf << Json::FastWriter().write(jroot);

        if(show && found)
        {
            cv::imshow("ciao",frame);
            cv::waitKey(1);
        }
        if(!saveframe.empty() && found)
        {
            // Multiframe => overwrite
            if(!singleframe)
                std::cout << "Warning: overwriting output " << saveframe << std::endl;
            cv::imwrite(saveframe.c_str(),frame);
        }
    }
    return 0;
}


#pragma once
#include "numeric/dense.hpp"
#include <Eigen/Jacobi>

namespace cpb { namespace compute {

namespace detail {
    template<class real_t>
    static void tridiagonal_qr_step(real_t* diag, real_t* subdiag, int start, int end) {
        auto td = (diag[end-1] - diag[end]) * real_t(0.5);
        auto e = subdiag[end-1];
        auto mu = diag[end];
        if (td == 0) {
            mu -= std::abs(e);
        }
        else {
            auto e2 = Eigen::numext::abs2(subdiag[end-1]);
            auto h = Eigen::numext::hypot(td, e);
            if (e2 == 0)
                mu -= (e / (td + (td>0 ? 1 : -1))) * (e / h);
            else
                mu -= e2 / (td + (td>0 ? h : -h));
        }

        auto x = diag[start] - mu;
        auto z = subdiag[start];
        for (auto k = start; k < end; ++k) {
            Eigen::JacobiRotation<real_t> rot;
            rot.makeGivens(x, z);

            // do T = G' T G
            auto sdk = rot.s() * diag[k] + rot.c() * subdiag[k];
            auto dkp1 = rot.s() * subdiag[k] + rot.c() * diag[k+1];

            diag[k] = rot.c() * (rot.c() * diag[k] - rot.s() * subdiag[k])
                - rot.s() * (rot.c() * subdiag[k] - rot.s() * diag[k+1]);
            diag[k+1] = rot.s() * sdk + rot.c() * dkp1;
            subdiag[k] = rot.c() * sdk - rot.s() * dkp1;

            if (k > start)
                subdiag[k - 1] = rot.c() * subdiag[k-1] - rot.s() * z;

            x = subdiag[k];
            if (k < end - 1) {
                z = -rot.s() * subdiag[k+1];
                subdiag[k + 1] = rot.c() * subdiag[k+1];
            }
        }
    }
}

template<class Derived, class scalar_t = typename Derived::Scalar>
inline ArrayX<scalar_t> tridiagonal_eigenvalues(const DenseBase<Derived>& alpha,
                                                const DenseBase<Derived>& beta)
{
    ArrayX<scalar_t> eigenvalues = alpha;
    ArrayX<scalar_t> temp = beta;

    auto start = 0;
    auto end = static_cast<int>(eigenvalues.size()) - 1;
    auto iter = 0;
    constexpr auto max_iterations = 30;

    while (end > 0) {
        for (auto i = start; i < end; ++i) {
            auto a = std::abs(temp[i]);
            auto b = std::abs(eigenvalues[i]) + std::abs(eigenvalues[i + 1]);
            // if a is much smaller than b
            if (a < b * std::numeric_limits<scalar_t>::epsilon())
                temp[i] = 0;
        }

        while (end > 0 && temp[end-1] == 0)
            end--;

        if (end <= 0)
            break;

        if (++iter > max_iterations * eigenvalues.size())
            throw std::runtime_error{"Tridiagonal QR error"};

        start = end - 1;
        while (start > 0 && temp[start-1] != 0)
            start--;

        detail::tridiagonal_qr_step(eigenvalues.data(), temp.data(), start, end);
    }

    return eigenvalues;
}

}} // namespace cpb::compute


#include "../include/simple_fft/fft_settings.h"

#ifdef __USE_SQUARE_BRACKETS_FOR_ELEMENT_ACCESS_OPERATOR
#undef __USE_SQUARE_BRACKETS_FOR_ELEMENT_ACCESS_OPERATOR
#endif

#include "../include/simple_fft/fft.h"
#include "test_fft.h"
#include <iostream>
#include <blitz/array.h>

namespace simple_fft {
namespace fft_test {

int testBlitz()
{
    std::cout << "Testing FFT algorithms with blitz" << std::endl;

    using namespace pulse_params;

    std::vector<real_type> t, x, y;
    makeGridsForPulse3D(t, x, y);

    // typedefing arrays
    typedef blitz::Array<real_type,int(1)> RealArray1D;
    typedef blitz::Array<complex_type,int(1)> ComplexArray1D;
    typedef blitz::Array<real_type,int(2)> RealArray2D;
    typedef blitz::Array<complex_type,int(2)> ComplexArray2D;
    typedef blitz::Array<real_type,int(3)> RealArray3D;
    typedef blitz::Array<complex_type,int(3)> ComplexArray3D;

    // 1D fields and spectrum
    RealArray1D E1_real(nt);
    ComplexArray1D E1_complex(nt), G1(nt), E1_restored(nt);

    // 2D fields and spectrum,
    RealArray2D E2_real(nt,nx);
    ComplexArray2D E2_complex(nt,nx), G2(nt,nx), E2_restored(nt,nx);

    // 3D fields and spectrum
    RealArray3D E3_real(nt,nx,ny);
    ComplexArray3D E3_complex(nt,nx,ny), G3(nt,nx,ny), E3_restored(nt,nx,ny);

    if (!commonPartsForTests3D(E1_real, E2_real, E3_real, E1_complex, E2_complex,
                               E3_complex, G1, G2, G3, E1_restored, E2_restored,
                               E3_restored, t, x, y))
    {
        std::cout << "Tests of FFT with blitz++ arrays returned with errors!" << std::endl;
        return FAILURE;
    }

    std::cout << "Tests of FFT with blitz++ arrays completed successfully!" << std::endl;
    return SUCCESS;
}

} // namespace fft_test
} // namespace simple_fft


#include <stan/math/mix/scal.hpp>
#include <gtest/gtest.h>
#include <boost/math/special_functions/fpclassify.hpp>
#include <test/unit/math/rev/scal/fun/util.hpp>
#include <test/unit/math/mix/scal/fun/nan_util.hpp>

TEST(AgradFwdLmgamma,FvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::lmgamma;

  fvar<var> x(3.2,2.1);
  fvar<var> a = lmgamma(3, x);

  EXPECT_FLOAT_EQ(lmgamma(3,3.2), a.val_.val());
  EXPECT_FLOAT_EQ(4.9138227, a.d_.val());

  AVEC y = createAVEC(x.val_);
  VEC g;
  a.val_.grad(y,g);
  EXPECT_FLOAT_EQ(4.9138227 / 2.1, g[0]);
}
TEST(AgradFwdLmgamma,FvarVar_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::lmgamma;

  fvar<var> x(3.2,2.1);
  fvar<var> a = lmgamma(3, x);

  AVEC y = createAVEC(x.val_);
  VEC g;
  a.d_.grad(y,g);
  EXPECT_FLOAT_EQ(2.9115787, g[0]);
}
TEST(AgradFwdLmgamma,FvarFvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::lmgamma;

  fvar<fvar<var> > x;
  x.val_.val_ = 3.2;
  x.val_.d_ = 2.1;

  fvar<fvar<var> > a = lmgamma(3,x);

  EXPECT_FLOAT_EQ(lmgamma(3,3.2), a.val_.val_.val());
  EXPECT_FLOAT_EQ(4.9138227, a.val_.d_.val());
  EXPECT_FLOAT_EQ(0, a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.val_.val_.grad(p,g);
  EXPECT_FLOAT_EQ(4.9138227 / 2.1, g[0]);

  fvar<fvar<var> > y;
  y.val_.val_ = 3.2;
  y.d_.val_ = 2.1;

  fvar<fvar<var> > b = lmgamma(3,y);
  EXPECT_FLOAT_EQ(lmgamma(3,3.2), b.val_.val_.val());
  EXPECT_FLOAT_EQ(0, b.val_.d_.val());
  EXPECT_FLOAT_EQ(4.9138227, b.d_.val_.val());
  EXPECT_FLOAT_EQ(0, b.d_.d_.val());

  AVEC q = createAVEC(y.val_.val_);
  VEC r;
  b.val_.val_.grad(q,r);
  EXPECT_FLOAT_EQ(4.9138227 / 2.1, r[0]);
}
TEST(AgradFwdLmgamma,FvarFvarVar_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::lmgamma;

  fvar<fvar<var> > x;
  x.val_.val_ = 3.2;
  x.val_.d_ = 2.1;

  fvar<fvar<var> > a = lmgamma(3,x);

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.val_.d_.grad(p,g);
  EXPECT_FLOAT_EQ(2.9115787, g[0]);

  fvar<fvar<var> > y;
  y.val_.val_ = 3.2;
  y.d_.val_ = 2.1;

  fvar<fvar<var> > b = lmgamma(3,y);

  AVEC q = createAVEC(y.val_.val_);
  VEC r;
  b.d_.val_.grad(q,r);
  EXPECT_FLOAT_EQ(2.9115787, r[0]);
}
TEST(AgradFwdLmgamma,FvarFvarVar_3rdDeriv) {
  using stan::math::fvar;
  using stan::math::var;

  fvar<fvar<var> > x;
  x.val_.val_ = 3.2;
  x.val_.d_ = 1.0;
  x.d_.val_ = 1.0;

  fvar<fvar<var> > a = stan::math::lmgamma(3,x);

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.d_.d_.grad(p,g);
  EXPECT_FLOAT_EQ(-0.65043455, g[0]);
}

struct lmgamma_fun {
  template <typename T0>
  inline T0
  operator()(const T0& arg1) const {
    return lmgamma(3,arg1);
  }
};

TEST(AgradFwdLmgamma,lmgamma_NaN) {
  lmgamma_fun lmgamma_;
  test_nan_mix(lmgamma_,false);
}


/*
MIT License

Copyright (c) 2019 Kalu U. Ogbureke

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.

Authors: Kalu U. Ogbureke
Change Log: 01.04.2019 - Version 1.0.0
*/
#define BOOST_TEST_MODULE unitTest
#include <boost/test/included/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <cmath>
#include "libutil.h"
#include "network.h"
#include "data_reader.h"

using namespace mlearn;
template <class T>
/**
    A helper function used to validate/test if 2 vectors contain the same elements.

    @param v1 First vector
    @param v1 Second vector

    @return A boolean value (true or false)
*/
boost::test_tools::predicate_result validateVector(const mublas::vector<T>& v1,  const mublas::vector<T>& v2)
{
    if (v1.size() != v2.size()) return false;
    for (uint64_t i = 0; i < v1.size(); ++i)
    {
        if (fabs(v1[i] - v2[i]) > EPSILON)
        {
            return false;
        }
    }
    return true;
}
/**
    Unit test for the Node class. Tests constructors and operators.
*/
BOOST_AUTO_TEST_SUITE (NodeSuite)
BOOST_AUTO_TEST_CASE(contructor_test)
{
    std::cout << "Start Node class test" << std::endl;
    std::vector<int> sv1 {1, 2, 3, 4};
    std::vector<double> sv2{2, 3, 4.0, 2};
    std::vector<int> sv3{0, 0, 0, 0, 0};

    mublas::vector<int> v1(sv1.size());
    mublas::vector<double> v2(sv2.size());
    mublas::vector<int> v3(sv3.size());

    std::copy(sv1.begin(), sv1.end(), v1.begin());
    std::copy(sv2.begin(), sv2.end(), v2.begin());
    std::copy(sv3.begin(), sv3.end(), v3.begin());

    Node<int> n1(v1);
    Node<double> n2(v2);
    Node<double> n3;
    Node<int> n4{n1};
    Node<int> n5(5);
    BOOST_CHECK(validateVector(v1, n1.getData()));
    BOOST_CHECK(validateVector(v1, n4.getData()));
    BOOST_CHECK(validateVector(v2, n2.getData()));
    BOOST_CHECK(n3.getDataSize() == 0);
    BOOST_CHECK(n4.getDataSize() == n1.getDataSize());
    BOOST_CHECK(validateVector(v3, n5.getData()));
}

BOOST_AUTO_TEST_CASE(operator_test)
{

    std::vector<double> sv1 {1.2, 2.3, 3.0, 4.1};
    std::vector<double> sv2{2, 3, 4.0, 2};
    std::vector<double> sv3{3.2, 5.3, 7.0, 6.1};
    std::vector<double> sv4 {1.2, 2.3, 3, 4.1};
    std::vector<double> sv5 {2.4, 6.9, 12.0, 8.2};
    std::vector<double> sv6 {4.8, 13.8, 24.0, 16.4};

    mublas::vector<double> v1(sv1.size());
    mublas::vector<double> v2(sv2.size());
    mublas::vector<double> v3(sv3.size());
    mublas::vector<double> v4(sv4.size());
    mublas::vector<double> v5(sv5.size());
    mublas::vector<double> v6(sv6.size());

    std::copy(sv1.begin(), sv1.end(), v1.begin());
    std::copy(sv2.begin(), sv2.end(), v2.begin());
    std::copy(sv3.begin(), sv3.end(), v3.begin());
    std::copy(sv4.begin(), sv4.end(), v4.begin());
    std::copy(sv5.begin(), sv5.end(), v5.begin());
    std::copy(sv6.begin(), sv6.end(), v6.begin());

    Node<double> n1(v1);
    Node<double> n2(v2);
    //operator+
    Node<double> n3 = n1 + n2;
    BOOST_CHECK(validateVector(v3, n3.getData()));
    //operator-
    Node<double> n4 = n3 - n2;

    BOOST_CHECK(validateVector(v1, n4.getData()));
    //operator*
    Node<double> n5 = n2 * n4;
    BOOST_CHECK(validateVector(v5, n5.getData()));
    //operator=
    n4 = n5;
    BOOST_CHECK(validateVector(n4.getData(), n5.getData()));
    BOOST_CHECK(n4.getDataSize() == n5.getDataSize());
    //scalarMultiply
    Node<double> n6 = n5.scalarMultiply(2);
    BOOST_CHECK(validateVector(v6, n6.getData()));
    //sum
    BOOST_CHECK(n6.sum() == 59);
    std::cout << "End Node class test" << std::endl;

}
BOOST_AUTO_TEST_SUITE_END()

/**
    Unit test for the NetNode class. Tests constructors and operators.
*/
BOOST_AUTO_TEST_SUITE (NetNodeSuite)
BOOST_AUTO_TEST_CASE(contructor_test)
{
    std::cout << "Start NetNode class test" << std::endl;
    std::vector<int> sv1 {1, 2, 3, 4};
    std::vector<double> sv2{2, 3, 4.0, 2};
    std::vector<int> sv3{0, 0, 0, 0, 0};

    mublas::vector<int> v1(sv1.size());
    mublas::vector<double> v2(sv2.size());
    mublas::vector<int> v3(sv3.size());

    std::copy(sv1.begin(), sv1.end(), v1.begin());
    std::copy(sv2.begin(), sv2.end(), v2.begin());
    std::copy(sv3.begin(), sv3.end(), v3.begin());

    NetNode<int> n1(v1);
    NetNode<double> n2(v2);
    NetNode<double> n3;
    NetNode<int> n4{n1};
    NetNode<int> n5(5);
    BOOST_CHECK(validateVector(v1, n1.getData()));
    BOOST_CHECK(validateVector(v1, n4.getData()));
    BOOST_CHECK(validateVector(v2, n2.getData()));
    BOOST_CHECK(n3.getDataSize() == 0);
    BOOST_CHECK(n4.getDataSize() == n1.getDataSize());
    BOOST_CHECK(validateVector(v3, n5.getData()));
}

BOOST_AUTO_TEST_CASE(operator_test)
{
    std::vector<double> sv1 {1.2, 2.3, 3.0, 4.1};
    std::vector<double> sv2{2, 3, 4.0, 2};
    std::vector<double> sv3{3.2, 5.3, 7.0, 6.1};
    std::vector<double> sv4 {1.2, 2.3, 3, 4.1};
    std::vector<double> sv5 {2.4, 6.9, 12.0, 8.2};
    std::vector<double> sv6 {4.8, 13.8, 24.0, 16.4};
    std::vector<double> sv7 {0.76852478, 0.90887704, 0.95257413, 0.9836975};
    std::vector<double> sv8 {0.17789444, 0.08281957, 0.04517666, 0.01603673};

    mublas::vector<double> v1(sv1.size());
    mublas::vector<double> v2(sv2.size());
    mublas::vector<double> v3(sv3.size());
    mublas::vector<double> v4(sv4.size());
    mublas::vector<double> v5(sv5.size());
    mublas::vector<double> v6(sv6.size());
    mublas::vector<double> v7(sv7.size());
    mublas::vector<double> v8(sv8.size());

    std::copy(sv1.begin(), sv1.end(), v1.begin());
    std::copy(sv2.begin(), sv2.end(), v2.begin());
    std::copy(sv3.begin(), sv3.end(), v3.begin());
    std::copy(sv4.begin(), sv4.end(), v4.begin());
    std::copy(sv5.begin(), sv5.end(), v5.begin());
    std::copy(sv6.begin(), sv6.end(), v6.begin());
    std::copy(sv7.begin(), sv7.end(), v7.begin());
    std::copy(sv8.begin(), sv8.end(), v8.begin());

    NetNode<double> n1(v1);
    NetNode<double> n2(v2);
    //operator+
    NetNode<double> n3 = n1 + n2;
    BOOST_CHECK(validateVector(v3, n3.getData()));
    //operator-
    NetNode<double> n4 = n3 - n2;

    BOOST_CHECK(validateVector(v4, n4.getData()));
    BOOST_CHECK(validateVector(v4, n1.getData()));
    //operator*
    NetNode<double> n5 = n2 * n4;
    BOOST_CHECK(validateVector(v5, n5.getData()));
    //operator=   not working. Fix!
    //n4 = n5;
    //scalarMultiply
    NetNode<double> n6 = n5.scalarMultiply(2);
    BOOST_CHECK(validateVector(v6, n6.getData()));
    // sum
    BOOST_CHECK(n6.sum() == 59);
    //sigmoid
    NetNode<double> n7 = n1.sigmoid();
    BOOST_CHECK(validateVector(v7, n7.getData()));
    //sigmoidPrime
    NetNode<double> n8 = n7.sigmoidPrime();
    BOOST_CHECK(validateVector(v8, n8.getData()));
    std::cout << "End NetNode class test" << std::endl;
}
BOOST_AUTO_TEST_SUITE_END()

/**
    Unit test for the Activation class. Performs general tests.
*/
BOOST_AUTO_TEST_SUITE (ActivationSuite)
BOOST_AUTO_TEST_CASE(general_test)
{
    std::cout << "Start Activation class test" << std::endl;
    std::vector<double> sv1 {1.2, 2.3, 3.0, 4.1};
    std::vector<double> sv2{0.76852478, 0.90887704, 0.95257413, 0.9836975};
    std::vector<double> sv3{1.2, 2.3, 3.0, 4.1};
    std::vector<double> sv4{0.17789444, 0.08281957, 0.04517666, 0.01603673};

    mublas::vector<double> v1(sv1.size());
    mublas::vector<double> v2(sv2.size());
    mublas::vector<double> v3(sv3.size());
    mublas::vector<double> v4(sv4.size());

    std::copy(sv1.begin(), sv1.end(), v1.begin());
    std::copy(sv2.begin(), sv2.end(), v2.begin());
    std::copy(sv3.begin(), sv3.end(), v3.begin());
    std::copy(sv4.begin(), sv4.end(), v4.begin());

    NetNode<double> n1(v1);
    NetNode<double> n3(v3);
    Activation<double> act("sigmoid");
    act.compute(n1);
    BOOST_CHECK(validateVector(v2, n1.getData()));
    //computeDerivative
    act.computeDerivative(n1);
    BOOST_CHECK(validateVector(v4, n1.getData()));
    //softmax
    mublas::vector<double> v5(2), v6(2);
    v5[0] = 1.0, v5[1] = 2.0, v6[0] = 0.26894142, v6[1] = 0.73105858;
    //compute
    Activation<double> soft_act("softmax");
    NetNode<double> n5(v5);
    soft_act.compute(n5);
    BOOST_CHECK(validateVector(v6, n5.getData()));
    //computeDerivative
    mublas::matrix<double> m1(2, 2);
    soft_act.computeDerivative(n5, m1);
    //std::cout <<m1 <<std::endl; //[2,2]((0.196612,-0.196612),(-0.196612,0.196612))
    std::cout << "End Activation class test" << std::endl;
}
BOOST_AUTO_TEST_SUITE_END()

/**
    Unit test for the Layer class. Performs general tests.
*/
BOOST_AUTO_TEST_SUITE (LayerSuite)
BOOST_AUTO_TEST_CASE(general_test)
{
    std::cout << "Start Layer class test" << std::endl;
    std::vector<double> ax{0, 0}, bx{0, 1}, cx{1, 0}, dx{1, 1};
    std::vector<double> ay{0}, by{1}, cy{1}, dy{1};
    std::vector<double> sv1{-0.7, 0.8}, sv2{0.66}, sv3{0, 0}, sv4{0.331812, 0.689974}, sv5{0.763192}, sv6{-0.76};
    std::vector<double> sv7 {-0.14};
    std::vector<double> sv8 {-0.03, -0.01};

    mublas::vector<double> v1(sv1.size());
    mublas::vector<double> v2(sv2.size());
    mublas::vector<double> v3(sv3.size());
    mublas::vector<double> v4(sv4.size());
    mublas::vector<double> v5(sv5.size());
    mublas::vector<double> v6(sv6.size());
    mublas::vector<double> v7(sv7.size());
    mublas::vector<double> v8(sv8.size());

    std::copy(sv1.begin(), sv1.end(), v1.begin());
    std::copy(sv2.begin(), sv2.end(), v2.begin());
    std::copy(sv3.begin(), sv3.end(), v3.begin());
    std::copy(sv4.begin(), sv4.end(), v4.begin());
    std::copy(sv5.begin(), sv5.end(), v5.begin());
    std::copy(sv6.begin(), sv6.end(), v6.begin());
    std::copy(sv7.begin(), sv7.end(), v7.begin());
    std::copy(sv8.begin(), sv8.end(), v8.begin());

    Activation<double> hidden("sigmoid"), output("sigmoid");
    Layer<double> hidden_layer(2, 2, "hidden", hidden);
    Layer<double> output_layer(2, 1, "output", output);
    std::unique_ptr<std::vector<double>> v_ptr1(new std::vector<double>{0.62, 0.55});
    std::unique_ptr<std::vector<double>> v_ptr2(new std::vector<double>{0.42, -0.17});
    std::unique_ptr<std::vector<double>> v_ptr3(new std::vector<double>{0.81, 0.35});
    std::unique_ptr<NetNode<double>> v_ptr4(new NetNode<double>{v1});
    std::unique_ptr<NetNode<double>> v_ptr5(new NetNode<double>{v2});
    std::unique_ptr<NetNode<double>> v_ptr6(new NetNode<double>{v3});
    std::unique_ptr<NetNode<double>> v_ptr7(new NetNode<double>{v6});
    hidden_layer.push_row(0, *v_ptr1);
    hidden_layer.push_row(1, *v_ptr2);
    output_layer.push_row(0, *v_ptr3);
    hidden_layer.setBias(*v_ptr4);
    output_layer.setBias(*v_ptr5);

    output_layer.connect(hidden_layer);
    hidden_layer.setInputData(*v_ptr6);
    NetNode<double> n1 = hidden_layer.forwardProp();
    NetNode<double> n2 = output_layer.forwardProp();
    BOOST_CHECK(validateVector(v4, n1.getData()));
    BOOST_CHECK(validateVector(v5, n2.getData()));
    //backwardProp()
    output_layer.setInputDelta(*v_ptr7);
    NetNode<double> n3 = output_layer.backwardProp();
    NetNode<double> n4 = hidden_layer.backwardProp();

    BOOST_CHECK(validateVector(v7, n3.getData()));
    BOOST_CHECK(validateVector(v8, n4.getData()));
    hidden_layer.clearDeltas();
    mublas::matrix<double> m1 = hidden_layer.getDeltaWeight();
    //std::cout<<m1<<std::endl; //should print zero matrix
    NetNode<double> n5 = hidden_layer.getDeltaBias();
    BOOST_CHECK(validateVector(v3, n5.getData()));
    std::cout << "End Activation class test" << std::endl;
}
BOOST_AUTO_TEST_SUITE_END()
/**
    Unit test for the DataReader class. Performs general tests.
*/
BOOST_AUTO_TEST_SUITE (ReaderSuite)
BOOST_AUTO_TEST_CASE(mnist_test)
{

    std::cout << "Start Reader class test" << std::endl;
    MNIST_CIFARReader<double> mnist("data/mnist_sample.csv", 784, 10, ',',  false), train("data/mnist_train.csv", 784, 10, ',',  false), test, test2;
    mnist.read();
    BOOST_CHECK(mnist.getFeatureDim() == 784);
    BOOST_CHECK(mnist.getLabelDim() == 10);
    BOOST_CHECK(mnist.getRowDim() == 10);
    //constructor
    MNIST_CIFARReader<double> mnist2(mnist);
    BOOST_CHECK(mnist.getFeatureDim() == mnist2.getFeatureDim());
    BOOST_CHECK(mnist.getLabelDim() == mnist2.getLabelDim() );
    BOOST_CHECK(mnist.getRowDim() == mnist2.getRowDim());
    //shuffleIndex
    std::vector<int> indices;
    mnist.shuffleIndex(indices);
    mnist.trainTestSplit(test, 0.2);
    BOOST_CHECK(mnist.getFeatureDim() == 784);
    BOOST_CHECK(mnist.getLabelDim() == 10);
    BOOST_CHECK(mnist.getRowDim() == 8);
    BOOST_CHECK(test.getFeatureDim() == 784);
    BOOST_CHECK(test.getLabelDim() == 10);
    BOOST_CHECK(test.getRowDim() == 2);
    train.read();
    train.trainTestSplit(test2, 0.1);
    BOOST_CHECK(train.getFeatureDim() == 784);
    BOOST_CHECK(train.getLabelDim() == 10);
    BOOST_CHECK(train.getRowDim() == 54000);
    BOOST_CHECK(test2.getFeatureDim() == 784);
    BOOST_CHECK(test2.getLabelDim() == 10);
    BOOST_CHECK(test2.getRowDim() == 6000);
}
BOOST_AUTO_TEST_CASE(general_test)
{
    GenericReader<double> data("data/xor.dat", 2, 1, ' ', false);
    data.read();
    BOOST_CHECK(data.getFeatureDim() == 2);
    BOOST_CHECK(data.getLabelDim() == 1);
    BOOST_CHECK(data.getRowDim() == 12);
    //shuffleIndex
    std::vector<int> indices;
    data.shuffleIndex(indices);
    //////////////////////////////////////////////////////
    GenericReader<double> test("data/xor_header.dat", 2, 2, ' ', true);
    test.read();
    BOOST_CHECK(test.getFeatureDim() == 2);
    BOOST_CHECK(test.getLabelDim() == 2);
    BOOST_CHECK(test.getRowDim() == 12);
    std::cout << "End Reader class test" << std::endl;
}
BOOST_AUTO_TEST_SUITE_END()


/*
 * Copyright (c) 2020, Eberty Alves
 */

// C++ standard library
#include <bits/stdc++.h>

// Point cloud library
#include <pcl/io/ply_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <pcl/common/transforms.h>
#include <pcl/common/centroid.h>

// Boost library
#include <boost/program_options.hpp>

// Typedefs
typedef pcl::PointXYZRGBNormal PointT;
typedef pcl::PointCloud<PointT> PointC;

int main(int argc, char* argv[]) {
  try {
    // Declaration of variables
    std::string src_file_name;
    std::string tgt_file_name;
    std::string output_file_name;

    std::string accumulated_file_name;
    bool b_accumulated_file;

    double roll;
    double pitch;
    double yaw;
    double elevation;

    // Parse command-line options
    namespace po = boost::program_options;

    // Define command-line options
    po::options_description desc("Options");
    desc.add_options()
    ("help,h", "Print help message")
    ("input,i", po::value<std::string>(&src_file_name)->required(), "Input cloud file (.ply)")
    ("target,t", po::value<std::string>(&tgt_file_name)->required(), "Input target file (.ply)")
    ("output,o", po::value<std::string>(&output_file_name)->required(), "Output file (.ply)")
    ("accumulated,a", po::value<std::string>(&accumulated_file_name), "Saves the accumulated point cloud in a .ply file")
    ("roll,r", po::value<double>(&roll)->default_value(0.0), "Rotation in X - degrees (Roll)")
    ("pitch,p", po::value<double>(&pitch)->default_value(0.0), "Rotation in Y - degrees (Pitch)")
    ("yaw,y", po::value<double>(&yaw)->default_value(0.0), "Rotation in Z - degrees (Yaw)")
    ("elevation,e", po::value<double>(&elevation)->default_value(0.0), "Translation in Y");

    // Use a parser to evaluate the command line
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);

    // Store the command-line options evaluated by the parser
    if (vm.count("help")) {
      std::cout << "Rotation tool to translate one point cloud with regard to one reference point cloud." << std::endl << std::endl;
      std::cout << desc << std::endl;
      return 0;
    }

    if (vm.count("input") && vm.count("target") && vm.count("output")) {
      src_file_name = vm["input"].as<std::string>();
      tgt_file_name = vm["target"].as<std::string>();
      output_file_name = vm["output"].as<std::string>();
    } else {
      throw std::string("Correct mode of use: " + std::string(argv[0]) + " -i input.ply -t target.ply -o output.ply [opts]");
    }

    b_accumulated_file = vm.count("accumulated");
    if (b_accumulated_file) {
      accumulated_file_name = vm["accumulated"].as<std::string>();
    }

    roll = vm["roll"].as<double>();
    pitch = vm["pitch"].as<double>();
    yaw = vm["yaw"].as<double>();
    elevation = vm["elevation"].as<double>();

    PointC::Ptr cloud_src(new PointC);
    PointC::Ptr cloud_tgt(new PointC);

    // Load point clouds data from disk
    if (pcl::io::loadPLYFile<PointT>(src_file_name, *cloud_src) == -1) {
      throw std::string("Couldn't load input cloud file");
    }
    std::cout << "Loaded " << cloud_src->size() << " data points from " << src_file_name << std::endl;

    if (pcl::io::loadPLYFile<PointT>(tgt_file_name, *cloud_tgt) == -1) {
      throw std::string("Couldn't load input target file");
    }
    std::cout << "Loaded " << cloud_tgt->size() << " data points from " << tgt_file_name << std::endl;

    Eigen::Affine3f transform;

    // Translate point cloud centroid to origin
    Eigen::Vector4f src_centroid;
    pcl::compute3DCentroid(*cloud_src, src_centroid);
    transform = Eigen::Affine3f::Identity();
    transform.translation() << -src_centroid[0], -src_centroid[1], -src_centroid[2];
    pcl::transformPointCloudWithNormals(*cloud_src, *cloud_src, transform);

    // Rotation
    transform = Eigen::Affine3f::Identity();
    Eigen::Matrix3f rotation;
    rotation = Eigen::AngleAxisf(DEG2RAD(roll), Eigen::Vector3f::UnitX()) *
               Eigen::AngleAxisf(DEG2RAD(pitch), Eigen::Vector3f::UnitY()) *
               Eigen::AngleAxisf(DEG2RAD(yaw), Eigen::Vector3f::UnitZ());
    transform.rotate(rotation);
    pcl::transformPointCloudWithNormals(*cloud_src, *cloud_src, transform);

    // Translate src point cloud centroid to tgt centroid
    Eigen::Vector4f tgt_centroid;
    pcl::compute3DCentroid(*cloud_tgt, tgt_centroid);
    transform = Eigen::Affine3f::Identity();
    transform.translation() << tgt_centroid[0], tgt_centroid[1], tgt_centroid[2];
    pcl::transformPointCloudWithNormals(*cloud_src, *cloud_src, transform);

    // Translate src point cloud in Y axis
    transform = Eigen::Affine3f::Identity();
    transform.translation() << 0, elevation, 0;
    pcl::transformPointCloudWithNormals(*cloud_src, *cloud_src, transform);

    pcl::io::savePLYFileBinary(output_file_name, *cloud_src);

    if (b_accumulated_file) {
      PointC::Ptr accumulated(new PointC);
      pcl::copyPointCloud(*cloud_src, *accumulated);
      *accumulated += *cloud_tgt;
      pcl::io::savePLYFileBinary(accumulated_file_name, *accumulated);
    }

    return 0;
  } catch (boost::program_options::error& msg) {
    std::cerr << "ERROR: " << msg.what() << std::endl;
  } catch (std::string msg) {
    std::cerr << msg << std::endl;
  }

  return -1;
}


/* =========================================================================
   Copyright (c) 2010-2016, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/** \example armadillo-with-viennacl.cpp
*
*   This tutorial shows how data can be directly transferred from the Armadillo Library to ViennaCL objects using the built-in convenience wrappers.
*
*   The first step is to include the necessary headers and activate the Armadillo convenience functions in ViennaCL:
**/

// System headers
#include <iostream>

// Armadillo headers (disable BLAS and LAPACK to avoid linking issues)
#define ARMA_DONT_USE_BLAS
#define ARMA_DONT_USE_LAPACK
#include <armadillo>

// IMPORTANT: Must be set prior to any ViennaCL includes if you want to use ViennaCL algorithms on Armadillo objects
#define VIENNACL_WITH_ARMADILLO 1

// ViennaCL includes
#include "viennacl/vector.hpp"
#include "viennacl/matrix.hpp"
#include "viennacl/compressed_matrix.hpp"
#include "viennacl/linalg/prod.hpp"

// Helper functions for this tutorial:
#include "vector-io.hpp"

/**
*    The following function contains the main code for this tutorial.
*    It consists of the following steps:
*      - Creates Armadillo matrices and vectors
*      - Initializes them with data
*      - Create ViennaCL objects
*      - Copy them over to the respective ViennaCL objects
*      - Compute matrix-vector products in both Armadillo and ViennaCL and compare results.
*
**/
template<typename NumericT>
void run_tutorial()
{
  typedef arma::SpMat<NumericT>  ArmaSparseMatrix;
  typedef arma::Mat<NumericT>    ArmaMatrix;
  typedef arma::Col<NumericT>    ArmaVector;

  /**
  * Create and fill dense matrices from the Armadillo library:
  **/
  ArmaMatrix arma_densemat(6, 5);
  ArmaMatrix arma_densemat2(6, 5);
  arma_densemat(0,0) = 2.0;   arma_densemat(0,1) = -1.0;
  arma_densemat(1,0) = -1.0;  arma_densemat(1,1) =  2.0;  arma_densemat(1,2) = -1.0;
  arma_densemat(2,1) = -1.0;  arma_densemat(2,2) = -1.0;  arma_densemat(2,3) = -1.0;
  arma_densemat(3,2) = -1.0;  arma_densemat(3,3) =  2.0;  arma_densemat(3,4) = -1.0;
                              arma_densemat(5,4) = -1.0;  arma_densemat(4,4) = -1.0;

  /**
  * Create and fill sparse matrices from the Armadillo library:
  **/
  ArmaSparseMatrix arma_sparsemat(6, 5);
  ArmaSparseMatrix arma_sparsemat2(6, 5);
  arma_sparsemat(0,0) = 2.0;   arma_sparsemat(0,1) = -1.0;
  arma_sparsemat(1,1) = 2.0;   arma_sparsemat(1,2) = -1.0;
  arma_sparsemat(2,2) = -1.0;  arma_sparsemat(2,3) = -1.0;
  arma_sparsemat(3,3) = 2.0;   arma_sparsemat(3,4) = -1.0;
  arma_sparsemat(5,4) = -1.0;

  /**
  * Create and fill a few vectors from the Armadillo library:
  **/
  ArmaVector arma_rhs(5);
  ArmaVector arma_result(6);
  ArmaVector arma_temp(6);

  arma_rhs(0) = 10.0;
  arma_rhs(1) = 11.0;
  arma_rhs(2) = 12.0;
  arma_rhs(3) = 13.0;
  arma_rhs(4) = 14.0;

  /**
  * Create the corresponding ViennaCL objects:
  **/
  viennacl::vector<NumericT> vcl_rhs(5);
  viennacl::vector<NumericT> vcl_result(6);
  viennacl::matrix<NumericT> vcl_densemat(6, 5);
  viennacl::compressed_matrix<NumericT> vcl_sparsemat(6, 5);

  /**
  * Directly copy the Armadillo objects to ViennaCL objects
  **/
  viennacl::copy(arma_rhs.memptr(), arma_rhs.memptr() + arma_rhs.n_elem, vcl_rhs.begin());  //method 1: via iterator interface (cf. std::copy())
  viennacl::copy(arma_rhs, vcl_rhs);  //method 2: via built-in wrappers (convenience layer)

  viennacl::copy(arma_densemat, vcl_densemat);
  viennacl::copy(arma_sparsemat, vcl_sparsemat);
  std::cout << "VCL sparsematrix dimensions: " << vcl_sparsemat.size1() << ", " << vcl_sparsemat.size2() << std::endl;

  // For completeness: Copy matrices from ViennaCL back to Eigen:
  viennacl::copy(vcl_densemat, arma_densemat2);
  viennacl::copy(vcl_sparsemat, arma_sparsemat2);

  /**
  * Run dense matrix-vector products and compare results:
  **/
  arma_result = arma_densemat * arma_rhs;
  vcl_result = viennacl::linalg::prod(vcl_densemat, vcl_rhs);
  viennacl::copy(vcl_result, arma_temp);
  std::cout << "Difference for dense matrix-vector product: " << norm(arma_result - arma_temp) << std::endl;
  std::cout << "Difference for dense matrix-vector product (Armadillo -> ViennaCL -> Armadillo): "
            << norm(arma_densemat2 * arma_rhs - arma_temp) << std::endl;

  /**
  * Run sparse matrix-vector products and compare results:
  **/
  arma_result = arma_sparsemat * arma_rhs;
  vcl_result = viennacl::linalg::prod(vcl_sparsemat, vcl_rhs);
  viennacl::copy(vcl_result, arma_temp);
  std::cout << "Difference for sparse matrix-vector product: " << norm(arma_result - arma_temp) << std::endl;
  std::cout << "Difference for sparse matrix-vector product (Armadillo -> ViennaCL -> Armadillo): "
            << norm(arma_sparsemat2 * arma_rhs - arma_temp) << std::endl;
}

/**
*   In the main() routine we only call the worker function defined above with both single and double precision arithmetic.
**/
int main(int, char *[])
{
  std::cout << "----------------------------------------------" << std::endl;
  std::cout << "## Single precision" << std::endl;
  std::cout << "----------------------------------------------" << std::endl;
  run_tutorial<float>();

#ifdef VIENNACL_HAVE_OPENCL
  if ( viennacl::ocl::current_device().double_support() )
#endif
  {
    std::cout << "----------------------------------------------" << std::endl;
    std::cout << "## Double precision" << std::endl;
    std::cout << "----------------------------------------------" << std::endl;
    run_tutorial<double>();
  }

  /**
  *   That's it. Print a success message and exit.
  **/
  std::cout << std::endl;
  std::cout << "!!!! TUTORIAL COMPLETED SUCCESSFULLY !!!!" << std::endl;
  std::cout << std::endl;

}


#include <iostream>
#include <fstream>
#include <Eigen/Dense>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/core/eigen.hpp>

int main(){
  cv::Mat mat = cv::imread("sample.jpg", cv::IMREAD_COLOR);
  if(mat.empty()){
    std::cout << "There is no input file 'sample.jpg'" << std::endl;
    exit(EXIT_FAILURE);
  }

  const int mr = mat.rows;
  const int mc = mat.cols;
  const int nn = mr*mc;

  if(RAND_MAX < mr){
    std::cout << "rand_max " << RAND_MAX << std::endl;
    std::cout << "mat.rows " << mr << std::endl;
    std::cout << "RAND_MAX < mr, error!" << std::endl;
  }

// 1チャンネルに変換し，Eigenに変換
  Eigen::MatrixXd em;
  cv::cv2eigen(mat.reshape(1, mr*mc), em);

// Eigen側の処理
  int kk;
  std::ifstream fr;
  fr.open("input_kk", std::ios::in);
  if(! fr.fail()){
    fr >> kk;
    std::cout << "kk = " << kk << std::endl;
    fr.close();
  }
  else{
    std::cout << "There is no input file 'input_kk'" << std::endl;
    exit(EXIT_FAILURE);
  }
  Eigen::VectorXi gk(nn);
  Eigen::VectorXi numk = Eigen::VectorXi::Zero(kk);
  Eigen::MatrixXd::Index minIndex;
  int i;
  int j;
  int l;
  int sum_gk;
  int bef_sum_gk;

  std::cout << "nn " << nn << std::endl;
  em.transposeInPlace();
  Eigen::MatrixXd mu(3, kk);
  for(i = 0; i < kk; i++){
    mu.col(i) = em.col((rand() % mr));
  }
//  std::cout << "mu = " << mu << std::endl; //dbg

  bef_sum_gk = 0;
  for(i = 0; i < 100; i++){
    std::cout << "loop =" << i << std::endl;
    for(j = 0; j < nn; j++){
      (mu.colwise() - em.col(j)).colwise().squaredNorm().minCoeff(&minIndex);
      gk(j) = minIndex;
    }
    numk.array() = 0;
    for(l = 0; l < kk; l++){
      mu.col(l).array() = 0.0;
      for(j = 0; j < nn; j++){
	if(gk(j) == l){
	  numk(l) += 1;
	  mu.col(l) += em.col(j);
	}
      }
      if(numk(l) != 0){
	mu.col(l) /= static_cast<double> (numk(l));
      }
      else{
	mu.col(l) = em.col((rand() % mr));
      }
    }
    sum_gk = gk.sum();
    if(bef_sum_gk == sum_gk) break;
    bef_sum_gk = sum_gk;
//    std::cout << "numk =" << numk << std::endl; //dbg
//    std::cout << "mu =" << mu << std::endl; //dbg
  }

  //色を減らす
  for(l = 0; l < kk; l++){
    for(j = 0; j < nn; j++){
      if(gk(j) == l){
	em.col(j) = mu.col(l);
      }
    }
  }
//  std::cout << "mu =" << mu << std::endl; //dbg
  em.transposeInPlace();

// cvに戻す
  cv::Mat tmp;
  cv::eigen2cv(em, tmp);

// 3チャンネルに変換
  tmp = tmp.reshape(3, mr);

  cv::namedWindow("sample", cv::WINDOW_AUTOSIZE);
  cv::imshow("sample", mat);
  cv::waitKey(0);
  cv::destroyAllWindows();

  cv::namedWindow("tmp", cv::WINDOW_AUTOSIZE);
  cv::imshow("tmp", tmp/255);
  cv::waitKey(0);
  cv::destroyAllWindows();

  return 0;
}


#include <cmath>
#include <limits>
#include <memory>

#include <Eigen/Dense>
#include <boost/filesystem.hpp>

#include "catch.hpp"
// MPI
#ifdef USE_MPI
#include "mpi.h"
#endif

#include "constraints.h"
#include "contact.h"
#include "contact_friction.h"
#include "element.h"
#include "function_base.h"
#include "hexahedron_element.h"
#include "linear_function.h"
#include "material.h"
#include "mesh.h"
#include "mpm_scheme_usf.h"
#include "node.h"
#include "partio_writer.h"
#include "quadrilateral_element.h"

//! \brief Check stress update 3D case
TEST_CASE("Contact test case", "[contact][friction][3D]") {
  // Dimension
  const unsigned Dim = 3;
  // Degrees of freedom
  const unsigned Dof = 6;
  // Number of phases
  const unsigned Nphases = 1;
  // Number of nodes per cell
  const unsigned Nnodes = 8;
  // Tolerance
  const double Tolerance = 1.E-9;

  // Assign material
  unsigned mid = 0;
  std::vector<unsigned> mids(1, mid);
  // Initialise material
  Json jmaterial;
  jmaterial["density"] = 1000.;
  jmaterial["youngs_modulus"] = 1.0E+7;
  jmaterial["poisson_ratio"] = 0.3;

  auto material =
      Factory<mpm::Material<Dim>, unsigned, const Json&>::instance()->create(
          "LinearElastic3D", std::move(0), jmaterial);

  std::map<unsigned, std::shared_ptr<mpm::Material<Dim>>> materials;
  materials[mid] = material;

  // 8-noded hexahedron element
  std::shared_ptr<mpm::Element<Dim>> element =
      Factory<mpm::Element<Dim>>::instance()->create("ED3H8");

  // Particle 1
  mpm::Index id1 = 0;
  Eigen::Vector3d coords;
  coords.setZero();
  std::shared_ptr<mpm::ParticleBase<Dim>> particle1 =
      std::make_shared<mpm::Particle<Dim>>(id1, coords);

  // Particle 2
  mpm::Index id2 = 1;
  coords << 2., 2., 2.;
  std::shared_ptr<mpm::ParticleBase<Dim>> particle2 =
      std::make_shared<mpm::Particle<Dim>>(id2, coords);

  auto mesh = std::make_shared<mpm::Mesh<Dim>>(0);
  // Check mesh is active
  REQUIRE(mesh->status() == false);

  // Check nodal coordinates size
  REQUIRE(mesh->nodal_coordinates().size() == 0);
  // Check node pairs size
  REQUIRE(mesh->node_pairs().size() == 0);

  // Define nodes
  coords << 0, 0, 0;
  std::shared_ptr<mpm::NodeBase<Dim>> node0 =
      std::make_shared<mpm::Node<Dim, Dof, Nphases>>(0, coords);
  REQUIRE(mesh->add_node(node0) == true);

  coords << 2, 0, 0;
  std::shared_ptr<mpm::NodeBase<Dim>> node1 =
      std::make_shared<mpm::Node<Dim, Dof, Nphases>>(1, coords);
  REQUIRE(mesh->add_node(node1) == true);

  coords << 2, 2, 0;
  std::shared_ptr<mpm::NodeBase<Dim>> node2 =
      std::make_shared<mpm::Node<Dim, Dof, Nphases>>(2, coords);
  REQUIRE(mesh->add_node(node2) == true);

  coords << 0, 2, 0;
  std::shared_ptr<mpm::NodeBase<Dim>> node3 =
      std::make_shared<mpm::Node<Dim, Dof, Nphases>>(3, coords);
  REQUIRE(mesh->add_node(node3) == true);

  coords << 0, 0, 2;
  std::shared_ptr<mpm::NodeBase<Dim>> node4 =
      std::make_shared<mpm::Node<Dim, Dof, Nphases>>(4, coords);
  REQUIRE(mesh->add_node(node4) == true);

  coords << 2, 0, 2;
  std::shared_ptr<mpm::NodeBase<Dim>> node5 =
      std::make_shared<mpm::Node<Dim, Dof, Nphases>>(5, coords);
  REQUIRE(mesh->add_node(node5) == true);

  coords << 2, 2, 2;
  std::shared_ptr<mpm::NodeBase<Dim>> node6 =
      std::make_shared<mpm::Node<Dim, Dof, Nphases>>(6, coords);
  REQUIRE(mesh->add_node(node6) == true);

  coords << 0, 2, 2;
  std::shared_ptr<mpm::NodeBase<Dim>> node7 =
      std::make_shared<mpm::Node<Dim, Dof, Nphases>>(7, coords);
  REQUIRE(mesh->add_node(node7) == true);

  // Create cell1
  auto cell1 = std::make_shared<mpm::Cell<Dim>>(id1, Nnodes, element);

  // Add nodes to cell
  cell1->add_node(0, node0);
  cell1->add_node(1, node1);
  cell1->add_node(2, node2);
  cell1->add_node(3, node3);
  cell1->add_node(4, node4);
  cell1->add_node(5, node5);
  cell1->add_node(6, node6);
  cell1->add_node(7, node7);

  REQUIRE(cell1->nnodes() == 8);

  REQUIRE(mesh->add_cell(cell1) == true);

  REQUIRE(cell1->initialise() == true);

  // Check nodal coordinates size
  REQUIRE(mesh->nodal_coordinates().size() == 8);
  // Check node pairs size
  REQUIRE(mesh->node_pairs().size() == 12);

  // Add particle 1 and check
  REQUIRE(mesh->add_particle(particle1) == true);
  // Add particle 2 and check
  REQUIRE(mesh->add_particle(particle2) == true);
  // Add particle 2 again and check
  REQUIRE(mesh->add_particle(particle2) == false);

  REQUIRE(particle1->assign_material(material) == true);
  REQUIRE(particle2->assign_material(material) == true);

  // Check mesh is active
  REQUIRE(mesh->status() == true);
  // Check number of particles in mesh
  REQUIRE(mesh->nparticles() == 2);

  REQUIRE_NOTHROW(mesh->locate_particles_mesh());

  SECTION("Check ContactFriction") {

    unsigned phase = 0;

    // Initialise material
    Json jmaterial;
    jmaterial["density"] = 1000.;
    jmaterial["youngs_modulus"] = 1.0E+7;
    jmaterial["poisson_ratio"] = 0.3;

    auto material1 =
        Factory<mpm::Material<Dim>, unsigned, const Json&>::instance()->create(
            "LinearElastic3D", std::move(0), jmaterial);
    auto material2 =
        Factory<mpm::Material<Dim>, unsigned, const Json&>::instance()->create(
            "LinearElastic3D", std::move(1), jmaterial);
    std::map<unsigned, std::shared_ptr<mpm::Material<Dim>>> materials;
    materials[0] = material1;
    materials[1] = material2;

    // Assign materials to particles
    REQUIRE_NOTHROW(particle1->assign_material(material1));
    REQUIRE_NOTHROW(particle2->assign_material(material2));

    // Assign mass
    REQUIRE_NOTHROW(particle1->assign_mass(2.0));
    REQUIRE_NOTHROW(particle2->assign_mass(3.0));

    // Assign volume
    REQUIRE_NOTHROW(particle1->assign_volume(4.0));
    REQUIRE_NOTHROW(particle2->assign_volume(3.0));

    auto mpm_scheme = std::make_shared<mpm::MPMSchemeUSF<Dim>>(mesh, 0.01);

    auto contact = std::make_shared<mpm::ContactFriction<Dim>>(mesh);

    // Initialise material models
    REQUIRE_NOTHROW(mesh->initialise_material_models(materials));

    // Create nodal properties
    REQUIRE_NOTHROW(mesh->create_nodal_properties());
    // Initialise
    REQUIRE_NOTHROW(mpm_scheme->initialise());
    // Contact initialize
    REQUIRE_NOTHROW(contact->initialise());

    // Mass momentum and compute velocity at nodes
    REQUIRE_NOTHROW(mpm_scheme->compute_nodal_kinematics(phase));
    // Contact compute forces
    REQUIRE_NOTHROW(contact->compute_contact_forces());
  }
}


#ifndef SRC_PHYSICS_FLUIDS_FLIPSOLVER2D_H_
#define SRC_PHYSICS_FLUIDS_FLIPSOLVER2D_H_

#include <vector>

#include <boost/numeric/ublas/vector.hpp>
#include <glm/glm.hpp>

namespace mk
{
  namespace physics
  {
    enum CellType
    {
      kCellTypeAir = 0,
      kCellTypeFluid,
      kCellTypeSolid
    };

    class FLIPSolver2D
    {
    public:
      struct Particles
      {
      public:
        Particles();

        void addParticle(const glm::fvec2& pos, const glm::fvec2& vel);
        void clearParticles();

      public:
        std::vector<glm::fvec2> positions;
        std::vector<glm::fvec2> velocities;
        int	numParticles;
      };

    public:
      FLIPSolver2D(int grid_width, int grid_height, float dx);

      float timeStep();
      void simulate(float dt);
      void setBoundaryVel(const glm::fvec2& vel);
      float getPressure(int i, int j);
      glm::fvec2 getVelocity(int i, int j);
      glm::fvec2 getVelocity(float i, float j);
      CellType getCellType(int i, int j) const;
      void setCellType(int i, int j, CellType type);
      void setPicFlipFactor(float factor);

      float& u(int i, int j);
      float& v(int i, int j);
      int ix(int i, int j) const;
      int ixBig(int i, int j);

    public:
      Particles mParticles;

    private:
      void applyForce(float dt, float ax, float ay);
      void setBoundary();
      void project(float dt);

      void checkBoundary(float i_init_, float j_init_, float& i_end_, float& j_end_);
      void advectParticles(float dt);
      void particlesToGrid();
      void storeVel();
      float computePhi(float a, float b, float current);
      void computeGridPhi();
      void sweepU(int i0, int i1, int j0, int j1);
      void sweepV(int i0, int i1, int j0, int j1);
      void extrapolateVel();
      void subtractVel();
      void gridToParticles();
      void fillHoles();

      void solvePressure();
      void calcPrecond();
      void applyPrecond();
      void applyA();

      float uVel(float i, float j);
      float vVel(float i, float j);
      int uIndex_x(float x, float& wx);
      int uIndex_y(float y, float& wy);
      int vIndex_x(float x, float& wx);
      int vIndex_y(float y, float& wy);
      void swapVel();

    private:
      int mGridWidth;
      int mGridHeight;
      int mGridSize;
      float mDx;
      float mOverDx;
      glm::fvec2 mBoundaryVelocity;
      float mPicFlipFactor;
      std::vector<float> mVelX;
      std::vector<float> mVelY;
      std::vector<float> mDeltaVelX;
      std::vector<float> mDeltaVelY;
      std::vector<float> mWeightSum;
      std::vector<float> mPhi;
      std::vector<CellType> mCellType;
      std::vector<CellType> mCellTypeAux;

      boost::numeric::ublas::vector<double> mP;
      boost::numeric::ublas::vector<double> mR;
      boost::numeric::ublas::vector<double> mS;
      boost::numeric::ublas::vector<double> mZ;
      boost::numeric::ublas::vector<double> mAux;
      boost::numeric::ublas::vector<double> mRhs;
      boost::numeric::ublas::vector<double> mPrecond;
      boost::numeric::ublas::vector<double> mCoefDiag;
      boost::numeric::ublas::vector<double> mCoefPlusI;
      boost::numeric::ublas::vector<double> mCoefPlusJ;
    };
  }
}

#endif  // SRC_PHYSICS_FLUIDS_FLIPSOLVER2D_H_


#include <iostream>     // std::cout
#include <algorithm>    // std::count
#include <unordered_map>//unordered_map
#include <vector>       // std::vector
#include <string>       // std::string, std::to_string
#include <iostream>     // std::cout
#include <random>       // std::random_device std::default_random_engine std::uniform_int_distribution std::normal_distribution
#include <numeric>      // std::inner_product
#include <chrono>       // std::chrono:
#include <boost/math/distributions/normal.hpp> //boost::math::normal
#include <math.h>  //std::sqrt

constexpr size_t n = 4;
int p = 101;
int s[n] = { 1, 2, 3, 4}; //{ 0, 0, 0 }; //{ 1, 2, 3 };

// Declare EQUATION struct type
struct EQUATION {
    int a[n];
    int b;
    int e;

};

// mappings
std::unordered_map<std::string, std::vector<EQUATION>> equationMemo;
std::unordered_map<int, std::unordered_map<int, long>> guessCount;  // {position, {guess, count}

// error distributions
/// rounded gaussian
float mu = 0;
float sigma = 2;

std::random_device rd;
std::default_random_engine generator(rd());
std::uniform_int_distribution<int> uniform_int(0, p-1);  //min, max
std::normal_distribution<float> gaussian(mu, sigma);  // mean, std

/// centered binomial
constexpr size_t upper = 2;  // has to be odd
std::binomial_distribution<int> binomial(upper, 0.5);  //upper bound, probability of success

// oracle
void oracleLWE(EQUATION& equation){

    for (int i = 0; i < n; i++) {
        equation.a[i] = uniform_int(generator);
    }

    // discrete gaussian
    //equation.e = round(gaussian(generator));

    // centered binomial (used in NewHope reference implementation)
    equation.e = binomial(generator) - upper/2;

    equation.b = std::inner_product(s, s+n, equation.a, 0.0) + equation.e;
    equation.b %= p;
}

void encodePosOfZeros(std::string& key, int(&coeffs)[n]) {
    for (const auto& coeff : coeffs)
        if (coeff == 0)
            key.push_back('0');
        else
            key.push_back('1');
}
void initializeEquationMemo(int n, int arr[], int i) {
    if (i == n) {
        std::string key;
        for (int j=0; j < n; j++) {
            key += std::to_string(arr[j]);
        }
        std::vector<EQUATION> equations;
        equationMemo.insert({ key, equations });
        //std::cout << key << std::endl;
        return;
    }

    // First assign "0" at ith position
    // and try for all other permutations
    // for remaining positions
    arr[i] = 0;
    initializeEquationMemo(n, arr, i + 1);

    // And then assign "1" at ith position
    // and try for all other permutations
    // for remaining positions
    arr[i] = 1;
    initializeEquationMemo(n, arr, i + 1);
}

// fast pow for integer using square and multiply algo
int integerPow(int a, int ex, int mod) {
    if (a == 1) return 1;

    long r = 1;
    while (ex) {
        if (ex & 1)r = (r * a) % mod;
        ex = ex >> 1;
        a = (a * a) % mod;
    }
    return r;
}

void compareEquationsSub(std::string& key, EQUATION& prevEquation, EQUATION& equation) {
    for (int i = 0; i < n; ++i) {
        if ((prevEquation.a[i] - equation.a[i]) % p == 0)
            key.push_back('0');
        else
            key.push_back('1');
    }
}

void compareEquationsAdd(std::string& key, EQUATION& prevEquation, EQUATION& equation) {
    for (int i = 0; i < n; ++i) {
        if ((prevEquation.a[i] + equation.a[i]) % p == 0)
            key.push_back('0');
        else
            key.push_back('1');
    }
}

void furtherElimination(std::string& prevKey, EQUATION& prevEquation) {
    auto equations = equationMemo.at(prevKey);

    std::string key;
    for (auto& equation : equations) {
        EQUATION newEquation;

        // compare for subtraction
        key.clear();
        compareEquationsSub(key, prevEquation, equation);

        if (std::count(key.begin(), key.end(), '1') == 1) {
            for (int i = 0; i < n; ++i)
                newEquation.a[i] = (prevEquation.a[i] - equation.a[i]) % p;
            newEquation.b = (prevEquation.b - equation.b) % p;
            int pos = key.find('1');
            int coeff = newEquation.a[pos];
            int s = newEquation.b*integerPow(coeff, p-2, p)%p;  // solving linear congruence fast by using little fermat
            if (s < 0)
                s += p;

            guessCount.at(pos).at(s) += 1;
        }
        else {
            if (std::count(key.begin(), key.end(), '1') < std::count(prevKey.begin(), prevKey.end(), '1')) {
                for (int i = 0; i < n; ++i)
                    newEquation.a[i] = (prevEquation.a[i] - equation.a[i]) % p;
                newEquation.b = (prevEquation.b - equation.b) % p;

                furtherElimination(key, newEquation);

                equationMemo.at(key).push_back(equation);
            }
        }

        // compare for addition
        key.clear();
        compareEquationsAdd(key, prevEquation, equation);

        if (std::count(key.begin(), key.end(), '1') == 1) {
            for (int i = 0; i < n; ++i)
                newEquation.a[i] = (prevEquation.a[i] + equation.a[i]) % p;
            newEquation.b = (prevEquation.b + equation.b) % p;
            int pos = key.find('1');
            int coeff = newEquation.a[pos];
            int s = newEquation.b * integerPow(coeff, p - 2, p) % p;  // solving linear congruence fast by using little fermat
            if (s < 0)
                s += p;

            guessCount.at(pos).at(s) += 1;
        }
        else {
            if (std::count(key.begin(), key.end(), '1') < std::count(prevKey.begin(), prevKey.end(), '1')) {
                for (int i = 0; i < n; ++i)
                    newEquation.a[i] = (prevEquation.a[i] + equation.a[i]) % p;
                newEquation.b = (prevEquation.b + equation.b) % p;

                furtherElimination(key, newEquation);

                equationMemo.at(key).push_back(equation);
            }
        }
    }
}

int main(int argc, char** argv) {
    // initialize equation memory
    int arr[n];
    initializeEquationMemo(n, arr, 0);

    // initialize map to track the count of guesses for a possible solution
    for (int pos = 0; pos < n; pos++) {
        std::unordered_map<int, long> tmp;
        for (int guess = 0; guess < p; guess++)
            tmp.insert(std::make_pair(guess, 0));

        guessCount.insert(std::make_pair(pos, tmp));
    }

    std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();

    long i = 1;
    while (1) {
        EQUATION equation;
        oracleLWE(equation);

        std::string key;
        encodePosOfZeros(key, equation.a);

        furtherElimination(key, equation);

        equationMemo.at(key).push_back(equation);

        // early stopping
        if (i % 10 == 0) {
            bool ready = true;
            for (auto const& pos : guessCount) {
                float mean = 0;
                for (auto const& guess : pos.second)
                    mean += guess.second;
                if (mean < 20) {
                    std::cout << "mean: " << mean << std::endl;
                    goto endOfLoop;
                }
                mean /= p;

                float var = 0;
                for (auto const& guess : pos.second)
                    var += (guess.second - mean) * (guess.second - mean); // fast pow
                var /= p - 1;
                float sigma = std::sqrt(var);

                int guessCount = 0;
                for (auto const& guess : pos.second) {
                    if (mean + sigma < guess.second) {
                        guessCount += 1;
                        std::cout << "Pos: " << pos.first << "\t\tGuess: " << guess.first << "\t\tCount: " <<
                            guess.second << "\t\tRight solution: " << s[pos.first] << std::endl;
                    }
                }
                std::cout << std::endl;
                if (guessCount != 1) {
                    ready = false;
                }
            }
            if (ready) {
                std::cout << "Count of equations: " << i << std::endl << std::endl;
                break;
            }
        }
    endOfLoop: {}
        std::cout << "Actual equations: " << i << "\t\tProcessing Time = " <<
            std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - begin).count() / 1000 <<
            "[s]" << std::endl;
        i += 1;
    }

    std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();

    std::cout << "Time difference = " << std::chrono::duration_cast<std::chrono::milliseconds>(end - begin).count()/1000 << "[s]" << std::endl;
    //return 0;

    // p-value empirical distribution
    for (auto const& pos : guessCount) {
        float mean = 0;
        for (auto const& guess : pos.second)
            mean += guess.second;
        mean /= p;

        float var = 0;
        for (auto const& guess : pos.second)
            var += (guess.second - mean)*(guess.second - mean); // fast pow
        var /= p-1;  // empirical variance
        float sigma = std::sqrt(var);
        std::cout << "mean + sigma " << mean+sigma << std::endl;
    }
    std::cout << std::endl;

    // p-value gaussian approximation of binomial
    /*for (auto const& pos : guessCount) {
        long sum = 0;
        for (auto const& guess : pos.second)
            sum += guess.second;
        float mean = sum/p;
        float sigma = std::sqrt(mean * (1 - 1 / p));
        boost::math::normal dist(mean, sigma);
        double q = quantile(dist, 0.99);
        std::cout << "quantile = " << q << std::endl;
    }*/

    // determine guess

    // debug
    for (auto const& pos : guessCount) {
        float mean = 0;
        for (auto const& guess : pos.second)
            mean += guess.second;
        mean /= p;

        float var = 0;
        for (auto const& guess : pos.second)
            var += (guess.second - mean) * (guess.second - mean); // fast pow
        var /= p-1;
        float sigma = std::sqrt(var);

        for (auto const& guess : pos.second) {
            if (mean + sigma < guess.second) {
                std::cout << "Pos: " << pos.first << "\t\tguess: " << guess.first << "\t\tcount: " << guess.second << "\t\tright: " << s[pos.first] << '\n';
            }
            //if (guess.second != 0)
            //    std::cout << "guess: " << guess.first << "    count: " << guess.second <<'\n';
        }
    }
    return 0;

    // debug
    for (auto const& pos : guessCount) {
        int trackGuess=0, trackMax=0;
        for (auto const& guess : pos.second) {
            if (trackMax < guess.second) {
                trackGuess = guess.first;
                trackMax = guess.second;
            }
            //if (guess.second != 0)
            //    std::cout << "guess: " << guess.first << "    count: " << guess.second <<'\n';
        }
        std::cout << "Pos: " << pos.first << "\t\t\tguess: " << trackGuess << "\t\t\count: " << trackMax << "\t\t\tright: " << s[pos.first] << '\n';
    }
    return 0;

    for (auto const& tmp : equationMemo) {
        std::cout << "Key: " << tmp.first << '\n';
        std::cout << "tmp contains " << tmp.second.size() << " elements.\n";
        continue;
        for (auto const& equation : tmp.second) {
            for (int i = 0; i < n; i++)
                std::cout << "a[" << i << "]: " << equation.a[i] << '\n';

            std::cout << "b: " << equation.b << '\n';
            std::cout << "e: " << equation.e << '\n' << '\n';
        }

    }

    return 0;
}


#ifndef STAN_MATH_PRIM_SCAL_FUN_BESSEL_FIRST_KIND_HPP
#define STAN_MATH_PRIM_SCAL_FUN_BESSEL_FIRST_KIND_HPP

#include <boost/math/special_functions/bessel.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>

namespace stan {
namespace math {

/**
 *
   \f[
   \mbox{bessel\_first\_kind}(v, x) =
   \begin{cases}
     J_v(x) & \mbox{if } -\infty\leq x \leq \infty \\[6pt]
     \textrm{error} & \mbox{if } x = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{bessel\_first\_kind}(v, x)}{\partial x} =
   \begin{cases}
     \frac{\partial\, J_v(x)}{\partial x} & \mbox{if } -\infty\leq x\leq \infty
 \\[6pt] \textrm{error} & \mbox{if } x = \textrm{NaN} \end{cases} \f]

   \f[
   J_v(x)=\left(\frac{1}{2}x\right)^v
   \sum_{k=0}^\infty \frac{\left(-\frac{1}{4}x^2\right)^k}{k!\, \Gamma(v+k+1)}
   \f]

   \f[
   \frac{\partial \, J_v(x)}{\partial x} = \frac{v}{x}J_v(x)-J_{v+1}(x)
   \f]
 *
 */
template <typename T2>
inline T2 bessel_first_kind(int v, const T2 z) {
  check_not_nan("bessel_first_kind", "z", z);
  return boost::math::cyl_bessel_j(v, z);
}

}  // namespace math
}  // namespace stan
#endif


#include "core/Initializer.h"

#include <Eigen/Core>
#include <Eigen/SVD>
#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

namespace EllipsoidSLAM
{
    Initializer::Initializer(int rows, int cols) {
        miImageRows = rows;
        miImageCols = cols;
    }

    // ascending sort
    bool cmp(pair<int, double>a, pair<int, double>b)
    {
        return a.second > b.second;
    }

    // Initialize an ellipsoid from several bounding boxes and camera poses.
    g2o::ellipsoid Initializer::initializeQuadric(MatrixXd &pose_mat, MatrixXd &detection_mat, Matrix3d &calib) {
        mbResult = false;

        int input_size = pose_mat.rows();

        // 1) Generate tangent planes from bounding boxes and camera cneter.
        //    Those invalid observations are ignored.
        MatrixXd planesHomo = getPlanesHomo(pose_mat, detection_mat, calib);

        int plane_size = planesHomo.cols();
        int invalid_plane = input_size*4 - plane_size;
        if( invalid_plane > 0)
            std::cout << " * invalid_plane: " << invalid_plane << std::endl;

        if(plane_size < 9)  // at least 9 planes are needed
        {
            g2o::ellipsoid e_bad;
            mbResult = false;
            return e_bad;
        }

        // Using SVD to generate a quadric
        MatrixXd planesVector = getVectorFromPlanesHomo(planesHomo);
        Matrix4d QStar = getQStarFromVectors(planesVector);

        // generate ellipsoid from the quadric
        g2o::ellipsoid e = getEllipsoidFromQStar(QStar);

        // Set color : blue.
        Eigen::Vector3d blueScalar(0,0,255);
        e.setColor(blueScalar);
        return e;
    }

    MatrixXd Initializer::getPlanesHomo(MatrixXd &pose_mat, MatrixXd &detection_mat, Matrix3d &calib) {
        assert( pose_mat.rows() == detection_mat.rows() && " Two matrices should match. " );
        assert( pose_mat.rows() > 2 && " At least 3 measurements are required. " );

        MatrixXd planes_all(4,0);

        int rows = pose_mat.rows();
        for(int i=0;i<rows;i++)
        {
            VectorXd pose = pose_mat.row(i);
            VectorXd detection = detection_mat.row(i);

            // filter invalid detections
            if( detection(0) < 1 && detection(1) < 1 && detection(2) < 1 && detection(3) < 1  )
                continue;

            Vector7d pose_vec = pose.tail(7);
            g2o::SE3Quat campose_wc(pose_vec);
            // get projection matrix
            MatrixXd P = generateProjectionMatrix(campose_wc.inverse(), calib);

            MatrixXd lines = fromDetectionsToLines(detection);
            MatrixXd planes = P.transpose() * lines;

            // add to matrix
            for( int m=0;m<planes.cols();m++)
            {
                planes_all.conservativeResize(planes_all.rows(), planes_all.cols()+1);
                planes_all.col(planes_all.cols()-1) = planes.col(m);
            }
        }

        return planes_all;
    }

    Matrix3Xd Initializer::generateProjectionMatrix(const SE3Quat& campose_cw, const Matrix3d& Kalib) const
    {
        Matrix3Xd identity_lefttop;
        identity_lefttop.resize(3, 4);
        identity_lefttop.col(3)=Vector3d(0,0,0);
        identity_lefttop.topLeftCorner<3,3>() = Matrix3d::Identity(3,3);

        Matrix3Xd proj_mat = Kalib * identity_lefttop;

        proj_mat = proj_mat * campose_cw.to_homogeneous_matrix();

        return proj_mat;
    }

    MatrixXd Initializer::fromDetectionsToLines(VectorXd &detections) {
        bool flag_openFilter = true;        // filter those lines lying on the image boundary

        double x1 = detections(0);
        double y1 = detections(1);
        double x2 = detections(2);
        double y2 = detections(3);

        Vector3d line1 (1, 0, -x1);
        Vector3d line2 (0, 1, -y1);
        Vector3d line3 (1, 0, -x2);
        Vector3d line4 (0, 1, -y2);

        // those lying on the image boundary have been marked -1
        MatrixXd line_selected(3, 0);
        MatrixXd line_selected_none(3, 0);
        if( !flag_openFilter || ( x1>0 && x1<miImageCols-1 ))
        {
            line_selected.conservativeResize(3, line_selected.cols()+1);
            line_selected.col(line_selected.cols()-1) = line1;
        }
        if( !flag_openFilter || (y1>0 && y1<miImageRows-1 ))
        {
            line_selected.conservativeResize(3, line_selected.cols()+1);
            line_selected.col(line_selected.cols()-1) = line2;
        }
        if( !flag_openFilter || (x2>0 && x2<miImageCols-1 ))
        {
            line_selected.conservativeResize(3, line_selected.cols()+1);
            line_selected.col(line_selected.cols()-1) = line3;
        }
        if( !flag_openFilter || (y2>0 && y2<miImageRows-1 ))
        {
            line_selected.conservativeResize(3, line_selected.cols()+1);
            line_selected.col(line_selected.cols()-1) = line4;
        }

        return line_selected;
    }

    MatrixXd Initializer::getVectorFromPlanesHomo(MatrixXd &planes) {
        int cols = planes.cols();

        MatrixXd planes_vector(10,0);

        for(int i=0;i<cols;i++)
        {
            VectorXd p = planes.col(i);
            Vector10d v;

            v << p(0)*p(0),2*p(0)*p(1),2*p(0)*p(2),2*p(0)*p(3),p(1)*p(1),2*p(1)*p(2),2*p(1)*p(3),p(2)*p(2),2*p(2)*p(3),p(3)*p(3);

            planes_vector.conservativeResize(planes_vector.rows(), planes_vector.cols()+1);
            planes_vector.col(planes_vector.cols()-1) = v;
        }

        return planes_vector;
    }

    Matrix4d Initializer::getQStarFromVectors(MatrixXd &planeVecs) {
        MatrixXd A = planeVecs.transpose();

        // svd decompose
        JacobiSVD<Eigen::MatrixXd> svd(A, ComputeThinU | ComputeThinV );
        MatrixXd V = svd.matrixV();

        VectorXd qj_hat = V.col(V.cols()-1);

        // Get QStar
        Matrix4d QStar;
        QStar <<
            qj_hat(0),qj_hat(1),qj_hat(2),qj_hat(3),
            qj_hat(1),qj_hat(4),qj_hat(5),qj_hat(6),
            qj_hat(2),qj_hat(5),qj_hat(7),qj_hat(8),
            qj_hat(3),qj_hat(6),qj_hat(8),qj_hat(9);

        return QStar;
    }

    g2o::ellipsoid Initializer::getEllipsoidFromQStar(Matrix4d &QStar) {
        g2o::ellipsoid e;

        Matrix4d Q = QStar.inverse() * cbrt(QStar.determinant());

        SelfAdjointEigenSolver<Matrix4d> es(Q);    // ascending order by default
        MatrixXd D = es.eigenvalues().asDiagonal();
        MatrixXd V = es.eigenvectors();

        VectorXd eigens = es.eigenvalues();

        // For an ellipsoid, the signs of the eigenvalues must be ---+ or +++-
        int num_pos = int(eigens(0)>0) +int(eigens(1)>0) +int(eigens(2)>0) +int(eigens(3)>0);
        int num_neg = int(eigens(0)<0) +int(eigens(1)<0) +int(eigens(2)<0) +int(eigens(3)<0);
        if( !(num_pos ==3 && num_neg == 1) && !(num_pos ==1 && num_neg == 3) ){
            cout << " Not Ellipsoid : pos/neg  " << num_pos << " / " << num_neg << endl;
            cout << "eigens :" << eigens.transpose() << endl;
            mbResult = false;
            return e;
        }
        else
            mbResult = true;

        if( eigens(3) > 0  )  // normalize to - - - +
        {
            Q=-Q;
            SelfAdjointEigenSolver<Matrix4d> es_2(Q);
            D = es_2.eigenvalues().asDiagonal();
            V = es_2.eigenvectors();

            eigens = es_2.eigenvalues();
        }

        // Solve ellipsoid parameters from matrix Q
        Vector3d lambda_mat = eigens.head(3).array().inverse();

        Matrix3d Q33 = Q.block(0,0,3,3);

        double k = Q.determinant()/Q33.determinant();

        Vector3d value = -k*(lambda_mat);
        Vector3d s = value.array().abs().sqrt();

        Vector4d t = QStar.col(3);
        t = t/t(3);
        Vector3d translation = t.head(3);

        SelfAdjointEigenSolver<Matrix3d> es2(Q33);
        MatrixXd D_Q33 = es2.eigenvalues().asDiagonal();
        MatrixXd rot = es2.eigenvectors();

        double r,p,y;
        rot_to_euler_zyx<double>(rot, r, p, y);
        Vector3d rpy(r,p,y);

        // generate ellipsoid
        Vector9d objectVec;
        objectVec << t(0),t(1),t(2),rpy(0),rpy(1),rpy(2),s(0),s(1),s(2);
        e.fromMinimalVector(objectVec);

        return e;

    }

    void Initializer::sortEigenValues(VectorXcd &eigens, MatrixXcd &V) {
        vector<pair<int, double>> pairs;
        for(int i=0;i<4;i++)
            pairs.push_back( make_pair( i, eigens(i).real() ) );
        sort(pairs.begin(), pairs.end(), cmp);

        // Construct a new matrix in order
        MatrixXcd V_new(4,4);
        Vector4cd eigens_new;

        for(int i=0;i<4;i++)
        {
            int oldID = pairs[i].first;
            V_new.col(i) = V.col(oldID);
            eigens_new(i) = complex<double>(pairs[i].second, 0);
        }

        eigens = eigens_new;
        V = V_new;
    }

    double Initializer::quadricErrorWithPlanes(MatrixXd &pose_mat, MatrixXd &detection_mat, Matrix3d &calib,
                                               g2o::ellipsoid &e) {
        Matrix4d QStar = e.generateQuadric();           // get Q^*
        Vector10d qj_hat;
        qj_hat << QStar(0,0),QStar(0,1),QStar(0,2),QStar(0,3),QStar(1,1),QStar(1,2),QStar(1,3),QStar(2,2),QStar(2,3),QStar(3,3);

        // Get planes vector
        MatrixXd planesHomo = getPlanesHomo(pose_mat, detection_mat, calib);
        MatrixXd planesVector = getVectorFromPlanesHomo(planesHomo);

        // Get error
        VectorXd result = planesVector.transpose() * qj_hat;
        return result.transpose() * result;
    }

    bool Initializer::getInitializeResult() {
        return mbResult;
    }

    g2o::ellipsoid Initializer::initializeQuadric(EllipsoidSLAM::Observations &obs, Matrix3d &calib) {
        MatrixXd pose_mat;
        MatrixXd detection_mat;

        // get pose matrix and detection matrix from observations
        getDetectionAndPoseMatFromObservations(obs, pose_mat, detection_mat);

        g2o::ellipsoid e = initializeQuadric(pose_mat, detection_mat, calib);

        if( getInitializeResult() )
        {
            e.miLabel = obs[0]->label;
        }

        return e;
    }

    void Initializer::getDetectionAndPoseMatFromObservations(EllipsoidSLAM::Observations &obs, MatrixXd &pose_mat,
                                                             MatrixXd &detection_mat) {
        int frameSize = obs.size();
        pose_mat.resize(frameSize, 7);  // x y z qx qy qz qw
        detection_mat.resize(frameSize, 5); // x1 y1 x2 y2 accuracy

        int id = 0;
        for (auto iter = obs.begin(); iter!=obs.end(); iter++)
        {
            Vector5d det_vec;
            det_vec << (*iter)->bbox, (*iter)->rate;
            Vector7d pos_vec = (*iter)->pFrame->cam_pose_Twc.toVector();

            pose_mat.row(id) = pos_vec;
            detection_mat.row(id) = det_vec;

            id ++;
        }
    }
}