#include "log.h"
#include "findstaffgauge.h"
#include <opencv2/imgproc.hpp>
#include <opencv2/calib3d.hpp>

using namespace cv;
using namespace std;

#ifndef __DEBUG_STAFFGAUGE
#define __DEBUG_STAFFGAUGE
#define SET_DEBUG_FOLDER
#endif

#ifdef __DEBUG_STAFFGAUGE_TEMPLATES
#undef __DEBUG_STAFFGAUGE_TEMPLATES
#undef SET_DEBUG_FOLDER
#endif

#ifdef SET_DEBUG_FOLDER
#include <opencv2/imgcodecs.hpp>
#include <boost/filesystem.hpp>
using namespace boost;
namespace fs = filesystem;
#ifdef WIN32
static const string "c:/gaugecam/debug"
#else
static const string DEBUG_FOLDER = "/var/tmp/water/";
#endif
#endif

static double DistToLine( const cv::Point2d pt, const cv::Point2d lnPt1, const cv::Point2d lnPt2 )
{
    double dNum = fabs( ( lnPt2.x - lnPt1.x ) * ( lnPt1.y - pt.y ) - ( lnPt1.x - pt.x ) * ( lnPt2.y - lnPt1.y ) );
    double dDenom = sqrt( ( lnPt2.x - lnPt1.x ) * ( lnPt2.x - lnPt1.x ) + ( lnPt2.y - lnPt1.y ) * ( lnPt2.y - lnPt1.y ) );
    return 0.0 == dDenom ? 0.0 : dNum / dDenom;
}

namespace gc
{

FindStaffGauge::FindStaffGauge()
{
    if ( !fs::exists( DEBUG_FOLDER ) )
    {
        bool bRet = fs::create_directories( DEBUG_FOLDER );
        if ( !bRet )
        {
            FILE_LOG( logERROR ) << "[FindStaffGauge::FindStaffGauge] Could not create debug folder " << DEBUG_FOLDER;
        }
    }
}
GC_STATUS FindStaffGauge::FindTicks( const cv::Mat &img, StaffGaugeTickType tickType )
{
    GC_STATUS retVal = GC_OK;
    try
    {
        retVal = CreateTemplates( tickType );
        if ( GC_OK == retVal )
        {
            int numToFind = -1;
            if ( BLACK_BOTTOM_LEFT_CORNER == tickType )
                numToFind = TICK_TARGET_COUNT_LEFT;
            else if ( BLACK_TOP_RIGHT_POINT == tickType )
                numToFind = TICK_TARGET_COUNT_RIGHT;
            retVal = FindTemplates( img, TICK_TEMPL_MATCH_MIN_SCORE, numToFind );
        }
    }
    catch( const Exception &e )
    {
        FILE_LOG( logERROR ) << "[FindStaffGauge::FindTicks] " << e.what();
        retVal = GC_EXCEPT;
    }

    return retVal;
}
GC_STATUS FindStaffGauge::Find( const cv::Mat &img, cv::Point2d ptTopTickPos, const double distTickToTick,
                                const std::vector< double > tickLengths )
{
    GC_STATUS retVal = GC_OK;
    try
    {
        Mat img8u;
        if ( img.type() == CV_8UC3 )
            cvtColor( img, img8u, COLOR_BGR2GRAY );
        else
            img8u = img;

        retVal = FindTicks( img8u, BLACK_TOP_RIGHT_POINT );
        if ( GC_OK == retVal )
        {
            retVal = CalcWorldPts( img8u, ptTopTickPos, distTickToTick, tickLengths );
#if 1 // test the calibration
            vector< Point2d > worldPts;
            worldPts.push_back( Point2d( 1.428571429, 24.0 ) );
            worldPts.push_back( Point2d( 0.0, 23.0 ) );
            worldPts.push_back( Point2d( 0.0, 22.0 ) );
            worldPts.push_back( Point2d( 0.0, 21.0 ) );
            worldPts.push_back( Point2d( 0.0, 20.0 ) );
            worldPts.push_back( Point2d( 0.0, 19.0 ) );
            worldPts.push_back( Point2d( 1.428571429, 18.0 ) );
            worldPts.push_back( Point2d( 0.0, 17.0 ) );
            worldPts.push_back( Point2d( 0.0, 16.0 ) );
            worldPts.push_back( Point2d( 0.0, 15.0 ) );
            worldPts.push_back( Point2d( 0.0, 14.0 ) );
            worldPts.push_back( Point2d( 0.0, 13.0 ) );
            worldPts.push_back( Point2d( 0.0, 11.0 ) );
            worldPts.push_back( Point2d( 0.0, 10.0 ) );
            worldPts.push_back( Point2d( 0.0, 9.0 ) );
            worldPts.push_back( Point2d( 0.0, 8.0 ) );
            worldPts.push_back( Point2d( 0.0, 7.0 ) );
            worldPts.push_back( Point2d( 1.428571429, 6.0 ) );
            worldPts.push_back( Point2d( 0.0, 5.0 ) );
            worldPts.push_back( Point2d( 0.0, 4.0 ) );
            worldPts.push_back( Point2d( 0.0, 3.0 ) );
            worldPts.push_back( Point2d( 0.0, 2.0 ) );
            worldPts.push_back( Point2d( 0.0, 1.0 ) );
            worldPts.push_back( Point2d( 1.2, 0.0 ) );
            worldPts.push_back( Point2d( 0.0, -3.0 ) );
            worldPts.push_back( Point2d( 0.0, -4.0 ) );
            Mat homogPixToWorld = findHomography( pixelPts, worldPts );
            Mat homogWorldToPix = findHomography( worldPts, pixelPts );

            Mat color;
            if ( img.type() == CV_8UC3 )
                img.copyTo( color );
            else
                cvtColor( img, color, COLOR_GRAY2BGR );

//            retVal = FindTicks( img8u, BLACK_BOTTOM_LEFT_CORNER );
//            if ( GC_OK == retVal )
//            {

//            }
            Point2d pt1, pt2;
            vector< Point2d > vecIn, vecOut;
            double y_pos = 24.0;
            for ( size_t i = 0; i < 25; ++i )
            {
                vecIn.clear();
                vecIn.push_back( Point2d( -1.428571429 * 2.0, y_pos ) );
                perspectiveTransform( vecIn, vecOut, homogWorldToPix );
                pt1 = vecOut[ 0 ];
                vecIn.clear();
                vecIn.push_back( Point2d( 1.428571429 * 2.0, y_pos ) );
                perspectiveTransform( vecIn, vecOut, homogWorldToPix );
                pt2 = vecOut[ 0 ];

                line( color, pt1, pt2, Scalar( 0, 255, 255 ), 1 );

                y_pos -= 1.0;
            }

            double x_pos = -1.428571429 * 2.0;
            for ( size_t i = 0; i < 5; ++i )
            {
                vecIn.clear();
                vecIn.push_back( Point2d( x_pos, 24.0 ) );
                perspectiveTransform( vecIn, vecOut, homogWorldToPix );
                pt1 = vecOut[ 0 ];
                vecIn.clear();
                vecIn.push_back( Point2d( x_pos, 0.0 ) );
                perspectiveTransform( vecIn, vecOut, homogWorldToPix );
                pt2 = vecOut[ 0 ];

                line( color, pt1, pt2, Scalar( 0, 255, 255 ), 1 );

                x_pos += 1.428571429;
            }

            imwrite( "/var/tmp/water/staffgauge_calib.png", color );
#endif
        }
    }
    catch( const Exception &e )
    {
        FILE_LOG( logERROR ) << "[FindStaffGauge::Find] " << e.what();
        retVal = GC_EXCEPT;
    }

    return retVal;
}
GC_STATUS FindStaffGauge::CalcWorldPts( const cv::Mat &img, cv::Point2d ptTopTickPos, const double distTickToTick,
                                        const std::vector< double > tickLengthsLowToHigh )
{
    GC_STATUS retVal = GC_OK;
    try
    {
        if ( TICK_POINT_COUNT_MIN > m_matchItems.size() )
        {
            FILE_LOG( logERROR ) << "[FindStaffGauge::CalcWorldPts] Too few tick points found";
            retVal = GC_ERR;
        }
        else
        {
            Point2d pt;
            vector< Point2d > intersectPts;
            for ( size_t i = 0; i < m_matchItems.size(); ++i )
            {

            }
        }
    }
    catch( const Exception &e )
    {
        FILE_LOG( logERROR ) << "[FindStaffGauge::CalcWorldPts] " << e.what();
        retVal = GC_EXCEPT;
    }

    return retVal;
}
GC_STATUS FindStaffGauge::CreateTemplates( const StaffGaugeTickType tickType )
{
    GC_STATUS retVal = GC_OK;
    try
    {
        Mat matTemp;
        int tempDim, templateDimEven, templateDim = -1;
        if ( BLACK_BOTTOM_LEFT_CORNER == tickType )
        {
            templateDim = 14;
            templateDimEven = templateDim + ( templateDim % 2 );
            tempDim = templateDimEven << 1;
            matTemp = Mat::zeros( Size( tempDim, tempDim ), CV_8UC1 );
            imwrite( "/var/tmp/water/corner_template_000.png", matTemp );
            matTemp( Rect( 0, 0, ( tempDim >> 1 ), tempDim ) ).setTo( 255 );
            imwrite( "/var/tmp/water/corner_template_001.png", matTemp );
            matTemp( Rect( 0, ( tempDim >> 1 ), tempDim, ( tempDim >> 1 ) ) ).setTo( 255 );
            matTemp = ~matTemp;
            imwrite( "/var/tmp/water/corner_template.png", matTemp );
        }
        else if ( BLACK_TOP_RIGHT_POINT == tickType )
        {
            templateDim = 14;
            templateDimEven = templateDim + ( templateDim % 2 );
            tempDim = templateDimEven << 1;
            matTemp = Mat::ones( Size( tempDim, tempDim ), CV_8UC1 );
            Mat matDraw = matTemp( Rect( tempDim >> 2, tempDim >> 2, tempDim >> 1, tempDim >> 1 ) );
            vector< Point > contour = { Point( 0, matDraw.rows >> 1 ),
                                        Point( matDraw.cols >> 1, matDraw.rows >> 1 ),
                                        Point( 0, matDraw.rows - 1 ),
                                        Point( 0, matDraw.rows >> 1 ) };
            drawContours( matDraw, vector< vector< Point > >( 1, contour ), -1, Scalar( 255 ), FILLED );
            matTemp = ~matTemp;
#ifdef __DEBUG_STAFFGAUGE_TEMPLATE
            imwrite( "/var/tmp/water/base_templ.png", matTemp );
#endif
        }
        else
        {
            FILE_LOG( logERROR ) << "[FindStaffGauge::CreateTemplates] Invalid tick type";
            retVal = GC_ERR;
        }

        if ( GC_OK == retVal )
        {
            int center = TICK_TEMPL_COUNT / 2;

            // create templates
            m_templates.clear();

            Mat matTempRot( Size( tempDim, tempDim ), CV_8U );

            for ( int i = 0; i < TICK_TEMPL_COUNT; ++i )
                m_templates.push_back( Mat( Size( templateDimEven, templateDimEven ), CV_8U ) );

            // create the rotated templates
            Rect roiRotate( templateDimEven >> 1, templateDimEven >> 1, templateDimEven, templateDimEven );
            Mat matTemplateTemp = matTemp( roiRotate );
            matTemplateTemp.copyTo( m_templates[ static_cast< size_t >( center ) ] );

            for ( int i = 0; i < center; ++i )
            {
                retVal = RotateImage( matTemp, matTempRot, static_cast< double >( i - center ) );
                if ( GC_OK != retVal )
                    break;

                matTemplateTemp = matTempRot( roiRotate );
                matTemplateTemp.copyTo( m_templates[ static_cast< size_t >( i ) ] );

                retVal = RotateImage( matTemp, matTempRot, static_cast< double >( i + 1 ) );
                if ( 0 != retVal )
                    break;

                matTemplateTemp = matTempRot( roiRotate );
                matTemplateTemp.copyTo( m_templates[ static_cast< size_t >( center + i + 1 ) ] );
            }
#ifdef __DEBUG_STAFFGAUGE_TEMPLATES
            for ( size_t i = 0; i < m_templates.size(); ++i )
                imwrite( DEBUG_FOLDER + "template_" + to_string( i ) + ".png", m_templates[ i ] );
#endif

            // allocate template match space
            m_matchSpace = Mat();
            m_matchSpaceSmall = Mat();
        }
    }
    catch( const Exception &e )
    {
        FILE_LOG( logERROR ) << "[FindStaffGauge::CreateBlackBotLftCornerTemplates] " << e.what();
        retVal = GC_EXCEPT;
    }

    return retVal;
}
GC_STATUS FindStaffGauge::RotateImage( const Mat &src, Mat &dst, const double angle )
{
    GC_STATUS retVal = GC_OK;
    try
    {
        Point2d ptCenter = Point2d( static_cast< double >( src.cols ) / 2.0, static_cast< double >( src.rows ) / 2.0 );
        Mat matRotMatrix = getRotationMatrix2D( ptCenter, angle, 1.0 );
        warpAffine( src, dst, matRotMatrix, dst.size(), INTER_CUBIC );
    }
    catch( Exception &e )
    {
        FILE_LOG( logERROR ) << "[FindStaffGauge::RotateImage] " << e.what();
        retVal = GC_EXCEPT;
    }
    return retVal;
}
GC_STATUS FindStaffGauge::FindTemplates( const Mat &img, const double minScore, const int targetCount, const string resultFilepath )
{
    GC_STATUS retVal = GC_OK;
    try
    {
        if ( m_templates.empty() )
        {
            FILE_LOG( logERROR ) << "[FindStaffGauge::FindTemplates] Templates not devined";
            retVal = GC_ERR;
        }
        else if ( img.empty() )
        {
            FILE_LOG( logERROR ) << "[FindStaffGauge::FindTemplates] Cannot find targets in a NULL image";
            retVal = GC_ERR;
        }
        if ( 0.01 > minScore || 1.0 < minScore )
        {
            FILE_LOG( logERROR ) << "[FindStaffGauge::FindTemplates] Invalid minimum target score " << minScore;
            retVal = GC_ERR;
        }
        else
        {
            retVal = MatchTemplate( m_templates.size() >> 1, img, minScore, targetCount * 2 );
            if ( GC_OK == retVal )
            {
                vector< TickItem > itemsTemp;
                for ( size_t i = 0; i < m_matchItems.size(); ++i )
                    itemsTemp.push_back( m_matchItems[ i ] );

                m_matchItems.clear();
                for ( size_t i = 0; i < itemsTemp.size(); ++i )
                {
                    for ( size_t j = 0; j < m_templates.size(); ++j )
                    {
                        retVal = MatchRefine( static_cast< int >( j ), img, minScore, 1, itemsTemp[ i ] );
                        if ( GC_OK != retVal )
                            break;
                    }
                    if ( GC_OK != retVal )
                        break;
                    m_matchItems.push_back( itemsTemp[ i ] );
                }

                if ( TICK_POINT_COUNT_MIN > m_matchItems.size() )
                {
                    FILE_LOG( logERROR ) << "[FindStaffGauge::FindTemplates] Ticks found=" << m_matchItems.size()
                                         << ". Should be at least " << TICK_POINT_COUNT_MIN;
                    retVal = GC_ERR;
                }
                else
                {
                    pixelPts.clear();
                    for ( size_t i = 0; i < m_matchItems.size(); ++i )
                    {
                        pixelPts.push_back( m_matchItems[ i ].pt );
                    }
                    retVal = FindTickTipLine( img, pixelPts, linePt1, linePt2 );
                    if ( GC_OK == retVal )
                    {
                        for ( size_t i = 0; i < m_matchItems.size(); ++i )
                        {
                            m_matchItems[ i ].x_length = DistToLine( m_matchItems[ i ].pt, linePt1, linePt2 );
                        }
                    }

                    if ( !resultFilepath.empty() )
                    {
                        Mat color;
                        cvtColor( img, color, COLOR_GRAY2BGR );
                        for ( size_t i = 0; i < m_matchItems.size(); ++i )
                        {
                            line( color, Point( m_matchItems[ i ].pt.x - 5, m_matchItems[ i ].pt.y ),
                                    Point( m_matchItems[ i ].pt.x + 5, m_matchItems[ i ].pt.y ), Scalar( 0, 0, 255 ) );
                            line( color, Point( m_matchItems[ i ].pt.x,  m_matchItems[ i ].pt.y - 5 ),
                                    Point( m_matchItems[ i ].pt.x, m_matchItems[ i ].pt.y + 5 ), Scalar( 0, 0, 255 ) );
                        }
                        if ( GC_OK == retVal )
                        {
                            line( color, linePt1, linePt2, Scalar( 0, 255, 255 ), 1 );
                        }

                        bool isOK = imwrite( resultFilepath, color );
                        if ( !isOK )
                        {
                            FILE_LOG( logERROR ) << "[" << __func__ << "[FindStaffGauge::FindTargets]"
                                                                       " Could not save result calib grid find to cache";
                            retVal = GC_ERR;
                        }
                    }
#ifdef DEBUG_FIND_CALIB_GRID   // output template matches to CSV file
                    FILE *fp = fopen( ( DEBUG_RESULT_FOLDER + "matches.csv" ).c_str(), "w" );
                    if ( nullptr != fp )
                    {
                        fprintf( fp, "Score, X, Y\n" );
                        for ( size_t i = 0; i < m_matchItems.size(); ++i )
                        {
                            fprintf( fp, "%.3f, %.3f, %.3f\n", m_matchItems[ i ].score,
                                     m_matchItems[ i ].pt.x, m_matchItems[ i ].pt.y );
                        }
                        fclose( fp );
                    }
#endif
                }
            }
        }
    }
    catch( Exception &e )
    {
        FILE_LOG( logERROR ) << "[FindStaffGauge::RotateImage] " << e.what();
        retVal = GC_EXCEPT;
    }
    return retVal;
}
GC_STATUS FindStaffGauge::MatchRefine( const int index, const cv::Mat &img, const double minScore,
                                       const int numToFind, TickItem &item )
{
    GC_STATUS retVal = GC_OK;

    if ( 0 > index || static_cast< int >( m_templates.size() ) <= index )
    {
        FILE_LOG( logERROR ) << "[FindStaffGauge::MatchRefine]"
                                                   " Attempted to find template index=" << index << \
                                                   " Must be in range 0-" << m_templates.size() - 1;
        retVal = GC_ERR;
    }
    else if ( 0.05 > minScore || 1.0 < minScore )
    {
        FILE_LOG( logERROR ) << "[FindStaffGauge::MatchRefine]  Min score %.3f must be in range 0.05-1.0" << minScore;
        retVal = GC_ERR;
    }
    else if ( 1 > numToFind || 1000 < numToFind )
    {
        FILE_LOG( logERROR ) << "[FindStaffGauge::MatchRefine]"
                                                   " Attempted to find " << numToFind << \
                                                   " matches.  Must be in range 1-1000";
        retVal = GC_ERR;
    }
    else
    {
        try
        {
            Rect rect;
            double minScore, maxScore;
            Point ptMin, ptMax;
            Point2d ptFinal;

            m_matchSpace = 0;
            TickItem itemTemp;

            matchTemplate( img, m_templates[ static_cast< size_t >( index ) ], m_matchSpace, TM_CCOEFF_NORMED );
#ifdef DEBUG_FIND_CALIB_GRID
            Mat matTemp;
            normalize( m_matchSpace, matTemp, 255.0 );
            imwrite( DEBUG_RESULT_FOLDER + "bowtie_match_fine.png", matTemp );
#endif
            rect.x = std::max( 0, cvRound( item.pt.x )- ( m_templates[ 0 ].cols >> 1 ) - ( m_templates[ 0 ].cols >> 2 ) );
            rect.y = std::max( 0, cvRound( item.pt.y ) - ( m_templates[ 0 ].rows >> 1 ) - ( m_templates[ 0 ].rows >> 2 ) );
            rect.width = m_templates[ 0 ].cols + ( m_templates[ 0 ].cols >> 1 );
            rect.height = m_templates[ 0 ].rows + ( m_templates[ 0 ].rows >> 1 );
            if ( rect.x + rect.width >= img.cols )
                rect.x = img.cols - rect.width;
            if ( rect.y + rect.height >= img.rows )
                rect.y = img.rows - rect.height;

            Mat matROI = img( rect );
            m_matchSpaceSmall = 0;

            matchTemplate( matROI, m_templates[ static_cast< size_t >( index ) ], m_matchSpaceSmall, TM_CCOEFF_NORMED );
            minMaxLoc( m_matchSpaceSmall, &minScore, &maxScore, &ptMin, &ptMax );
            if ( maxScore > item.score )
            {
                retVal = SubpixelPointRefine( m_matchSpaceSmall, ptMax, ptFinal );
                if ( GC_OK == retVal )
                {
                    // ptFinal = Point2d( static_cast< double >( ptMax.x ), static_cast< double >( ptMax.y ) );
                    item.score = maxScore;
                    item.pt.x = static_cast< double >( rect.x ) + ptFinal.x + static_cast< double >( m_templates[ 0 ].cols ) / 2.0;
                    item.pt.y = static_cast< double >( rect.y ) + ptFinal.y + static_cast< double >( m_templates[ 0 ].rows ) / 2.0;
                }
                else
                {
                    item.score = 0.0;
                    item.pt.x = static_cast< double >( rect.x ) + ptMax.x + static_cast< double >( m_templates[ 0 ].cols ) / 2.0;
                    item.pt.y = static_cast< double >( rect.y ) + ptMax.y + static_cast< double >( m_templates[ 0 ].rows ) / 2.0;
                    retVal = GC_OK;
                }
            }
        }
        catch( Exception &e )
        {
            FILE_LOG( logERROR ) << "[FindStaffGauge::MatchRefine] " << e.what();
            retVal = GC_EXCEPT;
        }
    }
    return retVal;
}
GC_STATUS FindStaffGauge::MatchTemplate( const int index, const Mat &img, const double minScore, const int numToFind )
{
    GC_STATUS retVal = GC_OK;

    if ( 0 > index || static_cast< int >( m_templates.size() ) <= index )
    {
        FILE_LOG( logERROR ) << "[FindStaffGauge::MatchTemplate]"
                                " Attempted to find template index=" << index << \
                                " Must be in range 0-" << m_templates.size() - 1;
        retVal = GC_ERR;
    }
    else if ( 0.05 > minScore || 1.0 < minScore )
    {
        FILE_LOG( logERROR ) << "[FindStaffGauge::MatchTemplate]"
                                " Min score %.3f must be in range 0.05-1.0" << minScore;
        retVal = GC_ERR;
    }
    else if ( 1 > numToFind || 1000 < numToFind )
    {
        FILE_LOG( logERROR ) << "[FindStaffGauge::MatchTemplate]"
                                                   " Attempted to find " << numToFind << \
                                                   " matches.  Must be in range 1-1000";
        retVal = GC_ERR;
    }
    else
    {
        try
        {
            Rect rect;
            double dMin, dMax;
            Point ptMin, ptMax;
            Point2d ptFinal;
            TickItem itemTemp;

            m_matchSpace = 0.0;
            m_matchItems.clear();
            matchTemplate( img, m_templates[ static_cast< size_t >( index ) ], m_matchSpace, cv::TM_CCOEFF_NORMED );

            // TODO: Debug here for find move target FAIL
#ifdef DEBUG_FIND_CALIB_GRID
            Mat matTemp;
            normalize( m_matchSpace, matTemp, 255.0 );
            imwrite( DEBUG_RESULT_FOLDER + "bowtie_match_original.png", img );
            imwrite( DEBUG_RESULT_FOLDER + "bowtie_match_coarse.png", matTemp );
            imwrite( DEBUG_RESULT_FOLDER + "bowtie_match_coarse_double.tiff", m_matchSpace );
#endif

            for ( int i = 0; i < numToFind; i++ )
            {
                minMaxLoc( m_matchSpace, &dMin, &dMax, &ptMin, &ptMax );
                if (  0 < ptMax.x && 0 < ptMax.y && img.cols - 1 > ptMax.x && img.rows - 1 > ptMax.y )
                {
                    if ( dMax >= minScore )
                    {
                        itemTemp.score = dMax;
                        itemTemp.pt.x = static_cast< double >( ptMax.x ) + static_cast< double >( m_templates[ 0 ].cols ) / 2.0;
                        itemTemp.pt.y = static_cast< double >( ptMax.y ) + static_cast< double >( m_templates[ 0 ].rows ) / 2.0;
                        m_matchItems.push_back( itemTemp );
                    }
                    else
                        break;
                }
                circle( m_matchSpace, ptMax, 17, Scalar( 0.0 ), FILLED );
            }
            if ( m_matchItems.empty() )
            {
                FILE_LOG( logERROR ) << "[FindStaffGauge::MatchTemplate] No template matches found";
                retVal = GC_ERR;
            }
            else
            {
                sort( m_matchItems.begin(), m_matchItems.end(), []( const TickItem &a, const TickItem &b ) { return a.pt.y < b.pt.y; } );
                for ( size_t i = 1; i < m_matchItems.size(); ++i )
                {
                    m_matchItems[ i ].y_interval = m_matchItems[ i ].pt.y - m_matchItems[ i - 1 ].pt.y;
                }
            }
        }
        catch( Exception &e )
        {
            FILE_LOG( logERROR ) << "[FindStaffGauge::MatchTemplate] " << e.what();
            return GC_EXCEPT;
        }
    }
    return retVal;
}
GC_STATUS FindStaffGauge::SubpixelPointRefine( const Mat &matchSpace, const Point ptMax, Point2d &ptResult )
{
    GC_STATUS retVal = GC_OK;
    if ( 1 > ptMax.x || 1 > ptMax.y || matchSpace.cols - 2 < ptMax.x || matchSpace.rows - 2 < ptMax.y )
    {
#ifdef DEBUG_FIND_CALIB_GRID
        FILE_LOG( logWARNING ) << "[FindStaffGauge::SubpixelPointRefine] Invalid point (not on image) for subpixel refinement";
#endif
        retVal = GC_WARN;
    }
    else if ( CV_32FC1 != matchSpace.type() )
    {
        FILE_LOG( logERROR ) << "[FindStaffGauge::SubpixelPointRefine] Invalid image format for subpixel refinement";
        retVal = GC_ERR;
    }
    else
    {
        try
        {
            float val;
            int col, row;
            float total = 0.0f;
            float totalX = 0.0f;
            float totalY = 0.0f;
            for ( row = ptMax.y - 1; row < ptMax.y + 2; ++row )
            {
                for ( col = ptMax.x - 1; col < ptMax.x + 2; ++col )
                {
                    val = matchSpace.at< float >( Point( col, row ) );
                    total += val;
                    totalX += val * static_cast< float >( col );
                    totalY += val * static_cast< float >( row );
                }
            }
            ptResult = Point2d( static_cast< double >( totalX / total ),
                                static_cast< double >( totalY / total ) );
        }
        catch( Exception &e )
        {
            FILE_LOG( logERROR ) << "[FindCalibGrid::SubpixelPointRefine] " << e.what();
            retVal = GC_EXCEPT;
        }
    }
    return retVal;
}
GC_STATUS FindStaffGauge::FindTickTipLine( const cv::Mat &img, const std::vector< cv::Point2d > pts, cv::Point2d &pt1, cv::Point2d &pt2 )
{
    GC_STATUS retVal = GC_OK;
    try
    {
        Vec4d lne;
        fitLine( pts, lne, DIST_L12, 0.0, 0.01, 0.01 );

        // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
        // line equation: y = mx + b
        // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
        Point2d pt1x0, pt1y0, pt2x0, pt2y0;

        double a = lne[ 1 ];
        double b = -lne[ 0 ];
        double c = lne[ 0 ] * lne[ 3 ] - lne[ 1 ] * lne[ 2 ];

        double denom = ( 0.0 == a ? std::numeric_limits< double >::epsilon() : a );
        pt1y0.y = 0.0;
        pt1y0.x = c / -denom;
        pt2y0.y = static_cast< double >( img.rows - 1 );
        pt2y0.x = ( b * pt2y0.y + c ) / -denom;

        denom = ( 0.0 == b ? std::numeric_limits< double >::epsilon() : b );
        pt1x0.x = 0.0;
        pt1x0.y = c / -denom;
        pt2x0.x = static_cast< double >( img.cols - 1 );
        pt2x0.y = ( a * pt2x0.x + c ) / -denom;

        if ( 0.0 <= pt1y0.x && 0.0 <= pt1y0.y && img.cols > pt1y0.x && img.rows > pt1y0.y )
        {
            pt1 = pt1y0;
        }
        else
        {
            pt1 = pt1x0;
        }

        if ( 0.0 <= pt2y0.x && 0.0 <= pt2y0.y && img.cols > pt2y0.x && img.rows > pt2y0.y )
        {
            pt2 = pt2y0;
        }
        else
        {
            pt2 = pt2x0;
        }
    }
    catch( Exception &e )
    {
        FILE_LOG( logERROR ) << "[FindCalibGrid::FindTickTipLine] " << e.what();
        retVal = GC_EXCEPT;
    }

    return retVal;
}
GC_STATUS FindStaffGauge::GetClosestPointOnLine( const cv::Point2d linePt1, const cv::Point2d linePt2,
                                                 const cv::Point2d pt, cv::Point &ptOnLine )
{
    GC_STATUS retVal = GC_OK;
    try
    {
        double APx = pt.x - linePt1.x;
        double APy = pt.y - linePt1.y;
        double ABx = linePt2.x - linePt1.x;
        double ABy = linePt2.y - linePt1.y;
        double magAB2 = ABx * ABx + ABy * ABy;
        double ABdotAP = ABx * APx + ABy * APy;
        double t = ABdotAP / magAB2;

        if ( 0 > t )
        {
            ptOnLine = linePt1;
        }
        else if ( 1 < t )
        {
            ptOnLine = linePt2;
        }
        else
        {
            ptOnLine.x = linePt1.x + ABx * t;
            ptOnLine.y = linePt2.y + ABy * t;
        }
    }
    catch( Exception &e )
    {
        FILE_LOG( logERROR ) << "[FindCalibGrid::FindTickTipLine] " << e.what();
        retVal = GC_EXCEPT;
    }

    return retVal;
}

} // namespace gc


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_SDK_COMPLEX_DETAILS_DRY_META_HPP_INCLUDED
#define NT2_SDK_COMPLEX_DETAILS_DRY_META_HPP_INCLUDED

#include <nt2/sdk/complex/hierarchy.hpp>
#include <nt2/sdk/complex/meta/real_of.hpp>
#include <boost/simd/sdk/simd/meta/as_simd.hpp>
#include <boost/dispatch/meta/property_of.hpp>
#include <boost/dispatch/meta/hierarchy_of.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>
#include <boost/type_traits/has_trivial_copy.hpp>
#include <boost/type_traits/has_trivial_constructor.hpp>
#include <boost/type_traits/has_trivial_destructor.hpp>
#include <boost/mpl/bool.hpp>

namespace nt2
{
  template<class T>
  struct dry;
}

namespace nt2 { namespace meta
{
  template<class T>
  struct real_of< nt2::dry<T> >
  {
    typedef T type;
  };
} }

namespace boost { namespace simd { namespace meta
{
  template<class T, class X>
  struct as_simd<nt2::dry<T>, X>
    : as_simd<T, X>
  {
  };
} } }

namespace boost { namespace dispatch { namespace meta
{
  template<class T, class Origin>
  struct property_of< nt2::dry<T>, Origin >
  {
    typedef dry_< typename property_of<T, Origin>::type > type;
  };

  template<class T, class Origin>
  struct hierarchy_of< nt2::dry<T>, Origin >
  {
    typedef typename remove_const<Origin>::type stripped;
    typedef typename mpl::if_< is_same< nt2::dry<T>, stripped >, stripped, Origin >::type origin_;
    typedef scalar_< dry_< typename property_of<T, origin_>::type > > type;
  };

  template<class T>
  struct scalar_of< nt2::dry<T> >
  {
    typedef nt2::dry<T> type;
  };

  template<class T>
  struct value_of< nt2::dry<T> >
  {
    typedef T type;
  };

} } }

namespace boost { namespace simd
{
  template<class T>
  struct is_value;

  template<class T>
  struct is_value< nt2::dry<T> >
       : boost::mpl::true_
  {
  };
} }

namespace boost
{
  template<class T>
  struct has_trivial_copy< nt2::dry<T> >
       : boost::mpl::true_
  {
  };

  template<class T>
  struct has_trivial_default_constructor< nt2::dry<T> >
       : boost::mpl::true_
  {
  };

  template<class T>
  struct has_trivial_destructor< nt2::dry<T> >
       : boost::mpl::true_
  {
  };
}

#endif


/**
 * @file PyOcpDescriptionLqr.cpp
 * @author Brahayam Ponton (brahayam.ponton@tuebingen.mpg.de)
 * @license License BSD-3-Clause
 * @copyright Copyright (c) 2019, New York University and Max Planck Gesellschaft.
 * @date 2019-10-07
 */

#include <pybind11/eigen.h>
#include <pybind11/stl_bind.h>
#include <pybind11/pybind11.h>

#include <Eigen/Dense>
#include <solver_lqr/PyOcpBase.hpp>
#include <solver_lqr/OcpDescription.hpp>

namespace py = pybind11;
using namespace solverlqr;

void init_ocp_description_lqr(py::module &m)
{
  // binding of state base class
  py::class_<StateBase>(m, "StateBase")
    .def(py::init<>());

  // binding of control base class
  py::class_<ControlBase>(m, "ControlBase")
    .def(py::init<>());

  // binding of OCP description
  py::class_<OcpBase, PyOcpBase>(m, "OcpBase")
    .def(py::init<>())
    .def("dynamics", &OcpBase::dynamics)
    .def("objective", &OcpBase::objective)
    .def("configure", &OcpBase::configure)
    .def("initialize", &OcpBase::initialize);
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2007 Chiara Fornarola

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 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 license for more details.
*/

#ifndef quantlib_test_assetswap_hpp
#define quantlib_test_assetswap_hpp

#include <boost/test/unit_test.hpp>

/* remember to document new and/or updated tests in the Doxygen
   comment block of the corresponding class */

class AssetSwapTest {
  public:
    static void testConsistency();
    static void testImpliedValue();
    static void testMarketASWSpread();
    static void testZSpread();
    static void testGenericBondImplied();
    static void testMASWWithGenericBond();
    static void testZSpreadWithGenericBond();
    static void testSpecializedBondVsGenericBond();
    static void testSpecializedBondVsGenericBondUsingAsw();
    static  boost::unit_test_framework::test_suite* suite();
};

#endif


#include <iostream>
#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/random.hpp>

#include "Generator.h"

using namespace std;

void Generator::init(int interval, int reps) {
    Generator::reps = reps;
    Generator::interval = interval;
    int count = 0;
    boost::asio::io_context io;
    boost::asio::steady_timer t(io, boost::asio::chrono::milliseconds(interval));
    t.async_wait(boost::bind(Generator::send, boost::asio::placeholders::error, &t, &count));
    io.run();
}

void Generator::send(const boost::system::error_code& ec, boost::asio::steady_timer* timer, int* count) {
    int value = dist(gen);
    char char_value = (char) (((int) 'a') + value);
    cout << "tick " << value << " " << char_value << endl;

    ++(*count);
    if (*count < Generator::reps)
    {
        timer->expires_at(timer->expiry() + boost::asio::chrono::milliseconds(Generator::interval));
        timer->async_wait(boost::bind(Generator::send, boost::asio::placeholders::error, timer, count));
    }
}

int Generator::reps = 1;
int Generator::interval = 1;
boost::random::mt19937 Generator::gen{static_cast<std::uint32_t>(1234567)};
boost::random::uniform_int_distribution<> Generator::dist{0, 25};


/**
 * @file test/polygon_iterator.cpp
 */
/*****************************************************************************
** Includes
*****************************************************************************/

#include "../include/cost_map_core/common.hpp"
#include <Eigen/Core>

// gtest
#include <gtest/gtest.h>

// Limits
#include <cfloat>

// Vector
#include <vector>
#include "../include/cost_map_core/cost_map.hpp"
#include <grid_map_core/Polygon.hpp>
#include "../include/cost_map_core/iterators/polygon_iterator.hpp"

using namespace std;
using namespace Eigen;

TEST(PolygonIterator, FullCover)
{
  vector<string> types;
  types.push_back("type");
  cost_map::CostMap map(types);
  map.setGeometry(cost_map::Length(8.0, 5.0), 1.0, cost_map::Position(0.0, 0.0)); // bufferSize(8, 5)

  cost_map::Polygon polygon;
  polygon.addVertex(cost_map::Position(-100.0, 100.0));
  polygon.addVertex(cost_map::Position(100.0, 100.0));
  polygon.addVertex(cost_map::Position(100.0, -100.0));
  polygon.addVertex(cost_map::Position(-100.0, -100.0));

  cost_map::PolygonIterator iterator(map, polygon);

  EXPECT_FALSE(iterator.isPastEnd());
  EXPECT_EQ(0, (*iterator)(0));
  EXPECT_EQ(0, (*iterator)(1));

  ++iterator;
  EXPECT_FALSE(iterator.isPastEnd());
  EXPECT_EQ(0, (*iterator)(0));
  EXPECT_EQ(1, (*iterator)(1));

  ++iterator;
  EXPECT_FALSE(iterator.isPastEnd());
  EXPECT_EQ(0, (*iterator)(0));
  EXPECT_EQ(2, (*iterator)(1));

  for (int i = 0; i < 37; ++i) ++iterator;

  EXPECT_FALSE(iterator.isPastEnd());
  EXPECT_EQ(7, (*iterator)(0));
  EXPECT_EQ(4, (*iterator)(1));

  ++iterator;
  EXPECT_TRUE(iterator.isPastEnd());
}

TEST(PolygonIterator, Outside)
{
  cost_map::CostMap map({"types"});
  map.setGeometry(cost_map::Length(8.0, 5.0), 1.0, cost_map::Position(0.0, 0.0)); // bufferSize(8, 5)

  cost_map::Polygon polygon;
  polygon.addVertex(cost_map::Position(99.0, 101.0));
  polygon.addVertex(cost_map::Position(101.0, 101.0));
  polygon.addVertex(cost_map::Position(101.0, 99.0));
  polygon.addVertex(cost_map::Position(99.0, 99.0));

  cost_map::PolygonIterator iterator(map, polygon);

  EXPECT_TRUE(iterator.isPastEnd());
}

TEST(PolygonIterator, Square)
{
  cost_map::CostMap map({"types"});
  map.setGeometry(cost_map::Length(8.0, 5.0), 1.0, cost_map::Position(0.0, 0.0)); // bufferSize(8, 5)

  cost_map::Polygon polygon;
  polygon.addVertex(cost_map::Position(-1.0, 1.5));
  polygon.addVertex(cost_map::Position(1.0, 1.5));
  polygon.addVertex(cost_map::Position(1.0, -1.5));
  polygon.addVertex(cost_map::Position(-1.0, -1.5));

  cost_map::PolygonIterator iterator(map, polygon);

  EXPECT_FALSE(iterator.isPastEnd());
  EXPECT_EQ(3, (*iterator)(0));
  EXPECT_EQ(1, (*iterator)(1));

  ++iterator;
  EXPECT_FALSE(iterator.isPastEnd());
  EXPECT_EQ(3, (*iterator)(0));
  EXPECT_EQ(2, (*iterator)(1));

  ++iterator;
  EXPECT_FALSE(iterator.isPastEnd());
  EXPECT_EQ(3, (*iterator)(0));
  EXPECT_EQ(3, (*iterator)(1));

  ++iterator;
  EXPECT_FALSE(iterator.isPastEnd());
  EXPECT_EQ(4, (*iterator)(0));
  EXPECT_EQ(1, (*iterator)(1));

  ++iterator;
  EXPECT_FALSE(iterator.isPastEnd());
  EXPECT_EQ(4, (*iterator)(0));
  EXPECT_EQ(2, (*iterator)(1));

  ++iterator;
  EXPECT_FALSE(iterator.isPastEnd());
  EXPECT_EQ(4, (*iterator)(0));
  EXPECT_EQ(3, (*iterator)(1));

  ++iterator;
  EXPECT_TRUE(iterator.isPastEnd());
}

TEST(PolygonIterator, TopLeftTriangle)
{
  cost_map::CostMap map({"types"});
  map.setGeometry(cost_map::Length(8.0, 5.0), 1.0, cost_map::Position(0.0, 0.0)); // bufferSize(8, 5)

  cost_map::Polygon polygon;
  polygon.addVertex(cost_map::Position(-40.1, 20.6));
  polygon.addVertex(cost_map::Position(40.1, 20.4));
  polygon.addVertex(cost_map::Position(-40.1, -20.6));

  cost_map::PolygonIterator iterator(map, polygon);

  EXPECT_FALSE(iterator.isPastEnd());
  EXPECT_EQ(0, (*iterator)(0));
  EXPECT_EQ(0, (*iterator)(1));

  ++iterator;
  EXPECT_FALSE(iterator.isPastEnd());
  EXPECT_EQ(1, (*iterator)(0));
  EXPECT_EQ(0, (*iterator)(1));

  // TODO Extend.
}

int main(int argc, char **argv)
{
  testing::InitGoogleTest(&argc, argv);
  srand((int)time(0));
  return RUN_ALL_TESTS();
}


#include <Configuration.h> // Package Path
#include <avatar_locomanipulation/enable_pinocchio_with_hpp_fcl.h> // Enable HPP FCL
// Multibody
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/multibody/geometry.hpp"
// Parsers
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/srdf.hpp"
// Algorithms
#include "pinocchio/algorithm/geometry.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
// Standard
#include <math.h>
#include <iostream>
#include <iomanip>
#include <vector>
#include <boost/shared_ptr.hpp>

int main(int argc, char ** argv){
  std::string filename = (argc<=1) ? THIS_PACKAGE_PATH"models/valkyrie_simplified.urdf" : argv[1];
  //const std::string srdf_filename = PINOCCHIO_SOURCE_DIR"/models/romeo/romeo_description/srdf/romeo.srdf";
  std::vector < std::string > packageDirs;
  std::string meshDir  = THIS_PACKAGE_PATH"../val_model/";
  packageDirs.push_back(meshDir);

  // build URDF model
  pinocchio::Model model;
  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),model);

  // build Geometry model
  pinocchio::GeometryModel geomModel;
  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geomModel, packageDirs );

  geomModel.addAllCollisionPairs();

  pinocchio::Data data(model);
  pinocchio::GeometryData geomData(geomModel);
  pinocchio::fcl::CollisionResult result;

  // Initialize Configuration -----------------------
  Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq); // Config
  Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv); // Config Velocity
  Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv); // Config Acceleration

  // floating base joints: x, y, z
  q[0] = 0.0;  q[1] = 0.0;  q[2] = 0.0;

  // floating base quaternion: qx, qy, qz, qw
  double theta = M_PI/4.0;
  Eigen::AngleAxis<double> aa(theta, Eigen::Vector3d(0.0, 0.0, 1.0)); // yaw pi/4 to the left
  Eigen::Quaternion<double> quat_init; quat_init =  aa;

  q[3] = quat_init.x();// 0.0;
  q[4] = quat_init.y(); //0.0;
  q[5] = quat_init.z(); //sin(theta/2.0);
  q[6] = quat_init.w(); //cos(theta/2.0);

  // std::cout << "Initial " << q.transpose() << std::endl;
  // std::cout << "q = " << q.transpose() << std::endl;

  pinocchio::PairIndex PairId;
  int i;
  int j;

  pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q);
  pinocchio::Index idx = geomModel.findCollisionPair(pinocchio::CollisionPair(1,10));

  // this computes a collision between a single collision pair and the result is stored in the collisionResults vector
  //pinocchio::computeCollision(geomModel,geomData,idx);

  // alternatively, there is the function computeCollisions
  	// Compute forward kinematics, updates geom placements, and calls computeCollision for every active pairs in GeometryData
  	// rtype is bool
    // computeCollision which checks for each collision pair
      //computeCollision calls the function collide which returns if there is a collision
    // collisionResult struct: https://github.com/flexible-collision-library/fcl/blob/9dba579158109c0164bfe0e8b4a75c16bfc788f6/include/fcl/narrowphase/collision_result.h
  pinocchio::computeCollisions(model,data,geomModel,geomData,q);

  std::cout << "------------Collision Results: " << std::endl;
  for(j=0; j<geomData.collisionResults.size(); j++)
  {
    // computeCollisions sets collisionPairIndex to the first colliding pair
      PairId = geomData.collisionPairIndex;
      std::cout << "PairIndex PairId: " << PairId << std::endl;
    //We iterate through the collisionResults vector so we can determine if each pair is in collision
      result = geomData.collisionResults[j];
      std::vector<pinocchio::fcl::Contact> contacts;
      result.getContacts(contacts);
      std::cout << contacts.size() << " contacts found" << std::endl;
      for(i=0; i<contacts.size(); i++)
      {
        std::cout << "position: " << contacts[i].pos << std::endl;
      }
  }
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <array>
#include <boost/functional/hash.hpp>  // IWYU pragma: keep
#include <cstddef>
#include <limits>
#include <string>
#include <type_traits>
#include <unordered_map>
#include <utility>

#include "DataStructures/Tensor/Tensor.hpp"
#include "DataStructures/Variables.hpp"
#include "Domain/Structure/Direction.hpp"
#include "Domain/Structure/Element.hpp"
#include "Domain/Structure/ElementId.hpp"
#include "Evolution/DiscontinuousGalerkin/Limiters/WenoGridHelpers.hpp"
#include "Evolution/DiscontinuousGalerkin/Limiters/WenoHelpers.hpp"
#include "Evolution/DiscontinuousGalerkin/Limiters/WenoOscillationIndicator.hpp"
#include "NumericalAlgorithms/Interpolation/RegularGridInterpolant.hpp"
#include "NumericalAlgorithms/LinearOperators/MeanValue.hpp"
#include "NumericalAlgorithms/Spectral/Mesh.hpp"
#include "Utilities/Gsl.hpp"

namespace Limiters::Weno_detail {

// Compute the Simple WENO solution for one tensor component
//
// This interface is intended for use in limiters that check the troubled-cell
// indicator independently for each tensor component. These limiters generally
// need to limit only a subset of the tensor components.
//
// When calling `simple_weno_impl`,
// - `interpolator_buffer` may be empty
// - `modified_neighbor_solution_buffer` should contain one DataVector for each
//   neighboring element (i.e. for each entry in `neighbor_data`)
template <typename Tag, size_t VolumeDim, typename PackagedData>
void simple_weno_impl(
    const gsl::not_null<std::unordered_map<
        std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>,
        intrp::RegularGrid<VolumeDim>,
        boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>*>
        interpolator_buffer,
    const gsl::not_null<std::unordered_map<
        std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>, DataVector,
        boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>*>
        modified_neighbor_solution_buffer,
    const gsl::not_null<typename Tag::type*> tensor,
    const double neighbor_linear_weight, const size_t tensor_storage_index,
    const Mesh<VolumeDim>& mesh, const Element<VolumeDim>& element,
    const std::unordered_map<
        std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>, PackagedData,
        boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>&
        neighbor_data) noexcept {
  // Check that basis is LGL or LG
  // Note that the SimpleWeno implementation should generalize well to other
  // bases beyond LGL or LG, once the oscillation_indicator function has been
  // generalized.
  ASSERT(mesh.basis() == make_array<VolumeDim>(Spectral::Basis::Legendre),
         "Unsupported basis: " << mesh);
  ASSERT(mesh.quadrature() ==
                 make_array<VolumeDim>(Spectral::Quadrature::GaussLobatto) or
             mesh.quadrature() ==
                 make_array<VolumeDim>(Spectral::Quadrature::Gauss),
         "Unsupported quadrature: " << mesh);

  ASSERT(
      modified_neighbor_solution_buffer->size() == neighbor_data.size(),
      "modified_neighbor_solution_buffer->size() = "
          << modified_neighbor_solution_buffer->size()
          << "\nneighbor_data.size() = " << neighbor_data.size()
          << "\nmodified_neighbor_solution_buffer was incorrectly initialized "
             "before calling simple_weno_impl.");

  // Compute the modified neighbor solutions.
  // First extrapolate neighbor data onto local grid points, then shift the
  // extrapolated data so its mean matches the local mean.
  DataVector& component_to_limit = (*tensor)[tensor_storage_index];
  const double local_mean = mean_value(component_to_limit, mesh);
  for (const auto& neighbor_and_data : neighbor_data) {
    const auto& neighbor = neighbor_and_data.first;
    const auto& data = neighbor_and_data.second;

    if (interpolator_buffer->find(neighbor) == interpolator_buffer->end()) {
      // No interpolator found => create one
      const auto& direction = neighbor.first;
      const auto& source_mesh = data.mesh;
      const auto target_1d_logical_coords =
          Weno_detail::local_grid_points_in_neighbor_logical_coords(
              mesh, source_mesh, element, direction);
      interpolator_buffer->insert(std::make_pair(
          neighbor, intrp::RegularGrid<VolumeDim>(source_mesh, mesh,
                                                  target_1d_logical_coords)));
    }

    // Avoid allocations by working directly in the preallocated buffer
    DataVector& buffer = modified_neighbor_solution_buffer->at(neighbor);

    interpolator_buffer->at(neighbor).interpolate(
        make_not_null(&buffer),
        get<Tag>(data.volume_data)[tensor_storage_index]);
    const double neighbor_mean = mean_value(buffer, mesh);
    buffer += (local_mean - neighbor_mean);
  }

  // Sum local and modified neighbor polynomials for the WENO reconstruction
  Weno_detail::reconstruct_from_weighted_sum(
      make_not_null(&component_to_limit), neighbor_linear_weight,
      Weno_detail::DerivativeWeight::PowTwoEll, mesh,
      *modified_neighbor_solution_buffer);
}

// Compute the Simple WENO solution for one tensor
//
// This interface is intended for use in limiters that check the troubled-cell
// indicator for the whole cell, and apply the limiter to all fields.
//
// When calling `simple_weno_impl`,
// - `interpolator_buffer` may be empty
// - `modified_neighbor_solution_buffer` should contain one DataVector for each
//   neighboring element (i.e. for each entry in `neighbor_data`)
template <typename Tag, size_t VolumeDim, typename PackagedData>
void simple_weno_impl(
    const gsl::not_null<std::unordered_map<
        std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>,
        intrp::RegularGrid<VolumeDim>,
        boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>*>
        interpolator_buffer,
    const gsl::not_null<std::unordered_map<
        std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>, DataVector,
        boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>*>
        modified_neighbor_solution_buffer,
    const gsl::not_null<typename Tag::type*> tensor,
    const double neighbor_linear_weight, const Mesh<VolumeDim>& mesh,
    const Element<VolumeDim>& element,
    const std::unordered_map<
        std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>, PackagedData,
        boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>&
        neighbor_data) noexcept {
  for (size_t tensor_storage_index = 0; tensor_storage_index < tensor->size();
       ++tensor_storage_index) {
    simple_weno_impl<Tag>(interpolator_buffer,
                          modified_neighbor_solution_buffer, tensor,
                          neighbor_linear_weight, tensor_storage_index, mesh,
                          element, neighbor_data);
  }
}

}  // namespace Limiters::Weno_detail


#ifndef FSTCRITERIONNORMALDIVERGENCE_H
#define FSTCRITERIONNORMALDIVERGENCE_H

/*!======================================================================
   Feature Selection Toolbox 3 source code
   ---------------------------------------

   \file    criterion_normal_divergence.hpp
   \brief   Implements Divergence (distance) based on normal (gaussian) model to serve as feature selection criterion
   \author  Petr Somol (somol@utia.cas.cz) with collaborators, see Contacts at http://fst.utia.cz
   \date    October 2010
   \version 3.0.0.beta
   \note    FST3 was developed using gcc 4.3 and requires
   \note    \li Boost library (http://www.boost.org/, tested with versions 1.33.1 and 1.44),
   \note    \li (\e optionally) LibSVM (http://www.csie.ntu.edu.tw/~cjlin/libsvm/,
                tested with version 3.00)
   \note    Note that LibSVM is required for SVM related tools only,
            as demonstrated in demo12t.cpp, demo23.cpp, demo25t.cpp, demo32t.cpp, etc.

*/ /*
=========================================================================
Copyright:
  * FST3 software (with exception of any externally linked libraries)
    is copyrighted by Institute of Information Theory and Automation (UTIA),
    Academy of Sciences of the Czech Republic.
  * FST3 source codes as presented here do not contain code of third parties.
    FST3 may need linkage to external libraries to exploit its functionality
    in full. For details on obtaining and possible usage restrictions
    of external libraries follow their original sources (referenced from
    FST3 documentation wherever applicable).
  * FST3 software is available free of charge for non-commercial use.
    Please address all inquires concerning possible commercial use
    of FST3, or if in doubt, to FST3 maintainer (see http://fst.utia.cz)
  * Derivative works based on FST3 are permitted as long as they remain
    non-commercial only.
  * Re-distribution of FST3 software is not allowed without explicit
    consent of the copyright holder.
Disclaimer of Warranty:
  * FST3 software is presented "as is", without warranty of any kind,
    either expressed or implied, including, but not limited to, the implied
    warranties of merchantability and fitness for a particular purpose.
    The entire risk as to the quality and performance of the program
    is with you. Should the program prove defective, you assume the cost
    of all necessary servicing, repair or correction.
Limitation of Liability:
  * The copyright holder will in no event be liable to you for damages,
    including any general, special, incidental or consequential damages
    arising out of the use or inability to use the code (including but not
    limited to loss of data or data being rendered inaccurate or losses
    sustained by you or third parties or a failure of the program to operate
    with any other programs).
========================================================================== */

#include <boost/smart_ptr.hpp>
#include <iostream>
#include <cmath>
#include "error.hpp"
#include "global.hpp"
#include "criterion_normal.hpp"
#include "indexed_vector.hpp"
#include "indexed_matrix.hpp"

/*============== Template parameter type naming conventions ==============
--------- Numeric types: -------------------------------------------------
DATATYPE - data sample values - usually real numbers (but may be integers
          in text processing etc.)
REALTYPE - must be real numbers - for representing intermediate results of
          calculations like mean, covariance etc.
IDXTYPE - index values for enumeration of data samples - (nonnegative) integers,
          extent depends on numbers of samples in data
DIMTYPE - index values for enumeration of features (dimensions), or classes (not
          class sizes) - (nonnegative) integers, usually lower extent than IDXTYPE,
          but be aware of expressions like _classes*_features*_features !
          in linearized representations of feature matrices for all classes
BINTYPE - feature selection marker type - represents ca. <10 different feature
          states (selected, deselected, sel./desel. temporarily 1st nested loop, 2nd...)
RETURNTYPE - criterion value: real value, but may be extended in future to support
          multiple values
--------- Class types: ---------------------------------------------------
SUBSET       - class of class type Subset
CLASSIFIER   - class implementing interface defined in abstract class Classifier
EVALUATOR    - class implementing interface defined in abstract class Sequential_Step
DISTANCE     - class implementing interface defined in abstract class Distance
DATAACCESSOR - class implementing interface defined in abstract class Data_Accessor
INTERVALCONTAINER - class of class type TIntervaller
CONTAINER    - STL container of class type TInterval
========================================================================== */

namespace FST {

/*! \brief Implements Divergence (distance) based on normal (gaussian) model to serve as feature selection criterion
    \note Defined for two-class problems only */
template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
class Criterion_Normal_Divergence : public Criterion_Normal<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR> {
public:
	typedef Criterion_Normal<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR> parent;
	typedef boost::shared_ptr<DATAACCESSOR> PDataAccessor;
	typedef boost::shared_ptr<SUBSET> PSubset;
	Criterion_Normal_Divergence() {notify("Criterion_Normal_Divergence constructor.");}
	virtual ~Criterion_Normal_Divergence() {notify("Criterion_Normal_Divergence destructor.");}

	virtual bool evaluate(RETURNTYPE &result, const PSubset sub);
	virtual bool initialize(PDataAccessor da);

	Criterion_Normal_Divergence* clone() const;
	Criterion_Normal_Divergence* sharing_clone() const {throw fst_error("Criterion_Normal_Divergence::sharing_clone() not supported, use Criterion_Normal_Divergence::clone() instead.");}
	Criterion_Normal_Divergence* stateless_clone() const {throw fst_error("Criterion_Normal_Divergence::stateless_clone() not supported, use Criterion_Normal_Divergence::clone() instead.");}

	virtual std::ostream& print(std::ostream& os) const {os << "Criterion_Normal_Divergence()"; return os;}
private:
	Criterion_Normal_Divergence(const Criterion_Normal_Divergence& cnd); // copy-constructor
private:
	boost::scoped_ptr<Indexed_Vector<DATATYPE,DIMTYPE,SUBSET> > _meandif; // to store mean[i]-mean[j]
	boost::scoped_ptr<Indexed_Vector<DATATYPE,DIMTYPE,SUBSET> > _diag; //
	boost::scoped_ptr<Indexed_Matrix<REALTYPE,DIMTYPE,SUBSET> > _invbuf1; // to store matrix inversions
	boost::scoped_ptr<Indexed_Matrix<REALTYPE,DIMTYPE,SUBSET> > _invbuf2; // to store matrix inversions
	boost::scoped_ptr<Indexed_Matrix<REALTYPE,DIMTYPE,SUBSET> > _LUtemp; // to store matrix LU decomposition
};

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
Criterion_Normal_Divergence<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::Criterion_Normal_Divergence(const Criterion_Normal_Divergence& cnd) :
	Criterion_Normal<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>(cnd)
{
	notify("Criterion_Normal_Divergence copy-constructor.");
	if(cnd._meandif) _meandif.reset(new Indexed_Vector<DATATYPE,DIMTYPE,SUBSET>(*cnd._meandif));
	if(cnd._diag) _diag.reset(new Indexed_Vector<DATATYPE,DIMTYPE,SUBSET>(*cnd._diag));
	if(cnd._invbuf1) _invbuf1.reset(new Indexed_Matrix<DATATYPE,DIMTYPE,SUBSET>(*cnd._invbuf1));
	if(cnd._invbuf2) _invbuf2.reset(new Indexed_Matrix<DATATYPE,DIMTYPE,SUBSET>(*cnd._invbuf2));
	if(cnd._LUtemp) _LUtemp.reset(new Indexed_Matrix<DATATYPE,DIMTYPE,SUBSET>(*cnd._LUtemp));
}

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
Criterion_Normal_Divergence<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>* Criterion_Normal_Divergence<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::clone() const
{
	Criterion_Normal_Divergence<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR> *clone=new Criterion_Normal_Divergence<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>(*this);
	clone->set_cloned();
	return clone;
}

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
bool Criterion_Normal_Divergence<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::evaluate(RETURNTYPE &result, PSubset sub)
{
	notify("Criterion_Normal_Divergence::evaluate().");
	assert(parent::_model);
	assert(parent::get_n()>0);
	assert(_meandif);
	assert(_diag);
	assert(_invbuf1);
	assert(_invbuf2);
	assert(_LUtemp);
	assert(parent::get_n()==_meandif->get_n());
	assert(parent::get_n()==_diag->get_n());
	assert(parent::get_n()<=_invbuf1->get_n_max());
	assert(parent::get_n()<=_invbuf2->get_n_max());
	assert(parent::get_n()<=_LUtemp->get_n_max());
	assert(sub);

	if(sub->get_d_raw()==0) return false;

	parent::_model->narrow_to(sub);
	_meandif->narrow_to(sub);

	_invbuf1->redim(parent::get_d());
	_invbuf2->redim(parent::get_d());
	_LUtemp->redim(parent::get_d());

	Indexed_Matrix<REALTYPE,DIMTYPE,SUBSET> &_cov1=(parent::_model->get_cov()[0]);
	Indexed_Matrix<REALTYPE,DIMTYPE,SUBSET> &_cov2=(parent::_model->get_cov()[1]);

	_cov1.LUdecompose(*_LUtemp);
	_cov1.invert(*_invbuf1,*_LUtemp);
	_cov2.LUdecompose(*_LUtemp);
	_cov2.invert(*_invbuf2,*_LUtemp);

	RETURNTYPE left=0.0;
	RETURNTYPE pom;
	for(DIMTYPE j=0;j<parent::get_d();j++)
	{
		pom=0.0;
		for(DIMTYPE i=0;i<parent::get_d();i++)
		{
			pom+=_meandif->at(i)*(_invbuf1->at_raw(i,j)+_invbuf2->at_raw(i,j));
		}
		left+=pom*_meandif->at(j);
	}

	RETURNTYPE right=0.0;
	for(DIMTYPE i=0;i<parent::get_d();i++)
	{
		_diag->at_raw(i)=0.0;
		for(DIMTYPE k=0;k<parent::get_d();k++)
		{
			_diag->at_raw(i)+=_invbuf1->at_raw(i,k)*_cov2.at(k,i);
		}
	}
	for(DIMTYPE i=0;i<parent::get_d();i++)
	{
		for(DIMTYPE k=0;k<parent::get_d();k++)
		{
			_diag->at_raw(i)+=_invbuf2->at_raw(i,k)*_cov1.at(k,i);
		}
	}
	for(DIMTYPE i=0;i<parent::get_d();i++) right+=_diag->at_raw(i)-2.0;

	result=0.5*left+0.5*right;
	return true;
}

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
bool Criterion_Normal_Divergence<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::initialize(PDataAccessor da)
{
	if(da->getNoOfClasses()!=2) throw fst_error("Criterion_Normal_Divergence() defined for two-class problems only.");
	notify("Criterion_Normal_Divergence::initialize().");
	parent::initialize(da);

	if(!_meandif) _meandif.reset(new Indexed_Vector<DATATYPE,DIMTYPE,SUBSET>);
	if(parent::get_n()>_meandif->get_n()) _meandif->reset(parent::get_n());
	if(!_diag) _diag.reset(new Indexed_Vector<DATATYPE,DIMTYPE,SUBSET>);
	if(parent::get_n()>_diag->get_n()) _diag->reset(parent::get_n());
	if(!_invbuf1) _invbuf1.reset(new Indexed_Matrix<REALTYPE,DIMTYPE,SUBSET>(parent::get_n()));
	if(parent::get_n()>_invbuf1->get_n()) _invbuf1->reset(parent::get_n());
	if(!_invbuf2) _invbuf2.reset(new Indexed_Matrix<REALTYPE,DIMTYPE,SUBSET>(parent::get_n()));
	if(parent::get_n()>_invbuf2->get_n()) _invbuf2->reset(parent::get_n());
	if(!_LUtemp) _LUtemp.reset(new Indexed_Matrix<REALTYPE,DIMTYPE,SUBSET>(parent::get_n(),true));
	if(parent::get_n()>_LUtemp->get_n()) _LUtemp->reset(parent::get_n(),true);

	_meandif->copy_raw(parent::_model->get_mean()[0]);
	_meandif->subtract_raw(parent::_model->get_mean()[1]);

	return true; }

} // namespace
#endif // FSTCRITERIONNORMALDIVERGENCE_H ///:~


/* -*- c++ -*- */
/*
 * Copyright 2015 Free Software Foundation, Inc.
 *
 * This file is part of GNU Radio
 *
 * GNU Radio 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 3, or (at your option)
 * any later version.
 *
 * GNU Radio 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 GNU Radio; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <gnuradio/io_signature.h>
#include <gnuradio/math.h>
#include "corr_est_cc_impl.h"
#include <volk/volk.h>
#include <boost/format.hpp>
#include <boost/math/special_functions/round.hpp>
#include <gnuradio/filter/pfb_arb_resampler.h>
#include <gnuradio/filter/firdes.h>

namespace gr {
  namespace digital {

    corr_est_cc::sptr
    corr_est_cc::make(const std::vector<gr_complex> &symbols,
                      float sps, unsigned int mark_delay,
                      float threshold)
    {
      return gnuradio::get_initial_sptr
        (new corr_est_cc_impl(symbols, sps, mark_delay, threshold));
    }

    corr_est_cc_impl::corr_est_cc_impl(const std::vector<gr_complex> &symbols,
                                       float sps, unsigned int mark_delay,
                                       float threshold)
      : sync_block("corr_est_cc",
                   io_signature::make(1, 1, sizeof(gr_complex)),
                   io_signature::make(1, 2, sizeof(gr_complex))),
        d_src_id(pmt::intern(alias()))
    {
      d_sps = sps;

      // In order to easily support the optional second output,
      // don't deal with an unbounded max number of output items.
      // For the common case of not using the optional second output,
      // this ensures we optimally call the volk routines.
      const size_t nitems = 24*1024;
      set_max_noutput_items(nitems);
      d_corr = (gr_complex *)
               volk_malloc(sizeof(gr_complex)*nitems, volk_get_alignment());
      d_corr_mag = (float *)
                   volk_malloc(sizeof(float)*nitems, volk_get_alignment());

      // Create time-reversed conjugate of symbols
      d_symbols = symbols;
      for(size_t i=0; i < d_symbols.size(); i++) {
          d_symbols[i] = conj(d_symbols[i]);
      }
      std::reverse(d_symbols.begin(), d_symbols.end());

      set_mark_delay(mark_delay);
      set_threshold(threshold);

      // Correlation filter
      d_filter = new kernel::fft_filter_ccc(1, d_symbols);

      // Per comments in gr-filter/include/gnuradio/filter/fft_filter.h,
      // set the block output multiple to the FFT filter kernel's internal,
      // assumed "nsamples", to ensure the scheduler always passes a
      // proper number of samples.
      int nsamples;
      nsamples = d_filter->set_taps(d_symbols);
      set_output_multiple(nsamples);

      // It looks like the kernel::fft_filter_ccc stashes a tail between
      // calls, so that contains our filtering history (I think).  The
      // fft_filter_ccc block (which calls the kernel::fft_filter_ccc) sets
      // the history to 1 (0 history items), so let's follow its lead.
      //set_history(1);

      // We'll (ab)use the history for our own purposes of tagging back in time.
      // Keep a history of the length of the sync word to delay for tagging.
      set_history(d_symbols.size()+1);

      declare_sample_delay(1, 0);
      declare_sample_delay(0, d_symbols.size());

      // Setting the alignment multiple for volk causes problems with the
      // expected behavior of setting the output multiple for the FFT filter.
      // Don't set the alignment multiple.
      //const int alignment_multiple =
      //  volk_get_alignment() / sizeof(gr_complex);
      //set_alignment(std::max(1,alignment_multiple));

      d_scale = 1.0f;
    }

    corr_est_cc_impl::~corr_est_cc_impl()
    {
      delete d_filter;
      volk_free(d_corr);
      volk_free(d_corr_mag);
    }

    std::vector<gr_complex>
    corr_est_cc_impl::symbols() const
    {
      return d_symbols;
    }

    void
    corr_est_cc_impl::set_symbols(const std::vector<gr_complex> &symbols)
    {
      gr::thread::scoped_lock lock(d_setlock);

      d_symbols = symbols;

      // Per comments in gr-filter/include/gnuradio/filter/fft_filter.h,
      // set the block output multiple to the FFT filter kernel's internal,
      // assumed "nsamples", to ensure the scheduler always passes a
      // proper number of samples.
      int nsamples;
      nsamples = d_filter->set_taps(d_symbols);
      set_output_multiple(nsamples);

      // It looks like the kernel::fft_filter_ccc stashes a tail between
      // calls, so that contains our filtering history (I think).  The
      // fft_filter_ccc block (which calls the kernel::fft_filter_ccc) sets
      // the history to 1 (0 history items), so let's follow its lead.
      //set_history(1);

      // We'll (ab)use the history for our own purposes of tagging back in time.
      // Keep a history of the length of the sync word to delay for tagging.
      set_history(d_symbols.size()+1);

      declare_sample_delay(1, 0);
      declare_sample_delay(0, d_symbols.size());

      _set_mark_delay(d_stashed_mark_delay);
      _set_threshold(d_stashed_threshold);
    }

    unsigned int
    corr_est_cc_impl::mark_delay() const
    {
      return d_mark_delay;
    }

    void
    corr_est_cc_impl::_set_mark_delay(unsigned int mark_delay)
    {
      d_stashed_mark_delay = mark_delay;

      if(mark_delay >= d_symbols.size()) {
        d_mark_delay = d_symbols.size()-1;
        GR_LOG_WARN(d_logger, boost::format("set_mark_delay: asked for %1% but due "
                                            "to the symbol size constraints, "
                                            "mark delay set to %2%.") \
                    % mark_delay % d_mark_delay);
      }
      else {
        d_mark_delay = mark_delay;
      }
    }

    void
    corr_est_cc_impl::set_mark_delay(unsigned int mark_delay)
    {
      gr::thread::scoped_lock lock(d_setlock);
      _set_mark_delay(mark_delay);
    }

    float
    corr_est_cc_impl::threshold() const
    {
      return d_thresh;
    }

    void
    corr_est_cc_impl::_set_threshold(float threshold)
    {
      d_stashed_threshold = threshold;
      d_pfa = -logf(1.0f-threshold);
    }

    void
    corr_est_cc_impl::set_threshold(float threshold)
    {
      gr::thread::scoped_lock lock(d_setlock);
      _set_threshold(threshold);
    }

    int
    corr_est_cc_impl::work(int noutput_items,
                           gr_vector_const_void_star &input_items,
                           gr_vector_void_star &output_items)
    {
      gr::thread::scoped_lock lock(d_setlock);

      const gr_complex *in = (gr_complex *)input_items[0];
      gr_complex *out = (gr_complex*)output_items[0];
      gr_complex *corr;
      if (output_items.size() > 1)
          corr = (gr_complex *) output_items[1];
      else
          corr = d_corr;

      // Our correlation filter length
      unsigned int hist_len = history() - 1;

      // Calculate the correlation of the non-delayed input with the
      // known symbols.
      d_filter->filter(noutput_items, &in[hist_len], corr);

      // Find the magnitude squared of the correlation
      volk_32fc_magnitude_squared_32f(&d_corr_mag[0], corr, noutput_items);

      float detection = 0;
      for(int i = 0; i < noutput_items; i++) {
        detection += d_corr_mag[i];
      }
      detection /= static_cast<float>(noutput_items);
      detection *= d_pfa;

      int isps = (int)(d_sps + 0.5f);
      int i = 0;
      while(i < noutput_items) {
        // Look for the correlator output to cross the threshold.
        // Sum power over two consecutive symbols in case we're offset
        // in time. If off by 1/2 a symbol, the peak of any one point
        // is much lower.
        float corr_mag = d_corr_mag[i] + d_corr_mag[i+1];
        if(corr_mag <= 4*detection) {
          i++;
          continue;
        }

        // Go to (just past) the current correlator output peak
        while ((i < (noutput_items-1)) &&
               (d_corr_mag[i] < d_corr_mag[i+1])) {
          i++;
        }
        // Delaying the primary signal output by the matched filter
        // length using history(), means that the the peak output of
        // the matched filter aligns with the start of the desired
        // sync word in the primary signal output.  This corr_start
        // tag is not offset to another sample, so that downstream
        // data-aided blocks (like adaptive equalizers) know exactly
        // where the start of the correlated symbols are.
        add_item_tag(0, nitems_written(0) + i, pmt::intern("corr_start"),
                     pmt::from_double(d_corr_mag[i]), d_src_id);

#if 0
        // Use Parabolic interpolation to estimate a fractional
        // sample delay. There are more accurate methods as
        // the sample delay estimate using this method is biased.
        // But this method is simple and fast.
        // center between [-0.5,0.5] units of samples
        // Paper Reference: "Discrete Time Techniques for Time Delay
        // Estimation" G. Jacovitti and G. Scarano
        double center = 0.0;
        if( i > 0 && i < (noutput_items - 1 )){
          double nom = d_corr_mag[i-1]-d_corr_mag[i+1];
          double denom = 2*(d_corr_mag[i-1]-2*d_corr_mag[i]+d_corr_mag[i+1]);
          center = nom/denom;
        }
#else
        // Calculates the center of mass between the three points around the peak.
        // Estimate is linear.
        double nom = 0, den = 0;
        nom = d_corr_mag[i-1] + 2*d_corr_mag[i] + 3*d_corr_mag[i+1];
        den = d_corr_mag[i-1] + d_corr_mag[i] + d_corr_mag[i+1];
        double center = nom / den;
        center = (center - 2.0); // adjust for bias in center of mass calculation
#endif

        // Estimated scaling factor for the input stream to normalize
        // the output to +/-1.
        uint32_t maxi;
        volk_32fc_index_max_32u_manual(&maxi, (gr_complex*)in, noutput_items, "generic");
        d_scale = 1 / std::abs(in[maxi]);

        // Calculate the phase offset of the incoming signal.
        //
        // The analytic cross-correlation is:
        //
        // 2A*e_bb(t-t_d)*exp(-j*2*pi*f*(t-t_d) - j*phi_bb(t-t_d) - j*theta_c)
        //

        // The analytic auto-correlation's envelope, e_bb(), has its
        // peak at the "group delay" time, t = t_d.  The analytic
        // cross-correlation's center frequency phase shift, theta_c,
        // is determined from the argument of the analytic
        // cross-correlation at the "group delay" time, t = t_d.
        //
        // Taking the argument of the analytic cross-correlation at
        // any other time will include the baseband auto-correlation's
        // phase term, phi_bb(t-t_d), and a frequency dependent term
        // of the cross-correlation, which I don't believe maps simply
        // to expected symbol phase differences.
        float phase = fast_atan2f(corr[i].imag(), corr[i].real());
        int index = i + d_mark_delay;

        add_item_tag(0, nitems_written(0) + index, pmt::intern("phase_est"),
                     pmt::from_double(phase), d_src_id);
        add_item_tag(0, nitems_written(0) + index, pmt::intern("time_est"),
                     pmt::from_double(center), d_src_id);
        // N.B. the appropriate d_corr_mag[] index is "i", not "index".
        add_item_tag(0, nitems_written(0) + index, pmt::intern("corr_est"),
                     pmt::from_double(d_corr_mag[i]), d_src_id);
        add_item_tag(0, nitems_written(0) + index, pmt::intern("amp_est"),
                     pmt::from_double(d_scale), d_src_id);

        if (output_items.size() > 1) {
          // N.B. these debug tags are not offset to avoid walking off out buf
          add_item_tag(1, nitems_written(0) + i, pmt::intern("phase_est"),
                       pmt::from_double(phase), d_src_id);
          add_item_tag(1, nitems_written(0) + i, pmt::intern("time_est"),
                       pmt::from_double(center), d_src_id);
          add_item_tag(1, nitems_written(0) + i, pmt::intern("corr_est"),
                       pmt::from_double(d_corr_mag[i]), d_src_id);
          add_item_tag(1, nitems_written(0) + i, pmt::intern("amp_est"),
                       pmt::from_double(d_scale), d_src_id);
        }

        // Skip ahead to the next potential symbol peak
        // (for non-offset/interleaved symbols)
        i += isps;
      }

      //if (output_items.size() > 1)
      //  add_item_tag(1, nitems_written(0) + noutput_items - 1,
      //               pmt::intern("ce_eow"), pmt::from_uint64(noutput_items),
      //               d_src_id);

      // Delay the output by our correlation filter length so we can
      // tag backwards in time
      memcpy(out, &in[0], sizeof(gr_complex)*noutput_items);

      return noutput_items;
    }

  } /* namespace digital */
} /* namespace gr */


/**************************************************************************
 * Copyright (c) 2017-2019 by the mfmg authors                            *
 * All rights reserved.                                                   *
 *                                                                        *
 * This file is part of the mfmg library. mfmg is distributed under a BSD *
 * 3-clause license. For the licensing terms see the LICENSE file in the  *
 * top-level directory                                                    *
 *                                                                        *
 * SPDX-License-Identifier: BSD-3-Clause                                  *
 *************************************************************************/

#include <mfmg/common/exceptions.hpp>
#include <mfmg/common/instantiation.hpp>
#include <mfmg/dealii/dealii_matrix_operator.hpp>

#include <deal.II/lac/vector.h>

namespace mfmg
{
template <typename VectorType>
DealIIMatrixOperator<VectorType>::DealIIMatrixOperator(
    std::shared_ptr<dealii::SparsityPattern> sparsity_pattern,
    std::shared_ptr<dealii::SparseMatrix<typename VectorType::value_type>>
        sparse_matrix)
    : _sparsity_pattern(std::move(sparsity_pattern)),
      _sparse_matrix(std::move(sparse_matrix))
{
  ASSERT(_sparsity_pattern != nullptr,
         "deal.II matrices require a sparsity pattern");

  ASSERT(_sparse_matrix != nullptr, "The matrix must exist");
}

template <typename VectorType>
void DealIIMatrixOperator<VectorType>::apply(VectorType const &x, VectorType &y,
                                             OperatorMode mode) const
{
  (mode == OperatorMode::NO_TRANS ? _sparse_matrix->vmult(y, x)
                                  : _sparse_matrix->Tvmult(y, x));
}

template <typename VectorType>
std::shared_ptr<Operator<VectorType>>
DealIIMatrixOperator<VectorType>::multiply_transpose(
    std::shared_ptr<Operator<VectorType> const> /*b*/) const
{
  ASSERT_THROW_NOT_IMPLEMENTED();

  return nullptr;
}

template <typename VectorType>
std::shared_ptr<VectorType>
DealIIMatrixOperator<VectorType>::build_domain_vector() const
{
  return std::make_shared<VectorType>(_sparse_matrix->n());
}

template <typename VectorType>
std::shared_ptr<VectorType>
DealIIMatrixOperator<VectorType>::build_range_vector() const
{
  return std::make_shared<VectorType>(_sparse_matrix->m());
}

template <typename VectorType>
size_t DealIIMatrixOperator<VectorType>::grid_complexity() const
{
  return _sparse_matrix->m();
}

template <typename VectorType>
size_t DealIIMatrixOperator<VectorType>::operator_complexity() const
{
  return _sparse_matrix->n_nonzero_elements();
}

} // namespace mfmg

// Explicit Instantiation
INSTANTIATE_SERIALVECTORTYPE(TUPLE(DealIIMatrixOperator))


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2008-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is 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)

#ifndef BOOST_GEOMETRY_PROJECTIONS_FACTORY_HPP
#define BOOST_GEOMETRY_PROJECTIONS_FACTORY_HPP

#include <map>
#include <string>

#include <boost/shared_ptr.hpp>

#include <boost/geometry/extensions/gis/projections/impl/factory_entry.hpp>
#include <boost/geometry/extensions/gis/projections/parameters.hpp>
#include <boost/geometry/extensions/gis/projections/proj/aea.hpp>
#include <boost/geometry/extensions/gis/projections/proj/aeqd.hpp>
#include <boost/geometry/extensions/gis/projections/proj/airy.hpp>
#include <boost/geometry/extensions/gis/projections/proj/aitoff.hpp>
#include <boost/geometry/extensions/gis/projections/proj/august.hpp>
#include <boost/geometry/extensions/gis/projections/proj/bacon.hpp>
#include <boost/geometry/extensions/gis/projections/proj/bipc.hpp>
#include <boost/geometry/extensions/gis/projections/proj/boggs.hpp>
#include <boost/geometry/extensions/gis/projections/proj/bonne.hpp>
#include <boost/geometry/extensions/gis/projections/proj/cass.hpp>
#include <boost/geometry/extensions/gis/projections/proj/cc.hpp>
#include <boost/geometry/extensions/gis/projections/proj/cea.hpp>
#include <boost/geometry/extensions/gis/projections/proj/chamb.hpp>
#include <boost/geometry/extensions/gis/projections/proj/collg.hpp>
#include <boost/geometry/extensions/gis/projections/proj/crast.hpp>
#include <boost/geometry/extensions/gis/projections/proj/denoy.hpp>
#include <boost/geometry/extensions/gis/projections/proj/eck1.hpp>
#include <boost/geometry/extensions/gis/projections/proj/eck2.hpp>
#include <boost/geometry/extensions/gis/projections/proj/eck3.hpp>
#include <boost/geometry/extensions/gis/projections/proj/eck4.hpp>
#include <boost/geometry/extensions/gis/projections/proj/eck5.hpp>
#include <boost/geometry/extensions/gis/projections/proj/eqc.hpp>
#include <boost/geometry/extensions/gis/projections/proj/eqdc.hpp>
#include <boost/geometry/extensions/gis/projections/proj/etmerc.hpp>
#include <boost/geometry/extensions/gis/projections/proj/fahey.hpp>
#include <boost/geometry/extensions/gis/projections/proj/fouc_s.hpp>
#include <boost/geometry/extensions/gis/projections/proj/gall.hpp>
#include <boost/geometry/extensions/gis/projections/proj/geocent.hpp>
#include <boost/geometry/extensions/gis/projections/proj/geos.hpp>
#include <boost/geometry/extensions/gis/projections/proj/gins8.hpp>
#include <boost/geometry/extensions/gis/projections/proj/gn_sinu.hpp>
#include <boost/geometry/extensions/gis/projections/proj/gnom.hpp>
#include <boost/geometry/extensions/gis/projections/proj/goode.hpp>
#include <boost/geometry/extensions/gis/projections/proj/gstmerc.hpp>
#include <boost/geometry/extensions/gis/projections/proj/hammer.hpp>
#include <boost/geometry/extensions/gis/projections/proj/hatano.hpp>
#include <boost/geometry/extensions/gis/projections/proj/healpix.hpp>
#include <boost/geometry/extensions/gis/projections/proj/krovak.hpp>
#include <boost/geometry/extensions/gis/projections/proj/igh.hpp>
#include <boost/geometry/extensions/gis/projections/proj/imw_p.hpp>
#include <boost/geometry/extensions/gis/projections/proj/isea.hpp>
#include <boost/geometry/extensions/gis/projections/proj/laea.hpp>
#include <boost/geometry/extensions/gis/projections/proj/labrd.hpp>
#include <boost/geometry/extensions/gis/projections/proj/lagrng.hpp>
#include <boost/geometry/extensions/gis/projections/proj/larr.hpp>
#include <boost/geometry/extensions/gis/projections/proj/lask.hpp>
#include <boost/geometry/extensions/gis/projections/proj/latlong.hpp>
#include <boost/geometry/extensions/gis/projections/proj/lcc.hpp>
#include <boost/geometry/extensions/gis/projections/proj/lcca.hpp>
#include <boost/geometry/extensions/gis/projections/proj/loxim.hpp>
#include <boost/geometry/extensions/gis/projections/proj/lsat.hpp>
#include <boost/geometry/extensions/gis/projections/proj/mbtfpp.hpp>
#include <boost/geometry/extensions/gis/projections/proj/mbtfpq.hpp>
#include <boost/geometry/extensions/gis/projections/proj/mbt_fps.hpp>
#include <boost/geometry/extensions/gis/projections/proj/merc.hpp>
#include <boost/geometry/extensions/gis/projections/proj/mill.hpp>
#include <boost/geometry/extensions/gis/projections/proj/mod_ster.hpp>
#include <boost/geometry/extensions/gis/projections/proj/moll.hpp>
#include <boost/geometry/extensions/gis/projections/proj/natearth.hpp>
#include <boost/geometry/extensions/gis/projections/proj/nell.hpp>
#include <boost/geometry/extensions/gis/projections/proj/nell_h.hpp>
#include <boost/geometry/extensions/gis/projections/proj/nocol.hpp>
#include <boost/geometry/extensions/gis/projections/proj/nsper.hpp>
#include <boost/geometry/extensions/gis/projections/proj/nzmg.hpp>
#include <boost/geometry/extensions/gis/projections/proj/ob_tran.hpp>
#include <boost/geometry/extensions/gis/projections/proj/ocea.hpp>
#include <boost/geometry/extensions/gis/projections/proj/oea.hpp>
#include <boost/geometry/extensions/gis/projections/proj/omerc.hpp>
#include <boost/geometry/extensions/gis/projections/proj/ortho.hpp>
#include <boost/geometry/extensions/gis/projections/proj/qsc.hpp>
#include <boost/geometry/extensions/gis/projections/proj/poly.hpp>
#include <boost/geometry/extensions/gis/projections/proj/putp2.hpp>
#include <boost/geometry/extensions/gis/projections/proj/putp3.hpp>
#include <boost/geometry/extensions/gis/projections/proj/putp4p.hpp>
#include <boost/geometry/extensions/gis/projections/proj/putp5.hpp>
#include <boost/geometry/extensions/gis/projections/proj/putp6.hpp>
#include <boost/geometry/extensions/gis/projections/proj/robin.hpp>
#include <boost/geometry/extensions/gis/projections/proj/rouss.hpp>
#include <boost/geometry/extensions/gis/projections/proj/rpoly.hpp>
#include <boost/geometry/extensions/gis/projections/proj/sconics.hpp>
#include <boost/geometry/extensions/gis/projections/proj/somerc.hpp>
#include <boost/geometry/extensions/gis/projections/proj/stere.hpp>
#include <boost/geometry/extensions/gis/projections/proj/sterea.hpp>
#include <boost/geometry/extensions/gis/projections/proj/sts.hpp>
#include <boost/geometry/extensions/gis/projections/proj/tcc.hpp>
#include <boost/geometry/extensions/gis/projections/proj/tcea.hpp>
#include <boost/geometry/extensions/gis/projections/proj/tmerc.hpp>
#include <boost/geometry/extensions/gis/projections/proj/tpeqd.hpp>
#include <boost/geometry/extensions/gis/projections/proj/urm5.hpp>
#include <boost/geometry/extensions/gis/projections/proj/urmfps.hpp>
#include <boost/geometry/extensions/gis/projections/proj/vandg.hpp>
#include <boost/geometry/extensions/gis/projections/proj/vandg2.hpp>
#include <boost/geometry/extensions/gis/projections/proj/vandg4.hpp>
#include <boost/geometry/extensions/gis/projections/proj/wag2.hpp>
#include <boost/geometry/extensions/gis/projections/proj/wag3.hpp>
#include <boost/geometry/extensions/gis/projections/proj/wag7.hpp>
#include <boost/geometry/extensions/gis/projections/proj/wink1.hpp>
#include <boost/geometry/extensions/gis/projections/proj/wink2.hpp>

namespace boost { namespace geometry { namespace projections
{

template <typename LatLong, typename Cartesian, typename Parameters = parameters>
class factory : public detail::base_factory<LatLong, Cartesian, Parameters>
{
private:

    typedef std::map
        <
            std::string,
            boost::shared_ptr
                <
                    detail::factory_entry
                        <
                            LatLong,
                            Cartesian,
                            Parameters
                        >
                >
        > prj_registry;
    prj_registry m_registry;

public:

    factory()
    {
        detail::aea_init(*this);
        detail::aeqd_init(*this);
        detail::airy_init(*this);
        detail::aitoff_init(*this);
        detail::august_init(*this);
        detail::bacon_init(*this);
        detail::bipc_init(*this);
        detail::boggs_init(*this);
        detail::bonne_init(*this);
        detail::cass_init(*this);
        detail::cc_init(*this);
        detail::cea_init(*this);
        detail::chamb_init(*this);
        detail::collg_init(*this);
        detail::crast_init(*this);
        detail::denoy_init(*this);
        detail::eck1_init(*this);
        detail::eck2_init(*this);
        detail::eck3_init(*this);
        detail::eck4_init(*this);
        detail::eck5_init(*this);
        detail::eqc_init(*this);
        detail::eqdc_init(*this);
        detail::etmerc_init(*this);
        detail::fahey_init(*this);
        detail::fouc_s_init(*this);
        detail::gall_init(*this);
        detail::geocent_init(*this);
        detail::geos_init(*this);
        detail::gins8_init(*this);
        detail::gn_sinu_init(*this);
        detail::gnom_init(*this);
        detail::goode_init(*this);
        detail::gstmerc_init(*this);
        detail::hammer_init(*this);
        detail::hatano_init(*this);
        detail::healpix_init(*this);
        detail::krovak_init(*this);
        detail::igh_init(*this);
        detail::imw_p_init(*this);
        detail::isea_init(*this);
        detail::labrd_init(*this);
        detail::laea_init(*this);
        detail::lagrng_init(*this);
        detail::larr_init(*this);
        detail::lask_init(*this);
        detail::latlong_init(*this);
        detail::lcc_init(*this);
        detail::lcca_init(*this);
        detail::loxim_init(*this);
        detail::lsat_init(*this);
        detail::mbtfpp_init(*this);
        detail::mbtfpq_init(*this);
        detail::mbt_fps_init(*this);
        detail::merc_init(*this);
        detail::mill_init(*this);
        detail::mod_ster_init(*this);
        detail::moll_init(*this);
        detail::natearth_init(*this);
        detail::nell_init(*this);
        detail::nell_h_init(*this);
        detail::nocol_init(*this);
        detail::nsper_init(*this);
        detail::nzmg_init(*this);
        detail::ob_tran_init(*this);
        detail::ocea_init(*this);
        detail::oea_init(*this);
        detail::omerc_init(*this);
        detail::ortho_init(*this);
        detail::qsc_init(*this);
        detail::poly_init(*this);
        detail::putp2_init(*this);
        detail::putp3_init(*this);
        detail::putp4p_init(*this);
        detail::putp5_init(*this);
        detail::putp6_init(*this);
        detail::robin_init(*this);
        detail::rouss_init(*this);
        detail::rpoly_init(*this);
        detail::sconics_init(*this);
        detail::somerc_init(*this);
        detail::stere_init(*this);
        detail::sterea_init(*this);
        detail::sts_init(*this);
        detail::tcc_init(*this);
        detail::tcea_init(*this);
        detail::tmerc_init(*this);
        detail::tpeqd_init(*this);
        detail::urm5_init(*this);
        detail::urmfps_init(*this);
        detail::vandg_init(*this);
        detail::vandg2_init(*this);
        detail::vandg4_init(*this);
        detail::wag2_init(*this);
        detail::wag3_init(*this);
        detail::wag7_init(*this);
        detail::wink1_init(*this);
        detail::wink2_init(*this);
    }

    virtual ~factory() {}

    virtual void add_to_factory(std::string const& name,
                    detail::factory_entry<LatLong, Cartesian, Parameters>* sub)
    {
        m_registry[name].reset(sub);
    }

    inline projection<LatLong, Cartesian>* create_new(Parameters const& parameters)
    {
        typename prj_registry::iterator it = m_registry.find(parameters.name);
        if (it != m_registry.end())
        {
            return it->second->create_new(parameters);
        }

        return 0;
    }
};

}}} // namespace boost::geometry::projections

#endif // BOOST_GEOMETRY_PROJECTIONS_FACTORY_HPP