#include "calibration_manager.h"
#include <boost/lexical_cast.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/ini_parser.hpp>
#include <boost/tokenizer.hpp>
#include <boost/math/special_functions/round.hpp>

#include <opencv2/flann/miniflann.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <scan_calculator.h>

#include <fstream>
#include <graycode.h>

CalibrationManager::CalibrationManager()
{
    m_calibParams.resize(2);
    m_detectionResult.resize(2);

    m_calibDescription.ReadFromIni("C:/Users/yildirim/Dropbox/Vision/myOCV/images/hosteo1/Calib.ini");
}

CalibrationManager::~CalibrationManager()
{

}

bool CalibrationManager::StereoCalibrate()
{
    if(!IsReadyToGo())
        return false;

    std::vector<std::vector<std::vector<cv::Point2f> > > ring_markers_image_wrapper;
    ring_markers_image_wrapper.resize(2);
    ring_markers_image_wrapper[0].resize(1);
    ring_markers_image_wrapper[1].resize(1);

    for(int cam_idx = 0; cam_idx < 2; cam_idx++)
    {
        //calculate initial guess for Ps
        m_stereoResult.P[cam_idx] = DLT(m_detectionResult[cam_idx].id_markers_world[0], m_detectionResult[cam_idx].id_markers_image[0]);

        //decompose Ps
        cv::decomposeProjectionMatrix(m_stereoResult.P[cam_idx], m_stereoResult.K[cam_idx], m_stereoResult.rvecs[cam_idx][0], m_stereoResult.tvecs[cam_idx][0]);
        m_stereoResult.K[cam_idx] = m_stereoResult.K[cam_idx] / m_stereoResult.K[cam_idx].at<float>(2,2);
        m_stereoResult.K[cam_idx].at<float>(0,1) = 0.0;
        m_stereoResult.K[cam_idx].at<float>(1,0) = 0.0;

        cv::Mat dummy;
        if(!MatchMarkers(m_calibDescription, m_stereoResult.P[cam_idx], m_detectionResult[cam_idx], cam_idx, dummy))
            return false;

        //Tum noktalari buraya dose, bu Levenberg-Marquard kullaniyor guya
        m_stereoResult.RMS[cam_idx] = cv::calibrateCamera(
                                        m_detectionResult[cam_idx].ring_markers_world,
                                        m_detectionResult[cam_idx].ring_markers_image,
                                        m_detectionResult[cam_idx].img_size,
                                        m_stereoResult.K[cam_idx],
                                        m_stereoResult.D[cam_idx],
                                        m_stereoResult.rvecs[cam_idx],
                                        m_stereoResult.tvecs[cam_idx],
                                        CV_CALIB_USE_INTRINSIC_GUESS); //| CV_CALIB_FIX_K1|CV_CALIB_FIX_K2|CV_CALIB_FIX_K3|CV_CALIB_FIX_K4|CV_CALIB_FIX_K5);

        cv::projectPoints(m_calibDescription.calib_points,
                          m_stereoResult.rvecs[cam_idx][0],
                          m_stereoResult.tvecs[cam_idx][0],
                          m_stereoResult.K[cam_idx],
                          m_stereoResult.D[cam_idx],
                          ring_markers_image_wrapper[cam_idx][0]);
    }

    //stereo calibration
    std::vector<std::vector<cv::Point3f> > calib_points_wrapper;
    calib_points_wrapper.push_back(m_calibDescription.calib_points);

    m_stereoResult.stereoRMS = cv::stereoCalibrate(calib_points_wrapper,
                                     ring_markers_image_wrapper[0],
                                     ring_markers_image_wrapper[1],
                                     m_stereoResult.K[0],
                                     m_stereoResult.D[0],
                                     m_stereoResult.K[1],
                                     m_stereoResult.D[1],
                                     m_detectionResult[0].img_size,
                                     m_stereoResult.R,
                                     m_stereoResult.T,
                                     m_stereoResult.E,
                                     m_stereoResult.F,
                                     cv::TermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, 50, 1e-5),
                                     CV_CALIB_USE_INTRINSIC_GUESS );
    return true;
}

void CalibrationManager::WriteRawCal(const char* filename, std::vector<std::vector<cv::Mat> >& sequence)
{
	int height = sequence[0][0].rows;
	int width = sequence[0][0].cols;

	ScanCalculator sc;
	sc.CalculateAbsPhase(sequence);
	cv::Mat absL = cv::Mat(height, width, CV_16S);
	cv::Mat absR = cv::Mat(height, width, CV_16S);
	LoadAbsolutePhase("absL.tiff", absL);
	LoadAbsolutePhase("absR.tiff", absR);

	int i = 0;
	boost::property_tree::ptree pt;
	char bufferHeader[32];
	char bufferData[256];
	pt.put("Cam1.NP", m_detectionResult[0].ring_markers_image[0].size());
	for(; i < m_detectionResult[0].ring_markers_image[0].size(); i++)
	{
		double imgX = m_detectionResult[0].ring_markers_image[0][i].x;
		double imgY = m_detectionResult[0].ring_markers_image[0][i].y;
		double wrdX = m_detectionResult[0].ring_markers_world[0][i].x;
		double wrdY = m_detectionResult[0].ring_markers_world[0][i].y;
		double wrdZ = m_detectionResult[0].ring_markers_world[0][i].z;
		double srcW = BLInterpolate(imgX, imgY, absL);

		sprintf(bufferHeader, "Cam1.Mark_%.3i", i);
		sprintf(bufferData, "%f %f %f %f %f %f", wrdX, wrdY, wrdZ, imgX, imgY, srcW);
		pt.put(bufferHeader, bufferData);
	}

	i = 0;
	pt.put("Cam2.NP", m_detectionResult[1].ring_markers_image[0].size());
	for(; i < m_detectionResult[1].ring_markers_image[0].size(); i++)
	{
		double imgX = m_detectionResult[1].ring_markers_image[0][i].x;
		double imgY = m_detectionResult[1].ring_markers_image[0][i].y;
		double wrdX = m_detectionResult[1].ring_markers_world[0][i].x;
		double wrdY = m_detectionResult[1].ring_markers_world[0][i].y;
		double wrdZ = m_detectionResult[1].ring_markers_world[0][i].z;
		double srcW = BLInterpolate(imgX, imgY, absR);

		sprintf(bufferHeader, "Cam2.Mark_%.3i", i);
		sprintf(bufferData, "%f %f %f %f %f %f", wrdX, wrdY, wrdZ, imgX, imgY, srcW);
		pt.put(bufferHeader, bufferData);
	}
	boost::property_tree::write_ini(filename, pt);
}

bool CalibrationManager::DetectMarkers(cv::Mat& camImg, int idxCam)
{
    m_detectionResult[idxCam].amount_id_markers = 0;
    m_detectionResult[idxCam].img_size = cv::Size(camImg.cols, camImg.rows);

    std::map<int, int> id_markers_index;
    std::vector<cv::Point3f> id_markers_world;
    std::vector<cv::Point2f> id_markers_image;
    std::vector<cv::Point2f> all_markers_image;

    //convert source image to gray
    cv::Mat src_gray;
    cv::cvtColor(camImg, src_gray, CV_BGR2GRAY);

    cv::GaussianBlur( src_gray, src_gray, cv::Size(m_calibParams[idxCam].kernel_size, m_calibParams[idxCam].kernel_size), 2, 2 );

    //binarize image
    cv::RNG rng(12345);
    cv::threshold( src_gray, src_gray, m_calibParams[idxCam].binary_threshold, 255, CV_THRESH_BINARY);

    cv::Mat src_binary;
    src_gray.copyTo(src_binary);

    std::vector<std::vector<cv::Point> > contours;
    std::vector<cv::Vec4i> hierarchy;
    cv::findContours( src_gray, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) );

    //Find the rotated rectangles and ellipses for each contour
    std::vector<cv::RotatedRect> minRect( contours.size() );
    std::vector<cv::RotatedRect> minEllipse( contours.size() );

    for( int i = 0; i < contours.size(); i++ )
    {
        minRect[i] = cv::minAreaRect( cv::Mat(contours[i]) );
        if( contours[i].size() > 5 )
        {
            minEllipse[i] = cv::fitEllipse( cv::Mat(contours[i]) );
        }
    }

    //Draw contours + rotated rects + ellipses
    for( int i = 0; i< contours.size(); i++ )
    {
        if(minEllipse[i].size.width < 20 || minEllipse[i].size.width > 100)continue;
        if(minEllipse[i].size.height < 20 || minEllipse[i].size.height > 100)continue;
        if(minEllipse[i].size.area() < 1500 || minEllipse[i].size.area() > 5000)continue;
        if( abs((minEllipse[i].size.width / (double)minEllipse[i].size.height)-1.0) > 0.15 )continue;

        cv::Scalar color = cv::Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) );
        // contour
        //cv::drawContours( camImg, contours, i, cv::Scalar(255,255,0), 1, 8, std::vector<cv::Vec4i>(), 0, cv::Point() );
        // ellipse

        // rotated rectangle
        cv::Point2f rect_points[4]; minRect[i].points( rect_points );
        //for( int j = 0; j < 4; j++ )
          //cv::line( camImg, rect_points[j], rect_points[(j+1)%4], color, 1, 8 );

        //detected_img_points.push_back(minEllipse[i].center);
        //m_detectionResult[idxCam].all_markers_image[0].push_back(minEllipse[i].center);

        cv::Point2f p = cv::Point2f(minEllipse[i].size.width/2-m_calibParams[idxCam].id_marker_position_offset);
        cv::Point2f image_coord = minEllipse[i].center + p;
        int prev_val = src_binary.at<uchar>(image_coord);
        int tooth_counter = 0;

        double step_size = M_PI * 2 / m_calibParams[idxCam].radial_step_amount;
        for(int j = 0; j < m_calibParams[idxCam].radial_step_amount; j++)
        {
            cv::Point2f newp;
            newp.x = (p.x*std::cos(step_size*j) - p.y*std::sin(step_size*j));
            newp.y = (p.x*std::sin(step_size*j) + p.y*std::cos(step_size*j));
            image_coord = minEllipse[i].center + newp;
            //cv::circle( camImg, image_coord, 2,cv::Scalar(255,255,255));

            //analyze this value to detect teeth
            uchar val = src_binary.at<uchar>(image_coord);
            if(abs(val - prev_val) == 255)
                tooth_counter++;
                prev_val = val;
        }

        if(tooth_counter>2){
            m_detectionResult[idxCam].amount_id_markers++;
            id_markers_index[std::ceil(tooth_counter/2.0)] = i;
            cv::circle( camImg, minEllipse[i].center, 20,cv::Scalar(255,0,0), 5);
            std::ostringstream oss;
            oss << std::ceil(tooth_counter/2.0);
            cv::putText(camImg, oss.str(), minEllipse[i].center-cv::Point2f(15,0), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 255, 0), 3);
        }
        else
        {
            cv::ellipse( camImg, minEllipse[i], cv::Scalar(0,255,255), 2, 8 );
            all_markers_image.push_back(minEllipse[i].center);
        }
    }

    //find id markers
    for(std::map<int, int>::const_iterator it = id_markers_index.begin(); it != id_markers_index.end();it++)
    {
        id_markers_world.push_back(m_calibDescription.id_markers_pos[(*it).first]);
        id_markers_image.push_back(minEllipse[id_markers_index[(*it).first]].center);
        //std::cout << "id: " << (*it).first << " pos: " << (*it).second << std::endl;
    }
    m_detectionResult[idxCam].id_markers_world[0].swap(id_markers_world);
    m_detectionResult[idxCam].id_markers_image[0].swap(id_markers_image);
    m_detectionResult[idxCam].all_markers_image[0].swap(all_markers_image);

    return true;
}

bool CalibrationManager::MatchMarkers(const CalibrationDescription& desc, const cv::Mat& P, DetectionResult& detectionResult, int idxCam, cv::Mat& camImg)
{
    std::vector<cv::Point3f> ring_markers_world;
    std::vector<cv::Point2f> ring_markers_image;

    cv::flann::KDTreeIndexParams indexParams;
    cv::flann::Index kdtree(cv::Mat(detectionResult.all_markers_image[0]).reshape(1), indexParams);
    for(int i = 0; i < desc.calib_points.size(); i++)
    {
        cv::Mat cp(4,1,CV_32F);
        cp.at<float>(cv::Point2i(0,0)) = desc.calib_points[i].x;
        cp.at<float>(cv::Point2i(0,1)) = desc.calib_points[i].y;
        cp.at<float>(cv::Point2i(0,2)) = desc.calib_points[i].z;
        cp.at<float>(cv::Point2i(0,3)) = 1;

        cv::Mat projected = P*cp;
        projected /= projected.at<float>(2);
        cv::Point2f proj(projected.at<float>(0), projected.at<float>(1));

        cv::circle( camImg, proj, 3,cv::Scalar(0,0,255));
        std::ostringstream oss;
        oss << i;
        cv::putText(camImg, oss.str(), cv::Point2f(proj.x - 25, proj.y), cv::FONT_HERSHEY_SIMPLEX, .5, cv::Scalar(0, 255, 255));

        std::vector<float> query;
        query.push_back(proj.x); //Insert the 2D point we need to find neighbours to the query
        query.push_back(proj.y); //Insert the 2D point we need to find neighbours to the query
        std::vector<int> indices;
        std::vector<float> dists;
        bool result = (bool)kdtree.radiusSearch(query, indices, dists, m_calibParams[idxCam].marker_search_radius, 1);

        if(!result)
            continue;

        //item found
        ring_markers_world.push_back(desc.calib_points[i]);
        ring_markers_image.push_back(detectionResult.all_markers_image[0][indices[0]]);
    }

    m_detectionResult[idxCam].ring_markers_world[0].swap(ring_markers_world);
    m_detectionResult[idxCam].ring_markers_image[0].swap(ring_markers_image);

    return true;
}

DetectionResult& CalibrationManager::GetDetectionResult(const int idx_cam)
{
    return m_detectionResult[idx_cam];
}

CalibrationParameters& CalibrationManager::GetCalibrationParameters(int idxCam)
{
    return m_calibParams[idxCam];
}

CalibrationDescription& CalibrationManager::GetCalibrationDescription()
{
    return m_calibDescription;
}

cv::Mat CalibrationManager::Calculate2DConditioning(std::vector<cv::Point2f> in)const
{
    //translation x ve y lerin ortalamasi t
    cv::Scalar t = cv::mean(in);

    //transform olmus hallerinin abs li ortalamasi s
    for(int i = 0; i < in.size(); i++)
    {
        in[i].x = abs(in[i].x - t[0]);
        in[i].y = abs(in[i].y - t[1]);
    }

    cv::Scalar s = cv::mean(in);

    cv::Mat S = cv::Mat::eye(3,3, CV_32F);
    S.at<float>(cv::Point2i(0,0)) = 1.0/s[0];
    S.at<float>(cv::Point2i(1,1)) = 1.0/s[1];

    cv::Mat Tr = cv::Mat::eye(3,3, CV_32F);
    Tr.at<float>(cv::Point2i(2,0)) = -t[0];
    Tr.at<float>(cv::Point2i(2,1)) = -t[1];

    cv::Mat T;
    T = S*Tr;
    return T;
}

cv::Mat CalibrationManager::Calculate3DConditioning(std::vector<cv::Point3f> in)const
{
    //translation x ve y lerin ortalamasi t
    cv::Scalar t = cv::mean(in);

    //transform olmus hallerinin abs li ortalamasi s
    for(int i = 0; i < in.size(); i++)
    {
        in[i].x = abs(in[i].x - t[0]);
        in[i].y = abs(in[i].y - t[1]);
        in[i].z = abs(in[i].z - t[2]);
    }

    cv::Scalar s = cv::mean(in);

    cv::Mat S = cv::Mat::eye(4,4, CV_32F);
    if(s[0])S.at<float>(cv::Point2i(0,0)) = 1.0/s[0];
    if(s[1])S.at<float>(cv::Point2i(1,1)) = 1.0/s[1];
    if(s[2])S.at<float>(cv::Point2i(2,2)) = 1.0/s[2];

    cv::Mat Tr = cv::Mat::eye(4,4, CV_32F);
    Tr.at<float>(cv::Point2i(3,0)) = -t[0];
    Tr.at<float>(cv::Point2i(3,1)) = -t[1];
    Tr.at<float>(cv::Point2i(3,2)) = -t[2];

    cv::Mat T;
    T = S*Tr;
    return T;
}

cv::Mat CalibrationManager::DLT(const std::vector<cv::Point3f>& ptsObject, const std::vector<cv::Point2f>& ptsImage)const
{
    cv::Mat P = cv::Mat::zeros(3,4,CV_32F);

    if(!ptsObject.size() || !ptsImage.size())
        return P;

    //conditioning
    cv::Mat To = Calculate3DConditioning(ptsObject);
    cv::Mat Ti = Calculate2DConditioning(ptsImage);
    //std::cout <<"To: "<< To << "\n\n";
    //std::cout <<"Ti: "<< Ti << "\n\n";

    std::vector<cv::Mat> conObjPts;
    std::vector<cv::Mat> conImgPts;

    for(int i = 0; i < 6;i++)
    {
        //object points
        cv::Mat om(4,1,CV_32F);
        om.at<float>(cv::Point2i(0,0)) = ptsObject[i].x;
        om.at<float>(cv::Point2i(0,1)) = ptsObject[i].y;
        om.at<float>(cv::Point2i(0,2)) = ptsObject[i].z;
        om.at<float>(cv::Point2i(0,3)) = 1;
        conObjPts.push_back(To*om);
     }

    for(int i = 0; i < 6;i++)
    {
        //image points
        cv::Mat im(3,1,CV_32F);
        im.at<float>(cv::Point2i(0,0)) = ptsImage[i].x;
        im.at<float>(cv::Point2i(0,1)) = ptsImage[i].y;
        im.at<float>(cv::Point2i(0,2)) = 1;
        conImgPts.push_back(Ti*im);
    }

    //Direct Linear Transformation
    //Prepare design(A) matrix
    cv::Mat A(12, 12, CV_32F);
    for(int i = 0; i < conObjPts.size(); i++ )
    {
        A.at<float>(cv::Point2i(0,i*2)) = -conImgPts[i].at<float>(2)*conObjPts[i].at<float>(0);
        A.at<float>(cv::Point2i(1,i*2)) = -conImgPts[i].at<float>(2)*conObjPts[i].at<float>(1);
        A.at<float>(cv::Point2i(2,i*2)) = -conImgPts[i].at<float>(2)*conObjPts[i].at<float>(2);
        A.at<float>(cv::Point2i(3,i*2)) = -conImgPts[i].at<float>(2)*conObjPts[i].at<float>(3);
        A.at<float>(cv::Point2i(4,i*2)) = 0.0;
        A.at<float>(cv::Point2i(5,i*2)) = 0.0;
        A.at<float>(cv::Point2i(6,i*2)) = 0.0;
        A.at<float>(cv::Point2i(7,i*2)) = 0.0;
        A.at<float>(cv::Point2i(8,i*2)) = conImgPts[i].at<float>(0)*conObjPts[i].at<float>(0);
        A.at<float>(cv::Point2i(9,i*2)) = conImgPts[i].at<float>(0)*conObjPts[i].at<float>(1);
        A.at<float>(cv::Point2i(10,i*2)) = conImgPts[i].at<float>(0)*conObjPts[i].at<float>(2);
        A.at<float>(cv::Point2i(11,i*2)) = conImgPts[i].at<float>(0)*conObjPts[i].at<float>(3);

        A.at<float>(cv::Point2i(0,i*2+1)) = 0.0;
        A.at<float>(cv::Point2i(1,i*2+1)) = 0.0;
        A.at<float>(cv::Point2i(2,i*2+1)) = 0.0;
        A.at<float>(cv::Point2i(3,i*2+1)) = 0.0;
        A.at<float>(cv::Point2i(4,i*2+1)) = -conImgPts[i].at<float>(2)*conObjPts[i].at<float>(0);
        A.at<float>(cv::Point2i(5,i*2+1)) = -conImgPts[i].at<float>(2)*conObjPts[i].at<float>(1);
        A.at<float>(cv::Point2i(6,i*2+1)) = -conImgPts[i].at<float>(2)*conObjPts[i].at<float>(2);
        A.at<float>(cv::Point2i(7,i*2+1)) = -conImgPts[i].at<float>(2)*conObjPts[i].at<float>(3);
        A.at<float>(cv::Point2i(8,i*2+1)) = conImgPts[i].at<float>(1)*conObjPts[i].at<float>(0);
        A.at<float>(cv::Point2i(9,i*2+1)) = conImgPts[i].at<float>(1)*conObjPts[i].at<float>(1);
        A.at<float>(cv::Point2i(10,i*2+1)) = conImgPts[i].at<float>(1)*conObjPts[i].at<float>(2);
        A.at<float>(cv::Point2i(11,i*2+1)) = conImgPts[i].at<float>(1)*conObjPts[i].at<float>(3);
    }

    //std::cout << A << std::endl;

    //prepare parameter space
    cv::Mat p(12,1,CV_32F);

    //Solve for 12 parameters
    cv::SVD::solveZ(A, p);

    //Reshape P
    P.at<float>(cv::Point2i(0,0)) = p.at<float>(0);
    P.at<float>(cv::Point2i(1,0)) = p.at<float>(1);
    P.at<float>(cv::Point2i(2,0)) = p.at<float>(2);
    P.at<float>(cv::Point2i(3,0)) = p.at<float>(3);
    P.at<float>(cv::Point2i(0,1)) = p.at<float>(4);
    P.at<float>(cv::Point2i(1,1)) = p.at<float>(5);
    P.at<float>(cv::Point2i(2,1)) = p.at<float>(6);
    P.at<float>(cv::Point2i(3,1)) = p.at<float>(7);
    P.at<float>(cv::Point2i(0,2)) = p.at<float>(8);
    P.at<float>(cv::Point2i(1,2)) = p.at<float>(9);
    P.at<float>(cv::Point2i(2,2)) = p.at<float>(10);
    P.at<float>(cv::Point2i(3,2)) = p.at<float>(11);

    //Reverse condition
    P = Ti.inv()*P*To;
    P = P/P.at<float>(2,3); //make homogenous
    //std::cout << "P " << P << "\n";

    return P;
}

bool CalibrationManager::IsReadyToGo() const
{
    if(m_detectionResult[0].amount_id_markers < 6 || m_detectionResult[1].amount_id_markers < 6 )
        return false;

    if(m_detectionResult[0].ring_markers_world.size() != m_detectionResult[0].ring_markers_image.size())
        return false;

    if(m_detectionResult[1].ring_markers_world.size() != m_detectionResult[1].ring_markers_image.size())
        return false;

    return true;
}

DetectionResult::DetectionResult()
{
    amount_points = 0;
    amount_id_markers = 0;
    img_size = cv::Size(0,0);
    id_markers_world.resize(1);
    id_markers_image.resize(1);
    ring_markers_world.resize(1);
    ring_markers_image.resize(1);
    all_markers_image.resize(1);
}

CalibrationResult& CalibrationManager::GetCalibrationResult()
{
    return m_stereoResult;
}

CalibrationDescription::CalibrationDescription(){
    amount_points = 0;
    amount_id_markers = 0;
}

void CalibrationDescription::ReadFromIni(const char* filename)
{
    //todo: add if file exist condition, this implementation is error prone

    amount_id_markers = 0;
    boost::property_tree::ptree pt;
    boost::property_tree::ini_parser::read_ini(filename, pt);

    int amount_points = pt.get<int>("CalibInfo.MarkCount");
    (*this).amount_points = amount_points;
    char buffer[32];
    typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
    boost::char_separator<char> sep(" ");

    for(int i = 0; i < amount_points; i++)
    {
        sprintf(buffer, "Marks.Mark_%.3i", i);
        std::string line = pt.get<std::string>(buffer);
        tokenizer tokens(line, sep);
        cv::Point3f calib_p;
        tokenizer::iterator tok_iter = tokens.begin();
        calib_p.x = ::atof(std::string(*tok_iter++).c_str());
        calib_p.y = ::atof(std::string(*tok_iter++).c_str());
        calib_p.z = ::atof(std::string(*tok_iter).c_str());

        sprintf(buffer, "Marks.Mark_%.3i_ID", i);
        boost::optional<int> id = pt.get_optional<int>(buffer);

        if(id.is_initialized())
        {
            amount_id_markers++;
            id_markers_pos[id.get()] = calib_p;
            continue;
        }

        calib_points.push_back(calib_p);
    }
}

CalibrationParameters::CalibrationParameters()
{
    binary_threshold = 30;
    kernel_size = 9;
    id_marker_position_offset = 3;
    radial_step_amount = 72;
    marker_search_radius = 500;
}


#include <string>
#include <iostream>
#include <algorithm>    // std::partial_sort

//Used for time measurement
#include <chrono>

//Support for messages in the cluster
#include <mpi.h>

//Used for interaction with the user
#include <boost/program_options.hpp>

//Used for image manipulation
#include<opencv2/imgproc/imgproc.hpp>
#include<opencv2/highgui/highgui.hpp>

namespace po = boost::program_options;

void display(std::string imgAdd, int windowSize){
	double start, end;

	cv::Mat src, dst;

	unsigned char* inData;
	unsigned char* outData;
	unsigned char* displayData;

	int imgSize;
	int imgRows;
	int imgCols;

	MPI_Init(NULL, NULL);

	int rank;
	int size;
	MPI_Comm_rank(MPI_COMM_WORLD, &rank);
	MPI_Comm_size(MPI_COMM_WORLD, &size);

	MPI_Barrier(MPI_COMM_WORLD); /* IMPORTANT */
	start = MPI_Wtime();

	//Broadcast image dimenstions to all nodes so that they can reserve memory
	if (rank == 0) {
		src = cv::imread(imgAdd, CV_LOAD_IMAGE_GRAYSCALE);
		dst = cv::Mat(src.rows, src.cols, CV_8U);

		if( !src.data ){
			printf("Image not found\n");
			return;
		}

		imgSize = src.cols*src.rows;
		imgRows = src.rows;
		imgCols = src.cols;

		//Third argument indicates that 0 sends the data
		MPI_Bcast(&imgSize, 1, MPI_INT, 0, MPI_COMM_WORLD);
		MPI_Bcast(&imgRows, 1, MPI_INT, 0, MPI_COMM_WORLD);
		MPI_Bcast(&imgCols, 1, MPI_INT, 0, MPI_COMM_WORLD);
	}
	else {
		MPI_Bcast(&imgSize, 1, MPI_INT, 0, MPI_COMM_WORLD);
		MPI_Bcast(&imgRows, 1, MPI_INT, 0, MPI_COMM_WORLD);
		MPI_Bcast(&imgCols, 1, MPI_INT, 0, MPI_COMM_WORLD);

		inData =  (unsigned char*)malloc(imgSize*sizeof(unsigned char));
	}

	//Waits for all procs to reach this point
	MPI_Barrier(MPI_COMM_WORLD);

	//Broadcast image to all elements and process their data
	if (rank == 0) {
		//Third argument indicates that 0 sends the data
		MPI_Bcast(src.data, src.cols*src.rows, MPI_UNSIGNED_CHAR, 0, MPI_COMM_WORLD);

		outData = (unsigned char*) malloc((imgSize/size) * sizeof(unsigned char));

		int windowElements = (windowSize*2 + 1)*(windowSize*2 + 1);

		int* window = (int*) malloc(windowElements*sizeof(int));

		int j;
		for(int i = 0; i < (imgSize/size); i++){
			j = i + (imgSize/size)*rank;

			int cnt = 0;
			// Pick up window element
			for(int u = -windowSize; u <= windowSize; u++){
				for(int v = -windowSize; v <= windowSize; v++){
					int row = (j + v + u*src.rows) / imgRows;
					int col = (j + v + u*src.rows) % imgRows;

					if( (row > 0) && (row < imgRows) && (col > 0) && (col < imgCols) )
						window[cnt] = src.data[j + v + u*src.rows];
					else
						window[cnt] = 128;
					cnt++;
				}
			}

			// sort the window to find median
			std::partial_sort (window, window + windowSize/2 + 1, window + windowSize);

			// assign the median to centered element of the matrix
			outData[i] = window[windowSize/2];
		}
	}
	else {
		MPI_Bcast(inData, imgSize, MPI_UNSIGNED_CHAR, 0, MPI_COMM_WORLD);

		outData = (unsigned char*) malloc((imgSize/size) * sizeof(unsigned char));

		int windowElements = (windowSize*2 + 1)*(windowSize*2 + 1);

		int* window = (int*) malloc(windowElements*sizeof(int));

		int j;
		for(int i = 0; i < (imgSize/size); i++){
			j = i + (imgSize/size)*rank;

			int cnt = 0;
			// Pick up window element
			for(int u = -windowSize; u <= windowSize; u++){
				for(int v = -windowSize; v <= windowSize; v++){
					int row = (j + v + u*src.rows) / imgRows;
					int col = (j + v + u*src.rows) % imgRows;

					if( (row > 0) && (row < imgRows) && (col > 0) && (col < imgCols) )
						window[cnt] = inData[j + v + u*src.rows];
					else
						window[cnt] = 128;
					cnt++;
				}
			}

			// sort the window to find median
			std::partial_sort (window, window + windowSize/2 + 1, window + windowSize);

			// assign the median to centered element of the matrix
			outData[i] = window[windowSize/2];
		}
	}

	//Waits for all procs to reach this point
	MPI_Barrier(MPI_COMM_WORLD);

	if(rank == 0){
		displayData = (unsigned char*) malloc(imgSize*sizeof(unsigned char));

		MPI_Gather(
			outData,
			(imgSize/size),
			MPI_UNSIGNED_CHAR,

			displayData,
			(imgSize/size),//This is per process
			MPI_UNSIGNED_CHAR,

			0,
			MPI_COMM_WORLD);

	}
	else{
		MPI_Gather(
			outData,
			(imgSize/size),
			MPI_UNSIGNED_CHAR,

			NULL,
			0,
			MPI_UNSIGNED_CHAR,

			0,
			MPI_COMM_WORLD);
	}

	//Waits for all procs to reach this point
	MPI_Barrier(MPI_COMM_WORLD); /* IMPORTANT */
	end = MPI_Wtime();

	if(rank == 0){
			cv::Mat dst = cv::Mat(cvSize(src.cols, src.rows), CV_8U, displayData);

			cv::namedWindow("final");
			cv::imshow("final", dst);

			cv::namedWindow("initial");
			cv::imshow("initial", src);

			printf("time: %.6fs\n", end -start);

			cv::waitKey();
	}
	MPI_Finalize();
}

int main(int ac, char* av[]){
	try{
		std::string address;
		int kernelSize;

		po::options_description desc("Allowed Options");
		desc.add_options()
			("help,h", "Produce help message")
			("file,f", po::value<std::string>(&address), "File to be processed")
			("kernelSize,k", po::value<int>(&kernelSize)->default_value(1), "Size of the kernel to apply")
			;

		po::variables_map vm;
		po::store(po::parse_command_line(ac, av, desc), vm);
		po::notify(vm);

		if(vm.count("file")){
			display(address, kernelSize);
		}
		else {//Includes help message
			std::cout << "This program applies a median filter to an image, and distributes the job between several processors and machines\n" << desc << "\n";
			return 0;
		}
	}
	catch(std::exception& e) {
		std::cerr << "error: " << e.what() << "\n";
		return 1;
	}
	catch(...) {
		std::cerr << "Exception of unknown type!\n";
	}
	return 0;
}


#ifndef SPHERE_HPP
#define	SPHERE_HPP

#include "Object.hpp"
#include <armadillo>
#include <vector>

using namespace arma;
using namespace std;

class Sphere :public Object {

private:
    double raio;
    vec centro, min, max;

public:

    Sphere();
    Sphere(vec esfera);

    bool colide(const vec &d, double &T, const vec &origem);

    void setMax(vec max) {
        this->max = max;
    }

    vec getMax() {
        return max;
    }

    void setMin(vec min) {
        this->min = min;
    }

    vec getMin()  {
        return min;
    }

    void setCentro(vec centro) {
        this->centro = centro;
    }

    vec getCentro() {
        return centro;
    }

    void setRaio(double raio) {
        this->raio = raio;
    }

    double getRaio(){
        return raio;
    }

};

#endif	/* SPHERE_HPP */


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#include <boost/simd/function/shuffle.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>

using namespace boost::simd;

STF_CASE_TPL( "Unary broadcast shuffle", STF_NUMERIC_TYPES)
{
  // cardinal 4
  {
    bs::pack<T,4> a{ Valmax<T>(), T(42), T(69), Valmin<T>() };
    STF_ALL_EQUAL( (shuffle<-1,-1,-1,-1>(a)), (bs::Zero<bs::pack<T,4>>())  );
    STF_ALL_EQUAL( (shuffle< 0, 0, 0, 0>(a)), (bs::pack<T,4>(a[0])) );
    STF_ALL_EQUAL( (shuffle< 1, 1, 1, 1>(a)), (bs::pack<T,4>(a[1])) );
    STF_ALL_EQUAL( (shuffle< 2, 2, 2, 2>(a)), (bs::pack<T,4>(a[2])) );
    STF_ALL_EQUAL( (shuffle< 3, 3, 3, 3>(a)), (bs::pack<T,4>(a[3])) );
  }

  // cardinal 8
  {
    bs::pack<T,8> a { boost::simd::Valmax<T>(),T(66),T(99),T(55)
                    , T(-1),T(77), T(23), boost::simd::Valmin<T>()
                    };

    STF_ALL_EQUAL( (shuffle<-1,-1,-1,-1,-1,-1,-1,-1>(a)), (bs::Zero<bs::pack<T,8>>()) );
    STF_ALL_EQUAL( (shuffle< 0, 0, 0, 0, 0, 0, 0, 0>(a)), (bs::pack<T,8>(a[0])) );
    STF_ALL_EQUAL( (shuffle< 1, 1, 1, 1, 1, 1, 1, 1>(a)), (bs::pack<T,8>(a[1])) );
    STF_ALL_EQUAL( (shuffle< 2, 2, 2, 2, 2, 2, 2, 2>(a)), (bs::pack<T,8>(a[2])) );
    STF_ALL_EQUAL( (shuffle< 3, 3, 3, 3, 3, 3, 3, 3>(a)), (bs::pack<T,8>(a[3])) );
    STF_ALL_EQUAL( (shuffle< 4, 4, 4, 4, 4, 4, 4, 4>(a)), (bs::pack<T,8>(a[4])) );
    STF_ALL_EQUAL( (shuffle< 5, 5, 5, 5, 5, 5, 5, 5>(a)), (bs::pack<T,8>(a[5])) );
    STF_ALL_EQUAL( (shuffle< 6, 6, 6, 6, 6, 6, 6, 6>(a)), (bs::pack<T,8>(a[6])) );
    STF_ALL_EQUAL( (shuffle< 7, 7, 7, 7, 7, 7, 7, 7>(a)), (bs::pack<T,8>(a[7])) );
  }
}


/******************************************************************************
* Coherent Point Drift
* Copyright (C) 2014 Pete Gadomski <pete.gadomski@gmail.com>
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
******************************************************************************/

#pragma once

#include <armadillo>

namespace cpdArma
{

void construct_affinity_matrix(const arma::mat& X, const arma::mat& Y,
                               float beta, arma::mat& G);

}


#include <Eigen/Core>
#include <algorithm>
#include <ceres/rotation.h>
#include <glog/logging.h>

#include "sfm/twoview_info.h"

namespace GraphSfM {

void SwapCameras(TwoViewInfo* twoview_info)
{

    // Invert the translation.
    Eigen::Vector3d neg_of_new_position;
    ceres::AngleAxisRotatePoint(twoview_info->rotation_2.data(),
                                twoview_info->position_2.data(),
                                neg_of_new_position.data());
    twoview_info->position_2 = -neg_of_new_position;

    // Invert the rotation.
    twoview_info->rotation_2 *= -1.0;
}

}  // namespace GraphSfM


////////////////////////////////////////////////////////////////////////////////
/// Copyright 2018-present Xinyan DAI<xinyan.dai@outlook.com>
///
/// 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 ofthe 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.

/// @version 0.1
/// @author  Xinyan DAI
/// @contact xinyan.dai@outlook.com
//////////////////////////////////////////////////////////////////////////////

#pragma once

#include <cmath>
#include <utility>
#include <vector>
#include <index/pq.hpp>
#include <boost/math/special_functions/beta.hpp>

#include "query/sorter/bucket_sorter.hpp"

#include "parameters.hpp"
#include "query.hpp"
#include "index/transformer/norm_range.hpp"

namespace ss {

    template<
            class DataType,
            class KeyType,
            class HashingIndexType,
            class Transformer
    >
    class RationalCrossPolytopeRanking :
            public Query <DataType > {

        using MapType   = unordered_map<KeyType, vector<int>, ss::SSHasher<KeyType> >;
        using IndexType = NormRangeIndex<DataType, KeyType, HashingIndexType, Transformer >;
    private:
        const MapType &                          _index_map;
        BucketSorter<DataType, KeyType > *      _sorter;

    public:
        ~RationalCrossPolytopeRanking() {}

        explicit RationalCrossPolytopeRanking(
                IndexType *                        index,
                const DataType *                   query,
                const Metric<DataType > &          metric,
                const Matrix<DataType > &          data,
                const parameter &                  para)
                : Query<DataType >(index, query, metric, data, para),
                  _index_map(index->getIndexMap()){

            { /// construction here

                CrossPolytopeIndex<DataType> * cross_polytope = index->GetIndex();
                vector<int >  query_hash = cross_polytope->HashQuery(query, -1);
                vector<vector<DataType > > xs = cross_polytope->Rotate(query);
                vector<int >      max_indexes = cross_polytope->ArgMaxAbs(xs);
                /// distance function from query to bucket(in each sub data set)
                auto inner_distor = [&xs, &max_indexes, &query_hash, &para, index] (const KeyType & index_key) {
                    DataType cost_metric = 0.0f;
                    for (int k = 0; k < xs.size(); ++k) {

                        DataType max_absolute = std::abs(xs[k][max_indexes[k]]);
                        DataType query_value = xs[k][ std::abs(index_key[k]) - 1 ]; ///index begin with 0
                        /// the cost to make absolute value of query[index_key[k]] bigger than actual 'max_absolute'
                        cost_metric += max_absolute - query_value * (index_key[k]>0?1:-1);
                    }

                    DataType distance_square = 2.0 - 2.0 / std::sqrt(cost_metric+1.0);
                    DataType u = index->Percentile(index_key);
                    return  - u * (8.0 - distance_square);
                };

                /// sorter helper
                _sorter = new BucketSorter<DataType, KeyType>(_index_map, inner_distor);
            } /// end construction
        }

        const vector<int> &  NextBucket() override {
            typename MapType::const_iterator bucket;
            while ( (bucket = _index_map.find(_sorter->NextBucket()) ) ==_index_map.end()) {}
            return bucket->second;
        }
    };
}  // end namespace ss


/*
	All rights reserved. Use of this source code is governed by the
	3-clause BSD License in LICENSE.txt.
*/
#include <iostream>
#include <Eigen/Core>
#include <vector>

using nanogui::Vector2i;
using nanogui::Vector2d;
using nanogui::MatrixXd;
using nanogui::Matrix3d;

#if 0
class Vector2i {
public:
	Vector2i(int xP, int yP) : xPos(xP), yPos(xP) {}
	int x() { return xPos; }
	int y() { return yPos; }
protected:
	int xPos, yPos;
};
#endif

#define TO_STRING( ID ) #ID

class Widget {
public:
	Vector2i position() { return mPos; }
	void setPosition(Vector2i p) { mPos = p; }
	Vector2i size() { return mSize; }
	void setSize(Vector2i s) { mSize = s; }
protected:
	Vector2i mPos;
	Vector2i mSize;
};

/*
Handles have a mode and a position. Each object has 9 handles but since only one object's handles are
active at a time we can use a singleton ObjectHandles to hold the handles and initialise it based on
the position and size of an object.

*/
class Handle {
public:

enum Mode { NONE, POSITION, RESIZE_TL, RESIZE_T, RESIZE_TR, RESIZE_R, RESIZE_BL, RESIZE_L, RESIZE_BR, RESIZE_B};

static Handle create(Mode which, Vector2i pos, Vector2i size) {
	Handle result;
	result.setMode(which);
	switch(which) {
		case NONE:
			break;
		case POSITION:
			result.setPosition( Vector2i(pos.x() + size.x()/2, pos.y()+size.y()/2) );
			break;
		case RESIZE_TL:
			result.setPosition( Vector2i(pos.x(), pos.y()) );
			break;
		case RESIZE_T:
			result.setPosition( Vector2i(pos.x() + size.x()/2, pos.y()) );
			break;
		case RESIZE_TR:
			result.setPosition( Vector2i(pos.x() + size.x(), pos.y()) );
			break;
		case RESIZE_R:
			result.setPosition( Vector2i(pos.x() + size.x(), pos.y()+size.y()/2) );
			break;
		case RESIZE_BL:
			result.setPosition( Vector2i(pos.x(), pos.y()+size.y()) );
			break;
		case RESIZE_L:
			result.setPosition( Vector2i(pos.x(), pos.y()+size.y()/2) );
			break;
		case RESIZE_BR:
			result.setPosition( Vector2i(pos.x() + size.x(), pos.y()+size.y()) );
			break;
		case RESIZE_B:
			result.setPosition( Vector2i(pos.x() + size.x()/2, pos.y()+size.y()) );
			break;
	}
	return result;
}

std::ostream &operator<<(std::ostream& out) const{
	return out;
}

Handle() : mMode(NONE) {}

void setPosition(Vector2i newpos) { pos = newpos; }
Vector2i position() const { return pos; }

void setMode(Mode newmode) { mMode = newmode; }
Mode mode() { return mMode; }

Handle closest(Vector2i pt) {
	Handle result;

	return result;
}

protected:
	Mode mMode;
	Vector2i pos;
};

std::ostream &operator<<(std::ostream& out, Handle::Mode m) {
	switch(m) {
		case Handle::NONE:
			out << TO_STRING(NONE);
			break;
		case Handle::POSITION:
			out << TO_STRING(POSITION);
			break;
		case Handle::RESIZE_TL:
			out << TO_STRING(RESIZE_TL);
			break;
		case Handle::RESIZE_T:
			out << TO_STRING(RESIZE_T);
			break;
		case Handle::RESIZE_TR:
			out << TO_STRING(RESIZE_TR);
			break;
		case Handle::RESIZE_R:
			out << TO_STRING(RESIZE_R);
			break;
		case Handle::RESIZE_BL:
			out << TO_STRING(RESIZE_BL);
			break;
		case Handle::RESIZE_L:
			out << TO_STRING(RESIZE_L);
			break;
		case Handle::RESIZE_BR:
			out << TO_STRING(RESIZE_BR);
			break;
		case Handle::RESIZE_B:
			out << TO_STRING(RESIZE_B);
			break;
	}
	return out;
}

//std::vector<Handle>handles(9);
Handle::Mode all_handles[] = {
	Handle::POSITION,
	Handle::RESIZE_TL,
	Handle::RESIZE_T,
	Handle::RESIZE_TR,
	Handle::RESIZE_R,
	Handle::RESIZE_BL,
	Handle::RESIZE_L,
	Handle::RESIZE_BR,
	Handle::RESIZE_B
};

using namespace std;
int main(int argc, char *argv[]) {
	const int ROWS = 5;
	Vector2i pos;
	nanogui::MatrixXd pts(ROWS,2);
	pts = nanogui::MatrixXd::Random(ROWS,2);
	std::cout << "pts:\n" << pts << "\n";
	Vector2d pt; pt(0,0) = 0.5; pt(1,0) = 0.5;
	std::cout << "rowwise sum:\n" << pts.rowwise().sum() << "\n";
	// find the distances to each pt

	std::cout << "by hand:\n" << ((pts(0,0)-0.5)*(pts(0,0)-0.5) + (pts(0,1)-0.5)*(pts(0,1)-0.5)) << "\n";
	std::cout << "by hand:\n" << ((pts(1,0)-0.5)*(pts(1,0)-0.5) + (pts(1,1)-0.5)*(pts(1,1)-0.5)) << "\n";

	nanogui::VectorXd dist = (pts.rowwise() -= pt.transpose()).rowwise().squaredNorm();
	std::cout << "distances:\n" <<dist << "\n";
	double min = dist.x();
	int idx = 0;
	for (int i=1; i<ROWS; ++i) {
		if (dist.row(i).x() < min){ min = dist.row(i).x(); idx = i; }
	}
	std::cout << "smallest distance: " << min << "\n";

	Widget w;
	w.setPosition( Vector2i(100,100) );
	w.setSize( Vector2i(50, 30) );
	MatrixXd z(9,2);
	std::vector<Handle> handles(9);
	for (int i=0; i<9; ++i) {
		Handle h = Handle::create(all_handles[i], w.position(), w.size());
		z(i,0) = h.position().x();
		z(i,1) = h.position().y();
		handles[i] = h;
	}
	pt = Vector2d(124, 114);
	nanogui::VectorXd distances = (z.rowwise() - pt.transpose()).rowwise().squaredNorm();
	std::cout << "distances\n" << distances << "\n";
	min = distances.row(0).x();
	idx = 0;
	for (int i=1; i<9; ++i) {
		if (distances.row(i).x() < min){ min = distances.row(i).x(); idx = i; }
	}
	if (min >= 30)
		std::cout << "no handle\n";
	else
		std::cout << "smallest distance: " << min << " " << handles[idx].mode() << "\n";

/*
	nanogui::Matrix3d m = Matrix3d::Random();
	cout << "Here is the matrix m:" << endl << m << endl;
	cout << "Here is the sum of each row:" << endl << m.rowwise().sum() << endl;
*/
}


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

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/pack.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/dispatch/hierarchy/functions.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/simd/arch/common/generic/function/autofold.hpp>
#include <simd_test.hpp>

namespace bs = boost::simd;
namespace bd = boost::dispatch;

namespace boost { namespace simd
{
  namespace tag
  {
    BOOST_DISPATCH_MAKE_TAG ( ext, fake_sum_
                            , (boost::dispatch::reduction_<fake_sum_,plus_,zero_>)
                            );
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag, fake_sum_);
  }

  BOOST_DISPATCH_CALLABLE_DEFINITION(tag::fake_sum_, fake_sum);

  namespace ext
  {
    BOOST_DISPATCH_OVERLOAD( fake_sum_, (typename T), bd::cpu_, bd::scalar_<bd::fundamental_<T>> )
    {
      T operator()(T const& a0) const BOOST_NOEXCEPT { return a0; }
    };
  }
} }

template <typename T, std::size_t N, typename Env> void test(Env& $)
{
  using p_t = bs::pack<T,N>;

  T k = 1;
  p_t p;

  for(std::size_t i=0;i<N;i++) p[i] = k++;

  STF_EQUAL( boost::simd::fake_sum(p), T( (N*(N+1))/2 ) );
}

STF_CASE_TPL("Check autofold behaviour on pack", STF_NUMERIC_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;

  test<T,N>($);
  test<T,N/2>($);
  test<T,N*2>($);
}


#define BOOST_TEST_MODULE example
#include <boost/test/included/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>

//____________________________________________________________________________//

BOOST_AUTO_TEST_CASE( test )
{
    double v = -1.23456e-3;

    BOOST_CHECK_SMALL( v, 0.000001 );
}

//____________________________________________________________________________//


//
//! Copyright © 2022
//! Brandon Kohn
//
//  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)
//
#pragma once

#include <boost/config.hpp>
#include <boost/units/cmath.hpp>
#include <geometrix/utility/tagged_quantity_cmath.hpp>

namespace geometrix {

    struct std_math_kernel
    {
        template<typename T>
        static auto exp(T&& q) -> auto
        {
            using std::exp;
            return exp(std::forward<T>(q));
        }

        template<typename T>
        static auto log(T&& q) -> auto
        {
            using std::log;
            return log(std::forward<T>(q));
        }

        template<typename T>
        static auto log10(T&& q) -> auto
        {
            using std::log10;
            return log10(std::forward<T>(q));
        }

        template <typename T>
        static auto sqrt(T&& q) -> auto
        {
            using std::sqrt;
            return sqrt(std::forward<T>(q));
        }

        template<typename T>
        static auto cos(T&& q) -> auto
        {
            using std::cos;
            return cos(std::forward<T>(q));
        }

        template<typename T>
        static auto sin(T&& q) -> auto
        {
            using std::sin;
            return sin(std::forward<T>(q));
        }

        template<typename T>
        static auto tan(T&& q) -> auto
        {
            using std::tan;
            return tan(std::forward<T>(q));
        }

        template<typename T>
        static auto acos(T&& q) -> auto
        {
            using std::acos;
            return acos(std::forward<T>(q));
        }

        template<typename T>
        static auto asin(T&& q) -> auto
        {
            using std::asin;
            return asin(std::forward<T>(q));
        }

        template<typename T>
        static auto atan(T&& q) -> auto
        {
            using std::atan;
            return atan(std::forward<T>(q));
        }

        template<typename T, typename U>
        static auto pow(T&& q1, U&& q2) -> auto
        {
            using std::pow;
            return pow(std::forward<T>(q1), std::forward<U>(q2));
        }

        template<typename T, typename U>
        static auto atan2(T&& y, U&& x) -> auto
        {
            using std::atan2;
            return atan2(std::forward<T>(y), std::forward<U>(x));
        }
    };

}//! namespace geometrix;


// Copyright (c) 2014 The Bitcoin Core developers
// Copyright (c) 2014-2015 The Dash developers
// Copyright (c) 2015-2017 The PIVX developers
// Copyright (c) 2018 The Fargocash developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "primitives/transaction.h"
#include "main.h"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(main_tests)

BOOST_AUTO_TEST_CASE(subsidy_limit_test)
{
    //Basic rewards
    BOOST_CHECK(GetBlockValue(500) == 999 * COIN);
    BOOST_CHECK(GetBlockValue(21904) == 5 * COIN);

    //Halvings
    int nHeight = 720 /*1d*/ * 365 * 2; //2y
    BOOST_CHECK(GetBlockValue(nHeight) == 5/2 * COIN);

    //Halvings
    nHeight = 720 /*1d*/ * 365 * 4; //4y
    BOOST_CHECK(GetBlockValue(nHeight) == 5/4 * COIN);
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef INLASSLE_H
#define INLASSLE_H

#include <RcppArmadillo.h>
#include <RcppEigen.h>
#include <RcppParallel.h>
#include <boost/math/special_functions/polygamma.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/bessel.hpp>
#include <dlib/matrix.h>

// [[Rcpp::depends(RcppArmadillo,RcppParallel,BH)]]
// [[Rcpp::plugins(cpp11)]]

using vec   = arma::vec;
using uvec  = arma::uvec;
using mat   = arma::mat;
using uword = arma::uword;
using cube  = arma::cube;

using dlib_mat = dlib::matrix<double>;

using boost::math::polygamma;
using boost::math::lgamma;
using boost::math::cyl_bessel_k;

using Worker = RcppParallel::Worker;
using Split  = RcppParallel::Split;

struct uvecComp
/* 'less-than' comparison functor for uvec, where each vector
 * is the positions of "set bits" in a possibly infinite but
 * sparse binary vector */
{
  bool operator() (uvec const&, uvec const&) const;
};

using uvec_map_uword = std::map<uvec, uword, uvecComp>;
using uvec_map_mat   = std::map<uvec,   mat, uvecComp>;
using uvec_map_vec   = std::map<uvec,   vec, uvecComp>;
using uvec_list      = std::vector<uvec>;

/* transforms */
vec logit (vec);
double logit (double);
vec plogis (vec);
double plogis (double);
uword tri_diag_ind (uword, uword);
dlib_mat arma_to_dlib (const mat&);
mat dlib_to_arma (const dlib_mat&);

/* utilities */
void cholesky_rev_smith (mat&, const mat&); // reverse autodiff of Cholesky decomposition

#endif /* INLASSLE_H */


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/function/div.hpp>
#include <boost/simd/function/fnms.hpp>
#include <boost/simd/function/ceil.hpp>
#include <boost/simd/function/is_nez.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/if_minus.hpp>
#include <boost/simd/function/if_nan_else.hpp>
#include <boost/simd/function/logical_or.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD_IF(rem_
                             , (typename A0, typename X)
                             , (detail::is_native<X>)
                             , bd::cpu_
                             , bs::pedantic_tag
                             , bs::tag::ceil_
                             , bs::pack_<bd::int_<A0>, X>
                             , bs::pack_<bd::int_<A0>, X>
                             )
   {
      BOOST_FORCEINLINE A0 operator()(pedantic_tag const &
                                     , bd::functor<bs::tag::ceil_> const&
                                     , const A0& a0, const A0& a1) const BOOST_NOEXCEPT
      {
        return if_minus(is_nez(a1), a0, div(ceil,a0,a1)*a1);
      }
   };
   BOOST_DISPATCH_OVERLOAD_IF(rem_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::tag::ceil_
                          , bs::pack_<bd::signed_<A0>, X>
                          , bs::pack_<bd::signed_<A0>, X>
                          )
   {
     BOOST_FORCEINLINE A0 operator()( bd::functor<bs::tag::ceil_> const&
                                     , const A0& a0, const A0& a1) const BOOST_NOEXCEPT
      {
        return fnms(div(ceil, a0,a1), a1, a0);
      }
   };
  BOOST_DISPATCH_OVERLOAD_IF(rem_
                            , (typename A0, typename X)
                            , (detail::is_native<X>)
                            , bd::cpu_
                            , bs::pedantic_tag
                            , bs::tag::ceil_
                            , bs::pack_<bd::floating_<A0>, X>
                            , bs::pack_<bd::floating_<A0>, X>
                            )
   {
      BOOST_FORCEINLINE A0 operator()(pedantic_tag const &
                                     , bd::functor<bs::tag::ceil_> const&
                                     , const A0& a0, const A0& a1) const BOOST_NOEXCEPT
      {
        return if_nan_else(logical_or(is_invalid(a1),is_invalid(a0)) ,
                           if_nan_else(is_eqz(a1),
                                       if_minus(is_nez(a1), a0, div(ceil, a0,a1)*a1)
                                      )
                          );
      }
   };

} } }

#endif


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

#include <boost/math/special_functions/next.hpp>
#include <boost/random.hpp>

#include <limits>

#include "caffe/common.hpp"
#include "caffe/util/math_functions.hpp"
#include "caffe/util/rng.hpp"
#include "caffe/util/ocl_util.hpp"
#include "caffe/util/ocl_wrapper.hpp"

static const clblasOrder order = clblasColumnMajor;
#define pi 3.1415926

namespace caffe {

template <>
void caffe_cpu_gemm<float>(const CBLAS_TRANSPOSE TransA,
    const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
    const float alpha, const float* A, const float* B, const float beta,
    float* C) {
  int lda = (TransA == CblasNoTrans) ? K : M;
  int ldb = (TransB == CblasNoTrans) ? N : K;
  cblas_sgemm(CblasRowMajor, TransA, TransB, M, N, K, alpha, A, lda, B, ldb,
      beta, C, N);
}

template <>
void caffe_cpu_gemm<double>(const CBLAS_TRANSPOSE TransA,
    const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
    const double alpha, const double* A, const double* B, const double beta,
    double* C) {
  int lda = (TransA == CblasNoTrans) ? K : M;
  int ldb = (TransB == CblasNoTrans) ? N : K;
  cblas_dgemm(CblasRowMajor, TransA, TransB, M, N, K, alpha, A, lda, B, ldb,
      beta, C, N);
}

template <>
void caffe_cpu_gemv<float>(const CBLAS_TRANSPOSE TransA, const int M,
    const int N, const float alpha, const float* A, const float* x,
    const float beta, float* y) {
  cblas_sgemv(CblasRowMajor, TransA, M, N, alpha, A, N, x, 1, beta, y, 1);
}

template <>
void caffe_cpu_gemv<double>(const CBLAS_TRANSPOSE TransA, const int M,
    const int N, const double alpha, const double* A, const double* x,
    const double beta, double* y) {
  cblas_dgemv(CblasRowMajor, TransA, M, N, alpha, A, N, x, 1, beta, y, 1);
}

template <>
void caffe_axpy<float>(const int N, const float alpha, const float* X,
    float* Y) {
  cblas_saxpy(N, alpha, X, 1, Y, 1);
}

template <>
void caffe_axpy<double>(const int N, const double alpha, const double* X,
    double* Y) {
  cblas_daxpy(N, alpha, X, 1, Y, 1);
}

template <>
void caffe_set(const int N, const float alpha, float* Y) {
  if (alpha == 0) {
    memset(Y, 0, sizeof(float) * N);
    return;
  }
  for (int i = 0; i < N; ++i) {
    Y[i] = alpha;
  }
}

template <>
void caffe_set(const int N, const double alpha, double* Y) {
  if (alpha == 0) {
    memset(Y, 0, sizeof(double) * N);
    return;
  }
  for (int i = 0; i < N; ++i) {
    Y[i] = alpha;
  }
}

/*
template <>
void caffe_copy<float>(const int N, const float* X, float* Y) {
  cblas_scopy(N, X, 1, Y, 1);
}

template <>
void caffe_copy<double>(const int N, const double* X, double* Y) {
  cblas_dcopy(N, X, 1, Y, 1);
}
*/
template <>
void caffe_add_scalar(const int N, const float alpha, float* Y) {
  for (int i = 0; i < N; ++i) {
    Y[i] += alpha;
  }
}

template <>
void caffe_add_scalar(const int N, const double alpha, double* Y) {
  for (int i = 0; i < N; ++i) {
    Y[i] += alpha;
  }
}

template <>
void caffe_scal<float>(const int N, const float alpha, float *X) {
  cblas_sscal(N, alpha, X, 1);
}

template <>
void caffe_scal<double>(const int N, const double alpha, double *X) {
  cblas_dscal(N, alpha, X, 1);
}

template <>
void caffe_cpu_axpby<float>(const int N, const float alpha, const float* X,
    const float beta, float* Y) {
  cblas_saxpby(N, alpha, X, 1, beta, Y, 1);
}

template <>
void caffe_cpu_axpby<double>(const int N, const double alpha, const double* X,
    const double beta, double* Y) {
  cblas_daxpby(N, alpha, X, 1, beta, Y, 1);
}

template <>
void caffe_add<float>(const int n, const float* a, const float* b, float* y) {
  vsAdd(n, a, b, y);
}

template <>
void caffe_add<double>(const int n, const double* a, const double* b,
    double* y) {
  vdAdd(n, a, b, y);
}

template <>
void caffe_sub<float>(const int n, const float* a, const float* b, float* y) {
  vsSub(n, a, b, y);
}

template <>
void caffe_sub<double>(const int n, const double* a, const double* b,
    double* y) {
  vdSub(n, a, b, y);
}

template <>
void caffe_mul<float>(const int n, const float* a, const float* b, float* y) {
  vsMul(n, a, b, y);
}

template <>
void caffe_mul<double>(const int n, const double* a, const double* b,
    double* y) {
  vdMul(n, a, b, y);
}

template <>
float caffe_cpu_strided_dot<float>(const int n, const float* x, const int incx,
    const float* y, const int incy) {
  return cblas_sdot(n, x, incx, y, incy);
}

template <>
double caffe_cpu_strided_dot<double>(const int n, const double* x,
    const int incx, const double* y, const int incy) {
  return cblas_ddot(n, x, incx, y, incy);
}

template <typename Dtype>
void caffe_set(const int N, const Dtype alpha, Dtype* Y) {
  if (alpha == 0) {
    memset(Y, 0, sizeof(Dtype) * N);  // NOLINT(caffe/alt_fn)
    return;
  }
  for (int i = 0; i < N; ++i) {
	    Y[i] = alpha;
  }
}

template void caffe_set<int>(const int N, const int alpha, int* Y);
template void caffe_set<float>(const int N, const float alpha, float* Y);
template void caffe_set<double>(const int N, const double alpha, double* Y);

template <>
void caffe_log<float>(const int n, const float* a, float* y) {
  vsLn(n, a, y);
}

template <>
void caffe_log<double>(const int n, const double* a, double* y) {
  vdLn(n, a, y);
}

template <typename Dtype>
void caffe_copy(const int N, const Dtype* X, Dtype* Y) {
  if (X != Y) {
      memcpy(Y, X, sizeof(Dtype) * N);  // NOLINT(caffe/alt_fn)
  }
}

template void caffe_copy<int>(const int N, const int* X, int* Y);
template void caffe_copy<unsigned int>(const int N, const unsigned int* X,
    unsigned int* Y);
template void caffe_copy<float>(const int N, const float* X, float* Y);
template void caffe_copy<double>(const int N, const double* X, double* Y);

template <>
void caffe_abs<float>(const int n, const float* a, float* y) {
  vsAbs(n, a, y);
}

template <>
void caffe_abs<double>(const int n, const double* a, double* y) {
  vdAbs(n, a, y);
}

template <>
void caffe_div<float>(const int n, const float* a, const float* b, float* y) {
  vsDiv(n, a, b, y);
}

template <>
void caffe_div<double>(const int n, const double* a, const double* b,
    double* y) {
  vdDiv(n, a, b, y);
}

template <>
void caffe_powx<float>(const int n, const float* a, const float b, float* y) {
  vsPowx(n, a, b, y);
}

template <>
void caffe_powx<double>(const int n, const double* a, const double b,
    double* y) {
  vdPowx(n, a, b, y);
}

template <>
void caffe_sqr<float>(const int n, const float* a, float* y) {
  vsSqr(n, a, y);
}

template <>
void caffe_sqr<double>(const int n, const double* a, double* y) {
  vdSqr(n, a, y);
}

template <>
void caffe_exp<float>(const int n, const float* a, float* y) {
  vsExp(n, a, y);
}

template <>
void caffe_exp<double>(const int n, const double* a, double* y) {
  vdExp(n, a, y);
}

unsigned int caffe_rng_rand() {
  return (*caffe_rng())();
}

template <typename Dtype>
Dtype caffe_nextafter(const Dtype b) {
  return boost::math::nextafter < Dtype
      > (b, std::numeric_limits < Dtype > ::max());
}
template float caffe_nextafter(const float b);
template double caffe_nextafter(const double b);

template <typename Dtype>
void caffe_rng_uniform(const int n, const Dtype a, const Dtype b, Dtype* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_LE(a, b);
  boost::uniform_real < Dtype
      > random_distribution(a, caffe_nextafter<Dtype>(b));
  boost::variate_generator<caffe::rng_t*, boost::uniform_real<Dtype> > variate_generator(
      caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }

  //LOG(INFO) << "caffe_rng_uniform";
}

template void caffe_rng_uniform<float>(const int n, const float a, const float b,
    float* r);
template void caffe_rng_uniform<double>(const int n, const double a, const double b,
    double* r);

template <typename Dtype>
void caffe_rng_gaussian(const int n, const Dtype a, const Dtype sigma,
    Dtype* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GT(sigma, 0);
  boost::normal_distribution < Dtype > random_distribution(a, sigma);
  boost::variate_generator<caffe::rng_t*, boost::normal_distribution<Dtype> > variate_generator(
      caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template void caffe_rng_gaussian<float>(const int n, const float mu, const float sigma,
    float* r);
template void caffe_rng_gaussian<double>(const int n, const double mu,
    const double sigma, double* r);

template <typename Dtype>
void caffe_rng_bernoulli(const int n, const Dtype p, int* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GE(p, 0);
  CHECK_LE(p, 1);
  boost::bernoulli_distribution < Dtype > random_distribution(p);
  boost::variate_generator<caffe::rng_t*, boost::bernoulli_distribution<Dtype> > variate_generator(
      caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template void caffe_rng_bernoulli<double>(const int n, const double p, int* r);
template void caffe_rng_bernoulli<float>(const int n, const float p, int* r);

template <typename Dtype>
void caffe_rng_bernoulli(const int n, const Dtype p, unsigned int* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GE(p, 0);
  CHECK_LE(p, 1);
  boost::bernoulli_distribution < Dtype > random_distribution(p);
  boost::variate_generator<caffe::rng_t*, boost::bernoulli_distribution<Dtype> > variate_generator(
      caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = static_cast<unsigned int>(variate_generator());
  }
}

template void caffe_rng_bernoulli<double>(const int n, const double p, unsigned int* r);
template void caffe_rng_bernoulli<float>(const int n, const float p, unsigned int* r);

template <>
float caffe_cpu_dot<float>(const int n, const float* x, const float* y) {
  return cblas_sdot(n, x, 1, y, 1);
}

template <>
double caffe_cpu_dot<double>(const int n, const double* x, const double* y) {
  return cblas_ddot(n, x, 1, y, 1);
}

template <>
int caffe_cpu_hamming_distance<float>(const int n, const float* x,
    const float* y) {
  int dist = 0;
  for (int i = 0; i < n; ++i) {
    dist += __builtin_popcount(
        static_cast<uint32_t>(x[i]) ^ static_cast<uint32_t>(y[i]));
  }
  return dist;
}

template <>
int caffe_cpu_hamming_distance<double>(const int n, const double* x,
    const double* y) {
  int dist = 0;
  for (int i = 0; i < n; ++i) {
    dist += __builtin_popcountl(
        static_cast<uint64_t>(x[i]) ^ static_cast<uint64_t>(y[i]));
  }
  return dist;
}

template <>
float caffe_cpu_asum<float>(const int n, const float* x) {
  return cblas_sasum(n, x, 1);
}

template <>
double caffe_cpu_asum<double>(const int n, const double* x) {
  return cblas_dasum(n, x, 1);
}

INSTANTIATE_CAFFE_CPU_UNARY_FUNC (sign);
INSTANTIATE_CAFFE_CPU_UNARY_FUNC (sgnbit);
INSTANTIATE_CAFFE_CPU_UNARY_FUNC (fabs);

template <>
void caffe_cpu_scale<float>(const int n, const float alpha, const float *x,
    float* y) {
  cblas_scopy(n, x, 1, y, 1);
  cblas_sscal(n, alpha, y, 1);
}

template <>
void caffe_cpu_scale<double>(const int n, const double alpha, const double *x,
    double* y) {
  cblas_dcopy(n, x, 1, y, 1);
  cblas_dscal(n, alpha, y, 1);
}

#ifndef CPU_ONLY
//DEFINE_AND_INSTANTIATE_GPU_UNARY_FUNC(sign, y[index] = (Dtype(0) < x[index])
//  - (x[index] < Dtype(0)));
//DEFINE_AND_INSTANTIATE_GPU_UNARY_FUNC(sgnbit, y[index] = signbit(x[index]));

template <>
void caffe_gpu_gemm<float>(const CBLAS_TRANSPOSE TransA,
    const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
    const float alpha, const float* A, const float* B, const float beta,
    float* C) {
  clblasTranspose transA =
      (TransA == CblasNoTrans) ? clblasNoTrans : clblasTrans;
  clblasTranspose transB =
      (TransB == CblasNoTrans) ? clblasNoTrans : clblasTrans;
  int lda = (TransA == CblasNoTrans) ? K : M;
  int ldb = (TransB == CblasNoTrans) ? N : K;
  int ldc = N;
  CLBLAS_CHECK(
      clblasSgemm(amdDevice.col, transB, transA, N, M, K, (cl_float) alpha,
          (cl_mem) B, 0, ldb, (cl_mem) A, 0, lda, (cl_float) beta, (cl_mem) C,
          0, ldc, 1, &(amdDevice.CommandQueue), 0, NULL, NULL));
}

template <>
void caffe_gpu_gemm<double>(const CBLAS_TRANSPOSE TransA,
    const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
    const double alpha, const double* A, const double* B, const double beta,
    double* C) {
  clblasTranspose transA =
      (TransA == CblasNoTrans) ? clblasNoTrans : clblasTrans;
  clblasTranspose transB =
      (TransB == CblasNoTrans) ? clblasNoTrans : clblasTrans;
  int lda = (TransA == CblasNoTrans) ? K : M;
  int ldb = (TransB == CblasNoTrans) ? N : K;
  int ldc = N;
  CLBLAS_CHECK(
      clblasDgemm(amdDevice.col, transB, transA, N, M, K,  alpha,
          (cl_mem) B, 0, ldb, (cl_mem) A, 0, lda,  beta, (cl_mem) C,
          0, ldc, 1, &(amdDevice.CommandQueue), 0, NULL, NULL));
}

template <>
cl_event caffe_gpu_gemm<float>(const CBLAS_TRANSPOSE TransA,
    const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
    const float alpha, const float* A, const int offA, const float* B,
    const int offB, const float beta, float* C, const int offC) {
  cl_event event;
  clblasTranspose transA =
      (TransA == CblasNoTrans) ? clblasNoTrans : clblasTrans;
  clblasTranspose transB =
      (TransB == CblasNoTrans) ? clblasNoTrans : clblasTrans;
  int lda = (TransA == CblasNoTrans) ? K : M;
  int ldb = (TransB == CblasNoTrans) ? N : K;
  int ldc = N;
  CLBLAS_CHECK(
      clblasSgemm(amdDevice.col, transB, transA, N, M, K, (cl_float) alpha,
          (cl_mem) B, offB, ldb, (cl_mem) A, offA, lda, (cl_float) beta,
          (cl_mem) C, offC, ldc, 1, &(amdDevice.CommandQueue), 0, NULL,
          &event));
  return event;
}

template <>
cl_event caffe_gpu_gemm<double>(const CBLAS_TRANSPOSE TransA,
    const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
    const double alpha, const double* A, const int offA, const double* B,
    const int offB, const double beta, double* C, const int offC) {
  cl_event event;
  clblasTranspose transA =
      (TransA == CblasNoTrans) ? clblasNoTrans : clblasTrans;
  clblasTranspose transB =
      (TransB == CblasNoTrans) ? clblasNoTrans : clblasTrans;
  int lda = (TransA == CblasNoTrans) ? K : M;
  int ldb = (TransB == CblasNoTrans) ? N : K;
  int ldc = N;
  CLBLAS_CHECK(
      clblasDgemm(amdDevice.col, transB, transA, N, M, K, alpha,
          (cl_mem) B, offB, ldb, (cl_mem) A, offA, lda, beta,
          (cl_mem) C, offC, ldc, 1, &(amdDevice.CommandQueue), 0, NULL,
          &event));
  return event;
}

template <>
cl_event caffe_gpu_gemm<float>(cl_command_queue *queue,
    const CBLAS_TRANSPOSE TransA, const CBLAS_TRANSPOSE TransB, const int M,
    const int N, const int K, const float alpha, const float* A, const int offA,
    const float* B, const int offB, const float beta, float* C,
    const int offC) {
  cl_event event;
  clblasTranspose transA =
      (TransA == CblasNoTrans) ? clblasNoTrans : clblasTrans;
  clblasTranspose transB =
      (TransB == CblasNoTrans) ? clblasNoTrans : clblasTrans;
  int lda = (TransA == CblasNoTrans) ? K : M;
  int ldb = (TransB == CblasNoTrans) ? N : K;
  int ldc = N;
  CLBLAS_CHECK(
      clblasSgemm(amdDevice.col, transB, transA, N, M, K, (cl_float) alpha,
          (cl_mem) B, offB, ldb, (cl_mem) A, offA, lda, (cl_float) beta,
          (cl_mem) C, offC, ldc, 1, queue, 0, NULL, &event));
  return event;
}

template <>
cl_event caffe_gpu_gemm<double>(cl_command_queue *queue,
    const CBLAS_TRANSPOSE TransA, const CBLAS_TRANSPOSE TransB, const int M,
    const int N, const int K, const double alpha, const double* A,
    const int offA, const double* B, const int offB, const double beta,
    double* C, const int offC) {
  cl_event event;
  clblasTranspose transA =
      (TransA == CblasNoTrans) ? clblasNoTrans : clblasTrans;
  clblasTranspose transB =
      (TransB == CblasNoTrans) ? clblasNoTrans : clblasTrans;
  int lda = (TransA == CblasNoTrans) ? K : M;
  int ldb = (TransB == CblasNoTrans) ? N : K;
  int ldc = N;
  CLBLAS_CHECK(
      clblasDgemm(amdDevice.col, transB, transA, N, M, K,  alpha,
          (cl_mem) B, offB, ldb, (cl_mem) A, offA, lda, beta,
          (cl_mem) C, offC, ldc, 1, queue, 0, NULL, &event));
  return event;
}

template <>
void caffe_gpu_gemv<float>(const CBLAS_TRANSPOSE TransA, const int M,
    const int N, const float alpha, const float* A, size_t offA, int lda,
    const float* x, size_t offx, const float beta, int incx, float* y,
    size_t offy, int incy) {
  clblasTranspose transA =
      (TransA == CblasNoTrans) ? clblasNoTrans : clblasTrans;
  CLBLAS_CHECK(
      clblasSgemv(amdDevice.row, transA, M, N, (cl_float) alpha, (cl_mem) A,
          offA, lda, (cl_mem) x, offx, incx, (cl_float) beta, (cl_mem) y, offy,
          incy, 1, &(amdDevice.CommandQueue), 0, NULL, NULL));
}

template <>
void caffe_gpu_gemv<double>(const CBLAS_TRANSPOSE TransA, const int M,
    const int N, const double alpha, const double* A, size_t offA, int lda,
    const double* x, size_t offx, const double beta, int incx, double* y,
    size_t offy, int incy) {
  clblasTranspose transA =
      (TransA == CblasNoTrans) ? clblasNoTrans : clblasTrans;
  CLBLAS_CHECK(
      clblasDgemv(amdDevice.row, transA, M, N, (cl_double) alpha, (cl_mem) A,
          offA, lda, (cl_mem) x, offx, incx, (cl_double) beta, (cl_mem) y, offy,
          incy, 1, &(amdDevice.CommandQueue), 0, NULL, NULL));
}

template <>
void caffe_gpu_gemv<float>(const CBLAS_TRANSPOSE TransA, const int M,
    const int N, const float alpha, const float* A, const float* x,
    const float beta, float* y) {
  clblasTranspose transA =
      (TransA == CblasNoTrans) ? clblasNoTrans : clblasTrans;
  CLBLAS_CHECK(
      clblasSgemv(amdDevice.row, transA, M, N, (cl_float) alpha, (cl_mem) A, 0,
          N, (cl_mem) x, 0, 1, (cl_float) beta, (cl_mem) y, 0, 1, 1,
          &(amdDevice.CommandQueue), 0, NULL, NULL));
}

template <>
void caffe_gpu_gemv<double>(const CBLAS_TRANSPOSE TransA, const int M,
    const int N, const double alpha, const double* A, const double* x,
    const double beta, double* y) {
  clblasTranspose transA =
      (TransA == CblasNoTrans) ? clblasNoTrans : clblasTrans;
  CLBLAS_CHECK(
      clblasDgemv(amdDevice.row, transA, M, N, (cl_double) alpha, (cl_mem) A, 0,
          N, (cl_mem) x, 0, 1, (cl_double) beta, (cl_mem) y, 0, 1, 1,
          &(amdDevice.CommandQueue), 0, NULL, NULL));
}

template <>
void caffe_gpu_axpy<float>(const int N, const float alpha, const float* X,
    float* Y) {
  CLBLAS_CHECK(
      clblasSaxpy(N, alpha, (cl_mem) X, 0, 1, (cl_mem) Y, 0, 1, 1,
          &(amdDevice.CommandQueue), 0, NULL, NULL));
}

template <>
void caffe_gpu_axpy<double>(const int N, const double alpha, const double* X,
    double* Y) {
  CLBLAS_CHECK(
      clblasDaxpy(N, alpha, (cl_mem) X, 0, 1, (cl_mem) Y, 0, 1, 1,
          &(amdDevice.CommandQueue), 0, NULL, NULL));
}

template <>
void caffe_gpu_sgnbit<float>(const int n, const float* x, float* y) {
  caffe_gpu_signbit(n, x, y);
}

template <>
void caffe_gpu_sgnbit<double>(const int n, const double* x, double* y) {
  caffe_gpu_signbit(n, x, y);
}

template <>
void caffe_gpu_abs<float>(const int n, const float* x, float* y) {
  caffe_gpu_abs_ocl(n, x, y);
}

template <>
void caffe_gpu_abs<double>(const int n, const double* x, double* y) {
  caffe_gpu_abs_ocl(n, x, y);
}

void caffe_gpu_memcpy(const size_t N, const void *X, void *Y) {
  clEnqueueReadBuffer(amdDevice.CommandQueue, (cl_mem) X, CL_TRUE, 0, N, Y, 0,
      NULL, NULL);
}
template <>
void caffe_gpu_memcpy<float>(const size_t N, const float* X, float* Y) {
  OCL_CHECK(
      clEnqueueCopyBuffer(amdDevice.CommandQueue, (cl_mem) X, (cl_mem) Y, 0, 0,
          N, 0, NULL, NULL));
}

template <>
void caffe_gpu_memcpy<double>(const size_t N, const double* X, double* Y) {
  OCL_CHECK(
      clEnqueueCopyBuffer(amdDevice.CommandQueue, (cl_mem) X, (cl_mem) Y, 0, 0,
          N, 0, NULL, NULL));
}

template <typename Dtype>
void caffe_gpu_copy(const int N, const Dtype* X, Dtype* Y) {
  if (X != Y) {
     OCL_CHECK(
       clEnqueueCopyBuffer(amdDevice.CommandQueue, (cl_mem) X, (cl_mem) Y, 0, 0,
          N * sizeof(Dtype), 0, NULL, NULL));
  }
}
template void caffe_gpu_copy<float>(const int N, const float* X, float* Y);
template void caffe_gpu_copy<double>(const int N, const double* X, double* Y);
template void caffe_gpu_copy<int>(const int N, const int* X, int* Y);
template void caffe_gpu_copy<unsigned int>(const int N, const unsigned int* X, unsigned int* Y);

template <>
void caffe_gpu_copy<float>(const int N, const float* X, const int offx, float* Y, const int offy) {
  if (X != Y) {
    CLBLAS_CHECK(
        clblasScopy(N, (cl_mem) X, offx, 1, (cl_mem) Y, offy, 1, 1,
            &(amdDevice.CommandQueue), 0, NULL, NULL));
  }
}

template <>
void caffe_gpu_copy<double>(const int N, const double* X, const int offx, double* Y, const int offy) {
  if (X != Y) {
    CLBLAS_CHECK(
        clblasDcopy(N, (cl_mem) X, offx, 1, (cl_mem) Y, offy, 1, 1,
            &(amdDevice.CommandQueue), 0, NULL, NULL));
  }
}

template <>
void caffe_gpu_scal<float>(const int N, const float alpha, float *X, const int offx) {
  CLBLAS_CHECK(
      clblasSscal(N, alpha, (cl_mem) X, offx, 1, 1, &(amdDevice.CommandQueue), 0,
          NULL, NULL));
}

template <>
void caffe_gpu_scal<double>(const int N, const double alpha, double *X, const int offx) {
  CLBLAS_CHECK(
      clblasDscal(N, alpha, (cl_mem) X, offx, 1, 1, &(amdDevice.CommandQueue), 0,
          NULL, NULL));
}

template <>
void caffe_gpu_axpby<float>(const int N, const float alpha, const float* X,
    const float beta, float* Y) {
  caffe_gpu_scal<float>(N, beta, Y);
  caffe_gpu_axpy<float>(N, alpha, X, Y);
}

template <>
void caffe_gpu_axpby<double>(const int N, const double alpha, const double* X,
    const double beta, double* Y) {
  caffe_gpu_scal<double>(N, beta, Y);
  caffe_gpu_axpy<double>(N, alpha, X, Y);
}

template <>
void caffe_gpu_dot<float>(const int n, const float* x, const float* y,
    float* out) {
  cl_mem scratchBuff = clCreateBuffer(amdDevice.Context, CL_MEM_READ_WRITE,
      (n * sizeof(float)), NULL, NULL);
  cl_mem d_out = clCreateBuffer(amdDevice.Context, CL_MEM_READ_WRITE,
      (1 * sizeof(float)), NULL, NULL);
  clblasSdot(n, d_out, 0, (cl_mem) x, 0, 1, (cl_mem) y, 0, 1, scratchBuff, 1,
      &(amdDevice.CommandQueue), 0, NULL, NULL);
  clEnqueueReadBuffer(amdDevice.CommandQueue, d_out, CL_TRUE, 0, sizeof(float),
      out, 0, NULL, NULL);
  clReleaseMemObject(scratchBuff);
  clReleaseMemObject(d_out);
}

template <>
void caffe_gpu_dot<double>(const int n, const double* x, const double* y,
    double * out) {
  //need to pass in scratchBuff
  cl_mem scratchBuff = clCreateBuffer(amdDevice.Context, CL_MEM_READ_WRITE,
      (n * sizeof(double)), NULL, NULL);
  cl_mem d_out = clCreateBuffer(amdDevice.Context, CL_MEM_READ_WRITE,
      (1 * sizeof(double)), NULL, NULL);
  clblasDdot(n, d_out, 0, (cl_mem) x, 0, 1, (cl_mem) y, 0, 1, scratchBuff, 1,
      &(amdDevice.CommandQueue), 0, NULL, NULL);
  clEnqueueReadBuffer(amdDevice.CommandQueue, d_out, CL_TRUE, 0, sizeof(double),
      out, 0, NULL, NULL);
  clReleaseMemObject(scratchBuff);
  clReleaseMemObject(d_out);
}

template <>
void caffe_gpu_dot<float>(const int n, const float* x, size_t offx, const float* y, size_t offy, float* out) {
  cl_mem scratchBuff = clCreateBuffer(amdDevice.Context, CL_MEM_READ_WRITE,
      (n * sizeof(float)), NULL, NULL);
  cl_mem d_out = clCreateBuffer(amdDevice.Context, CL_MEM_READ_WRITE,
      (1 * sizeof(float)), NULL, NULL);
  clblasSdot(n, d_out, 0, (cl_mem) x, offx, 1, (cl_mem) y, offy, 1, scratchBuff, 1,
      &(amdDevice.CommandQueue), 0, NULL, NULL);
  clEnqueueReadBuffer(amdDevice.CommandQueue, d_out, CL_TRUE, 0, sizeof(float),
      out, 0, NULL, NULL);
  clReleaseMemObject(scratchBuff);
  clReleaseMemObject(d_out);
}

template <>
void caffe_gpu_dot<double>(const int n, const double* x, size_t offx, const double* y, size_t offy, double * out) {
  //need to pass in scratchBuff
  cl_mem scratchBuff = clCreateBuffer(amdDevice.Context, CL_MEM_READ_WRITE,
      (n * sizeof(double)), NULL, NULL);
  cl_mem d_out = clCreateBuffer(amdDevice.Context, CL_MEM_READ_WRITE,
      (1 * sizeof(double)), NULL, NULL);
  clblasDdot(n, d_out, 0, (cl_mem) x, offx, 1, (cl_mem) y, offy, 1, scratchBuff, 1,
      &(amdDevice.CommandQueue), 0, NULL, NULL);
  clEnqueueReadBuffer(amdDevice.CommandQueue, d_out, CL_TRUE, 0, sizeof(double),
      out, 0, NULL, NULL);
  clReleaseMemObject(scratchBuff);
  clReleaseMemObject(d_out);
}

template <>
void caffe_gpu_asum<float>(const int n, const float* x, float* y) {
  cl_mem scratchBuff = clCreateBuffer(amdDevice.Context, CL_MEM_READ_WRITE,
      (n * sizeof(cl_float)), NULL, NULL);
  cl_mem d_y = clCreateBuffer(amdDevice.Context, CL_MEM_READ_WRITE,
      (1 * sizeof(cl_float)), NULL, NULL);
  clblasSasum(n, d_y, 0, (cl_mem) x, 0, 1, scratchBuff, 1,
      &(amdDevice.CommandQueue), 0, NULL, NULL);
  clEnqueueReadBuffer(amdDevice.CommandQueue, d_y, CL_TRUE, 0, sizeof(float), y,
      0, NULL, NULL);
  clReleaseMemObject(scratchBuff);
  clReleaseMemObject(d_y);
}

template <>
void caffe_gpu_asum<double>(const int n, const double* x, double* y) {
  cl_mem scratchBuff = clCreateBuffer(amdDevice.Context, CL_MEM_READ_WRITE,
      (n * sizeof(cl_double)), NULL, NULL);
  cl_mem d_y = clCreateBuffer(amdDevice.Context, CL_MEM_READ_WRITE,
      (1 * sizeof(cl_double)), NULL, NULL);
  clblasDasum(n, d_y, 0, (cl_mem) x, 0, 1, scratchBuff, 1,
      &(amdDevice.CommandQueue), 0, NULL, NULL);
  clEnqueueReadBuffer(amdDevice.CommandQueue, d_y, CL_TRUE, 0, sizeof(double),
      y, 0, NULL, NULL);
  clReleaseMemObject(scratchBuff);
  clReleaseMemObject(d_y);
}

template <>
void caffe_gpu_asum<float>(const int n, const float* x, size_t offx, float* y) {
  cl_mem scratchBuff = clCreateBuffer(amdDevice.Context, CL_MEM_READ_WRITE,
      (n * sizeof(cl_float)), NULL, NULL);
  cl_mem d_y = clCreateBuffer(amdDevice.Context, CL_MEM_READ_WRITE,
      (1 * sizeof(cl_float)), NULL, NULL);
  clblasSasum(n, d_y, 0, (cl_mem) x, offx, 1, scratchBuff, 1,
      &(amdDevice.CommandQueue), 0, NULL, NULL);
  clEnqueueReadBuffer(amdDevice.CommandQueue, d_y, CL_TRUE, 0, sizeof(float), y,
      0, NULL, NULL);
  clReleaseMemObject(scratchBuff);
  clReleaseMemObject(d_y);
}

template <>
void caffe_gpu_asum<double>(const int n, const double* x, size_t offx, double* y) {
  cl_mem scratchBuff = clCreateBuffer(amdDevice.Context, CL_MEM_READ_WRITE,
      (n * sizeof(cl_double)), NULL, NULL);
  cl_mem d_y = clCreateBuffer(amdDevice.Context, CL_MEM_READ_WRITE,
      (1 * sizeof(cl_double)), NULL, NULL);
  clblasDasum(n, d_y, 0, (cl_mem) x, offx, 1, scratchBuff, 1,
      &(amdDevice.CommandQueue), 0, NULL, NULL);
  clEnqueueReadBuffer(amdDevice.CommandQueue, d_y, CL_TRUE, 0, sizeof(double),
      y, 0, NULL, NULL);
  clReleaseMemObject(scratchBuff);
  clReleaseMemObject(d_y);
}

template <>
void caffe_gpu_scale<float>(const int n, const float alpha, const float *x,
    float* y) {
  caffe_gpu_copy(n, x, y);
  caffe_gpu_scal(n, alpha, y);
}

template <>
void caffe_gpu_scale<double>(const int n, const double alpha, const double *x,
    double* y) {
  caffe_gpu_copy(n, x, y);
  caffe_gpu_scal(n, alpha, y);
}

template <>
void caffe_gpu_scale<float>(const int n, const float alpha, const float *x,
    const int offx, float* y, const int offy) {
  caffe_gpu_copy(n, x, offx, y, offy);
  caffe_gpu_scal(n, alpha, y, offy);
}

template <>
void caffe_gpu_scale<double>(const int n, const double alpha, const double *x,
    const int offx, double* y, const int offy) {
  caffe_gpu_copy(n, x, offx, y, offy);
  caffe_gpu_scal(n, alpha, y, offy);
}

template <typename Dtype>
void set_kernel(const int n, const Dtype alpha, Dtype* y) {
  NOT_IMPLEMENTED;
}

template <>
void caffe_gpu_set<float>(const int N, const float alpha, float* Y, const int offy) {
  ocl_memset(Y, alpha, N, offy);
}

template <>
void caffe_gpu_set<double>(const int N, const double alpha, double* Y, const int offy) {
  ocl_memset(Y, alpha, N, offy);
}

template <>
void caffe_gpu_add_scalar(const int N, const float alpha, float* Y) {
  kernel_add_scalar(N, alpha, Y);
}

template <>
void caffe_gpu_add_scalar(const int N, const double alpha, double* Y) {
  kernel_add_scalar(N, alpha, Y);
}

template <>
void caffe_gpu_exp<float>(const int N, const float* a, float* y) {
  kernel_exp(N, a, y);
}

template <>
void caffe_gpu_exp<double>(const int N, const double* a, double* y) {
  kernel_exp(N, a, y);
}

template <>
void caffe_gpu_sign<float>(const int N, const float *X, float *Y) {
  caffe_gpu_sign_ocl(N, X, Y);
}

template <>
void caffe_gpu_sign<double>(const int N, const double *X, double *Y) {
  caffe_gpu_sign_ocl(N, X, Y);
}

template <>
void caffe_gpu_sign<float>(const int N, const float *X, const int offx, float *Y, const int offy) {
  caffe_gpu_sign_with_offset_ocl(N, X, offx, Y, offy);
}

template <>
void caffe_gpu_sign<double>(const int N, const double *X, const int offx, double *Y, const int offy) {
  caffe_gpu_sign_with_offset_ocl(N, X, offx, Y, offy);
}

template <>
void caffe_gpu_sub<float>(const int N, const float* a, const float* b,
    float* y) {
  // NOLINT_NEXT_LINE(whitespace/operators)
  kernel_sub(N, a, b, y);
}

template <>
void caffe_gpu_sub<double>(const int N, const double* a, const double* b,
    double* y) {
  // NOLINT_NEXT_LINE(whitespace/operators)
  kernel_sub(N, a, b, y);
}

template <>
void caffe_gpu_mul<float>(const int N, const float* a, const float* b,
    float* y) {
  kernel_mul(N, a, b, y);
}

template <>
void caffe_gpu_mul<double>(const int N, const double* a, const double* b,
    double* y) {
  kernel_mul(N, a, b, y);
}

template <>
void caffe_gpu_div<float>(const int N, const float* a, const float* b,
    float* y) {
  kernel_div(N, a, b, y);
}

template <>
void caffe_gpu_div<double>(const int N, const double* a, const double* b,
    double* y) {
  kernel_div(N, a, b, y);
}

template <>
void caffe_gpu_powx<float>(const int N, const float* a, const float alpha,
    float* y) {
  // NOLINT_NEXT_LINE(whitespace/operators)
  kernel_powx(N, a, alpha, y);
}

template <>
void caffe_gpu_powx<double>(const int N, const double* a, const double alpha,
    double* y) {
  // NOLINT_NEXT_LINE(whitespace/operators)
  kernel_powx(N, a, alpha, y);
}

void popc_kernel(const int n, const float* a, const float* b, uint8_t* y) {
  NOT_IMPLEMENTED;
}

void popcll_kernel(const int n, const double* a, const double* b, uint8_t* y) {
  NOT_IMPLEMENTED;
}

template <>
uint32_t caffe_gpu_hamming_distance<float>(const int n, const float* x,
    const float* y) {
  NOT_IMPLEMENTED;
}

template <>
uint32_t caffe_gpu_hamming_distance<double>(const int n, const double* x,
    const double* y) {
  NOT_IMPLEMENTED;
}

void caffe_gpu_rng_uniform(const int n, unsigned int* r) {
        caffe_gpu_uniform(n, r);
}

template <>
void caffe_gpu_rng_uniform<float>(const int n, const float a, const float b,
    float* r) {
  caffe_gpu_uniform(r, n, a, b);	// r is a cl_mem object
}
template <>
void caffe_gpu_rng_uniform<double>(const int n, const double a, const double b,
    double* r) {
  caffe_gpu_uniform(r, n, a, b);  // r is a cl_mem object
}

template <>
void caffe_gpu_rng_gaussian<float>(const int n, const float mu,
    const float sigma, float* r) {
  caffe_gpu_gaussian(r, n, mu, sigma);  // r is a cl_mem object
}

template <>
void caffe_gpu_rng_gaussian<double>(const int n, const double mu,
    const double sigma, double* r) {
  caffe_gpu_gaussian(r, n, mu, sigma);  // r is a cl_mem object
}

template <>
void caffe_gpu_log<float>(const int N, const float* a, float* y) {
  // NOLINT_NEXT_LINE(whitespace/operators)
  kernel_log(N, a, y);
}

template <>
void caffe_gpu_log<double>(const int N, const double* a, double* y) {
  // NOLINT_NEXT_LINE(whitespace/operators)
  kernel_log(N, a, y);
}

template <>
void caffe_gpu_add<float>(const int N, const float* a, const float* b,
    float* y) {
  // NOLINT_NEXT_LINE(whitespace/operators)
  kernel_add(N, a, b, y);
}

template <>
void caffe_gpu_add<double>(const int N, const double* a, const double* b,
    double* y) {
  // NOLINT_NEXT_LINE(whitespace/operators)
  kernel_add(N, a, b, y);
}
#endif
}  // namespace caffe


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

  Program:   Small Body Geophysical Analysis
  Module:    SBGATObsLightcurve.hpp

  Class derived from VTK's vtkPolyDataAlgorithm by Benjamin Bercovici

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

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

/**
@file SBGATObsLightcurve.hpp
@class  SBGATObsLightcurve
@author Benjamin Bercovici
@author Jay McMahon
@date October 2018
@brief Defines the SBGATObsLightcurve class
@details Defines the SBGATObsLightcurve class, used to generate lightcurves over a
 single/binary asteroid system at a fixed phase angle
@copyright MIT License, Benjamin Bercovici and Jay McMahon
*/

#ifndef SBGATObsLightcurve_H
#define SBGATObsLightcurve_H

#include <vtkFiltersCoreModule.h> // For export macro

#include <vtkModifiedBSPTree.h>
#include <vtkImageData.h>

#include <armadillo>
#include <array>

#include <SBGATObs.hpp>

class VTKFILTERSCORE_EXPORT SBGATObsLightcurve : public SBGATObs{
public:

  static SBGATObsLightcurve *New();

  vtkTypeMacro(SBGATObsLightcurve,SBGATObs);
  void PrintSelf(std::ostream& os, vtkIndent indent) override;
  void PrintHeader(std::ostream& os, vtkIndent indent) override;
  void PrintTrailer(std::ostream& os, vtkIndent indent) override;

  /**
  Computes collected luminosity over the surface of the considered small bodies at the
  specified time after epoch. Exposure is instantaneous. The luminosity is computed as the
  number of sample points in view of both the sun and the observer at each time (the "hit count"), normalized by the largest hit count in the observation sequence.
  The positions of each body is specified. The attitude of each body is supposed to derive from a constant-spin rotation such that
  each of the dcms BN is equal to identity when dt == 0.
  @param measurements reference to a vector of std::arrays holding (times,luminosity)
  @param time measurement time
  @param N minimum number of measurements to produce over the smallest facet in the shape. The number of samples for any other facet will be equal to N * facet_surface_area / smallest_facet_surface_area
  @param sun_dir unit direction towards sun from target in inertial frame
  @param observer_dir unit direction towards observer from target in inertial frame
  @param positions_vec vector of positions of each target's center-of-mass expressed in the primary's body frame
  @param velocities_vec vector of inertial velocities of each target's center-of-mass expressed in the primary's body frame
  @param mrps_vec vector of MRPs defining the inertial-to-body DCM [BN]
  @param omegas_vec vector of angular velocities of each body expressed in the inertial frame
  @param penalize_incidence if true, each measurement will be weighed by the cos(incidence) angle between
  the sampled point and the radar squared. If false, all measurements (in view of the radar and not blocked) are weighed equally
  have the same weight

  */
  void CollectMeasurements(
    std::vector<std::array<double, 2> > & measurements,
    const double & time,
    const int & N,
    const arma::vec & sun_dir,
    const arma::vec & observer_dir,
    const std::vector<arma::vec> & positions_vec,
    const std::vector<arma::vec> & velocities_vec,
    const std::vector<arma::vec> & mrps_vec,
    const std::vector<arma::vec> & omegas_vec,
    const bool & penalize_indicence);

  /**
  Save the raw data points from the unnormalized (times,luminosity) time series to file
  @param measurements reference to a vector of std::arrays holding (times,luminosity)
  @param savepath path to file where raw lightcurve data will be saved (ex: "output/lightcurve.txt")
  */
  void SaveLightCurveData(const std::vector<std::array<double, 2> > & measurements, std::string savepath);

protected:
  SBGATObsLightcurve();
  ~SBGATObsLightcurve() override;

  /**
  Ray traces all of the facets in view to the sun/observer and increment measurement
  counter if in view
  @param measurements_temp reference to an std::array holding (times,luminosity)
  @param facets_in_view reference to a vector of vector holding indices of (maybe) illuminated facets for all considered bodies
  @param sun_dir sun direction expressed in inertial frame
  @param observer_dir observer direction expressed in inertial frame
  @param BN_dcms_vec vector holding the DCMs orienting the body frame of each body w/r to inertial
  @param positions_vec vector holding the position vector of the CM of each body w/r to the primary
  */
  void reverse_ray_trace(std::array<double, 2>  & measurements_temp,
    const std::vector<std::vector<int> > & facets_in_view,
    const arma::vec & sun_dir,
    const arma::vec & observer_dir,
    const int N,
    const bool penalize_indicence,
    const std::vector<arma::mat> & BN_dcms_vec,
    const std::vector<arma::vec> & positions_vec);

private:
  SBGATObsLightcurve(const SBGATObsLightcurve&) = delete;
  void operator=(const SBGATObsLightcurve&) = delete;
};

#endif


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Vincent Rouvreau
 *
 *    Copyright (C) 2016 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#include <boost/program_options.hpp>

#include <CGAL/Epick_d.h>
#include <CGAL/Epeck_d.h>

#include <gudhi/Alpha_complex.h>
#include <gudhi/Persistent_cohomology.h>
// to construct a simplex_tree from alpha complex
#include <gudhi/Simplex_tree.h>

#include <iostream>
#include <string>
#include <limits>  // for numeric_limits

using Simplex_tree = Gudhi::Simplex_tree<>;
using Filtration_value = Simplex_tree::Filtration_value;

void program_options(int argc, char *argv[], std::string &off_file_points, bool &exact, bool &fast,
                     std::string &output_file_diag, Filtration_value &alpha_square_max_value,
                     int &coeff_field_characteristic, Filtration_value &min_persistence);

int main(int argc, char **argv) {
  std::string off_file_points;
  std::string output_file_diag;
  bool exact_version = false;
  bool fast_version = false;
  Filtration_value alpha_square_max_value;
  int coeff_field_characteristic;
  Filtration_value min_persistence;

  program_options(argc, argv, off_file_points, exact_version, fast_version, output_file_diag, alpha_square_max_value,
                  coeff_field_characteristic, min_persistence);

  if ((exact_version) && (fast_version)) {
    std::cerr << "You cannot set the exact and the fast version." << std::endl;
    exit(-1);
  }

  Simplex_tree simplex;
  if (fast_version) {
    // WARNING : CGAL::Epick_d is fast but not safe (unlike CGAL::Epeck_d)
    // (i.e. when the points are on a grid)
    using Fast_kernel = CGAL::Epick_d<CGAL::Dynamic_dimension_tag>;

    // Init of an alpha complex from an OFF file
    Gudhi::alpha_complex::Alpha_complex<Fast_kernel> alpha_complex_from_file(off_file_points);

    if (!alpha_complex_from_file.create_complex(simplex, alpha_square_max_value)) {
      std::cerr << "Fast Alpha complex simplicial complex creation failed." << std::endl;
      exit(-1);
    }
  } else {
    using Kernel = CGAL::Epeck_d<CGAL::Dynamic_dimension_tag>;

    // Init of an alpha complex from an OFF file
    Gudhi::alpha_complex::Alpha_complex<Kernel> alpha_complex_from_file(off_file_points);

    if (!alpha_complex_from_file.create_complex(simplex, alpha_square_max_value, exact_version)) {
      std::cerr << "Alpha complex simplicial complex creation failed." << std::endl;
      exit(-1);
    }
  }
  // ----------------------------------------------------------------------------
  // Display information about the alpha complex
  // ----------------------------------------------------------------------------
  std::clog << "Simplicial complex is of dimension " << simplex.dimension() << " - " << simplex.num_simplices()
            << " simplices - " << simplex.num_vertices() << " vertices." << std::endl;

  std::clog << "Simplex_tree dim: " << simplex.dimension() << std::endl;
  // Compute the persistence diagram of the complex
  Gudhi::persistent_cohomology::Persistent_cohomology<Simplex_tree, Gudhi::persistent_cohomology::Field_Zp> pcoh(
      simplex);
  // initializes the coefficient field for homology
  pcoh.init_coefficients(coeff_field_characteristic);

  pcoh.compute_persistent_cohomology(min_persistence);

  // Output the diagram in filediag
  if (output_file_diag.empty()) {
    pcoh.output_diagram();
  } else {
    std::clog << "Result in file: " << output_file_diag << std::endl;
    std::ofstream out(output_file_diag);
    pcoh.output_diagram(out);
    out.close();
  }
  return 0;
}

void program_options(int argc, char *argv[], std::string &off_file_points, bool &exact, bool &fast,
                     std::string &output_file_diag, Filtration_value &alpha_square_max_value,
                     int &coeff_field_characteristic, Filtration_value &min_persistence) {
  namespace po = boost::program_options;
  po::options_description hidden("Hidden options");
  hidden.add_options()("input-file", po::value<std::string>(&off_file_points),
                       "Name of file containing a point set. Format is one point per line:   X1 ... Xd ");

  po::options_description visible("Allowed options", 100);
  visible.add_options()("help,h", "produce help message")(
      "exact,e", po::bool_switch(&exact),
      "To activate exact version of Alpha complex (default is false, not available if fast is set)")(
      "fast,f", po::bool_switch(&fast),
      "To activate fast version of Alpha complex (default is false, not available if exact is set)")(
      "output-file,o", po::value<std::string>(&output_file_diag)->default_value(std::string()),
      "Name of file in which the persistence diagram is written. Default print in std::clog")(
      "max-alpha-square-value,r", po::value<Filtration_value>(&alpha_square_max_value)
                                      ->default_value(std::numeric_limits<Filtration_value>::infinity()),
      "Maximal alpha square value for the Alpha complex construction.")(
      "field-charac,p", po::value<int>(&coeff_field_characteristic)->default_value(11),
      "Characteristic p of the coefficient field Z/pZ for computing homology.")(
      "min-persistence,m", po::value<Filtration_value>(&min_persistence),
      "Minimal lifetime of homology feature to be recorded. Default is 0. Enter a negative value to see zero length "
      "intervals");

  po::positional_options_description pos;
  pos.add("input-file", 1);

  po::options_description all;
  all.add(visible).add(hidden);

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).options(all).positional(pos).run(), vm);
  po::notify(vm);

  if (vm.count("help") || !vm.count("input-file")) {
    std::clog << std::endl;
    std::clog << "Compute the persistent homology with coefficient field Z/pZ \n";
    std::clog << "of an Alpha complex defined on a set of input points.\n \n";
    std::clog << "The output diagram contains one bar per line, written with the convention: \n";
    std::clog << "   p   dim b d \n";
    std::clog << "where dim is the dimension of the homological feature,\n";
    std::clog << "b and d are respectively the birth and death of the feature and \n";
    std::clog << "p is the characteristic of the field Z/pZ used for homology coefficients." << std::endl << std::endl;

    std::clog << "Usage: " << argv[0] << " [options] input-file" << std::endl << std::endl;
    std::clog << visible << std::endl;
    exit(-1);
  }
}