/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
   Copyright 2021 Kenneth W. Chapman

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

     http://www.apache.org/licenses/LICENSE-2.0

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

#include "log.h"
#include "calib.h"
#include "findline.h"
#include <vector>
#include <limits>
#include <cmath>
#include <iostream>
#include <fstream>
#include <sstream>
#include <boost/filesystem.hpp>
#include <boost/foreach.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/exception/exception.hpp>
#include <boost/exception/diagnostic_information.hpp>
#include <boost/format.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/imgcodecs.hpp>

#ifdef __DEBUG_CALIB
#undef __DEBUG_CALIB
#endif

#ifdef LOG_CALIB_VALUES
#undef LOG_CALIB_VALUES
#endif

using namespace cv;
using namespace std;
using namespace boost;
namespace fs = boost::filesystem;

namespace gc
{

enum PIX_POS_INDEX
{
    PIX_POS_X = 0,
    PIX_POS_Y = 1
};

void Calib::clear()
{
    m_model.clear();
    m_matHomogPixToWorld = Mat();
    m_matHomogWorldToPix = Mat();
}
GC_STATUS Calib::Calibrate( const vector< Point2d > pixelPts, const vector< Point2d > worldPts,
                            const Size gridSize, const Size imgSize, const Mat &img, Mat &imgOut,
                            const bool drawCalib, const bool drawMoveROIs, const bool drawSearchROI )
{
    GC_STATUS retVal = GC_OK;
    if ( pixelPts.size() != worldPts.size() || pixelPts.empty() || worldPts.empty() ||
         gridSize.width * gridSize.height != static_cast< int >( pixelPts.size() ) || 0 >= gridSize.width )
    {
        FILE_LOG( logERROR ) << "[Calib::Calibrate] Calibration world/pixel coordinate point counts do not match or are empty";
        retVal = GC_ERR;
    }
    else
    {
        try
        {
            m_model.clear();
            m_model.imgSize = imgSize;
            m_model.gridSize = gridSize;
            m_model.pixelPoints.clear();
            m_model.worldPoints.clear();
            for ( size_t i = 0; i < pixelPts.size(); ++i )
            {
                m_model.pixelPoints.push_back( pixelPts[ i ] );
                m_model.worldPoints.push_back( worldPts[ i ] );
            }
            m_matHomogPixToWorld = findHomography( m_model.pixelPoints, m_model.worldPoints );
            m_matHomogWorldToPix = findHomography( m_model.worldPoints, m_model.pixelPoints );

            retVal = CalcSearchSwaths();
            if ( GC_OK == retVal )
            {
                m_model.moveSearchRegionLft = Rect( std::max( 0, cvRound( m_model.pixelPoints[ 0 ].x ) - GC_BOWTIE_TEMPLATE_DIM ),
                                           std::max( 0, cvRound( m_model.pixelPoints[ 0 ].y )- GC_BOWTIE_TEMPLATE_DIM ),
                                           std::min( imgSize.width - cvRound( m_model.pixelPoints[ 0 ].x ), GC_BOWTIE_TEMPLATE_DIM * 2 ),
                                           std::min( imgSize.height - cvRound( m_model.pixelPoints[ 0 ].y ), GC_BOWTIE_TEMPLATE_DIM * 2 ) );
                size_t idx = static_cast< size_t >( m_model.gridSize.width - 1 );
                m_model.moveSearchRegionRgt = Rect( std::max( 0, cvRound( m_model.pixelPoints[ idx ].x ) - GC_BOWTIE_TEMPLATE_DIM ),
                                           std::max( 0, cvRound( m_model.pixelPoints[ idx ].y )- GC_BOWTIE_TEMPLATE_DIM ),
                                           std::min( imgSize.width - cvRound( m_model.pixelPoints[ idx ].x ), GC_BOWTIE_TEMPLATE_DIM * 2 ),
                                           std::min( imgSize.height - cvRound( m_model.pixelPoints[ idx ].y ), GC_BOWTIE_TEMPLATE_DIM * 2 ) );

                int width = std::max( cvRound( m_model.pixelPoints[ idx ].x - m_model.pixelPoints[ 0 ].x ),
                                      cvRound( m_model.pixelPoints[ m_model.pixelPoints.size() - 1 ].x - m_model.pixelPoints[ m_model.pixelPoints.size() - 2 ].x ) );
                int height = std::max( cvRound( m_model.pixelPoints[ m_model.pixelPoints.size() - 2 ].y - m_model.pixelPoints[ 0 ].y ),
                                       cvRound( m_model.pixelPoints[ m_model.pixelPoints.size() - 1 ].y - m_model.pixelPoints[ idx ].y ) );
                int left = std::max( 0, cvRound( m_model.pixelPoints[ 0 ].x ) - width / 2 );
                int top = std::max( 0, cvRound( m_model.pixelPoints[ 0 ].y ) - height / 2 );

                width *= 2;
                height *= 2;
                if ( left + width > imgSize.width - 1 )
                {
                    width = imgSize.width - left - 1;
                }
                if ( top + height > imgSize.height - 1 )
                {
                    height = imgSize.height - top - 1;
                }

                m_model.wholeTargetRegion = Rect( left, top, width, height );
            }

            if ( ( drawCalib || drawMoveROIs || drawSearchROI ) && !img.empty() )
            {
                if ( CV_8UC1 == img.type() )
                {
                     cvtColor( img, imgOut, COLOR_GRAY2BGR );
                }
                else if ( CV_8UC3 == img.type() )
                {
                    imgOut = img.clone();
                }
                else
                {
                    FILE_LOG( logERROR ) << "[Calib::Calibrate] Invalid image format for calibration";
                    retVal = GC_ERR;
                }

                if ( GC_OK == retVal )
                {
                    int textOffset = cvRound( static_cast< double >( imgOut.rows ) / 6.6666667 );
                    int circleSize =  std::max( 5, cvRound( static_cast< double >( imgOut.rows ) / 120.0 ) );
                    int textStroke = std::max( 1, cvRound( static_cast< double >( imgOut.rows ) / 300.0 ) );
                    double fontScale = 1.0 + static_cast< double >( imgOut.rows ) / 1200.0;

                    if ( drawMoveROIs )
                    {
                        rectangle( imgOut, m_model.moveSearchRegionLft, Scalar( 0, 0, 255 ), textStroke );
                        rectangle( imgOut, m_model.moveSearchRegionRgt, Scalar( 0, 0, 255 ), textStroke );
                    }

                    if ( drawSearchROI )
                    {
                        if ( m_model.searchLines.empty() )
                        {
                            FILE_LOG( logWARNING ) << "[Calib::Calibrate] Search lines not calculated properly so they cannot be drawn";
                            retVal = GC_WARN;
                        }
                        else
                        {
                            line( imgOut, m_model.searchLines[ 0 ].top, m_model.searchLines[ 0 ].bot, Scalar( 255, 0, 0 ), textStroke );
                            line( imgOut, m_model.searchLines[ 0 ].top, m_model.searchLines[ m_model.searchLines.size() - 1 ].top, Scalar( 255, 0, 0 ), textStroke );
                            line( imgOut, m_model.searchLines[ m_model.searchLines.size() - 1 ].top, m_model.searchLines[ m_model.searchLines.size() - 1 ].bot, Scalar( 255, 0, 0 ), textStroke );
                            line( imgOut, m_model.searchLines[ 0 ].bot, m_model.searchLines[ m_model.searchLines.size() - 1 ].bot, Scalar( 255, 0, 0 ), textStroke );
                        }
                    }

                    if ( drawCalib )
                    {
                        Point2d topLft, botRgt;
                        retVal = PixelToWorld( m_model.pixelPoints[ 0 ], topLft );
                        if ( GC_OK == retVal )
                        {
                            retVal = PixelToWorld( m_model.pixelPoints[ m_model.pixelPoints.size() - 1 ], botRgt );
                            if ( GC_OK == retVal )
                            {
                                Point2d pt1, pt2;
                                double minCol = std::min( topLft.x, botRgt.x );
                                double maxCol = std::max( topLft.x, botRgt.x );
                                double minRow = std::min( topLft.y, botRgt.y );
                                double maxRow = std::max( topLft.y, botRgt.y );
                                double rowInc = ( maxRow - minRow ) / static_cast< double >( m_model.gridSize.height + 2 );
                                double colInc = ( maxCol - minCol ) / static_cast< double >( m_model.gridSize.width );
                                minRow -= rowInc;
                                maxRow += rowInc;
                                stringstream buf;

                                bool first;
                                double row, col;
                                int rowInt, colInt;
                                for ( rowInt = 0, row = maxRow; row > minRow; row -= rowInc, ++rowInt )
                                {
                                    first = true;
                                    for ( colInt = 0, col = minCol; col < maxCol; col += colInc, ++colInt )
                                    {
                                        retVal = WorldToPixel( Point2d( col, row ), pt1 );
                                        if ( GC_OK == retVal )
                                        {
                                            retVal = WorldToPixel( Point2d( col + colInc, row ), pt2 );
                                            if ( GC_OK == retVal )
                                            {
                                                line( imgOut, pt1, pt2, Scalar( 0, 255, 255 ), textStroke );
                                                retVal = WorldToPixel( Point2d( col, row - rowInc ), pt2 );
                                                if ( GC_OK == retVal && pt1.y < imgOut.rows )
                                                {
                                                    line( imgOut, pt1, pt2, Scalar( 0, 255, 255 ), textStroke );
                                                    if ( ( ( rowInt % 2 ) == 1 ) && ( ( colInt % 2 ) == 0 ) )
                                                        circle( imgOut, pt1, circleSize, Scalar( 0, 255, 0 ), textStroke );
                                                }
                                            }
                                        }
                                        if ( first )
                                        {
                                            first = false;
                                            buf.str( string() ); buf << boost::format( "%.1f" ) % row;
                                            putText( imgOut, buf.str(), Point( cvRound( pt1.x ) - textOffset, cvRound( pt1.y ) + 5 ),
                                                     FONT_HERSHEY_COMPLEX, fontScale * 0.5, Scalar( 0, 255, 255 ), textStroke );
                                        }
                                    }
                                    retVal = WorldToPixel( Point2d( maxCol, row ), pt1 );
                                    if ( GC_OK == retVal && pt1.y < imgOut.rows )
                                    {
                                        retVal = WorldToPixel( Point2d( maxCol, row - rowInc ), pt2 );
                                        if ( GC_OK == retVal )
                                        {
                                            line( imgOut, pt1, pt2, Scalar( 0, 255, 255 ), textStroke );
                                            if ( ( rowInt % 2 ) == 1 )
                                                circle( imgOut, pt1, circleSize, Scalar( 0, 255, 0 ), textStroke );
                                        }
                                    }
                                }
                                first = true;
                                for ( double col = minCol; col < maxCol; col += colInc )
                                {
                                    retVal = WorldToPixel( Point2d( col, minRow ), pt1 );
                                    if ( GC_OK == retVal )
                                    {
                                        retVal = WorldToPixel( Point2d( col + colInc, minRow ), pt2 );
                                        if ( GC_OK == retVal )
                                        {
                                            line( imgOut, pt1, pt2, Scalar( 0, 255, 255 ), textStroke );
                                        }
                                    }
                                    if ( first )
                                    {
                                        first = false;
                                        buf.str( string() ); buf << boost::format( "%.1f" ) % minRow;
                                        putText( imgOut, buf.str(), Point( cvRound( pt1.x ) - textOffset, cvRound( pt1.y ) + 5 ),
                                                 FONT_HERSHEY_COMPLEX, fontScale * 0.5, Scalar( 0, 255, 255 ), textStroke );
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }
        catch( Exception &e )
        {
            FILE_LOG( logERROR ) << "[" << __func__ << "] " << e.what();
            return GC_EXCEPT;
        }
    }
    return retVal;
}
GC_STATUS Calib::PixelToWorld( const Point2d ptPixel, Point2d &ptWorld )
{
    GC_STATUS retVal = GC_OK;

    if ( m_matHomogPixToWorld.empty() )
    {
        FILE_LOG( logERROR ) << "[Calib::PixelToWorld] No calibration for pixel to world conversion";
        retVal = GC_ERR;
    }
    else
    {
        try
        {
            vector< Point2d > vecIn, vecOut;
            vecIn.push_back( ptPixel );
            perspectiveTransform( vecIn, vecOut, m_matHomogPixToWorld );
            ptWorld = vecOut[ 0 ];
        }
        catch( Exception &e )
        {
            FILE_LOG( logERROR ) << "[Calib::PixelToWorld] " << e.what();
            retVal = GC_EXCEPT;
        }
    }

    return retVal;
}
GC_STATUS Calib::WorldToPixel( const Point2d ptWorld, Point2d &ptPixel )
{
    GC_STATUS retVal = GC_OK;
    if ( m_matHomogWorldToPix.empty() )
    {
        FILE_LOG( logERROR ) << "[Calib::WorldToPixel] No calibration for world to pixel conversion";
        retVal = GC_ERR;
    }
    else
    {
        try
        {
            vector< Point2d > vecIn, vecOut;
            vecIn.push_back( ptWorld );
            perspectiveTransform( vecIn, vecOut, m_matHomogWorldToPix );
            ptPixel = vecOut[ 0 ];
        }
        catch( Exception &e )
        {
            FILE_LOG( logERROR ) << "[Calib::WorldToPixel] " << e.what();
            retVal = GC_EXCEPT;
        }
    }

    return retVal;
}
cv::Rect Calib::MoveSearchROI( const bool isLeft )
{
    return isLeft ? m_model.moveSearchRegionLft :
                    m_model.moveSearchRegionRgt;
}
GC_STATUS Calib::MoveRefPoint( cv::Point2d &lftRefPt, cv::Point2d &rgtRefPt )
{
    GC_STATUS retVal = GC_OK;
    Point2d pt( numeric_limits< double >::min(), numeric_limits< double >::min() );
    if ( m_model.pixelPoints.empty() )
    {
        FILE_LOG( logERROR ) << "[Calib::MoveRefPoint] Cannot retrieve move reference point from an uncalibrated system";
        retVal = GC_ERR;
    }
    else if ( static_cast< size_t >( m_model.gridSize.width * m_model.gridSize.height ) != m_model.pixelPoints.size() )
    {
        FILE_LOG( logERROR ) << "[Calib::MoveRefPoint] Cannot retrieve move reference point with invalid calibration";
        retVal = GC_ERR;
    }
    else
    {
        lftRefPt = m_model.pixelPoints[ 0 ];
        rgtRefPt = m_model.pixelPoints[ static_cast< size_t >( m_model.gridSize.width - 1 ) ];
    }
    return retVal;
}
GC_STATUS Calib::Load( const string &jsonCalibString )
{
    GC_STATUS retVal = GC_OK;

    try
    {
        if ( jsonCalibString.empty() )
        {
            FILE_LOG( logERROR ) << "[Calib::Load] Bow tie calibration string is empty";
            retVal = GC_ERR;
        }
        else
        {
            stringstream ss;
            ss << jsonCalibString;
            property_tree::ptree ptreeTop;
            property_tree::json_parser::read_json( ss, ptreeTop );

            m_model.imgSize.width = ptreeTop.get< int >( "imageWidth", 0 );
            m_model.imgSize.height = ptreeTop.get< int >( "imageHeight", 0 );
            property_tree::ptree ptreeCalib = ptreeTop.get_child( "PixelToWorld" );

            Point2d ptTemp;
            size_t cols = ptreeCalib.get< size_t >( "columns", 2 );
            size_t rows = ptreeCalib.get< size_t >( "rows", 4 );
            m_model.pixelPoints.clear();
            m_model.worldPoints.clear();

            BOOST_FOREACH( property_tree::ptree::value_type &node, ptreeCalib.get_child( "points" ) )
            {
                ptTemp.x = node.second.get< double >( "pixelX", 0.0 );
                ptTemp.y = node.second.get< double >( "pixelY", 0.0 );
                m_model.pixelPoints.push_back( ptTemp );
                ptTemp.x = node.second.get< double >( "worldX", 0.0 );
                ptTemp.y = node.second.get< double >( "worldY", 0.0 );
                m_model.worldPoints.push_back( ptTemp );
            }

            const property_tree::ptree &ptreeMoveSearch = ptreeTop.get_child( "MoveSearchRegions" );
            property_tree::ptree::const_iterator end = ptreeMoveSearch.end();
            for ( property_tree::ptree::const_iterator iter = ptreeMoveSearch.begin(); iter != end; ++iter )
            {
                if ( iter->first == "Left" )
                {
                    m_model.moveSearchRegionLft.x =      iter->second.get< int >( "x", 0 );
                    m_model.moveSearchRegionLft.y =      iter->second.get< int >( "y", 0 );
                    m_model.moveSearchRegionLft.width =  iter->second.get< int >( "width", 0 );
                    m_model.moveSearchRegionLft.height = iter->second.get< int >( "height", 0 );
                }
                else if ( iter->first == "Right" )
                {
                    m_model.moveSearchRegionRgt.x =      iter->second.get< int >( "x", 0 );
                    m_model.moveSearchRegionRgt.y =      iter->second.get< int >( "y", 0 );
                    m_model.moveSearchRegionRgt.width =  iter->second.get< int >( "width", 0 );
                    m_model.moveSearchRegionRgt.height = iter->second.get< int >( "height", 0 );
                }
                else if ( iter->first == "Target" )
                {
                    m_model.wholeTargetRegion.x =      iter->second.get< int >( "x", 0 );
                    m_model.wholeTargetRegion.y =      iter->second.get< int >( "y", 0 );
                    m_model.wholeTargetRegion.width =  iter->second.get< int >( "width", 0 );
                    m_model.wholeTargetRegion.height = iter->second.get< int >( "height", 0 );
                }
            }

            Point ptTop, ptBot;
            m_model.searchLines.clear();
            BOOST_FOREACH( property_tree::ptree::value_type &node, ptreeTop.get_child( "SearchLines" ) )
            {
                ptTop.x = node.second.get< int >( "topX", std::numeric_limits< int >::min() );
                ptTop.y = node.second.get< int >( "topY", std::numeric_limits< int >::min() );
                ptBot.x = node.second.get< int >( "botX", std::numeric_limits< int >::min() );
                ptBot.y = node.second.get< int >( "botY", std::numeric_limits< int >::min() );
                m_model.searchLines.push_back( LineEnds( ptTop, ptBot ) );
            }

#ifdef LOG_CALIB_VALUES
            FILE_LOG( logINFO ) << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~";
            FILE_LOG( logINFO ) << "Camera calibration association points";
            FILE_LOG( logINFO ) << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~";
            FILE_LOG( logINFO ) << "Columns=" << cols << " Rows=" << rows;
            FILE_LOG( logINFO ) << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~";
#endif
            if ( cols * rows != m_model.pixelPoints.size() )
            {
                FILE_LOG( logERROR ) << "[Calib::Load] Invalid association point count";
                retVal = GC_ERR;
            }
            else
            {
#ifdef LOG_CALIB_VALUES
                for ( size_t i = 0; i < m_settings.pixelPoints.size(); ++i )
                {
                    FILE_LOG( logINFO ) << "[r=" << i / cols << " c=" << i % cols << "] " << \
                                           " pixelX=" << m_settings.pixelPoints[ i ].x << " pixelY=" << m_settings.pixelPoints[ i ].y << \
                                           " worldX=" << m_settings.worldPoints[ i ].x << " worldY=" << m_settings.worldPoints[ i ].y;
                }
#endif
                m_model.gridSize = Size( static_cast< int >( cols ), static_cast< int >( rows ) );

                Mat matIn, matOut;
                retVal = Calibrate( m_model.pixelPoints, m_model.worldPoints, m_model.gridSize, m_model.imgSize, matIn, matOut, false, false );
            }
#ifdef LOG_CALIB_VALUES
            FILE_LOG( logINFO ) << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" << endl;
            FILE_LOG( logINFO ) << "Search lines";
            FILE_LOG( logINFO ) << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~";
            for ( size_t i = 0; i < m_settings.searchLines.size(); ++i )
            {
                FILE_LOG( logINFO ) << "[index=" << i << "] " << \
                                       " topX=" << m_settings.searchLines[ i ].top.x << " topY=" << m_settings.searchLines[ i ].top.y << \
                                       " botX=" << m_settings.searchLines[ i ].bot.x << " botY=" << m_settings.searchLines[ i ].bot.y;
            }
#endif
        }
    }
    catch( boost::exception &e )
    {
        FILE_LOG( logERROR ) << "[Calib::Load] " << diagnostic_information( e );
        retVal = GC_EXCEPT;
    }

    return retVal;
}
GC_STATUS Calib::Save( const string jsonCalFilepath )
{
    GC_STATUS retVal = GC_OK;

    if ( m_model.pixelPoints.empty() || m_model.worldPoints.empty() ||
         m_model.pixelPoints.size() != m_model.worldPoints.size() ||
         2 > m_model.gridSize.width || 4 > m_model.gridSize.height || m_model.searchLines.empty() )
    {
        FILE_LOG( logERROR ) << "[Calib::Save] Invalid calib grid dimension(s) or empty cal point vector(s)";
        retVal = GC_ERR;
    }
    else
    {
        try
        {
            ofstream fileStream( jsonCalFilepath, ios::out );
            if ( fileStream.is_open() )
            {
                fileStream << "{" << endl;
                fileStream << "  \"calibType\":\"BowTie\"" << "," << endl;
                fileStream << "  \"imageWidth\":" << m_model.imgSize.width << "," << endl;
                fileStream << "  \"imageHeight\":" << m_model.imgSize.height << "," << endl;
                fileStream << "  \"PixelToWorld\": " << endl;
                fileStream << "  {" << endl;
                fileStream << "    \"columns\": " << m_model.gridSize.width << "," << endl;
                fileStream << "    \"rows\": " <<    m_model.gridSize.height << "," << endl;
                fileStream << "    \"points\": [" << endl;
                fileStream << fixed << setprecision( 3 );
                for ( size_t i = 0; i < m_model.pixelPoints.size() - 1; ++i )
                {
                    fileStream << "      { \"pixelX\": " << m_model.pixelPoints[ i ].x << ", " << \
                                          "\"pixelY\": " << m_model.pixelPoints[ i ].y << ", " << \
                                          "\"worldX\": " << m_model.worldPoints[ i ].x << ", " << \
                                          "\"worldY\": " << m_model.worldPoints[ i ].y << " }," << endl;
                }
                fileStream << "      { \"pixelX\": " << m_model.pixelPoints[ m_model.pixelPoints.size() - 1 ].x << ", " << \
                                      "\"pixelY\": " << m_model.pixelPoints[ m_model.pixelPoints.size() - 1 ].y << ", " << \
                                      "\"worldX\": " << m_model.worldPoints[ m_model.pixelPoints.size() - 1 ].x << ", " << \
                                      "\"worldY\": " << m_model.worldPoints[ m_model.pixelPoints.size() - 1 ].y << " }" << endl;
                fileStream << "    ]" << endl;
                fileStream << "  }," << endl;
                fileStream << "  \"MoveSearchRegions\": " << endl;
                fileStream << "  {" << endl;
                fileStream << fixed << setprecision( 0 );
                fileStream << "    \"Left\":  { " << \
                                    "\"x\": " <<      m_model.moveSearchRegionLft.x << ", " << \
                                    "\"y\": " <<      m_model.moveSearchRegionLft.y << ", " << \
                                    "\"width\": " <<  m_model.moveSearchRegionLft.width << ", " << \
                                    "\"height\": " << m_model.moveSearchRegionLft.height << " }, " << endl;
                fileStream << "    \"Right\": { " << \
                                    "\"x\": " <<      m_model.moveSearchRegionRgt.x << ", " << \
                                    "\"y\": " <<      m_model.moveSearchRegionRgt.y << ", " << \
                                    "\"width\": " <<  m_model.moveSearchRegionRgt.width << ", " << \
                                    "\"height\": " << m_model.moveSearchRegionRgt.height << " }," << endl;
                fileStream << "    \"Target\": { " << \
                                    "\"x\": " <<      m_model.wholeTargetRegion.x << ", " << \
                                    "\"y\": " <<      m_model.wholeTargetRegion.y << ", " << \
                                    "\"width\": " <<  m_model.wholeTargetRegion.width << ", " << \
                                    "\"height\": " << m_model.wholeTargetRegion.height << " }" << endl;
                fileStream << "  }," << endl;
                fileStream << "  \"SearchLines\": [" << endl;
                for ( size_t i = 0; i < m_model.searchLines.size() - 1; ++i )
                {
                    fileStream << "      { \"topX\": " << m_model.searchLines[ i ].top.x << ", " << \
                                          "\"topY\": " << m_model.searchLines[ i ].top.y << ", " << \
                                          "\"botX\": " << m_model.searchLines[ i ].bot.x << ", " << \
                                          "\"botY\": " << m_model.searchLines[ i ].bot.y << " }," << endl;
                }
                fileStream << "      { \"topX\": " << m_model.searchLines[ m_model.searchLines.size() - 1 ].top.x << ", " << \
                                      "\"topY\": " << m_model.searchLines[ m_model.searchLines.size() - 1 ].top.y << ", " << \
                                      "\"botX\": " << m_model.searchLines[ m_model.searchLines.size() - 1 ].bot.x << ", " << \
                                      "\"botY\": " << m_model.searchLines[ m_model.searchLines.size() - 1 ].bot.y << " }" << endl;
                fileStream << "  ]" << endl;
                fileStream << "}" << endl;
                fileStream.close();
            }
            else
            {
                FILE_LOG( logERROR ) << "[Calib::Save] Could not open calibration save file " << jsonCalFilepath;
                retVal = GC_ERR;
            }
        }
        catch( boost::exception &e )
        {
            FILE_LOG( logERROR ) << "[Calib::Save] " << diagnostic_information( e );
            retVal = GC_EXCEPT;
        }
    }

    return retVal;
}
GC_STATUS Calib::CalcSearchSwaths()
{
    GC_STATUS retVal = GC_OK;

    if ( m_model.pixelPoints.empty() || m_model.worldPoints.empty() ||
         m_model.pixelPoints.size() != m_model.worldPoints.size() ||
         2 > m_model.gridSize.width || 4 > m_model.gridSize.height )
    {
        FILE_LOG( logERROR ) << "[" << __func__ << "] Invalid calib grid dimension(s) or empty cal point vector(s)";
        retVal = GC_ERR;
    }
    else
    {
        try
        {
            int widthTop = cvRound( ( m_model.pixelPoints[ static_cast< size_t >( m_model.gridSize.width ) - 1 ].x - m_model.pixelPoints[ 0 ].x )  / 3.0 );
            double widthBot = ( m_model.pixelPoints[ static_cast< size_t >( m_model.gridSize.width * m_model.gridSize.height ) - 1 ].x -
                                m_model.pixelPoints[ static_cast< size_t >( m_model.gridSize.width * ( m_model.gridSize.height - 1 ) ) ].x ) / 3.0;
            int height = cvRound( ( m_model.pixelPoints[ static_cast< size_t >( m_model.gridSize.width * ( m_model.gridSize.height - 1 ) ) ].y - m_model.pixelPoints[ 0 ].y ) * 1.25 );
            double topLftX = m_model.pixelPoints[ 0 ].x + static_cast< double >( widthTop );
            double topLftY = m_model.pixelPoints[ 0 ].y - ( static_cast< double >( height ) / 8.0 ) + static_cast< double >( height >> 4 );
            double botLftX = m_model.pixelPoints[ static_cast< size_t >( m_model.gridSize.width * ( m_model.gridSize.height - 1 ) ) ].x + static_cast< double >( widthBot );
            double botLftY = m_model.pixelPoints[ static_cast< size_t >( m_model.gridSize.width * ( m_model.gridSize.height - 1 ) ) ].y + static_cast< double >( height ) / 8.0 + static_cast< double >( height >> 4 );
            botLftY = std::min( botLftY, static_cast< double >( m_model.imgSize.height - 1 ) );

            double xInc = 1.0;
            double xIncBot = widthBot / static_cast< double >( widthTop );
            double yInc = ( m_model.pixelPoints[ static_cast< size_t >( m_model.gridSize.width ) - 1 ].y - m_model.pixelPoints[ 0 ].y ) / ( static_cast< double >( height ) * 3.0 );

            m_model.searchLines.clear();
            Point2d ptTop = Point2d( topLftX, topLftY );
            Point2d ptBot = Point2d( botLftX, botLftY );
            for ( int i = 0; i <= widthTop; ++i )
            {
                m_model.searchLines.push_back( LineEnds( Point( cvRound( ptTop.x ), cvRound( ptTop.y ) ),
                                                         Point( cvRound( ptBot.x ), cvRound( ptBot.y ) ) ) );
                ptTop.x += xInc;
                ptTop.y += yInc;
                ptBot.x += xIncBot;
                ptBot.y += yInc;
            }
        }
        catch( boost::exception &e )
        {
            FILE_LOG( logERROR ) << "[Calib::CalcSearchSwaths] " << diagnostic_information( e );
            retVal = GC_EXCEPT;
        }
    }

    return retVal;
}
string Calib::ModelJsonString()
{
    stringstream ss;

    if ( m_model.pixelPoints.empty() || m_model.worldPoints.empty() ||
         m_model.pixelPoints.size() != m_model.worldPoints.size() ||
         2 > m_model.gridSize.width || 4 > m_model.gridSize.height || m_model.searchLines.empty() )
    {
        FILE_LOG( logERROR ) << "[Calib::SettingsJsonString] Invalid calib grid dimension(s) or empty cal point vector(s)";
    }
    else
    {
        try
        {
            ss << "{" << endl;
            ss << "  \"PixelToWorld\": " << endl;
            ss << "  {" << endl;
            ss << "    \"columns\": " << m_model.gridSize.width << "," << endl;
            ss << "    \"rows\": " <<    m_model.gridSize.height << "," << endl;
            ss << "    \"points\": [" << endl;
            for ( size_t i = 0; i < m_model.pixelPoints.size() - 1; ++i )
            {
                ss << "      { \"pixelX\": " << m_model.pixelPoints[ i ].x << ", " << \
                                      "\"pixelY\": " << m_model.pixelPoints[ i ].y << ", " << \
                                      "\"worldX\": " << m_model.worldPoints[ i ].x << ", " << \
                                      "\"worldY\": " << m_model.worldPoints[ i ].y << " }," << endl;
            }
            ss << "      { \"pixelX\": " << m_model.pixelPoints[ m_model.pixelPoints.size() - 1 ].x << ", " << \
                                  "\"pixelY\": " << m_model.pixelPoints[ m_model.pixelPoints.size() - 1 ].y << ", " << \
                                  "\"worldX\": " << m_model.worldPoints[ m_model.pixelPoints.size() - 1 ].x << ", " << \
                                  "\"worldY\": " << m_model.worldPoints[ m_model.pixelPoints.size() - 1 ].y << " }" << endl;
            ss << "    ]" << endl;
            ss << "  }," << endl;
            ss << "  \"MoveSearchRegions\": " << endl;
            ss << "  {" << endl;
            ss << "    \"Left\":  { " << \
                                "\"x\": " <<      m_model.moveSearchRegionLft.x << ", " << \
                                "\"y\": " <<      m_model.moveSearchRegionLft.y << ", " << \
                                "\"width\": " <<  m_model.moveSearchRegionLft.width << ", " << \
                                "\"height\": " << m_model.moveSearchRegionLft.height << " }, " << endl;
            ss << "    \"Right\": { " << \
                                "\"x\": " <<      m_model.moveSearchRegionRgt.x << ", " << \
                                "\"y\": " <<      m_model.moveSearchRegionRgt.y << ", " << \
                                "\"width\": " <<  m_model.moveSearchRegionRgt.width << ", " << \
                                "\"height\": " << m_model.moveSearchRegionRgt.height << " }" << endl;
            ss << "  }," << endl;
            ss << "  \"SearchLines\": [" << endl;
            for ( size_t i = 0; i < m_model.searchLines.size() - 1; ++i )
            {
                ss << "      { \"topX\": " << m_model.searchLines[ i ].top.x << ", " << \
                                      "\"topY\": " << m_model.searchLines[ i ].top.y << ", " << \
                                      "\"botX\": " << m_model.searchLines[ i ].bot.x << ", " << \
                                      "\"botY\": " << m_model.searchLines[ i ].bot.y << " }," << endl;
            }
            ss << "      { \"topX\": " << m_model.searchLines[ m_model.searchLines.size() - 1 ].top.x << ", " << \
                                  "\"topY\": " << m_model.searchLines[ m_model.searchLines.size() - 1 ].top.y << ", " << \
                                  "\"botX\": " << m_model.searchLines[ m_model.searchLines.size() - 1 ].bot.x << ", " << \
                                  "\"botY\": " << m_model.searchLines[ m_model.searchLines.size() - 1 ].bot.y << " }" << endl;
            ss << "  ]" << endl;
            ss << "}" << endl;
        }
        catch( boost::exception &e )
        {
            FILE_LOG( logERROR ) << "[Calib::SettingsJsonString] " << diagnostic_information( e );
        }
    }

    return ss.str();
}

}   // namespace gc


#define BOOST_TEST_MODULE
#include <boost/test/unit_test.hpp>
#include <core/util/test_macros.hpp>
#include <stdlib.h>
#include <vector>
#include <string>
#include <functional>
#include <random>
#include <cfenv>

#include <ml/ml_data/ml_data.hpp>
#include <ml/optimization/optimization_interface.hpp>
#include <ml/optimization/utils.hpp>
#include <toolkits/supervised_learning/linear_regression.hpp>
#include <toolkits/supervised_learning/linear_regression_opt_interface.hpp>
#include <core/storage/sframe_data/testing_utils.hpp>

using namespace turi;
using namespace turi::supervised;

void run_linear_regression_test(std::map<std::string, flexible_type> opts) {

  size_t examples = opts.at("examples");
  size_t features = opts.at("features");
  std::string target_column_name = "target";
  std::vector<std::string> column_names = {"user", "item"};

  // Answers
  // -----------------------------------------------------------------------
  DenseVector coefs(features+1);
  coefs.setRandom();

  // Feature names
  std::vector<std::string> feature_names;
  std::vector<flex_type_enum> feature_types;
  for(size_t i=0; i < features; i++){
    feature_names.push_back(std::to_string(i));
    feature_types.push_back(flex_type_enum::FLOAT);
  }

  // Data
  std::vector<std::vector<flexible_type>> y_data;
  std::vector<std::vector<flexible_type>> X_data;
  for(size_t i=0; i < examples; i++){
    DenseVector x(features);
    x.setRandom();
    std::vector<flexible_type> x_tmp;
    for(size_t k=0; k < features; k++){
      x_tmp.push_back(x(k));
    }

    // Compute the prediction for this
    double t = x.dot(coefs.segment(0, features)) + coefs(features);
    std::vector<flexible_type> y_tmp;
    y_tmp.push_back(t);

    X_data.push_back(x_tmp);
    y_data.push_back(y_tmp);
  }

  // Options
  std::map<std::string, flexible_type> options = {
    {"convergence_threshold", 1e-2},
    {"step_size", 1.0},
    {"lbfgs_memory_level", 3},
    {"max_iterations", 10},
    {"solver", "newton"},
    {"l1_penalty", 0.0},
    {"l2_penalty", 0.0}
  };

  // Make the data
  sframe X = make_testing_sframe(feature_names, feature_types, X_data);
  sframe y = make_testing_sframe({"target"}, {flex_type_enum::INTEGER}, y_data);

  std::shared_ptr<linear_regression> model;
  model.reset(new linear_regression);
  model->init(X,y);
  model->init_options(options);
  model->train();

  // Construct the ml_data
  ml_data data = model->construct_ml_data_using_current_metadata(X, y);
  ml_data valid_data;

  // Check coefficients & options
  // ----------------------------------------------------------------------
  DenseVector _coefs(features+1);
  model->get_coefficients(_coefs);
  TS_ASSERT(_coefs.size() == features + 1);

  std::map<std::string, flexible_type> _options;
  _options = model->get_current_options();
  for (auto& kvp: options){
    TS_ASSERT(_options[kvp.first] == kvp.second);
  }
  TS_ASSERT(model->is_trained() == true);

  // Check predictions
  // ----------------------------------------------------------------------
  std::vector<flexible_type> pred_margin;
  std::shared_ptr<sarray<flexible_type>> _pred_margin = model->predict(data);

  // Save predictions made by the model
  auto reader = _pred_margin->get_reader();
  reader->read_rows(0, examples, pred_margin);

  // Check that the predictions made by the model are right!
  for(size_t i=0; i < examples; i++){
    DenseVector x(features + 1);
    for(size_t k=0; k < features; k++){
      x(k) = X_data[i][k];
    }
    x(features) = 1;
    double t = x.dot(_coefs);
    double p = pred_margin[i];
    TS_ASSERT(abs(p - t) < 1e-5);
  }

  // Check save and load
  // ----------------------------------------------------------------------
  dir_archive archive_write;
  archive_write.open_directory_for_write("regr_linear_regression_tests");
  turi::oarchive oarc(archive_write);
  oarc << *model;
  archive_write.close();

  // Load it
  dir_archive archive_read;
  archive_read.open_directory_for_read("regr_linear_regression_tests");
  turi::iarchive iarc(archive_read);
  iarc >> *model;

  // Check RMSE has been saved
  // ----------------------------------------------------------------------
  variant_type variant_rmse = model->get_value_from_state("training_rmse");
  double rmse = variant_get_value<flexible_type>(variant_rmse).to<double>();
  TS_ASSERT(rmse >= 0);
  TS_ASSERT(rmse <= 1);

  // Check coefficients after saving and loading.
  // ----------------------------------------------------------------------
  DenseVector _coefs_after_load(features+1);
  model->get_coefficients(_coefs_after_load);
  TS_ASSERT(_coefs_after_load.size() == features + 1);
  TS_ASSERT(_coefs_after_load.isApprox(_coefs, 1e-5));
  _options = model->get_current_options();
  for (auto& kvp: options){
    TS_ASSERT(_options[kvp.first] == kvp.second);
  }
  TS_ASSERT(model->is_trained() == true);

  // Check coefficients after saving and loading.
  // ----------------------------------------------------------------------
  _pred_margin = model->predict(data);

  // Check that the predictions made by the model are right!
  for(size_t i=0; i < examples; i++){
    DenseVector x(features + 1);
    for(size_t k=0; k < features; k++){
      x(k) = X_data[i][k];
    }
    x(features) = 1;
    double t = x.dot(_coefs);
    double p = pred_margin[i];
    TS_ASSERT(abs(p - t) < 1e-5);
  }

  model->get_coefficients(_coefs);
  TS_ASSERT(_coefs.size() == features + 1);
  model.reset();
}

/**
 *  Check linear regression
*/
struct linear_regression_test  {

  public:

  void test_linear_regression_basic_2d() {
    std::map<std::string, flexible_type> opts = {
      {"examples", 100},
      {"features", 1}};
    run_linear_regression_test(opts);
  }

  void test_linear_regression_small() {
    std::map<std::string, flexible_type> opts = {
      {"examples", 1000},
      {"features", 10}};
    run_linear_regression_test(opts);
  }
};

void run_linear_regression_opt_interface_test(std::map<std::string,
    flexible_type> opts) {

  size_t examples = opts.at("examples");
  size_t features = opts.at("features");
  std::string target_column_name = "target";
  std::vector<std::string> column_names = {"user", "item"};

  // Answers
  // -----------------------------------------------------------------------
  DenseVector coefs(features+1);
  coefs.setRandom();

  // Feature names
  std::vector<std::string> feature_names;
  std::vector<flex_type_enum> feature_types;
  for(size_t i=0; i < features; i++){
    feature_names.push_back(std::to_string(i));
    feature_types.push_back(flex_type_enum::FLOAT);
  }

  // Data
  std::vector<std::vector<flexible_type>> y_data;
  std::vector<std::vector<flexible_type>> X_data;
  for(size_t i=0; i < examples; i++){
    DenseVector x(features);
    x.setRandom();
    std::vector<flexible_type> x_tmp;
    for(size_t k=0; k < features; k++){
      x_tmp.push_back(x(k));
    }

    // Compute the prediction for this
    double t = x.dot(coefs.segment(0, features)) + coefs(features);
    std::vector<flexible_type> y_tmp;
    y_tmp.push_back(t);

    X_data.push_back(x_tmp);
    y_data.push_back(y_tmp);
  }

  // Options
  std::map<std::string, flexible_type> options = {
    {"convergence_threshold", 1e-2},
    {"step_size", 1.0},
    {"lbfgs_memory_level", 3},
    {"max_iterations", 10},
    {"solver", "auto"},
  };

  std::map<std::string, flex_type_enum> options_type = {
    {"convergence_threshold", flex_type_enum::FLOAT},
    {"step_size", flex_type_enum::FLOAT},
    {"lbfgs_memory_level", flex_type_enum::INTEGER},
    {"max_iterations", flex_type_enum::INTEGER},
    {"solver", flex_type_enum::STRING},
  };

  // Construct the ml_data
  // Make the data
  sframe X = make_testing_sframe(feature_names, feature_types, X_data);
  sframe y = make_testing_sframe({"target"}, {flex_type_enum::INTEGER}, y_data);
  std::shared_ptr<linear_regression> model;
  model.reset(new linear_regression);
  model->init(X,y);

  ml_data data = model->construct_ml_data_using_current_metadata(X, y);
  ml_data valid_data;

  std::shared_ptr<linear_regression_opt_interface> lr_interface;
  lr_interface.reset(new linear_regression_opt_interface(data, valid_data, *model));

  // Check examples & variables.
  TS_ASSERT(lr_interface->num_variables() == features + 1);
  TS_ASSERT(lr_interface->num_examples() == examples);

  size_t variables = lr_interface->num_variables();
  for(size_t i=0; i < 10; i++){

    DenseVector point(variables);
    point.setRandom();

    // Check gradients, functions and hessians.
    DenseVector gradient(variables);
    double func_value;
    DenseMatrix hessian(variables, variables);

    func_value = lr_interface->compute_function_value(point);
    lr_interface->compute_gradient(point, gradient);
    lr_interface->compute_hessian(point, hessian);
    TS_ASSERT(check_gradient(*lr_interface, point, gradient));
    if( variables <= 2){
      TS_ASSERT(check_hessian(*lr_interface, point, hessian));
    }

    // Check first order & second order computations
    DenseVector _gradient(variables);
    double _func_value;
    DenseMatrix _hessian(variables, variables);

    lr_interface->compute_first_order_statistics(point, _gradient,
      _func_value);
    TS_ASSERT(abs(func_value - _func_value) < 1e-5);
    TS_ASSERT(gradient.isApprox(_gradient));
    lr_interface->compute_second_order_statistics(point, _hessian, _gradient,
      _func_value);
    TS_ASSERT(abs(func_value - _func_value) < 1e-5);
    TS_ASSERT(gradient.isApprox(_gradient));
    TS_ASSERT(hessian.isApprox(_hessian));

  }

  model.reset();
  lr_interface.reset();
}

/**
 *  Check linear regression opt interface
*/
struct linear_regression_opt_interface_test  {
  public:

  void test_linear_regression_opt_interface_basic_2d() {
    std::map<std::string, flexible_type> opts = {
      {"examples", 100},
      {"features", 1}};
    run_linear_regression_opt_interface_test(opts);
  }

  void test_linear_regression_opt_interface_small() {
    std::map<std::string, flexible_type> opts = {
      {"examples", 1000},
      {"features", 10}};
    run_linear_regression_opt_interface_test(opts);
  }

};

BOOST_FIXTURE_TEST_SUITE(_linear_regression_test, linear_regression_test)
BOOST_AUTO_TEST_CASE(test_linear_regression_basic_2d) {
  linear_regression_test::test_linear_regression_basic_2d();
}
BOOST_AUTO_TEST_CASE(test_linear_regression_small) {
  linear_regression_test::test_linear_regression_small();
}
BOOST_AUTO_TEST_SUITE_END()
BOOST_FIXTURE_TEST_SUITE(_linear_regression_opt_interface_test, linear_regression_opt_interface_test)
BOOST_AUTO_TEST_CASE(test_linear_regression_opt_interface_basic_2d) {
  linear_regression_opt_interface_test::test_linear_regression_opt_interface_basic_2d();
}
BOOST_AUTO_TEST_CASE(test_linear_regression_opt_interface_small) {
  linear_regression_opt_interface_test::test_linear_regression_opt_interface_small();
}
BOOST_AUTO_TEST_SUITE_END()


#include <boost/stl_interfaces/iterator_interface.hpp>

#include <algorithm>
#include <array>
#include <vector>

template <typename Pred>
struct filtered_int_iterator
    : boost::stl_interfaces::iterator_interface<
          filtered_int_iterator<Pred>, std::bidirectional_iterator_tag, int> {
  filtered_int_iterator() : it_(nullptr) {}
  filtered_int_iterator(int *it, int *last, Pred pred)
      : it_(it), last_(last), pred_(std::move(pred)) {
    it_ = std::find_if(it_, last_, pred_);
  }

  filtered_int_iterator &operator++() {
    it_ = std::find_if(std::next(it_), last_, pred_);
    return *this;
  }

  int *base() const { return it_; }

private:
  friend boost::stl_interfaces::access;
  int *&base_reference() noexcept { return it_; }
  int *base_reference() const noexcept { return it_; }

  int *it_;
  int *last_;
  Pred pred_;
};

template <typename Pred>
filtered_int_iterator<Pred> make_filtered_int_iterator(int *it, int *last, Pred pred) {
  return filtered_int_iterator<Pred>(it, last, std::move(pred));
}

int main() {
  std::array<int, 8> ints = {{0, 1, 2, 3, 4, 5, 6, 7}};
  int *const ints_first = ints.data();
  int *const ints_last = ints.data() + ints.size();

  auto even = [](int x) { return (x % 2) == 0; };
  auto first = make_filtered_int_iterator(ints_first, ints_last, even);
  auto last = make_filtered_int_iterator(ints_last, ints_last, even);

  std::vector<int> ints_copy;
  std::copy(first, last, std::back_inserter(ints_copy));
  return ints_copy == (std::vector<int>{0, 2, 4, 6}) ? 0 : 1;
}


//  Copyright (C) Eric Niebler 2008.
//  Copyright (C) Pieter Bastiaan Ober 2014.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/rolling_mean.hpp>
#include <sstream>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>

using namespace boost;
using namespace unit_test;
using namespace accumulators;

template<typename T>
void assert_is_double(T const &)
{
    BOOST_MPL_ASSERT((is_same<T, double>));
}

// test_rolling_mean_test_impl
// implements a test for window_size = 5
size_t window_size = 5;

template<typename accumulator_set_type>
void
test_rolling_mean_test_impl(accumulator_set_type& acc)
{
    acc(1);
    BOOST_CHECK_CLOSE(1., rolling_mean(acc), 1e-5);

    acc(2);
    BOOST_CHECK_CLOSE(1.5, rolling_mean(acc), 1e-5);

    acc(3);
    BOOST_CHECK_CLOSE(2., rolling_mean(acc), 1e-5);

    acc(4);
    BOOST_CHECK_CLOSE(2.5, rolling_mean(acc), 1e-5);

    acc(5);
    BOOST_CHECK_CLOSE(3., rolling_mean(acc), 1e-5);

    acc(6);
    BOOST_CHECK_CLOSE(4., rolling_mean(acc), 1e-5);

    acc(7);
    BOOST_CHECK_CLOSE(5., rolling_mean(acc), 1e-5);

    assert_is_double(rolling_mean(acc));
}

template<typename accumulator_set_type>
void
test_rolling_mean_unsigned_test_impl(accumulator_set_type& acc)
{
    acc(7U);
    BOOST_CHECK_CLOSE(7., rolling_mean(acc), 1e-5);

    acc(6U);
    BOOST_CHECK_CLOSE(6.5, rolling_mean(acc), 1e-5);

    acc(5U);
    BOOST_CHECK_CLOSE(6., rolling_mean(acc), 1e-5);

    acc(4U);
    BOOST_CHECK_CLOSE(5.5, rolling_mean(acc), 1e-5);

    acc(3U);
    BOOST_CHECK_CLOSE(5., rolling_mean(acc), 1e-5);

    acc(2U);
    BOOST_CHECK_CLOSE(4., rolling_mean(acc), 1e-5);

    acc(1U);
    BOOST_CHECK_CLOSE(3., rolling_mean(acc), 1e-5);

    assert_is_double(rolling_mean(acc));
}

///////////////////////////////////////////////////////////////////////////////
// test_persistency_impl
//
template<typename accumulator_set_type>
void test_persistency_impl(accumulator_set_type& acc)
{
    std::stringstream ss;
    {
        acc(1);
        acc(2);
        acc(3);
        acc(4);
        acc(5);
        acc(6);
        acc(7);
        BOOST_CHECK_CLOSE(5., rolling_mean(acc), 1e-5);
        boost::archive::text_oarchive oa(ss);
        acc.serialize(oa, 0);
    }
    // initialize from acc to make sure all values are passed
    accumulator_set_type other_acc = acc;
    // accumulate more, to make sure that deserialization set the right value
    // and not the copy ctor
    other_acc(100);
    other_acc(100);
    other_acc(100);
    other_acc(100);
    other_acc(100);
    boost::archive::text_iarchive ia(ss);
    other_acc.serialize(ia, 0);
    BOOST_CHECK_CLOSE(5., rolling_mean(other_acc), 1e-5);
}

///////////////////////////////////////////////////////////////////////////////
// test_rolling_mean
void test_rolling_mean()
{
    accumulator_set<int,stats<tag::immediate_rolling_mean> >
        acc_immediate_rolling_mean(tag::immediate_rolling_mean::window_size = window_size),
        acc_immediate_rolling_mean2(tag::immediate_rolling_mean::window_size = window_size, sample = 0);

    accumulator_set<int,stats<tag::rolling_mean(immediate)> >
       acc_immediate_rolling_mean3(tag::immediate_rolling_mean::window_size = window_size);

    accumulator_set<int,stats<tag::lazy_rolling_mean> >
        acc_lazy_rolling_mean(tag::lazy_rolling_mean::window_size = window_size),
        acc_lazy_rolling_mean2(tag::lazy_rolling_mean::window_size = window_size, sample = 0);

    accumulator_set<int,stats<tag::rolling_mean(lazy)> >
       acc_lazy_rolling_mean3(tag::lazy_rolling_mean::window_size = window_size);

    accumulator_set<int,stats<tag::rolling_mean> >
        acc_default_rolling_mean(tag::rolling_mean::window_size = window_size),
        acc_default_rolling_mean2(tag::rolling_mean::window_size = window_size, sample = 0);

    //// test the different implementations
    test_rolling_mean_test_impl(acc_lazy_rolling_mean);
    test_rolling_mean_test_impl(acc_default_rolling_mean);
    test_rolling_mean_test_impl(acc_immediate_rolling_mean);

    test_rolling_mean_test_impl(acc_lazy_rolling_mean2);
    test_rolling_mean_test_impl(acc_default_rolling_mean2);
    test_rolling_mean_test_impl(acc_immediate_rolling_mean2);

    test_rolling_mean_test_impl(acc_lazy_rolling_mean3);
    test_rolling_mean_test_impl(acc_immediate_rolling_mean3);

    //// test that the default implementation is the 'immediate' computation
    BOOST_REQUIRE(sizeof(acc_lazy_rolling_mean) != sizeof(acc_immediate_rolling_mean));
    BOOST_CHECK  (sizeof(acc_default_rolling_mean) == sizeof(acc_immediate_rolling_mean));

    //// test the equivalence of the different ways to indicate a feature
    BOOST_CHECK  (sizeof(acc_lazy_rolling_mean) == sizeof(acc_lazy_rolling_mean2));
    BOOST_CHECK  (sizeof(acc_lazy_rolling_mean) == sizeof(acc_lazy_rolling_mean3));
    BOOST_CHECK  (sizeof(acc_immediate_rolling_mean) == sizeof(acc_immediate_rolling_mean2));
    BOOST_CHECK  (sizeof(acc_immediate_rolling_mean) == sizeof(acc_immediate_rolling_mean3));

    //// test unsigned int with both implementations
    accumulator_set<unsigned int,stats<tag::immediate_rolling_mean> >
    acc_immediate_rolling_mean4(tag::immediate_rolling_mean::window_size = window_size),
    acc_immediate_rolling_mean5(tag::immediate_rolling_mean::window_size = window_size, sample = 0);

    test_rolling_mean_unsigned_test_impl(acc_immediate_rolling_mean4);
    test_rolling_mean_unsigned_test_impl(acc_immediate_rolling_mean5);
}

///////////////////////////////////////////////////////////////////////////////
// test_persistency
void test_persistency()
{
    accumulator_set<int,stats<tag::immediate_rolling_mean> >
        acc_immediate_rolling_mean(tag::immediate_rolling_mean::window_size = window_size),
        acc_immediate_rolling_mean2(tag::immediate_rolling_mean::window_size = window_size, sample = 0);

    accumulator_set<int,stats<tag::rolling_mean(immediate)> >
       acc_immediate_rolling_mean3(tag::immediate_rolling_mean::window_size = window_size);

    accumulator_set<int,stats<tag::lazy_rolling_mean> >
        acc_lazy_rolling_mean(tag::lazy_rolling_mean::window_size = window_size),
        acc_lazy_rolling_mean2(tag::lazy_rolling_mean::window_size = window_size, sample = 0);

    accumulator_set<int,stats<tag::rolling_mean(lazy)> >
       acc_lazy_rolling_mean3(tag::lazy_rolling_mean::window_size = window_size);

    accumulator_set<int,stats<tag::rolling_mean> >
        acc_default_rolling_mean(tag::rolling_mean::window_size = window_size),
        acc_default_rolling_mean2(tag::rolling_mean::window_size = window_size, sample = 0);

    //// test the different implementations
    test_persistency_impl(acc_lazy_rolling_mean);
    test_persistency_impl(acc_default_rolling_mean);
    test_persistency_impl(acc_immediate_rolling_mean);

    test_persistency_impl(acc_lazy_rolling_mean2);
    test_persistency_impl(acc_default_rolling_mean2);
    test_persistency_impl(acc_immediate_rolling_mean2);

    test_persistency_impl(acc_lazy_rolling_mean3);
    test_persistency_impl(acc_immediate_rolling_mean3);
}
///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("rolling mean test");

    test->add(BOOST_TEST_CASE(&test_rolling_mean));
    test->add(BOOST_TEST_CASE(&test_persistency));

    return test;
}


// Copyright Louis Dionne 2013-2017
// 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/hana/assert.hpp>
#include <boost/hana/greater.hpp>
#include <boost/hana/greater_equal.hpp>
#include <boost/hana/less.hpp>
#include <boost/hana/less_equal.hpp>
#include <boost/hana/pair.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/base.hpp>
#include <laws/orderable.hpp>
namespace hana = boost::hana;
using hana::test::ct_ord;

int main() {
    BOOST_HANA_CONSTANT_CHECK(
        hana::make_pair(ct_ord<1>{}, ct_ord<2>{}) <
        hana::make_pair(ct_ord<3>{}, ct_ord<2>{})
    );

    BOOST_HANA_CONSTANT_CHECK(
        hana::make_pair(ct_ord<1>{}, ct_ord<2>{}) <=
        hana::make_pair(ct_ord<3>{}, ct_ord<2>{})
    );

    BOOST_HANA_CONSTANT_CHECK(
        hana::make_pair(ct_ord<3>{}, ct_ord<2>{}) >=
        hana::make_pair(ct_ord<1>{}, ct_ord<2>{})
    );

    BOOST_HANA_CONSTANT_CHECK(
        hana::make_pair(ct_ord<3>{}, ct_ord<2>{}) >
        hana::make_pair(ct_ord<1>{}, ct_ord<2>{})
    );

    hana::test::TestOrderable<hana::pair_tag>{hana::make_tuple(
          hana::make_pair(ct_ord<3>{}, ct_ord<3>{})
        , hana::make_pair(ct_ord<3>{}, ct_ord<4>{})
        , hana::make_pair(ct_ord<4>{}, ct_ord<3>{})
        , hana::make_pair(ct_ord<4>{}, ct_ord<4>{})
    )};
}


// -----------------------------------------------------------------------
// RTToolbox - DKFZ radiotherapy quantitative evaluation library
//
// Copyright (c) German Cancer Research Center (DKFZ),
// Software development for Integrated Diagnostics and Therapy (SIDT).
// ALL RIGHTS RESERVED.
// See rttbCopyright.txt or
// http://www.dkfz.de/en/sidt/projects/rttb/copyright.html
//
// This software is distributed WITHOUT ANY WARRANTY; without even
// the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
// PURPOSE.  See the above copyright notices for more information.
//
//------------------------------------------------------------------------

#include "rttbGeometricInfo.h"

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

namespace rttb
{
	namespace core
	{

		void GeometricInfo::setSpacing(const SpacingVectorType3D& aSpacingVector)
		{
			_spacing = aSpacingVector;
		}

		const SpacingVectorType3D& GeometricInfo::getSpacing() const
		{
			return _spacing;
		}

		void GeometricInfo::setImagePositionPatient(const WorldCoordinate3D& aImagePositionPatient)
		{
			_imagePositionPatient = aImagePositionPatient;
		}

		const WorldCoordinate3D& GeometricInfo::getImagePositionPatient() const
		{
			return _imagePositionPatient;
		}

		void GeometricInfo::setOrientationMatrix(const OrientationMatrix& anOrientationMatrix)
		{
			_orientationMatrix = anOrientationMatrix;
			computeInvertOrientation();
		}

		bool GeometricInfo::computeInvertOrientation()
		{
			using pmatrix = boost::numeric::ublas::permutation_matrix<std::size_t>;

			boost::numeric::ublas::matrix<WorldCoordinate> A(_orientationMatrix);

			// create a permutation matrix for the LU-factorization
			pmatrix pm(A.size1());

			size_t res = boost::numeric::ublas::lu_factorize(A, pm);

			if (res != 0)
			{
				return false;
			}

			_invertedOrientationMatrix.assign(boost::numeric::ublas::identity_matrix<WorldCoordinate>
			                                  (A.size1()));

			// backsubstitute to get the inverse
			boost::numeric::ublas::lu_substitute(A, pm, _invertedOrientationMatrix);

			return true;
		}

		void GeometricInfo::setImageSize(const ImageSize& aSize)
		{
			setNumColumns(aSize(0));
			setNumRows(aSize(1));
			setNumSlices(aSize(2));
		}

		const ImageSize GeometricInfo::getImageSize() const
		{
			return ImageSize(static_cast<Index1D>(getNumColumns()), static_cast<Index1D>(getNumRows()), static_cast<Index1D>(getNumSlices()));
		}

		void GeometricInfo::setNumColumns(const VoxelGridDimensionType aValue)
		{
			_numberOfColumns = aValue;
		}

		const VoxelGridDimensionType GeometricInfo::getNumColumns() const
		{
			return _numberOfColumns;
		}

		void GeometricInfo::setNumRows(const VoxelGridDimensionType aValue)
		{
			_numberOfRows = aValue;
		}

		const VoxelGridDimensionType GeometricInfo::getNumRows() const
		{
			return _numberOfRows;
		}

		void GeometricInfo::setNumSlices(const VoxelGridDimensionType aValue)
		{
			_numberOfFrames = aValue;
		}

		const VoxelGridDimensionType GeometricInfo::getNumSlices() const
		{
			return _numberOfFrames;
		}

		bool operator==(const GeometricInfo& gInfo, const GeometricInfo& gInfo1)
		{
			return (gInfo.getImagePositionPatient() == gInfo1.getImagePositionPatient()
			        && gInfo.getOrientationMatrix() == gInfo1.getOrientationMatrix()
			        && gInfo.getSpacing() == gInfo1.getSpacing() && gInfo.getNumColumns() == gInfo1.getNumColumns()
			        && gInfo.getNumRows() == gInfo1.getNumRows() && gInfo.getNumSlices() == gInfo1.getNumSlices());
		}

		bool GeometricInfo::equalsAlmost(const GeometricInfo& another,
		                                 double errorConstantGI /*= 1e-5*/) const
		{
			return (getImagePositionPatient().equalsAlmost(another.getImagePositionPatient(), errorConstantGI)
			        && getOrientationMatrix().equalsAlmost(another.getOrientationMatrix(), errorConstantGI)
			        && getSpacing().equalsAlmost(another.getSpacing(), errorConstantGI)
			        && getNumColumns() == another.getNumColumns()
			        && getNumRows() == another.getNumRows() && getNumSlices() == another.getNumSlices());
		}

		bool GeometricInfo::worldCoordinateToContinuousIndex(const WorldCoordinate3D& aWorldCoordinate,
			ContinuousVoxelGridIndex3D& aIndex)
		const
		{
			WorldCoordinate3D distanceToIP;
			distanceToIP = aWorldCoordinate - _imagePositionPatient;

			boost::numeric::ublas::vector<WorldCoordinate> result = boost::numeric::ublas::prod(
			            _invertedOrientationMatrix,
			            distanceToIP);

			boost::numeric::ublas::vector<WorldCoordinate> resultS = boost::numeric::ublas::element_div(result,
			        _spacing);

			aIndex = ContinuousVoxelGridIndex3D(resultS(0), resultS(1), resultS(2));

            //if we convert ContinuousVoxelGridIndex3D (double) to VoxelGridIndex3D (unsigned int), we can't find out if it's negative.
            //So we have to check before.
            if (aIndex(0) < -0.5 || aIndex(1) < -0.5 || aIndex(2) < -0.5){
                return false;
            }
            else {
                //check if it is inside
                VoxelGridIndex3D indexInt = VoxelGridIndex3D(GridIndexType(aIndex(0) + 0.5),
                    GridIndexType(aIndex(1) + 0.5),
                    GridIndexType(aIndex(2) + 0.5));

                return isInside(indexInt);
            }
		}

		bool GeometricInfo::worldCoordinateToIndex(const WorldCoordinate3D& aWorldCoordinate,
		        VoxelGridIndex3D& aIndex)
		const
		{
			ContinuousVoxelGridIndex3D doubleIndex;
			bool inside = worldCoordinateToContinuousIndex(aWorldCoordinate, doubleIndex);

			aIndex = VoxelGridIndex3D(GridIndexType(doubleIndex(0)+0.5), GridIndexType(doubleIndex(1)+0.5),
			                          GridIndexType(doubleIndex(2)+0.5));

			return inside;
		}

		bool GeometricInfo::continuousIndexToWorldCoordinate(const ContinuousVoxelGridIndex3D& aIndex,
		        WorldCoordinate3D& aWorldCoordinate)
		const
		{

			boost::numeric::ublas::vector<WorldCoordinate> resultS = boost::numeric::ublas::element_prod(
			            aIndex,
			            _spacing);

			boost::numeric::ublas::vector<WorldCoordinate> result = boost::numeric::ublas::prod(
			            _orientationMatrix,
			            resultS);

			aWorldCoordinate = result + _imagePositionPatient;

            //if we convert ContinuousVoxelGridIndex3D (double) to VoxelGridIndex3D (unsigned int), we can't find out if it's negative.
            //So we have to check before.
            if (aIndex(0) < -0.5 || aIndex(1) < -0.5 || aIndex(2) < -0.5){
                return false;
            }
            else {
                VoxelGridIndex3D indexInt = VoxelGridIndex3D(GridIndexType(aIndex(0) + 0.5),
                    GridIndexType(aIndex(1) + 0.5),
                    GridIndexType(aIndex(2) + 0.5));
                return isInside(indexInt);
            }
		}

		bool GeometricInfo::indexToWorldCoordinate(const VoxelGridIndex3D& aIndex,
		        WorldCoordinate3D& aWorldCoordinate)
		const
		{
			ContinuousVoxelGridIndex3D indexDouble = ContinuousVoxelGridIndex3D(aIndex(0), aIndex(1),
			                                     aIndex(2));
			return continuousIndexToWorldCoordinate(indexDouble, aWorldCoordinate);
		}

		bool GeometricInfo::isInside(const VoxelGridIndex3D& aIndex) const
		{
			return (aIndex(0) >= 0 && aIndex(1) >= 0 && aIndex(2) >= 0
			        && aIndex(0) < static_cast<unsigned int>(_numberOfColumns)
			        && aIndex(1) < static_cast<unsigned int>(_numberOfRows)
			        && aIndex(2) < static_cast<unsigned int>(_numberOfFrames));
		}

		bool GeometricInfo::isInside(const WorldCoordinate3D& aWorldCoordinate) const
		{
			VoxelGridIndex3D currentIndex;
			return (worldCoordinateToIndex(aWorldCoordinate, currentIndex));
		}

		const GridSizeType GeometricInfo::getNumberOfVoxels() const
		{
			auto nVoxels = static_cast<GridSizeType>(_numberOfRows * _numberOfColumns * _numberOfFrames);
			return nVoxels;
		}

		bool GeometricInfo::convert(const VoxelGridID& gridID, VoxelGridIndex3D& gridIndex) const
		{
			if (validID(gridID))
			{
				gridIndex(0) = gridID % getNumColumns();
				VoxelGridID tempID = (gridID - gridIndex.x()) / getNumColumns();
				gridIndex(1) = tempID % getNumRows();
				gridIndex(2) = (tempID - gridIndex.y()) / getNumRows();
				return true;
			}

			return false;
		}

		bool GeometricInfo::convert(const VoxelGridIndex3D& gridIndex, VoxelGridID& gridID) const
		{
			if ((gridIndex.x() >= static_cast<unsigned int>(getNumColumns()))
			    || (gridIndex.y() >= static_cast<unsigned int>(getNumRows()))
			    || (gridIndex.z() >= static_cast<unsigned int>(getNumSlices())))
			{
				return false;
			}
			else
			{
				gridID = gridIndex.z() * getNumColumns() * getNumRows() + gridIndex.y() *
				         getNumColumns()
				         + gridIndex.x();
				return validID(gridID);
			}
		}

		bool GeometricInfo::validID(const VoxelGridID aID) const
		{
			return (aID >= 0 && aID < getNumberOfVoxels());
		}

		bool GeometricInfo::validIndex(const VoxelGridIndex3D& aIndex) const
		{
			VoxelGridID aID;

			if (!convert(aIndex, aID))
			{
				return false;
			}
			else
			{
				return validID(aID);
			}
		}

		std::ostream& operator<<(std::ostream& s, const GeometricInfo& anGeometricInfo)
		{
			s << "[ " << anGeometricInfo.getImagePositionPatient() << "; " <<
			  anGeometricInfo.getOrientationMatrix()
			  << "; " << anGeometricInfo.getSpacing() << "; " << "; " << anGeometricInfo.getNumColumns()
			  << "; " << anGeometricInfo.getNumRows() << "; " << anGeometricInfo.getNumSlices() << " ]";
			return s;
		}

	}//end namespace core
}//end namespace rttb


/* 2D_Enhancement_plugin.cpp
 * This is a test plugin, you can use it as a demo.
 * 2013-06-26 : by Zhi Zhou
 */

#include "v3d_message.h"
#include <vector>
#include "2D_Enhancement_plugin.h"
#include <QtGui>
#include <fstream>
#include <iostream>
#include <math.h>
#include <stdlib.h>
#include <vector>
#include "stackutil.h"
#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

Q_EXPORT_PLUGIN2(2D_Enhancement, Enhancement);
void processImage(V3DPluginCallback2 &callback, QWidget *parent);
bool processImage(const V3DPluginArgList & input, V3DPluginArgList & output);
template <class T> void selectiveEnhance(T* data1d,
                     V3DLONG *in_sz,
                     unsigned int c,
                     unsigned int WS,
                     T* &outimg);

QStringList Enhancement::menulist() const
{
	return QStringList()
		<<tr("selectiveEnhance")
		<<tr("about");
}

QStringList Enhancement::funclist() const
{
	return QStringList()
		<<tr("selectiveEnhance")
		<<tr("help");
}

void Enhancement::domenu(const QString &menu_name, V3DPluginCallback2 &callback, QWidget *parent)
{
	if (menu_name == tr("selectiveEnhance"))
	{
		processImage(callback,parent);
	}
	else
	{
		v3d_msg(tr("This is a test plugin, you can use it as a demo.. "
			"Developed by Zhi Zhou, 2013-06-26"));
	}
}

bool Enhancement::dofunc(const QString & func_name, const V3DPluginArgList & input, V3DPluginArgList & output, V3DPluginCallback2 & callback,  QWidget * parent)
{
	vector<char*> infiles, inparas, outfiles;
	if(input.size() >= 1) infiles = *((vector<char*> *)input.at(0).p);
	if(input.size() >= 2) inparas = *((vector<char*> *)input.at(1).p);
	if(output.size() >= 1) outfiles = *((vector<char*> *)output.at(0).p);

	if (func_name == tr("selectiveEnhance"))
	{
	}
	else if (func_name == tr("help"))
	{
	}
	else return false;
}

void processImage(V3DPluginCallback2 &callback, QWidget *parent)
{
    v3dhandle curwin = callback.currentImageWindow();
	if (!curwin)
	{
        QMessageBox::information(0, "", "You don't have any image open in the main window.");
		return;
	}

    Image4DSimple* p4DImage = callback.getImage(curwin);

	if (!p4DImage)
	{
		QMessageBox::information(0, "", "The image pointer is invalid. Ensure your data is valid and try again!");
		return;
	}

    unsigned char* data1d = p4DImage->getRawData();
    //V3DLONG totalpxls = p4DImage->getTotalBytes();
    V3DLONG pagesz = p4DImage->getTotalUnitNumberPerChannel();

    V3DLONG N = p4DImage->getXDim();
    V3DLONG M = p4DImage->getYDim();
    V3DLONG P = p4DImage->getZDim();
    V3DLONG sc = p4DImage->getCDim();

	//input
    bool ok4,ok1;
    unsigned int c = 1, w = 1;

	c = QInputDialog::getInteger(parent, "Channel",
												  "Enter channel NO:",
												  1, 1, sc, 1, &ok4);
    if(ok4)
    {

        w = QInputDialog::getInteger(parent, "Window size",
                                          "Enter window size  (# voxels):",
                                            1, 1, M, 1, &ok1);
    }
    else
        return;

     // filter
     V3DLONG in_sz[4];
     in_sz[0] = N; in_sz[1] = M; in_sz[2] = P; in_sz[4] = sc;

    ImagePixelType pixeltype = p4DImage->getDatatype();
    void* outimg = 0;
    switch (pixeltype)
    {
        case V3D_UINT8: selectiveEnhance(data1d, in_sz, c,w,(unsigned char* &)outimg); break;
        case V3D_UINT16: selectiveEnhance((unsigned short int *)data1d, in_sz, c,w, (unsigned short int* &)outimg); break;
        case V3D_FLOAT32: selectiveEnhance((float *)data1d, in_sz, c,w, (float* &)outimg);break;
        default: v3d_msg("Invalid data type. Do nothing."); return;
    }

     // display
     Image4DSimple * new4DImage = new Image4DSimple();
     new4DImage->setData((unsigned char *)outimg, V3DLONG(N), V3DLONG(M), V3DLONG(P), 1, pixeltype);
     v3dhandle newwin = callback.newImageWindow();
     callback.setImage(newwin, new4DImage);
     callback.setImageName(newwin, "Enhancement result");
     callback.updateImageWindow(newwin);

}

template <class T> void selectiveEnhance(T* data1d,
                     V3DLONG *in_sz,
                     unsigned int c,
                     unsigned int WS,
                     T* &outimg)
{

     V3DLONG N = in_sz[0];
     V3DLONG M = in_sz[1];
     V3DLONG P = in_sz[2];
     V3DLONG sc = in_sz[3];
     V3DLONG pagesz = N*M*P;
    // V3DLONG WS = 1;

		//declare temporary pointer
		T *pImage = new T [pagesz];
		if (!pImage)
		{
			printf("Fail to allocate memory.\n");
			return;
		}
		else
		{
			for(V3DLONG i=0; i<pagesz; i++)
				pImage[i] = 0;
		}
		for(V3DLONG iy = WS; iy < M-WS; iy++)
		{
			V3DLONG offsetj = iy*N;
			for(V3DLONG ix = WS; ix < N-WS; ix++)
			{

				float  fxx = data1d[offsetj+ix+WS]+ data1d[offsetj+ix-WS]- 2*data1d[offsetj+ix];
				float  fyy = data1d[(iy+WS)*N+ix]+data1d[(iy-WS)*N+ix]-2*data1d[offsetj+ix];
				float  fxy = 0.25*(data1d[(iy+WS)*N+ix+WS]+data1d[(iy-WS)*N+ix-WS]-data1d[(iy+WS)*N+ix-WS]-data1d[(iy-WS)*N+ix+WS]);
				/*float	K = 0.5*(fxx+fyy);
				float 	Q = sqrt(fxx*fyy-fxy*fxy);
		   		float 	a1 = K + sqrt(K*K-Q*Q);
				float 	a2 = K - sqrt(K*K-Q*Q);
*/
 				Matrix2f A;
				A << fxx,fxy,fxy,fyy;
  				SelfAdjointEigenSolver<Matrix2f> eigensolver(A, false);
                                float a1 = eigensolver.eigenvalues()(0);
	   			float a2 = eigensolver.eigenvalues()(1);

			 	if(abs(a1)<abs(a2))
				{
						float temp = a2;
					        a2 = a1;
						a1 = temp;
				}
				float output1 = 0;
				float output2 = 0;
				if(a1<0)
				{
				 	output1 = abs(a1)-abs(a2);
		 			//pImage[offsetj+ix]=output;
				//	if(ix == 71 && iy == 46)
				//   	 printf("fxx = %f, fyy= %f,fxy = %f, a1 = %f,a2 = %f, value = %f\n",fxx,fyy,fxy,a1,a2,output);
				}
				if(a1<0 && a2<0)
				{
				 	output2 = 0;// abs(a2)*abs(a2)/abs(a1);

				}
					pImage[offsetj+ix]=sqrt(pow(output1,2)+pow(output2,2));
			}
		}

		outimg = pImage;

}


#include <iomanip>
#include <random>
#include <chrono>
#include <string>
#include <vector>
#include <fstream>
#include <sstream>
#include <iostream>
#include <typeinfo>
#include <boost/lexical_cast.hpp>

using Clock = std::chrono::high_resolution_clock;
using std::cout;
using std::cerr;
using std::endl;

auto makeNumbers(const unsigned count)
{
  std::mt19937_64                  gen(42);
  //constexpr auto                   range = 1000l*1000l*1000l*1000l*1000l;
  //constexpr auto                   range = 1000*1000;
  constexpr auto                   range = 1;
  //constexpr auto                   range = 0.0001;
  std::uniform_real_distribution<> prng(-range, +range);

  std::vector<std::string> out;
  out.reserve(count);
  for(auto i=0u; i<count; ++i)
  {
    std::ostringstream oss;
    oss << std::fixed << std::setprecision(20) << prng(gen);
    out.push_back( oss.str() );
  }

  return out;
}

template<typename C, typename F>
void measure(C const& c, F&& convert)
{
  const auto start = Clock::now();

  double out = 0;
  for(const auto& e: c)
    out += convert(e);

  const auto stop = Clock::now();
  const auto us   = std::chrono::duration_cast<std::chrono::microseconds>( stop - start );

  if(false)
    for(auto const& str: { c.at(0).c_str(), "123456789.123456789", "1.2", "42", "0.13", "0.00000000001234" })
      cout << "example conversion: " << str << " -> " << std::setprecision(20) << convert(str) << endl;
  cout << "resulting value: " <<  std::setprecision(20) << out << endl;
  cout << "time: \t" << us.count() << "[us]" << endl;
}

template<typename C, typename F>
void writeConvertedToFile(C const& c, F&& convert, std::string const& path)
{
  std::ofstream of;
  of.open(path);
  if( not of.is_open() )
    throw std::runtime_error{"failed to open " + path + " file"};
  for(const auto& e: c)
    of << e << " -> " << convert(e) << endl;
}

double implBoost(std::string const& in)
{
  return boost::lexical_cast<double>(in);
}

double implScanf(std::string const& in)
{
  double out;
  sscanf( in.c_str(), "%lf", &out );
  return out;
}

template<typename T>
double implManualNoSign(std::string::const_iterator begin, const std::string::const_iterator end)
{
  auto it  = begin;
  T    pre = 0;

  for(; it!=end; ++it)
  {
    if( !isdigit(*it) )
      break;
    pre *= 10;
    pre += *it-'0';
  }
  if(it==end)
    return pre;

  if( *it!='.' )
    throw std::runtime_error{"1: NaN..."};
  ++it;

  T div  = 1;
  T post = 0;
  for(; it!=end; ++it)
  {
    if( not isdigit(*it) )
      throw std::runtime_error{"2: NaN..."};
    post *= 10;
    post += *it-'0';
    div  *= 10;
  }

  return double(pre) + double(post)/double(div);
}

template<typename T>
double implManual(std::string const& in)
{
  if( in.empty() )
    throw std::runtime_error{"3: NaN..."};

  switch(in[0])
  {
    case '-': return -1 * implManualNoSign<T>( begin(in)+1, end(in) );
    case '.':
    case '+': return +1 * implManualNoSign<T>( begin(in)+1, end(in) );
  }
  if( not isdigit(in[0]) )
    throw std::runtime_error{"4: NaN..."};

  return implManualNoSign<T>( begin(in), end(in) );
}

template<typename C>
void saveToFile(C const& c)
{
  std::ofstream of;
  of.open("data.bc");
  if( not of.is_open() )
    throw std::runtime_error{"failed to open file"};

  of << "#!/usr/bin/bc -ql" << endl;
  of << "scale = 100" << endl;
  of << "out = 0" << endl;
  for(auto& e: c)
    of << "out += " << e << endl;
  of << "out" << endl;
  of << "quit" << endl;
}

int main()
{
  cout << std::fixed;

  cout << "preparing data... ";
  cout.flush();
  const auto in = makeNumbers(1000*1000);
  //const auto in = makeNumbers(5);
  cout << "done!" << endl;

  cout << "writing to file... ";
  cout.flush();
  saveToFile(in);
  cout << "done!" << endl;
  cout << endl;

  cout << "MANUAL<uint64_t>:" << endl;
  measure(in, implManual<uint64_t>);
  writeConvertedToFile(in, implManual<uint64_t>, "manual_uint64_t.log");
  cout << endl;

  cout << "MANUAL<double>:" << endl;
  measure(in, implManual<double>);
  writeConvertedToFile(in, implManual<double>, "manual_double.log");
  cout << endl;

  cout << "SCANF:" << endl;
  measure(in, implScanf);
  writeConvertedToFile(in, implScanf, "scanf.log");
  cout << endl;

  cout << "BOOST:" << endl;
  measure(in, implBoost);
  writeConvertedToFile(in, implBoost, "boost.log");
  cout << endl;
}


#include <iostream>
#include <vector>
#include <memory>
#include <boost/functional/hash.hpp>

// value object -> has no identity -> all state
struct Point {
    int x, y;

    friend std::size_t hash_value(const Point &obj) {
        std::size_t seed = 0x725C686F;
        boost::hash_combine(seed, obj.x);
        boost::hash_combine(seed, obj.y);
        return seed;
    }
};

// value object
struct Line {
    Point start, end;

    friend std::size_t hash_value(const Line &obj) {
        std::size_t seed = 0x719E6B16;
        boost::hash_combine(seed, obj.start);
        boost::hash_combine(seed, obj.end);
        return seed;
    }
};

struct Polygon { // interface segregation principle
    virtual std::vector<Line>::iterator begin() = 0;

    virtual std::vector<Line>::iterator end() = 0;
};

struct Rectangle : public Polygon {
    Rectangle(int x, int y, int width, int height) {
        lines.emplace_back(Line{Point{x, y},
                                Point{x + width, y}});
        lines.emplace_back(Line{Point{x + width, y},
                                Point{x + width, y + height}});
        lines.emplace_back(Line{Point{x, y},
                                Point{x, y + height}});
        lines.emplace_back(Line{Point{x, y + height},
                                Point{x + width, y + height}});
    }

    std::vector<Line>::iterator begin() override {
        return lines.begin();
    }

    std::vector<Line>::iterator end() override {
        return lines.end();
    }

private:
    std::vector<Line> lines;
};

std::ostream &operator<<(std::ostream &out, const Point &point) {
    out << "Point [ x=" << point.x << ", y=" << point.y << " ]";
    return out;
}

void DrawPoints(std::vector<Point>::iterator start, std::vector<Point>::iterator end) {
    for (auto point = start; point != end; point++)
        std::cout << "Drawing " << *point << std::endl;
}

// Line -> Point
struct LineToPointCachingAdapter {
    LineToPointCachingAdapter(Line &line) {
        static boost::hash<Line> hash;
        line_hash = hash(line); // note: line_hash is a field!
        if (cache.find(line_hash) != cache.end()){
            std::cout << "Found in the line_hash " << line_hash << std::endl;
            return; // we already have it
        }
        int left = std::min(line.start.x, line.end.x);
        int right = std::max(line.start.x, line.end.x);
        int top = std::min(line.start.y, line.end.y);
        int bottom = std::max(line.start.y, line.end.y);
        int dx = right - left;
        int dy = line.end.y - line.start.y;

        if (dx == 0) {
            for (int y = top; y <= bottom; ++y) {
                points.emplace_back(Point{left, y});
            }
        } else if (dy == 0) {
            for (int x = left; x <= right; ++x) {
                points.emplace_back(Point{x, top});
            }
        }
        cache[line_hash] = points;
    }

    virtual std::vector<Point>::iterator begin() {
        return cache[line_hash].begin();
    }

    virtual std::vector<Point>::iterator end() {
        return cache[line_hash].end();
    }

private:
    std::vector<Point> points;
    static std::map<size_t, std::vector<Point>> cache;
    std::size_t line_hash;
};

std::map<size_t, std::vector<Point>> LineToPointCachingAdapter::cache;

int main() {
    std::vector<std::shared_ptr<Rectangle>> rectangles{
            std::make_shared<Rectangle>(10, 10, 5, 5),
            std::make_shared<Rectangle>(20, 20, 10, 10),
            std::make_shared<Rectangle>(20, 20, 10, 10),
            std::make_shared<Rectangle>(10, 10, 5, 5)
    };
    for (auto &rec : rectangles) {
        for (auto &line: *rec) {
            LineToPointCachingAdapter lpo{line};
            DrawPoints(lpo.begin(), lpo.end());
        }
    }

    return 0;
}


//Copyright (c) 2010 Daniel Pfeifer

#ifndef __VMML_MATRIX_TRAITS_HPP__
#define __VMML_MATRIX_TRAITS_HPP__

#include <vmmlib/matrix.hpp>
#include <boost/la/matrix_traits.hpp>

namespace boost
{
namespace la
{

template<size_t M, size_t N, typename T>
struct matrix_traits<vmml::matrix<M, N, T> >
{
	typedef vmml::matrix<M, N, T> matrix_type;

	static const int rows = M;
	static const int cols = N;

	typedef T scalar_type;

	template<int Row, int Col>
	static scalar_type r(const matrix_type& m)
	{
		BOOST_STATIC_ASSERT(Row >= 0);
		BOOST_STATIC_ASSERT(Row < rows);
		BOOST_STATIC_ASSERT(Col >= 0);
		BOOST_STATIC_ASSERT(Col < cols);
		return m.at(Col, Row);
	}

	template<int Row, int Col>
	static scalar_type& w(matrix_type& m)
	{
		BOOST_STATIC_ASSERT(Row >= 0);
		BOOST_STATIC_ASSERT(Row < rows);
		BOOST_STATIC_ASSERT(Col >= 0);
		BOOST_STATIC_ASSERT(Col < cols);
		return m.at(Col, Row);
	}

	static scalar_type ir(int row, int col, const matrix_type& m)
	{
		BOOST_ASSERT(row >= 0);
		BOOST_ASSERT(row < rows);
		BOOST_ASSERT(col >= 0);
		BOOST_ASSERT(col < cols);
		return m.at(col, row);
	}

	static scalar_type& iw(int row, int col, matrix_type& m)
	{
		BOOST_ASSERT(row >= 0);
		BOOST_ASSERT(row < rows);
		BOOST_ASSERT(col >= 0);
		BOOST_ASSERT(col < cols);
		return m.at(col, row);
	}
};

} // namespace la
} // namespace boost

#endif /* VMML_MATRIX_TRAITS_HPP */


#include "dynamics_engine.hpp"

#include <Eigen>
#include <cstdint>
#include <cmath>
#include <queue>
#include <vector>
#include <thread>

/* class DynamicsEngine */
double DynamicsEngine::totalPotentialEnergy(const Eigen::Ref<const Eigen::Matrix3Xd>& x,
                                            const Eigen::Ref<const Eigen::RowVectorXd>& m) {
    // Iterate through each pair of distinct objects and adds their potential energy to a sum.
    double sumPotentialEnergies = 0;
    for (int i = 0; i < x.cols(); ++i) {
        for (int j = 0; j < x.cols(); ++j) {
            if (i == j) continue;
            sumPotentialEnergies += this->pairPotentialEnergy(x.col(i), x.col(j), m(i), m(j));
        }
    }
    return sumPotentialEnergies;
}

double DynamicsEngine::pairPotentialEnergy(const Eigen::Vector3d& x_i,
                                           const Eigen::Vector3d& x_j,
                                           double m_i,
                                           double m_j) {
    // Returns zero as a placeholder.
    // Should be overriden by subclasses to get actual potential energy.
    return 0;
}

/* class Abstract_Direct */

void Abstract_Direct::updateAccelerations(Eigen::Ref<Eigen::Matrix3Xd> a,
                                          const Eigen::Ref<const Eigen::Matrix3Xd>& x,
                                          const Eigen::Ref<const Eigen::RowVectorXd>& m) {
    // Iterate through each pair of objects and compute gravitational force
    uint64_t n = x.cols();
    for (uint64_t i = 0; i < n; ++i) {
        a.col(i).setZero(); // Clear net acceleration

        for (uint64_t j = 0; j < n; ++j) {
            if (i == j) continue;
            this->pairAcceleration(a.col(i), x.col(i), x.col(j), m(i), m(j)); // Force computation
        }
    }
}

/* class Abstract_BarnesHut */

Abstract_BarnesHut::Abstract_BarnesHut(double theta)
: root(nullptr)
, theta(theta) {}

Abstract_BarnesHut::~Abstract_BarnesHut() {
    // this->root guaranteed pointing to OctreeNode or nullptr
    delete this->root;
}

void Abstract_BarnesHut::threadUpdateAccelerations(Eigen::Ref<Eigen::Matrix3Xd> a,
                                                   const Eigen::Ref<const Eigen::Matrix3Xd>& x,
                                                   const Eigen::Ref<const Eigen::RowVectorXd>& m,
                                                   int startIdx, int endIdx) {
    for (int i = startIdx; i < endIdx; ++i) {
        // Set acceleration to zero
        a.col(i).setZero();

        // Iterate through tree using breadth-first traversal
        std::queue<OctreeNode*> bfsQ;
        bfsQ.push(this->root);
        while (bfsQ.size() > 0) {
            // Get current node
            OctreeNode* currNode = bfsQ.front();
            bfsQ.pop();

            // Compute s/d
            double s = currNode->xMax - currNode->xMin;
            double d = (x.col(i) - currNode->centerOfMass).norm();
            if (s/d < theta || currNode->isExternal) {
                // Current node is sufficiently far away from the current object
                this->pairAcceleration(a.col(i), x.col(i), currNode->centerOfMass, m(i), currNode->totalMass); // Force computation
            } else {
                // Current node not sufficiently far from the current object
                // Add currNode's children to queue
                for (int z = 0; z < 2; ++z) {
                    for (int y = 0; y < 2; ++y) {
                        for (int x = 0; x < 2; ++x) {
                            OctreeNode* child = currNode->children[z][y][x];
                            if (!child->isEmpty) bfsQ.push(child);
                        }
                    }
                }
            }
        }
    }
}

void Abstract_BarnesHut::updateAccelerations(Eigen::Ref<Eigen::Matrix3Xd> a,
                                             const Eigen::Ref<const Eigen::Matrix3Xd>& x,
                                             const Eigen::Ref<const Eigen::RowVectorXd>& m) {
    delete this->root;

    // Get octree root bounds and construct octree root
    Eigen::Matrix<double, 3, 1> minPos = x.rowwise().minCoeff();
    Eigen::Matrix<double, 3, 1> maxPos = x.rowwise().maxCoeff();
    Eigen::Matrix<double, 3, 1> widths = maxPos - minPos;
    double rootWidth = widths.maxCoeff();

    this->root = new OctreeNode(minPos(0), minPos(0) + rootWidth,
                                minPos(1), minPos(1) + rootWidth,
                                minPos(2), minPos(2) + rootWidth);

    // Construct Barnes-Hut tree
    for (int i = 0; i < x.cols(); ++i) {
        this->root->addObject(m(i), x.col(i));
    }

    // Compute accelerations
    int nThreads = std::thread::hardware_concurrency();
    int width = x.cols() / nThreads;
    int remainder = x.cols() % nThreads;
    std::vector<std::thread> threads;
    for (int i = 0; i < nThreads; ++i) {
        int startIdx = i*width;
        int endIdx = (i + 1)*width;
        if (i == nThreads - 1) {
            endIdx += remainder; // Add remaining indices to final thread
        }

        threads.emplace_back(&Abstract_BarnesHut::threadUpdateAccelerations, this, a, x, m, startIdx, endIdx);
    }

    for (int i = 0; i < threads.size(); ++i) {
        threads[i].join();
    }
}

/* class Gravitational_Direct */

Gravitational_Direct::Gravitational_Direct(double softening, unit_t l, unit_t m, unit_t t)
: softening(softening)
, G(6.67430e-11/l/l/l*m*t*t) {}

void Gravitational_Direct::pairAcceleration(Eigen::Ref<Eigen::Vector3d> a_i,
                                            const Eigen::Vector3d& x_i,
                                            const Eigen::Vector3d& x_j,
                                            double m_i, double m_j) {
    Eigen::VectorXd dx = x_i - x_j;
    a_i += -G*m_j*(dx)/std::pow(dx.squaredNorm() + softening*softening, 1.5);
}

double Gravitational_Direct::pairPotentialEnergy(const Eigen::Vector3d& x_i,
                                                 const Eigen::Vector3d& x_j,
                                                 double m_i, double m_j) {
    return -G*m_j*m_i/2.0/(x_j - x_i).norm();
}

/* class Gravitational_BarnesHut */

Gravitational_BarnesHut::Gravitational_BarnesHut(double theta, double softening, unit_t l, unit_t m, unit_t t)
: Abstract_BarnesHut(theta)
, softening(softening)
, G(6.67430e-11/l/l/l*m*t*t) {}

void Gravitational_BarnesHut::pairAcceleration(Eigen::Ref<Eigen::Vector3d> a_i,
                                               const Eigen::Vector3d& x_i,
                                               const Eigen::Vector3d& x_j,
                                               double m_i, double m_j) {
    Eigen::VectorXd dx = x_i - x_j;
    a_i += -G*m_j*(dx)/std::pow(dx.squaredNorm() + softening*softening, 1.5);
}

double Gravitational_BarnesHut::pairPotentialEnergy(const Eigen::Vector3d& x_i,
                                                    const Eigen::Vector3d& x_j,
                                                    double m_i, double m_j) {
    return -G*m_j*m_i/2.0/(x_j - x_i).norm();
}