/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
   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 "findline.h"
#include <chrono>
#include <opencv2/imgproc.hpp>
#include <opencv2/imgcodecs.hpp>

#ifdef DEBUG_FIND_LINE
#undef DEBUG_FIND_LINE
#include <iostream>
#include <boost/filesystem.hpp>
#ifdef WIN32
static const std::string DEBUG_RESULT_FOLDER = "c:/water/";
#else
static const std::string DEBUG_RESULT_FOLDER = "/var/tmp/water/";
#endif
#endif

using namespace cv;
using namespace std;
#ifdef DEBUG_FIND_LINE
using namespace boost;
namespace fs = boost::filesystem;
#endif

static const int MEDIAN_FILTER_KERN_SIZE = 9;

namespace gc
{

FindLine::FindLine() :
    m_minLineFindAngle( DEFAULT_MIN_LINE_ANGLE ),
    m_maxLineFindAngle( DEFAULT_MAX_LINE_ANGLE )
{
#ifdef DEBUG_FIND_LINE
    if ( !fs::exists( DEBUG_RESULT_FOLDER ) )
    {
        bool isOK = fs::create_directories( DEBUG_RESULT_FOLDER );
        if ( !isOK )
        {
            FILE_LOG( logERROR ) << "Could not create debug result folder: " << DEBUG_RESULT_FOLDER;
        }
    }
#endif
}

GC_STATUS FindLine::SetLineFindAngleBounds( const double minAngle, const double maxAngle )
{
    GC_STATUS retVal = GC_OK;
    if ( minAngle > maxAngle )
    {
        FILE_LOG( logERROR ) << "[FindLine::SetLineFindAngleBounds] Min angle must be less than max angle: min="
                             << minAngle << " max=" << maxAngle;
        retVal = GC_ERR;
    }
    else
    {
        m_minLineFindAngle = minAngle;
        m_maxLineFindAngle = maxAngle;
    }
    return retVal;
}
GC_STATUS FindLine::Find( const Mat &img, const vector< LineEnds > &lines, FindLineResult &result )
{
    result.findSuccess = false;
    GC_STATUS retVal = lines.empty() || img.empty() ? GC_ERR : GC_OK;
    if ( GC_OK != retVal )
    {
        FILE_LOG( logERROR ) << "[FindLine::Find] Cannot find lines with no search lines defined or in a NULL image";
    }
    else
    {
        try
        {
#ifdef DEBUG_FIND_LINE
            Mat outImg;
            if ( CV_8UC1 == img.type() )
                cvtColor( img, outImg, COLOR_GRAY2BGR );
            else if ( CV_8UC3 == img.type() )
                outImg = img.clone();
            else
            {
                FILE_LOG( logERROR ) << "[FindLine::Find] Invalid image type for drawing row sum must be 8-bit gray or 8-bit bgr";
                retVal = GC_ERR;
            }
#endif

            Mat inImg;
            if ( CV_8UC3 == img.type() )
                cvtColor( img, inImg, COLOR_BGR2GRAY );
            else if ( CV_8UC1 == img.type() )
                inImg = img.clone();
            else
            {
                FILE_LOG( logERROR ) << "[FindLine::Find] Invalid image type for find. Must be 8-bit gray or 8-bit bgr";
                retVal = GC_ERR;
            }

            if ( GC_OK == retVal )
            {
                // clean-up a little
                Mat scratch;
                retVal = Preprocess( inImg, scratch );
                if ( GC_OK == retVal )
                {
                    size_t start;
                    Point2d linePt;
                    vector< uint > rowSums;
                    string timestamp = result.timestamp;
                    result.timestamp = timestamp;
                    size_t linesPerSwath = lines.size() / 10;
                    for ( size_t i = 0; i < 9; ++i )
                    {
                        start = i * linesPerSwath;
                        retVal = EvaluateSwath( scratch, lines, start, start + linesPerSwath, linePt, result );
                        if ( GC_OK == retVal )
                            result.foundPoints.push_back( linePt );
                    }
                    start = lines.size() - linesPerSwath - 1;
                    retVal = EvaluateSwath( scratch, lines, start, lines.size() - 1, linePt, result );
                    if ( GC_OK == retVal )
                        result.foundPoints.push_back( linePt );

#ifdef DEBUG_FIND_LINE
                    line( outImg, lines[ 0 ].top, lines[ 0 ].bot, Scalar( 0, 255, 255 ), 3 );
                    line( outImg, lines[ lines.size() - 1 ].top, lines[ lines.size() - 1 ].bot, Scalar( 0, 255, 255 ), 3 );
                    bool isOK = imwrite( DEBUG_RESULT_FOLDER + "rowsums.png", outImg );
                    if ( !isOK )
                    {
                        FILE_LOG( logERROR ) << "[FindLine::Find] Could not write debug image " << DEBUG_RESULT_FOLDER << "rowsums.png";
                    }
#endif

                    FindPointSet findPtSet;
                    double xCenter = ( lines[ 0 ].bot.x + lines[ lines.size() - 1 ].bot.x ) / 2.0;
                    retVal = FitLineRANSAC( result.foundPoints, result.calcLinePts, xCenter, scratch );
                    if ( GC_OK == retVal )
                    {
                        result.findSuccess = true;
                    }
                }
            }
        }
        catch( cv::Exception &e )
        {
            result.findSuccess = false;
            FILE_LOG( logERROR ) << "[FindLine::Find] " << e.what();
            retVal = GC_EXCEPT;
        }
    }

    return retVal;
}
GC_STATUS FindLine::Preprocess( const cv::Mat &src, cv::Mat &dst )
{
    GC_STATUS retVal = GC_OK;
    if ( src.empty() )
    {
        FILE_LOG( logERROR ) << "[FindLine::Preprocess] Not possible to preprocess an empty image";
        retVal = GC_ERR;
    }
    else
    {
        try
        {
            Mat kern = getStructuringElement( MORPH_RECT, Size( 1, 9 ) );
            dilate( src, dst, kern, Point( -1, -1 ), 3 );
            erode( dst, dst, kern, Point( -1, -1 ), 3 );
        }
        catch( cv::Exception &e )
        {
            FILE_LOG( logERROR ) << "[FindLine::Preprocess] " << e.what();
            retVal = GC_EXCEPT;
        }
    }

    return retVal;
}
GC_STATUS FindLine::FitLineRANSAC( const std::vector< Point2d > &pts, FindPointSet &findPtSet,
                                   const double xCenter, const cv::Mat &img )
{
    GC_STATUS retVal = 5 > pts.size() ? GC_ERR : GC_OK;
    if ( GC_OK != retVal )
    {
        FILE_LOG( logERROR ) << "[FindLine::FitLineRANSAC] At least five points are needed to fit a line";
    }
    else
    {
        try
        {
#ifdef DEBUG_FIND_LINE
            Mat scratch;
            if ( CV_8UC1 == img.type() )
                cvtColor( img, scratch, COLOR_GRAY2BGR );
            else
                scratch = img.clone();
#endif
            Vec4d lineVec;
            vector< int > indices;
            vector< Point2d > ptSet;
            vector< FindPointSet > validLines;
            for ( int i = 0; i < FIT_LINE_RANSAC_TRIES_TOTAL; ++i )
            {
                retVal = GetRandomNumbers( 0, static_cast< int >( pts.size() ) - 1, FIT_LINE_RANSAC_POINT_COUNT, indices, 0 == i );
                if ( GC_OK == retVal )
                {
                    ptSet.clear();
                    for ( size_t i = 0; i < indices.size(); ++i )
                    {
#ifdef DEBUG_FIND_LINE
                        circle( scratch, pts[ indices[ i ] ], 5, Scalar( 0, 255, 255 ), 3 );
#endif
                        ptSet.push_back( pts[ indices[ i ] ] );
                    }

                    fitLine( ptSet, lineVec, DIST_L2, 0.0, 0.01, 0.01 );
                    findPtSet.lftPixel.x = lineVec[ 2 ] + ( lineVec[ 0 ] * -lineVec[ 2 ] );
                    findPtSet.lftPixel.y = lineVec[ 3 ] + ( lineVec[ 1 ] * -lineVec[ 2 ] );
                    findPtSet.rgtPixel.x = lineVec[ 2 ] + ( lineVec[ 0 ] * ( img.cols - lineVec[ 2 ] - 1 ) );
                    findPtSet.rgtPixel.y = lineVec[ 3 ] + ( lineVec[ 1 ] * ( img.cols - lineVec[ 2 ] - 1 ) );
                    findPtSet.ctrPixel.x = lineVec[ 2 ] + ( lineVec[ 0 ] * ( xCenter - lineVec[ 2 ] ) );
                    findPtSet.ctrPixel.y = lineVec[ 3 ] + ( lineVec[ 1 ] * ( xCenter - lineVec[ 2 ] ) );
                    findPtSet.anglePixel = atan2( ( findPtSet.rgtPixel.y - findPtSet.lftPixel.y ),
                                                 ( findPtSet.rgtPixel.x - findPtSet.lftPixel.x ) ) * ( 180.0 / CV_PI );
#ifdef DEBUG_FIND_LINE
                    line( scratch, findPtSet.lftPixel, findPtSet.rgtPixel, Scalar( 0, 0, 255 ), 1 );
#endif
                    if ( m_minLineFindAngle <= findPtSet.anglePixel && m_maxLineFindAngle >= findPtSet.anglePixel )
                    {
                        validLines.push_back( findPtSet );
                    }
                    if ( validLines.size() >= FIT_LINE_RANSAC_TRIES_EARLY_OUT )
                        break;
                }
            }
#ifdef DEBUG_FIND_LINE
            imwrite( DEBUG_RESULT_FOLDER + "ransac.png", scratch );
#endif
            if ( 9 > validLines.size() )
            {
                FILE_LOG( logERROR ) << "[FindLine::FitLineRANSAC] No valid lines found";
                retVal = GC_ERR;
            }
            else
            {
                sort( validLines.begin(), validLines.end(), []( FindPointSet a, FindPointSet b ) {
                    return a.ctrPixel.y > b.ctrPixel.y; } );

                double totalY = 0.0;
                double totalTheta = 0.0;
                size_t start = validLines.size() >> 2;
                size_t end = validLines.size() - start;
                for ( size_t i = start; i < end; ++i )
                {
                    totalY += validLines[ i ].ctrPixel.y;
                    totalTheta += validLines[ i ].anglePixel;
                }
                findPtSet.ctrPixel.x = xCenter;
                findPtSet.ctrPixel.y = totalY / static_cast< double >( end - start );
                findPtSet.anglePixel = totalTheta / static_cast< double >( end - start );

                double rads = findPtSet.anglePixel * CV_PI / 180.0;
                Point2d pt = Point2d( findPtSet.ctrPixel.x + cos( rads ) * 100.0, findPtSet.ctrPixel.y + sin( rads ) * 100 );

                double slope, intercept;
                retVal = GetSlopeIntercept( findPtSet.ctrPixel, pt, slope, intercept );
                if ( GC_OK == retVal )
                {
                    findPtSet.lftPixel.x = 0.0;
                    findPtSet.lftPixel.y = intercept;
                    findPtSet.rgtPixel.x = static_cast< double >( img.cols ) - 1.0;
                    findPtSet.rgtPixel.y = slope * findPtSet.rgtPixel.x + intercept;
                }
            }
        }
        catch( cv::Exception &e )
        {
            FILE_LOG( logERROR ) << "[FindLine::FitLineRANSAC] " << e.what();
            retVal = GC_EXCEPT;
        }
    }
#ifdef DEBUG_FIND_LINE
    cout << "FindLine::Find() retVal=" << retVal << endl;
#endif
    return retVal;
}
GC_STATUS FindLine::GetSlopeIntercept( const Point2d one, const Point2d two, double &slope, double &intercept )
{
    GC_STATUS retVal = GC_OK;
    try
    {
        slope = ( two.y - one.y ) / ( 0.0 == ( two.x - one.x ) ? std::numeric_limits< double >::epsilon() : ( two.x - one.x ) );
        intercept = one.y - slope * one.x;
    }
    catch( std::exception &e )
    {
        FILE_LOG( logERROR ) << "[FindLine::GetSlopeIntercept] " << e.what();
        retVal = GC_EXCEPT;
    }
    return retVal;
}
GC_STATUS FindLine::GetRandomNumbers( const int low_bound, const int high_bound, const int cnt_to_generate,
                                      vector< int > &numbers, const bool isFirst )
{
    GC_STATUS retVal = GC_OK;
    try
    {
        if ( high_bound - low_bound + 1 < static_cast< int >( numbers.size() ) / 2 )
        {
            FILE_LOG( logERROR ) << "[FindLine::GetRandomNumbers] Not enough points to find good numbers";
            retVal = GC_ERR;
        }
        else
        {
            if ( isFirst )
            {
                auto seed = std::chrono::system_clock::now().time_since_epoch().count(); //seed
                m_randomEngine.seed( static_cast< unsigned int >( seed ) );
            }

            std::uniform_int_distribution< int > di( low_bound, high_bound ); //distribution

            vector< int > tempVec;
            for ( int i = 0; i < cnt_to_generate; ++i )
                tempVec.push_back( 0 );

            bool foundIt;
            int tries = 10;
            numbers.clear();
            while ( tries > 0 && static_cast< int >( numbers.size() ) < cnt_to_generate )
            {
                std::generate( tempVec.begin(), tempVec.end(), [ & ]{ return di( m_randomEngine ); } );
                for ( size_t i = 0; i < tempVec.size(); ++i )
                {
                    foundIt = false;
                    for ( size_t j = 0; j < numbers.size(); ++j )
                    {
                        if ( tempVec[ i ] == numbers[ j ] )
                        {
                            foundIt = true;
                            break;
                        }
                    }
                    if ( !foundIt )
                    {
                        numbers.push_back( tempVec[ i ] );
                        if ( static_cast< int >( numbers.size() ) >= cnt_to_generate )
                            break;
                    }
                }
                --tries;
            }
            if ( static_cast< int >( numbers.size() ) < cnt_to_generate )
            {
                FILE_LOG( logERROR ) << "[FindLine::GetRandomNumbers] Not enough unique numbers found";
                retVal = GC_ERR;
            }

#ifdef DEBUG_FIND_LINE
            for ( size_t i = 0; i < static_cast< size_t >( cnt_to_generate ); ++i )
                std::cout << numbers[ i ] << ", ";
            std::cout << endl;
#endif
        }
    }
    catch( std::exception &e )
    {
        FILE_LOG( logERROR ) << "[FindLine::GetRandomNumbers] " << e.what();
        retVal = GC_EXCEPT;
    }
    return retVal;
}
GC_STATUS FindLine::DrawResult( const Mat &img, Mat &imgOut, const FindLineResult &result,
                                const LineDrawType overlayTypes )
{
    GC_STATUS retVal = img.empty() ? GC_ERR : GC_OK;
    if ( GC_OK != retVal )
    {
        FILE_LOG( logERROR ) << "[FindLine::DrawResult] Cannot draw find line results on a NULL image";
    }
    else
    {
        try
        {
            if ( &img != &imgOut )
            {
                if ( CV_8UC1 == img.type() )
                {
                    cvtColor( img, imgOut, COLOR_GRAY2BGR );
                }
                else if ( CV_8UC3 == img.type() )
                {
                    imgOut = img.clone();
                }
                else
                {
                    FILE_LOG( logERROR ) << "[FindLine::DrawResult] Invalid image type for drawing row sum must be 8-bit gray or 8-bit bgr";
                    retVal = GC_ERR;
                }
            }
            if ( GC_OK == retVal )
            {
                int circleSize =  std::max( 5, cvRound( static_cast< double >( imgOut.rows ) / 400.0 ) );
                int textStroke = std::max( 1, cvRound( static_cast< double >( imgOut.rows ) / 300.0 ) );
                int textRowSpacing = cvRound( static_cast< double >( imgOut.rows ) / 40.0 );
                double fontScale = static_cast< double >( imgOut.rows ) / 500.0;

                if ( ( overlayTypes & ROW_SUMS ) && !result.diagRowSums.empty() )
                {
                    for ( size_t i = 0; i < result.diagRowSums.size(); ++i )
                    {
                        if ( result.diagRowSums[ i ].size() > 1 )
                        {
                            for ( size_t j = 1; j < result.diagRowSums[ i ].size(); ++j )
                            {
                                line( imgOut, result.diagRowSums[ i ][ j - 1 ], result.diagRowSums[ i ][ j ], Scalar( 0, 255, 255 ), 2 );
                            }
                        }
                    }
                }

                if ( ( overlayTypes & FIRST_DERIVE ) && !result.diag1stDeriv.empty() )
                {
                    for ( size_t i = 0; i < result.diag1stDeriv.size(); ++i )
                    {
                        if ( result.diag1stDeriv[ i ].size() > 1 )
                        {
                            for ( size_t j = 1; j < result.diag1stDeriv[ i ].size(); ++j )
                            {
                                line( imgOut, result.diag1stDeriv[ i ][ j - 1 ], result.diag1stDeriv[ i ][ j ], Scalar( 0, 0, 255 ), 2 );
                            }
                        }
                    }
                }

                if ( ( overlayTypes & SECOND_DERIVE ) && !result.diag2ndDeriv.empty() )
                {
                    for ( size_t i = 0; i < result.diag2ndDeriv.size(); ++i )
                    {
                        if ( result.diag2ndDeriv[ i ].size() > 1 )
                        {
                            for ( size_t j = 1; j < result.diag2ndDeriv[ i ].size(); ++j )
                            {
                                line( imgOut, result.diag2ndDeriv[ i ][ j - 1 ], result.diag2ndDeriv[ i ][ j ], Scalar( 255, 127, 127 ), 2 );
                            }
                        }
                    }
                }

                if ( !result.findSuccess )
                {
                    line( imgOut, Point2d( 0.0, 0.0 ), Point2d( img.cols - 1, img.rows - 1 ), Scalar( 0, 0, 255 ), 3 );
                    line( imgOut, Point2d( 0.0, img.rows - 1 ), Point2d( img.cols - 1, 0.0 ), Scalar( 0, 0, 255 ), 3 );
                    putText( imgOut, "BAD FIND", Point( 5, textRowSpacing * 2 ), FONT_HERSHEY_PLAIN, fontScale, Scalar( 0, 0, 255 ), textStroke );
                    for ( size_t i = 0; i < result.foundPoints.size(); ++i )
                    {
                        circle( imgOut, result.foundPoints[ i ], max( 3, circleSize >> 1 ), Scalar( 0, 255, 255 ), FILLED );
                    }
                }
                else
                {
                    if ( ( overlayTypes & FOUND_LINE ) )
                    {
                        line( imgOut, result.calcLinePts.lftPixel, result.calcLinePts.rgtPixel, Scalar( 255, 0, 0 ), textStroke + 1 );
                        circle( imgOut, result.calcLinePts.ctrPixel, circleSize + textStroke, Scalar( 0, 255, 0 ), textStroke );
                        line( imgOut, Point2d( result.calcLinePts.ctrPixel.x - circleSize - textStroke * 2,
                                               result.calcLinePts.ctrPixel.y - circleSize - textStroke * 2 ),
                                      Point2d( result.calcLinePts.ctrPixel.x + circleSize + textStroke * 2,
                                               result.calcLinePts.ctrPixel.y + circleSize + textStroke * 2 ), Scalar( 0, 0, 255 ), textStroke );
                        line( imgOut, Point2d( result.calcLinePts.ctrPixel.x + circleSize + textStroke * 2,
                                               result.calcLinePts.ctrPixel.y - circleSize - textStroke * 2 ),
                                      Point2d( result.calcLinePts.ctrPixel.x - circleSize - textStroke * 2,
                                               result.calcLinePts.ctrPixel.y + circleSize + textStroke * 2 ), Scalar( 0, 0, 255 ), textStroke );
                    }

                    if ( ( overlayTypes & MOVE_FIND_RESULT ) )
                    {
                        line( imgOut, result.refMovePts.lftPixel, result.refMovePts.rgtPixel, Scalar( 0, 0, 255 ), circleSize );
                        line( imgOut, result.foundMovePts.lftPixel, result.foundMovePts.rgtPixel, Scalar( 0, 255, 0 ), ( circleSize >> 1 ) - 1 );
                    }
                }
                if ( 3 < result.foundPoints.size() && ( overlayTypes & RANSAC_POINTS ) )
                {
                    for ( size_t i = 0; i < result.foundPoints.size(); ++i )
                    {
                        circle( imgOut, result.foundPoints[ i ], max( 3, circleSize >> 1 ), Scalar( 0, 255, 255 ), FILLED );
                    }
                }
                for ( size_t i = 0; i < result.msgs.size(); ++i )
                {
                    putText( imgOut, result.msgs[ i ], Point( 3, ( static_cast< int >( i ) + 1 ) * textRowSpacing ), FONT_HERSHEY_PLAIN, fontScale, Scalar( 0, 255, 255 ), textStroke );
                }
            }
        }
        catch( cv::Exception &e )
        {
            FILE_LOG( logERROR ) << "[FindLine::DrawResult] " << e.what();
            retVal = GC_EXCEPT;
        }
    }
    return retVal;
}
GC_STATUS FindLine::EvaluateSwath( const Mat &img, const vector< LineEnds > &lines, const size_t startIndex,
                                   const size_t endIndex, Point2d &resultPt, FindLineResult &result )
{
    GC_STATUS retVal = ( lines.empty() || img.empty() || startIndex > endIndex ||
                         lines.size() - 1 < endIndex ) ? GC_ERR : GC_OK;
    if ( GC_OK != retVal )
    {
        FILE_LOG( logERROR ) << "[FindLine::EvaluateSwath] Cannot evalate swath with invalid indices or an empty line vector or image";
    }
    else
    {
        try
        {
            vector< LineEnds > swath;
            for ( size_t i = startIndex; i <= endIndex; ++i )
                swath.push_back( lines[ i ] );

            vector< uint > rowSums;
            retVal = CalcRowSums( img, swath, rowSums );
            if ( GC_OK == retVal )
            {
                retVal = CalculateRowSumsLines( rowSums, swath, result.diagRowSums,
                                                result.diag1stDeriv, result.diag2ndDeriv );
                if ( GC_OK != retVal )
                {
                    FILE_LOG( logWARNING ) << "[FindLine::EvaluateSwath] Cannot retrieve diagnostic line points";
                    retVal = GC_OK;
                }
                retVal = CalcSwathPoint( swath, rowSums, resultPt );
            }
        }
        catch( cv::Exception &e )
        {
            FILE_LOG( logERROR ) << "[FindLine::EvaluateSwath] " << e.what();
            retVal = GC_EXCEPT;
        }
    }

    return retVal;
}
GC_STATUS FindLine::CalcSwathPoint( const vector< LineEnds > &swath, const vector< uint > &rowSums, Point2d &resultPt )
{
    GC_STATUS retVal = ( swath.empty() || rowSums.empty() ) ? GC_ERR : GC_OK;
    if ( GC_OK != retVal )
    {
        FILE_LOG( logERROR ) << "[FindLine::CalcSwathPoint] Cannot calculate swath point with empty line or rowsums vector(s)";
    }
    else
    {
        try
        {
            int diff ;
            int index = -1;
            int diffMax = numeric_limits< int >::min();
            for ( size_t i = 1; i < rowSums.size() - 1; ++i )
            {
                diff = abs( static_cast< int >( rowSums[ i ] ) - static_cast< int >( rowSums[ i - 1 ] ) );
                if ( diff > diffMax )
                {
                    index = static_cast< int >( i );
                    diffMax = diff;
                }
            }
            if ( -1 == index )
            {
                FILE_LOG( logERROR ) << "[FindLine::CalcSwathPoint] No swath edge found";
                retVal = GC_WARN;
            }
            else
            {
                resultPt.x = static_cast< double >( swath[ 0 ].top.x + swath[ swath.size() - 1 ].top.x ) / 2.0;
                double d0 = rowSums[ static_cast< size_t >( index - 1 ) ] - rowSums[ static_cast< size_t >( index - 2 ) ];
                double d1 = rowSums[ static_cast< size_t >( index ) ] - rowSums[ static_cast< size_t >( index - 1 ) ];
                double d2 = rowSums[ static_cast< size_t >( index + 1 ) ] - rowSums[ static_cast< size_t >( index ) ];
                double dd1 = d1 - d0;
                double dd2 = d2 - d1;
                resultPt.y = static_cast< double >( index ) + fabs( dd1 ) / ( fabs( dd1 * dd1 ) + ( dd2 * dd2 ) ) +
                        static_cast< double >( swath[ 0 ].top.y + swath[ swath.size() - 1 ].top.y ) / 2.0;
            }
        }
        catch( cv::Exception &e )
        {
            FILE_LOG( logERROR ) << "[FindLine::CalcSwathPoint] " << e.what();
            retVal = GC_EXCEPT;
        }
    }
    return retVal;
}
GC_STATUS FindLine::CalcRowSums( const Mat &img, const vector< LineEnds > &lines, vector< uint > &rowSums )
{
    GC_STATUS retVal = lines.empty() || img.empty() ? GC_ERR : GC_OK;
    if ( GC_OK != retVal )
    {
        FILE_LOG( logERROR ) << "[FindLine::CalcRowSums] Cannot calculate row sums with no search lines defined or in a NULL image";
    }
    else
    {
        try
        {
            Mat color;
            cvtColor( img, color, COLOR_GRAY2BGR );
            line( color, lines[ 0 ].top, lines[ 0 ].bot, Scalar( 0, 0, 255 ), 3 );
            line( color, lines[ lines.size() - 1 ].top, lines[ lines.size() - 1 ].bot, Scalar( 0, 0, 255 ), 3 );
            // imwrite( "/var/tmp/water/orig.png", color );

            rowSums.clear();
            vector< uint > rowSumsTemp;
            int height = lines[ 0 ].bot.y - lines[ 0 ].top.y;
            for ( int i = 0; i < height; ++i )
                rowSumsTemp.push_back( 0 );

            for ( size_t i = 0; i < lines.size(); ++i )
            {
                LineIterator iter( img, lines[ i ].top, lines[ i ].bot );
                for ( int j = 0; j < std::min( height, iter.count ); ++j, ++iter )
                {
                    rowSumsTemp[ static_cast< size_t >( j ) ] += static_cast< uint >( **iter );
                }
            }
            retVal = MedianFilter( MEDIAN_FILTER_KERN_SIZE, rowSumsTemp, rowSums );
        }
        catch( cv::Exception &e )
        {
            FILE_LOG( logERROR ) << "[FindLine::CalcRowSums] " << e.what();
            retVal = GC_EXCEPT;
        }
    }

    return retVal;
}
GC_STATUS FindLine::CalculateRowSumsLines( const vector< uint > rowSums, const vector< LineEnds > lines, vector< vector< Point > > &rowSumsLines,
                                           vector< vector< Point > > &deriveOneLines,  vector< vector< Point > > &deriveTwoLines )
{
    GC_STATUS retVal = lines.empty() || rowSums.empty() ? GC_ERR : GC_OK;
    if ( GC_OK != retVal )
    {
        FILE_LOG( logERROR ) << "[FindLine::CalculateRowSumsLines] Cannot calculate row sums line points with empty search lines, image, or row sum array";
    }
    else
    {
        try
        {
            uint maxVal = 0;
            for ( size_t i = 0; i < rowSums.size(); ++i )
            {
                if ( maxVal < rowSums[ i ] )
                    maxVal = rowSums[ i ];
            }

            int beg = min( lines[ 0 ].top.x, lines[ 0 ].bot.x ); // + width_poly;
            // double wide = static_cast< double >( min( lines[ lines.size() - 1 ].top.x, lines[ lines.size() - 1 ].bot.x ) - beg - 3 );
            double wide = 64.0;

            int right;
            int y = lines[ 0 ].top.y;
            double dMaxVal = static_cast< double >( maxVal );

            vector< Point > rowSumsPts;
            right = beg + cvRound( wide * ( static_cast< double >( rowSums[ 0 ] ) / dMaxVal ) );
            Point pt = Point( right, y );
            rowSumsPts.push_back( pt );
            ++y;

            int diff;
            int minDiff = 9999999;
            int maxDiff = -9999999;
            vector< int > firstDeriv;
            for ( size_t i = 1; i < rowSums.size(); ++i )
            {
                diff = rowSums[ i ] - rowSums[ i - 1 ];
                right = beg + cvRound( wide * ( static_cast< double >( rowSums[ i ] ) / dMaxVal ) );
                pt = Point( right, y );
                rowSumsPts.push_back( pt );
                ++y;

                firstDeriv.push_back( diff );
                if ( diff > maxDiff )
                    maxDiff = diff;
                if ( diff < minDiff )
                    minDiff = diff;
            }

            rowSumsLines.push_back( rowSumsPts );

            if ( maxDiff == minDiff )
            {
                FILE_LOG( logERROR ) << "[FindLine::CalculateRowSumsLines] Cannot calculate row sums first deriv if all values are the same";
                retVal = GC_ERR;
            }
            else
            {
                beg += 35;
                y = lines[ 0 ].top.y + 1;
                rowSumsPts.clear();

                int minDiff2 = 9999999;
                int maxDiff2 = -9999999;
                vector< int > secondDeriv;
                double totDiff = maxDiff - minDiff;
                right = beg + cvRound( wide * ( static_cast< double >( firstDeriv[ 0 ] - minDiff ) / totDiff ) );
                pt = Point( right, y );
                rowSumsPts.push_back( pt );
                ++y;

                for ( size_t i = 1; i < firstDeriv.size(); ++i )
                {
                    diff = firstDeriv[ i ] - firstDeriv[ i - 1 ];
                    right = beg + cvRound( wide * ( static_cast< double >( firstDeriv[ i ] - minDiff ) / totDiff ) );
                    pt = Point( right, y );
                    rowSumsPts.push_back( pt );
                    ++y;

                    secondDeriv.push_back( diff );
                    if ( diff > maxDiff2 )
                        maxDiff2 = diff;
                    if ( diff < minDiff2 )
                        minDiff2 = diff;
                }

                deriveOneLines.push_back( rowSumsPts );

                if ( maxDiff2 == minDiff2 )
                {
                    FILE_LOG( logERROR ) << "[FindLine::CalculateRowSumsLines] Cannot calculate row sums second deriv if all values are the same";
                    retVal = GC_ERR;
                }
                else
                {
                    beg += 70;
                    y = lines[ 0 ].top.y + 1;
                    rowSumsPts.clear();

                    double totDiff = maxDiff2 - minDiff;
                    right = beg + cvRound( wide * ( static_cast< double >( secondDeriv[ 0 ] - minDiff2 ) / totDiff ) );
                    pt = Point( right, y );
                    rowSumsPts.push_back( pt );
                    ++y;

                    for ( size_t i = 1; i < secondDeriv.size(); ++i )
                    {
                        right = beg + cvRound( wide * ( static_cast< double >( secondDeriv[ i ] - minDiff2 ) / totDiff ) );
                        pt = Point( right, y );
                        rowSumsPts.push_back( pt );
                        ++y;
                    }

                    deriveTwoLines.push_back( rowSumsPts );
                }
            }
        }
        catch( cv::Exception &e )
        {
            FILE_LOG( logERROR ) << "[FindLine::CalculateRowSumsLines] " << e.what();
            retVal = GC_EXCEPT;
        }
    }

    return retVal;
}
GC_STATUS FindLine::MedianFilter( const size_t kernSize, const vector< uint > values, vector< uint > &valuesOut )
{
    GC_STATUS retVal = values.empty() || 3 > kernSize || kernSize * 2 > values.size() ? GC_ERR : GC_OK;
    if ( GC_OK != retVal )
    {
        FILE_LOG( logERROR ) << "[FindLine::MedianFilter] Median filter not possible with empty vector or bad kern size=" << kernSize;
    }
    else
    {
        try
        {
            valuesOut.clear();
            vector< uint > sortVec;
            size_t halfVec, kernHalf = kernSize >> 1;
            for ( size_t i = 0; i < kernHalf; ++i )
            {
                sortVec.clear();
                for ( size_t j = 0; j < kernHalf + i; j++ )
                    sortVec.push_back( values[ j ] );
                halfVec = sortVec.size() / 2;
                nth_element( sortVec.begin(), sortVec.begin() + static_cast< int >( halfVec ), sortVec.end() );
                if ( 0 == sortVec.size() % 2 )
                    valuesOut.push_back( ( sortVec[ halfVec ] + sortVec[ halfVec + 1 ] ) / 2 );
                else
                    valuesOut.push_back( sortVec[ halfVec ] );
            }
            for ( int i = static_cast< int >( kernHalf ); i < static_cast< int >( values.size() - kernHalf ); ++i )
            {
                sortVec.clear();
                for ( int j = -static_cast< int >( kernHalf ); j < static_cast< int >( kernHalf ); j++ )
                    sortVec.push_back( values[ static_cast< size_t >( i + j ) ] );
                nth_element( sortVec.begin(), sortVec.begin() + static_cast< int >( kernHalf ), sortVec.end() );
                valuesOut.push_back( sortVec[ kernHalf ] );
            }
            for ( size_t i = values.size() - kernHalf; i < values.size(); ++i )
            {
                sortVec.clear();
                for ( size_t j = i - kernHalf; j < values.size(); j++ )
                    sortVec.push_back( values[ j ] );
                halfVec = sortVec.size() / 2;
                nth_element( sortVec.begin(), sortVec.begin() + static_cast< int >( halfVec ), sortVec.end() );
                if ( 0 == sortVec.size() % 2 )
                    valuesOut.push_back( ( sortVec[ halfVec ] + sortVec[ halfVec + 1 ] ) / 2 );
                else
                    valuesOut.push_back( sortVec[ halfVec ] );
            }
        }
        catch( cv::Exception &e )
        {
            FILE_LOG( logERROR ) << "[FindLine::MedianFilter] " << e.what();
            retVal = GC_EXCEPT;
        }
    }

    return retVal;
}
} // namespace gc


#pragma once

#include <Eigen/Core>

namespace GooBalls {

template<typename Derived>
inline bool is_finite(const Eigen::MatrixBase<Derived>& x)
{
    return ( (x - x).array() == (x - x).array()).all();
}

} // GooBalls


/*******************************************************************************
 * Copyright (c) 2014, 2015  IBM Corporation and others
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 *******************************************************************************/

#ifndef State_hpp
#define State_hpp

#include <stdio.h>
#include <iostream>
#include <vector>
#include "Location.hpp"
#include "Pose.hpp"
#include <boost/circular_buffer.hpp>

namespace loc{
    class State;
    using States = std::vector<State>;
    using StatesPtr = std::shared_ptr<States>;

    class State : public Pose{
    private:
        double orientationBias_ = 0;
        double orientationAlignment_ = 0;
        double rssiBias_ = 0;
        double weight_ = 1.0;
        double negativeLogLikelihood_;
        double mahalanobisDistance_;

    public:
        using Ptr = std::shared_ptr<State>;

        State() = default;
        ~State() = default;

        State(const Pose& pose);
        double orientationBias() const;
        double orientationAlignment() const;
        double rssiBias() const;
        double weight() const;
        double negativeLogLikelihood() const;
        double mahalanobisDistance() const;
        State& orientationBias(double orientationBias);
        State& orientationAlignment(double orientationAlignment);
        State& rssiBias(double rssiBias);
        State& weight(double weight);
        State& negativeLogLikelihood(double negativeLogLikelihood);
        State& mahalanobisDistance(double mahalanobisDistance);
        static State mean(const std::vector<State>& states);
        static State weightedMean(const std::vector<State>& states);

        // for string stream
        friend std::ostream& operator<<(std::ostream&os, const State& pose);

        std::string header() const;

        // experimental feature
        long timestamp;
        boost::circular_buffer<State> history;
        static size_t history_capacity;
    };

    class StateProperty{

        double meanRssiBias_ = 0.0; // [dBm/s]
        double stdRssiBias_ = 0.0; // [dBm/s]
        double diffusionRssiBias_ = 0.0; // [dBm/s]
        double diffusionOrientationBias_ = 10.0/180.0*M_PI; // [radian/s]

        double minRssiBias_ = -10;
        double maxRssiBias_ = 10;

    public:
        using Ptr = std::shared_ptr<StateProperty>;

        StateProperty& meanRssiBias(double meanRssiBias);
        double meanRssiBias() const;
        StateProperty& stdRssiBias(double stdRssiBias);
        double stdRssiBias() const;
        StateProperty& diffusionRssiBias(double diffusionRssiBias);
        double diffusionRssiBias() const;
        StateProperty& diffusionOrientationBias(double diffusionOrientationBias);
        double diffusionOrientationBias();
        StateProperty& minRssiBias(double minRssiBias);
        double minRssiBias() const;
        StateProperty& maxRssiBias(double maxRssiBias);
        double maxRssiBias() const;

        template<class Archive>
        void serialize(Archive & ar, std::uint32_t const version)
        {
            //if (0 <= version) {
            ar(CEREAL_NVP(meanRssiBias_));
            ar(CEREAL_NVP(stdRssiBias_));
            ar(CEREAL_NVP(diffusionRssiBias_));
            ar(CEREAL_NVP(diffusionOrientationBias_));

            ar(CEREAL_NVP(minRssiBias_));
            ar(CEREAL_NVP(maxRssiBias_));
            //}
        }
    };
}

// assign version
CEREAL_CLASS_VERSION(loc::StateProperty, 0);
#endif /* State_hpp */


// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern feature feature
#include <boost/test/unit_test.hpp>
#include "fern/language/feature/core/attributes.h"

namespace fl = fern::language;

BOOST_AUTO_TEST_CASE(feature)
{
    // Earth feature. All attributes are relevant for this one planet.
    {
        fl::Feature earth;
        BOOST_CHECK_EQUAL(earth.nr_features(), 0u);
        BOOST_CHECK(!earth.contains_feature("continents"));
        BOOST_CHECK_EQUAL(earth.nr_attributes(), 0u);
        BOOST_CHECK(!earth.contains_attribute("acceleration"));

        earth.add_attribute("acceleration",
            std::make_shared<fl::ConstantAttribute<double>>(9.80665));
        BOOST_CHECK_EQUAL(earth.nr_features(), 0u);
        BOOST_CHECK_EQUAL(earth.nr_attributes(), 1u);
        BOOST_CHECK(earth.contains_attribute("acceleration"));

        // Add continents child-feature.
        earth.add_feature("continents", std::make_shared<fl::Feature>());
        BOOST_CHECK_EQUAL(earth.nr_features(), 1u);
        BOOST_CHECK(earth.contains_feature("continents"));
        BOOST_CHECK_EQUAL(earth.nr_attributes(), 1u);

        // Add attribute that is global to all continents.
        earth.add_attribute("/continents/is_land",
            std::make_shared<fl::ConstantAttribute<bool>>(true));
        BOOST_CHECK_EQUAL(earth.nr_features(), 1u);
        BOOST_CHECK_EQUAL(earth.nr_attributes(), 1u);
        BOOST_CHECK_EQUAL(earth.nr_features("continents"), 0u);
        BOOST_CHECK_EQUAL(earth.nr_attributes("continents"), 1u);
        BOOST_CHECK(earth.contains_attribute("continents/is_land"));
    }

    // Planets feature. Attributes are stored per planet (a point in space).
    {
        fl::Feature planets;

        using Value = int;
        using Point = fern::Point<int, 3>;
        using PointsAttribute = fl::SpatialAttribute<fl::SpatialDomain<
            Point>, Value>;
        using PointsAttributePtr = std::shared_ptr<PointsAttribute>;

        Point center_of_planet1{ 1, 1, 1 };
        PointsAttributePtr acceleration(std::make_shared<PointsAttribute>());
        acceleration->add(center_of_planet1, 5);
        planets.add_attribute("acceleration", acceleration);
        BOOST_CHECK_EQUAL(planets.nr_attributes(), 1);
    }
}


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// See LICENSE file in the project root for full license information.
//
#pragma once

#include "armnn/ArmNN.hpp"
#include "armnn/Tensor.hpp"

#include <boost/multi_array.hpp>
#include <boost/assert.hpp>
#include <array>

// Layer callables

namespace armnn
{
class IWorkloadFactory;
}

template <std::size_t n>
boost::array<unsigned int, n> GetTensorShapeAsArray(const armnn::TensorInfo& tensorInfo)
{
    BOOST_ASSERT_MSG(n == tensorInfo.GetNumDimensions(),
        "Attempting to construct a shape array of mismatching size");

    boost::array<unsigned int, n> shape;
    for (unsigned int i = 0; i < n; i++)
    {
        shape[i] = tensorInfo.GetShape()[i];
    }
    return shape;
}

template <typename T, std::size_t n>
struct LayerTestResult
{
    LayerTestResult(const armnn::TensorInfo& outputInfo)
    {
        auto shape( GetTensorShapeAsArray<n>(outputInfo) );
        output.resize(shape);
        outputExpected.resize(shape);
        supported = true;
    }

    boost::multi_array<T, n> output;
    boost::multi_array<T, n> outputExpected;
    bool supported;
};

LayerTestResult<float, 4> SimpleConvolution2d3x5Test(armnn::IWorkloadFactory& workloadFactory,
                                                     bool                     biasEnabled);

LayerTestResult<float, 4> SimpleConvolution2d3x3Test(armnn::IWorkloadFactory& workloadFactory,
                                                     bool                     biasEnabled);

LayerTestResult<float, 4>
Convolution2dAsymmetricPaddingLargerThanHalfKernelSizeTest(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float, 4> Convolution2dAsymmetricPaddingTest(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<float,   4> Convolution1dTest(armnn::IWorkloadFactory& workloadFactory, bool biasEnabled);
LayerTestResult<uint8_t, 4> Convolution1dUint8Test(armnn::IWorkloadFactory& workloadFactory, bool biasEnabled);

LayerTestResult<float, 4> DepthwiseConvolution2dTest(armnn::IWorkloadFactory& workloadFactory, bool biasEnabled);

LayerTestResult<float, 4> DepthwiseConvolution2dDepthMul1Test(armnn::IWorkloadFactory& workloadFactory,
                                                              bool biasEnabled);

LayerTestResult<float, 4> DepthwiseConvolution2dAsymmetricTest(armnn::IWorkloadFactory& workloadFactory,
                                                               bool biasEnabled);

LayerTestResult<float,   4> SimpleMaxPooling2dSize2x2Stride2x2Test(armnn::IWorkloadFactory& workloadFactory,
                                                                   bool forceNoPadding);
LayerTestResult<uint8_t, 4> SimpleMaxPooling2dSize2x2Stride2x2Uint8Test(armnn::IWorkloadFactory& workloadFactory,
                                                                        bool forceNoPadding);
LayerTestResult<float,   4> SimpleMaxPooling2dSize3x3Stride2x4Test(armnn::IWorkloadFactory& workloadFactory,
                                                                   bool forceNoPadding);
LayerTestResult<uint8_t, 4> SimpleMaxPooling2dSize3x3Stride2x4Uint8Test(armnn::IWorkloadFactory& workloadFactory,
                                                                        bool forceNoPadding );
LayerTestResult<float,   4> IgnorePaddingSimpleMaxPooling2dTest(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 4> IgnorePaddingSimpleMaxPooling2dUint8Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float,   4> IgnorePaddingMaxPooling2dSize3Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 4> IgnorePaddingMaxPooling2dSize3Uint8Test(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<float,   4> SimpleAveragePooling2dTest(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 4> SimpleAveragePooling2dUint8Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float,   4> IgnorePaddingAveragePooling2dSize3x2Stride2x2Test(armnn::IWorkloadFactory& workloadFactory,
                                                                              bool forceNoPadding);
LayerTestResult<float,   4> IgnorePaddingSimpleAveragePooling2dTest(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 4> IgnorePaddingSimpleAveragePooling2dUint8Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float, 4>   IgnorePaddingSimpleAveragePooling2dNoPaddingTest(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 4> IgnorePaddingSimpleAveragePooling2dNoPaddingUint8Test(
    armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float,   4> IgnorePaddingAveragePooling2dSize3Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 4> IgnorePaddingAveragePooling2dSize3Uint8Test(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<float,   4> SimpleL2Pooling2dTest(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 4> SimpleL2Pooling2dUint8Test(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<float,   4> L2Pooling2dSize3Stride1Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 4> L2Pooling2dSize3Stride1Uint8Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float,   4> L2Pooling2dSize3Stride3Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 4> L2Pooling2dSize3Stride3Uint8Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float,   4> L2Pooling2dSize3Stride4Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 4> L2Pooling2dSize3Stride4Uint8Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float,   4> L2Pooling2dSize7Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 4> L2Pooling2dSize7Uint8Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float,   4> L2Pooling2dSize9Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 4> L2Pooling2dSize9Uint8Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float,   4> LargeTensorsAveragePooling2dTest(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 4> LargeTensorsAveragePooling2dUint8Test(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<float,   4> IgnorePaddingSimpleL2Pooling2dTest(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 4> IgnorePaddingSimpleL2Pooling2dUint8Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float,   4> IgnorePaddingL2Pooling2dSize3Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 4> IgnorePaddingL2Pooling2dSize3Uint8Test(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<float,   4> AsymmetricNonSquarePooling2dTest(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 4> AsymmetricNonSquarePooling2dUint8Test(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<float, 4> ComparePooling2dTest(armnn::IWorkloadFactory& workloadFactory,
                                               armnn::IWorkloadFactory& refWorkloadFactory,
                                               armnn::PoolingAlgorithm  poolingType);
LayerTestResult<uint8_t, 4> ComparePooling2dUint8Test(armnn::IWorkloadFactory& workloadFactory,
                                                      armnn::IWorkloadFactory& refWorkloadFactory,
                                                      armnn::PoolingAlgorithm  poolingType);

LayerTestResult<float, 4> ConstantLinearActivationTest(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<float, 4> SimpleNormalizationAcrossTest(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float, 4> SimpleNormalizationWithinTest(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<float, 2> SimpleSoftmaxTest(armnn::IWorkloadFactory& workloadFactory, float beta);
LayerTestResult<uint8_t, 2> SimpleSoftmaxUint8Test(armnn::IWorkloadFactory& workloadFactory, float beta);

LayerTestResult<float, 4> SimpleSigmoidTest(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<float, 4> SimpleReshapeFloat32Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 4> SimpleReshapeUint8Test(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<float, 4> SimpleFloorTest(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<float, 1> Concatenation1dTest(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float, 2> Concatenation2dDim0Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float, 2> Concatenation2dDim1Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float, 2> Concatenation2dDim0DiffInputDimsTest(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float, 2> Concatenation2dDim1DiffInputDimsTest(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float, 3> Concatenation3dDim0Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float, 3> Concatenation3dDim1Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float, 3> Concatenation3dDim2Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float, 3> Concatenation3dDim0DiffInputDimsTest(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float, 3> Concatenation3dDim1DiffInputDimsTest(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float, 3> Concatenation3dDim2DiffInputDimsTest(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<uint8_t, 4> SimpleSigmoidUint8Test(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<float, 4> CompareConvolution2dTest(armnn::IWorkloadFactory& workloadFactory,
    armnn::IWorkloadFactory& refWorkloadFactory);

template<typename T>
LayerTestResult<T, 4> CompareDepthwiseConvolution2dTest(armnn::IWorkloadFactory& workloadFactory,
    armnn::IWorkloadFactory& refWorkloadFactory);

LayerTestResult<float, 4> CompareNormalizationTest(armnn::IWorkloadFactory& workloadFactory,
                                                   armnn::IWorkloadFactory& refWorkloadFactory,
                                                   armnn::NormalizationAlgorithmChannel normChannel,
                                                   armnn::NormalizationAlgorithmMethod normMethod);

LayerTestResult<float, 2> CompareSoftmaxTest(armnn::IWorkloadFactory& workloadFactory,
    armnn::IWorkloadFactory& refWorkloadFactory, float beta);

LayerTestResult<float, 2> FullyConnectedFloat32Test(armnn::IWorkloadFactory& workloadFactory,
                                             bool                     biasEnabled,
                                             bool                     transposeWeights);

std::vector<LayerTestResult<float, 3>> SplitterTest(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float, 3> CopyViaSplitterTest(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<float, 3> MergerTest(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<float, 4> AdditionTest(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float, 4> AdditionBroadcast1ElementTest(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float, 4> AdditionBroadcastTest(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<float, 4> CompareAdditionTest(armnn::IWorkloadFactory& workloadFactory,
                                       armnn::IWorkloadFactory& refWorkloadFactory);

LayerTestResult<float, 4> CompareActivationTest(armnn::IWorkloadFactory&  workloadFactory,
                                                armnn::IWorkloadFactory&  refWorkloadFactory,
                                                armnn::ActivationFunction f,
                                                unsigned int batchSize);

LayerTestResult<float, 4> MultiplicationTest(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float, 4> MultiplicationBroadcast1ElementTest(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float, 4> MultiplicationBroadcast1DVectorTest(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<float, 4> CompareMultiplicationTest(armnn::IWorkloadFactory& workloadFactory,
                                             armnn::IWorkloadFactory& refWorkloadFactory);

LayerTestResult<float, 4> BatchNormTest(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<float, 4> CompareBatchNormTest(armnn::IWorkloadFactory& workloadFactory,
                                        armnn::IWorkloadFactory& refWorkloadFactory);

LayerTestResult<float, 4> BoundedReLuUpperAndLowerBoundTest(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 4> BoundedReLuUint8UpperAndLowerBoundTest(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float, 4> BoundedReLuUpperBoundOnlyTest(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 4> BoundedReLuUint8UpperBoundOnlyTest(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<float, 4> CompareBoundedReLuTest(armnn::IWorkloadFactory& workloadFactory,
                                                 armnn::IWorkloadFactory& refWorkloadFactory,
                                                 float upperBound,
                                                 float lowerBound);

// Tests that the output should be identical to the input when the output dimensions match the input ones
LayerTestResult<float, 4> ResizeBilinearNopTest(armnn::IWorkloadFactory& workloadFactory);

// Tests the behaviour of the resize bilinear operation when rescaling a 2x2 image into a 1x1 image
LayerTestResult<float, 4> SimpleResizeBilinearTest(armnn::IWorkloadFactory& workloadFactory);

// Tests resize bilinear for minification of a square input matrix (also: input dimensions are a
// multiple of output dimensions)
LayerTestResult<float, 4> ResizeBilinearSqMinTest(armnn::IWorkloadFactory& workloadFactory);

// Tests resize bilinear for minification (output dimensions smaller than input dimensions)
LayerTestResult<float, 4> ResizeBilinearMinTest(armnn::IWorkloadFactory& workloadFactory);

// Tests resize bilinear for magnification (output dimensions bigger than input dimensions)
LayerTestResult<float, 4> ResizeBilinearMagTest(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<float, 4> BatchNormTest(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<float, 2> FakeQuantizationTest(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<float, 4> L2Normalization1dTest(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float, 4> L2Normalization2dTest(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float, 4> L2Normalization3dTest(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float, 4> L2Normalization4dTest(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<float, 4> ConstantTest(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<uint8_t, 4> ConstantTestUint8(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<uint8_t, 4> BoundedReLuUint8Test(armnn::IWorkloadFactory& workloadFactory, float upperBound);
LayerTestResult<uint8_t, 4> BoundedReLuUint8Test(armnn::IWorkloadFactory& workloadFactory,
    float upperBound,
    float lowerBound);

LayerTestResult<uint8_t, 2> FullyConnectedUint8Test(armnn::IWorkloadFactory& workloadFactory, bool biasEnabled);

std::vector<LayerTestResult<uint8_t, 3>> SplitterUint8Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 3> CopyViaSplitterUint8Test(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<uint8_t, 3> MergerUint8Test(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<uint8_t, 4> AdditionUint8Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 4> AdditionBroadcast1ElementUint8Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 4> AdditionBroadcastUint8Test(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<uint8_t, 4> CompareActivationUint8Test(armnn::IWorkloadFactory&  workloadFactory,
                                                       armnn::IWorkloadFactory&  refWorkloadFactory,
                                                       armnn::ActivationFunction f);

LayerTestResult<uint8_t, 2> CompareSoftmaxUint8Test(armnn::IWorkloadFactory& workloadFactory,
    armnn::IWorkloadFactory& refWorkloadFactory,
    float beta);

LayerTestResult<uint8_t, 4> MultiplicationUint8Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 4> MultiplicationBroadcast1ElementUint8Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 4> MultiplicationBroadcast1DVectorUint8Test(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<uint8_t, 4> SimpleConvolution2d3x5Uint8Test(armnn::IWorkloadFactory& workloadFactory,
                                                            bool                     biasEnabled);

LayerTestResult<uint8_t, 4> SimpleConvolution2d3x3Uint8Test(armnn::IWorkloadFactory& workloadFactory,
                                                            bool                     biasEnabled);

LayerTestResult<uint8_t, 4> DepthwiseConvolution2dUint8Test(armnn::IWorkloadFactory& workloadFactory,
                                                            bool                     biasEnabled);

LayerTestResult<uint8_t, 4> DepthwiseConvolution2dDepthMul1Uint8Test(armnn::IWorkloadFactory& workloadFactory,
                                                                     bool biasEnabled);

LayerTestResult<uint8_t, 4> ConstantLinearActivationUint8Test(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<uint8_t, 4> ResizeBilinearNopUint8Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 4> SimpleResizeBilinearUint8Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 4> ResizeBilinearSqMinUint8Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 4> ResizeBilinearMinUint8Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 4> ResizeBilinearMagUint8Test(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<uint8_t, 4> BatchNormUint8Test(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<uint8_t, 4> ConstantUint8Test(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<uint8_t, 1> Concatenation1dUint8Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 2> Concatenation2dDim0Uint8Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 2> Concatenation2dDim1Uint8Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 2> Concatenation2dDim0DiffInputDimsUint8Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 2> Concatenation2dDim1DiffInputDimsUint8Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 3> Concatenation3dDim0Uint8Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 3> Concatenation3dDim1Uint8Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 3> Concatenation3dDim2Uint8Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 3> Concatenation3dDim0DiffInputDimsUint8Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 3> Concatenation3dDim1DiffInputDimsUint8Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 3> Concatenation3dDim2DiffInputDimsUint8Test(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<float, 2> FullyConnectedLargeTest(armnn::IWorkloadFactory& workloadFactory,
                                                  bool transposeWeights);
LayerTestResult<float, 4> SimplePermuteFloat32Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<uint8_t, 4> SimplePermuteUint8Test(armnn::IWorkloadFactory& workloadFactory);

LayerTestResult<float, 4> PermuteFloat32ValueSet1Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float, 4> PermuteFloat32ValueSet2Test(armnn::IWorkloadFactory& workloadFactory);
LayerTestResult<float, 4> PermuteFloat32ValueSet3Test(armnn::IWorkloadFactory& workloadFactory);


/**
 * @file gradient_zero_visitor.hpp
 * @author Marcus Edel
 *
 * This file provides an abstraction for the Gradient() function for different
 * layers and automatically directs any parameter to the right layer type.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#ifndef MLPACK_METHODS_ANN_VISITOR_GRADIENT_ZERO_VISITOR_HPP
#define MLPACK_METHODS_ANN_VISITOR_GRADIENT_ZERO_VISITOR_HPP

#include <mlpack/methods/ann/layer/layer_traits.hpp>
#include <mlpack/methods/ann/layer/layer_types.hpp>

#include <boost/variant.hpp>

namespace mlpack {
namespace ann {

/*
 * GradientZeroVisitor set the gradient to zero for the given module.
 */
class GradientZeroVisitor : public boost::static_visitor<void>
{
 public:
  //! Set the gradient to zero for the given module.
  GradientZeroVisitor();

  //! Set the gradient to zero.
  template<typename LayerType>
  void operator()(LayerType* layer) const;

 private:
  //! Set the gradient to zero if the module implements the Gradient() function.
  template<typename T>
  typename std::enable_if<
      HasGradientCheck<T, arma::mat&(T::*)()>::value, void>::type
  LayerGradients(T* layer, arma::mat& input) const;

  //! Do not set the gradient to zero if the module doesn't implement the
  //! Gradient() function.
  template<typename T, typename P>
  typename std::enable_if<
      !HasGradientCheck<T, P&(T::*)()>::value, void>::type
  LayerGradients(T* layer, P& input) const;
};

} // namespace ann
} // namespace mlpack

// Include implementation.
#include "gradient_zero_visitor_impl.hpp"

#endif


#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <boost/filesystem.hpp>
#include <boost/foreach.hpp>
#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>
#include <iostream>
#include <string>

using namespace cv;
using namespace std;

namespace po = boost::program_options;
namespace fs = boost::filesystem;

Mat rotate(Mat src, int angle) {
	Mat dst;
	Point2f pc(src.cols / 2., src.rows / 2.);
	Mat r = getRotationMatrix2D(pc, angle, 1.0);
	warpAffine(src, dst, r, src.size());
	return dst;
}

void showImage(Mat img) {
	namedWindow("Image", WINDOW_AUTOSIZE);
	imshow("Image", img);
	waitKey(0);
}

void processImages(po::variables_map vm) {
	fs::path srcDir(vm["src"].as<string>());
	fs::path dstDir(vm["dst"].as<string>());
	fs::directory_iterator it(srcDir), eod;
	string filename;

	BOOST_FOREACH(fs::path const & path, std::make_pair(it, eod)) {
		filename = path.filename().string();
		if (fs::is_regular_file(path) && filename[0] != '.') {
			cout << path.string() << endl;
			Mat srcImage = imread(path.string(), 1);
			Mat dstImage = rotate(srcImage, -45);
			imwrite(dstDir.string() + '/' + filename, dstImage);
		}
	}
}

po::variables_map parseArguments(int argc, char const *argv[]) {
	po::variables_map vm;
	po::options_description desc("Allowed options");
	desc.add_options()
	("help", "produce help message")
	("src", po::value<string>(), "set source directory")
	("dst", po::value<string>(), "set destination directory")
	;

	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if (vm.count("help")) {
		std::cout << desc << "\n";
		return vm;
	}

	if (vm.count("src")) {
		cout << "Source directory was set to \""
		     << vm["src"].as<string>() << "\".\n";
	} else {
		cout << "Source directory was not set. Use option \'--src arg\'\n";
	}

	if (vm.count("dst")) {
		cout << "Destination directory was set to \""
		     << vm["dst"].as<string>() << "\".\n";
		if (!fs::is_directory(vm["dst"].as<string>())) {
			cout << "Destination directory was not found but will be created." << endl;
			fs::create_directory(vm["dst"].as<string>());
		}

	} else {
		cout << "Destination directory was not set. Use option \'--dst arg\'\n";
	}
	return vm;
}

int main(int argc, char const *argv[]) {
	processImages(parseArguments(argc, argv));
	return 0;
}


/**
 * @brief シャドウマッピングのテストシーン
 */

#include "SceneShadowMap.h"

#include <boost/assert.hpp>
#include <iostream>

#include <glm/gtc/constants.hpp>
#include <glm/gtc/matrix_transform.hpp>

// ********************************************************************************
// constexpr variables
// ********************************************************************************

static constexpr glm::mat4 kShadowBias{0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.5f,
                                       0.0f, 0.0f, 0.0f, 0.0f, 0.5f, 0.0f,
                                       0.5f, 0.5f, 0.5f, 1.0f};

static constexpr float kCameraCenter = 2.0f;

static constexpr glm::vec3 kLightColor{0.85f};
static constexpr float kLightCenter = 1.65f;
static constexpr glm::vec3 kDefaultLightPosition{0.0f, kLightCenter * 5.25f,
                                                 kLightCenter * 7.5f};

// ********************************************************************************
// Override functions
// ********************************************************************************

void SceneShadowMap::OnInit() {
  SetupCamera();
  SetupLight();

  if (const auto msg = CompileAndLinkShader()) {
    std::cerr << msg.value() << std::endl;
    BOOST_ASSERT_MSG(false, "failed to compile or link!");
  } else {
    progs_[kShadeWithShadow].Use();
    progs_[kShadeWithShadow].SetUniform("Light.La", kLightColor);
    progs_[kShadeWithShadow].SetUniform("Light.Ld", kLightColor);
    progs_[kShadeWithShadow].SetUniform("Light.Ls", kLightColor);
    progs_[kShadeWithShadow].SetUniform("ShadowMap", 0);
  }

  // フレームバッファオブジェクトの生成
  SetupFBO();
}

void SceneShadowMap::OnDestroy() {
  glDeleteBuffers(1, &shadowFBO_);
  glDeleteTextures(1, &depthTex_);
}

void SceneShadowMap::OnUpdate(float t) {
  const float deltaT = tPrev_ == 0.0f ? 0.0f : t - tPrev_;
  tPrev_ = t;

  angle_ += kRotSpeed * deltaT;
  if (angle_ > glm::two_pi<float>()) {
    angle_ -= glm::two_pi<float>();
  }

  const glm::vec3 kCamPt =
      glm::vec3(kCameraCenter * 11.5f * cos(angle_), kCameraCenter * 7.0f,
                kCameraCenter * 11.5f * sin(angle_));
  camera_.SetPosition(kCamPt);
}

void SceneShadowMap::OnRender() {
  glClearColor(0.9f, 0.9f, 0.9f, 1.0f);
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_CULL_FACE);

  // シャドウマップをチャンネル0に登録します。
  glActiveTexture(GL_TEXTURE0);
  glBindTexture(GL_TEXTURE_2D, depthTex_);
  {
    Pass1();
    Pass2();
  }
  glBindTexture(GL_TEXTURE_2D, 0);
}

void SceneShadowMap::OnResize(int w, int h) {
  SetDimensions(w, h);
  glViewport(0, 0, w, h);
}

// ********************************************************************************
// Shader settings
// ********************************************************************************

std::optional<std::string> SceneShadowMap::CompileAndLinkShader() {
  // compile and links
  if (auto msg = progs_[kRecordDepth].CompileAndLink(
          {{"./Assets/Shaders/ShadowMap/RecordDepth.vs.glsl",
            ShaderType::Vertex},
           {"./Assets/Shaders/ShadowMap/RecordDepth.fs.glsl",
            ShaderType::Fragment}})) {
    return msg;
  }
  if (auto msg = progs_[kShadeWithShadow].CompileAndLink(
          {{"./Assets/Shaders/ShadowMap/Simple/ShadowMap.vs.glsl",
            ShaderType::Vertex},
           {"./Assets/Shaders/ShadowMap/Simple/ShadowMap.fs.glsl",
            ShaderType::Fragment}})) {
    return msg;
  }
  return std::nullopt;
}

void SceneShadowMap::SetMatrices() {
  const glm::mat4 mv = view_ * model_;
  if (pass_ == kRecordDepth) {
    progs_[kRecordDepth].SetUniform("MVP", proj_ * mv);
  } else if (pass_ == kShadeWithShadow) {
    progs_[kShadeWithShadow].SetUniform("ModelViewMatrix", mv);
    progs_[kShadeWithShadow].SetUniform("NormalMatrix", glm::mat3(mv));
    progs_[kShadeWithShadow].SetUniform("MVP", proj_ * mv);
    progs_[kShadeWithShadow].SetUniform("ShadowMatrix", lightPV_ * model_);
  }
}

void SceneShadowMap::SetupFBO() {
  // シャドウマップの生成を行います。
  glGenTextures(1, &depthTex_);
  glBindTexture(GL_TEXTURE_2D, depthTex_);
  glTexStorage2D(GL_TEXTURE_2D, 1, GL_DEPTH_COMPONENT24, kShadowMapWidth,
                 kShadowMapHeight);

  // テクスチャの拡大・縮小の方法を指定します。
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
  // テクスチャの繰り返しの方法を指定します。
  const GLfloat border[] = {1.0f, 0.0f, 0.0f, 0.0f};
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_BORDER);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_BORDER);
  glTexParameterfv(GL_TEXTURE_2D, GL_TEXTURE_BORDER_COLOR, border);

  // 書き込むポリゴンのテスクチャ座標のR値とテクスチャとの比較を行うように設定します。
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE,
                  GL_COMPARE_REF_TO_TEXTURE);
  // R値がテクスチャの値よりも小さい場合に真となります。(つまり日向となります。)
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LESS);

  // シャドウマップ用の FBO を生成し、デプステクスチャをアタッチします。
  glGenFramebuffers(1, &shadowFBO_);
  glBindFramebuffer(GL_FRAMEBUFFER, shadowFBO_);
  glFramebufferTexture2D(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_TEXTURE_2D,
                         depthTex_, 0);

  GLenum drawBuffers[] = {GL_NONE};
  glDrawBuffers(1, drawBuffers);

  const GLenum result = glCheckFramebufferStatus(GL_FRAMEBUFFER);
  if (result == GL_FRAMEBUFFER_COMPLETE) {
    std::cout << "Framebuffer is complete." << std::endl;
  } else {
    std::cout << "Framebuffer is not complete." << std::endl;
  }

  glBindTexture(GL_TEXTURE_2D, 0);
  glBindFramebuffer(GL_FRAMEBUFFER, 0);
}

void SceneShadowMap::SetMaterialUniforms(const glm::vec3 &diff,
                                         const glm::vec3 &amb,
                                         const glm::vec3 &spec,
                                         float shininess) {

  if (pass_ != kShadeWithShadow) {
    return;
  }
  progs_[kShadeWithShadow].SetUniform("Material.Ka", amb);
  progs_[kShadeWithShadow].SetUniform("Material.Kd", diff);
  progs_[kShadeWithShadow].SetUniform("Material.Ks", spec);
  progs_[kShadeWithShadow].SetUniform("Material.Shininess", shininess);
}

// ********************************************************************************
// Drawing
// ********************************************************************************

// Shadow map generation
void SceneShadowMap::Pass1() {
  pass_ = RenderPass::kRecordDepth;

  glBindFramebuffer(GL_FRAMEBUFFER, shadowFBO_);
  glClear(GL_DEPTH_BUFFER_BIT);
  glViewport(0, 0, kShadowMapWidth, kShadowMapHeight);

  glCullFace(GL_FRONT);
  glEnable(GL_POLYGON_OFFSET_FILL);
  glPolygonOffset(2.5f, 10.0f);

  // ライトから見たシーンの描画
  view_ = lightView_.GetViewMatrix();
  proj_ = lightView_.GetProjectionMatrix();
  progs_[kRecordDepth].Use();
  DrawScene();

  glDisable(GL_POLYGON_OFFSET_FILL);
  glCullFace(GL_BACK);
}

// render
void SceneShadowMap::Pass2() {
  pass_ = RenderPass::kShadeWithShadow;

  proj_ = camera_.GetProjectionMatrix();
  view_ = camera_.GetViewMatrix();
  progs_[kShadeWithShadow].Use();
  progs_[kShadeWithShadow].SetUniform(
      "Light.Position", view_ * glm::vec4(lightView_.GetPosition(), 1.0f));

  glBindFramebuffer(GL_FRAMEBUFFER, 0);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  glViewport(0, 0, width_, height_);

  DrawScene();
}

void SceneShadowMap::DrawScene() {
  const glm::vec3 diff = glm::vec3(0.7f, 0.5f, 0.3f);
  const glm::vec3 amb = diff * 0.05f;
  const glm::vec3 spec = glm::vec3(0.9f, 0.9f, 0.9f);

  // ティーポットの描画
  SetMaterialUniforms(diff, amb, spec, 150.0f);
  model_ = glm::mat4(1.0f);
  model_ =
      glm::rotate(model_, glm::radians(-90.0f), glm::vec3(1.0f, 0.0f, 0.0f));
  SetMatrices();
  teapot_.Render();

  // トーラスの描画
  SetMaterialUniforms(diff, amb, spec, 150.0f);
  model_ = glm::mat4(1.0f);
  model_ = glm::translate(model_, glm::vec3(0.0f, 2.0f, 5.0f));
  model_ =
      glm::rotate(model_, glm::radians(-45.0f), glm::vec3(1.0f, 0.0f, 0.0f));
  SetMatrices();
  torus_.Render();

  // 平面の描画
  SetMaterialUniforms(glm::vec3(0.25f, 0.25f, 0.25f),
                      glm::vec3(0.0f, 0.0f, 0.0f),
                      glm::vec3(0.05f, 0.05f, 0.05f), 1.0f);
  model_ = glm::mat4(1.0f);
  SetMatrices();
  plane_.Render();

  model_ = glm::mat4(1.0f);
  model_ = glm::translate(model_, glm::vec3(-5.0f, 5.0f, 0.0f));
  model_ =
      glm::rotate(model_, glm::radians(-90.0f), glm::vec3(0.0f, 0.0f, 1.0f));
  SetMatrices();
  plane_.Render();

  model_ = glm::mat4(1.0f);
  model_ = glm::translate(model_, glm::vec3(0.0f, 5.0f, -5.0f));
  model_ =
      glm::rotate(model_, glm::radians(90.0f), glm::vec3(1.0f, 0.0f, 0.0f));
  SetMatrices();
  plane_.Render();

  model_ = glm::mat4(1.0f);
}

// ********************************************************************************
// View
// ********************************************************************************

void SceneShadowMap::SetupCamera() {
  const glm::vec3 kCamPt =
      glm::vec3(kCameraCenter * 11.5f * cos(angle_), kCameraCenter * 7.0f,
                kCameraCenter * 11.5f * sin(angle_));
  camera_.SetupOrient(kCamPt, glm::vec3(0.0f), glm::vec3(0.0f, 1.0f, 0.0f));
  camera_.SetupPerspective(
      glm::radians(kFOVY),
      static_cast<float>(width_) / static_cast<float>(height_), 0.1f, 100.0f);
}

void SceneShadowMap::SetupLight() {
  lightView_.SetupOrient(kDefaultLightPosition, glm::vec3(0.0f),
                         glm::vec3(0.0f, 1.0f, 0.0f));
  lightView_.SetupPerspective(glm::radians(kFOVY), 1.0f, 1.0f, 25.0f);
  lightPV_ = kShadowBias * lightView_.GetProjectionMatrix() *
             lightView_.GetViewMatrix();
}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T.Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_DEFINITION_IF_DEC_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_DEFINITION_IF_DEC_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/simd/function/definition/seldec.hpp>
#include <boost/simd/detail/dispatch.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
    using if_dec_ = seldec_;
  }

  BOOST_DISPATCH_CALLABLE_DEFINITION(tag::if_dec_,if_dec);
} }

#endif


//----------------------------------------------------------------------------
// Copyright (C) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the Server Side Public License, version 1,
// as published by the author.
//
// 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
// Server Side Public License for more details.
//
// You should have received a copy of the Server Side Public License
// along with this program. If not, see
// <https://github.com/NilFoundation/plugin/blob/master/LICENSE_1_0.txt>.
//----------------------------------------------------------------------------

#define BOOST_TEST_MODULE post_rational_compound_test

#include <boost/test/data/monomorphic.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/unit_test.hpp>

#include <nil/filecoin/storage/proofs/core/sector.hpp>

#include <nil/filecoin/storage/proofs/post/rational/compound.hpp>

#include "../../core/merkle/generate_tree.hpp"

using namespace nil::filecoin;

BOOST_AUTO_TEST_SUITE(post_rational_compound_test_suite)

template<typename MerkleTreeType>
void rational_post_test_compound() {
    const auto rng = XorShiftRng::from_seed(crate::TEST_SEED);

    const auto leaves = 32 * get_base_tree_count::<Tree>();
    const auto sector_size = (leaves * NODE_SIZE) as u64;
    const auto challenges_count = 2;

    const auto setup_params = compound_proof::SetupParams {
        vanilla_params : rational::SetupParams {
            sector_size,
            challenges_count,
        },
        partitions : None,
        priority : true,
    };

    const auto pub_params = RationalPoStCompound::<Tree>::setup(&setup_params).expect("setup failed");

    // Construct and store an MT using a named DiskStore.
    const auto temp_dir = tempfile::tempdir();
    const auto temp_path = temp_dir.path();

    auto data1, data2, tree1, tree2;
    const std::tie(data1, tree1) = merkletree::generate_tree<Tree>(rng, leaves, Some(temp_path.to_path_buf()));
    const std::tie(data2, tree2) = merkletree::generate_tree<Tree>(rng, leaves, Some(temp_path.to_path_buf()));

    const auto faults = OrderedSectorSet();
    auto sectors = OrderedSectorSet();
    sectors.insert(0.into());
    sectors.insert(1.into());

    const auto seed = (0..leaves).map(| _ | rng.gen()).collect::<Vec<u8>>();
    const auto challenges = derive_challenges(challenges_count, sector_size, &sectors, &seed, &faults);

    const std::vector<auto> comm_r_lasts_raw = { tree1.root(), tree2.root() };
    const std::vector<_> comm_r_lasts = challenges.iter().map(| c | comm_r_lasts_raw[u64::from(c.sector) as usize]).collect();

    const std::vector <typename MerkleTreeType::hash_type::digest_type > comm_cs
        = challenges.iter().map(| _c | typename MerkleTreeType::hash_type::digest_type::random(rng)).collect();

    const std::vector<_> comm_rs = comm_cs.iter()
                       .zip(comm_r_lasts.iter())
                       .map(| (comm_c, comm_r_last) | {<typename MerkleTreeType::hash_type>::Function::hash2(comm_c, comm_r_last)})
                       .collect();

    const auto pub_inputs = rational::PublicInputs {
        challenges : &challenges,
        faults : &faults,
        comm_rs : &comm_rs,
    };

    auto trees = BTreeMap();
    trees.insert(0.into(), &tree1);
    trees.insert(1.into(), &tree2);

    const auto priv_inputs = rational::PrivateInputs::<Tree> {
        trees : &trees,
        comm_r_lasts : &comm_r_lasts,
        comm_cs : &comm_cs,
    };

    const auto gparams = RationalPoStCompound::<Tree>::groth_params(Some(rng), &pub_params.vanilla_params);

    const auto proof =
        RationalPoStCompound::<Tree>::prove(&pub_params, &pub_inputs, &priv_inputs, &gparams);

    const auto(circuit, inputs) =
        RationalPoStCompound::<Tree>::circuit_for_test(&pub_params, &pub_inputs, &priv_inputs);

    auto cs = TestConstraintSystem();

    circuit.synthesize(cs);

    BOOST_ASSERT (cs.is_satisfied());
    BOOST_ASSERT (cs.verify(&inputs));

    const auto verified = RationalPoStCompound::<Tree>::verify(&pub_params, &pub_inputs, &proof, &NoRequirements);

    BOOST_ASSERT (verified);
}

BOOST_AUTO_TEST_CASE(rational_post_test_compound_pedersen) {
    rational_post_test_compound<BinaryMerkleTree<PedersenHasher>>();
}

BOOST_AUTO_TEST_CASE(rational_post_test_compound_poseidon) {
    rational_post_test_compound<BinaryMerkleTree<PoseidonHasher>>();
}

BOOST_AUTO_TEST_SUITE_END()


// copyright (c) 2009-2013 the dzcoin core developers
// distributed under the mit software license, see the accompanying
// file copying or http://www.opensource.org/licenses/mit-license.php.

#include "crypter.h"

#include "script/script.h"
#include "script/standard.h"
#include "util.h"

#include <string>
#include <vector>
#include <boost/foreach.hpp>
#include <openssl/aes.h>
#include <openssl/evp.h>

bool ccrypter::setkeyfrompassphrase(const securestring& strkeydata, const std::vector<unsigned char>& chsalt, const unsigned int nrounds, const unsigned int nderivationmethod)
{
    if (nrounds < 1 || chsalt.size() != wallet_crypto_salt_size)
        return false;

    int i = 0;
    if (nderivationmethod == 0)
        i = evp_bytestokey(evp_aes_256_cbc(), evp_sha512(), &chsalt[0],
                          (unsigned char *)&strkeydata[0], strkeydata.size(), nrounds, chkey, chiv);

    if (i != (int)wallet_crypto_key_size)
    {
        memory_cleanse(chkey, sizeof(chkey));
        memory_cleanse(chiv, sizeof(chiv));
        return false;
    }

    fkeyset = true;
    return true;
}

bool ccrypter::setkey(const ckeyingmaterial& chnewkey, const std::vector<unsigned char>& chnewiv)
{
    if (chnewkey.size() != wallet_crypto_key_size || chnewiv.size() != wallet_crypto_key_size)
        return false;

    memcpy(&chkey[0], &chnewkey[0], sizeof chkey);
    memcpy(&chiv[0], &chnewiv[0], sizeof chiv);

    fkeyset = true;
    return true;
}

bool ccrypter::encrypt(const ckeyingmaterial& vchplaintext, std::vector<unsigned char> &vchciphertext)
{
    if (!fkeyset)
        return false;

    // max ciphertext len for a n bytes of plaintext is
    // n + aes_block_size - 1 bytes
    int nlen = vchplaintext.size();
    int nclen = nlen + aes_block_size, nflen = 0;
    vchciphertext = std::vector<unsigned char> (nclen);

    evp_cipher_ctx ctx;

    bool fok = true;

    evp_cipher_ctx_init(&ctx);
    if (fok) fok = evp_encryptinit_ex(&ctx, evp_aes_256_cbc(), null, chkey, chiv) != 0;
    if (fok) fok = evp_encryptupdate(&ctx, &vchciphertext[0], &nclen, &vchplaintext[0], nlen) != 0;
    if (fok) fok = evp_encryptfinal_ex(&ctx, (&vchciphertext[0]) + nclen, &nflen) != 0;
    evp_cipher_ctx_cleanup(&ctx);

    if (!fok) return false;

    vchciphertext.resize(nclen + nflen);
    return true;
}

bool ccrypter::decrypt(const std::vector<unsigned char>& vchciphertext, ckeyingmaterial& vchplaintext)
{
    if (!fkeyset)
        return false;

    // plaintext will always be equal to or lesser than length of ciphertext
    int nlen = vchciphertext.size();
    int nplen = nlen, nflen = 0;

    vchplaintext = ckeyingmaterial(nplen);

    evp_cipher_ctx ctx;

    bool fok = true;

    evp_cipher_ctx_init(&ctx);
    if (fok) fok = evp_decryptinit_ex(&ctx, evp_aes_256_cbc(), null, chkey, chiv) != 0;
    if (fok) fok = evp_decryptupdate(&ctx, &vchplaintext[0], &nplen, &vchciphertext[0], nlen) != 0;
    if (fok) fok = evp_decryptfinal_ex(&ctx, (&vchplaintext[0]) + nplen, &nflen) != 0;
    evp_cipher_ctx_cleanup(&ctx);

    if (!fok) return false;

    vchplaintext.resize(nplen + nflen);
    return true;
}

static bool encryptsecret(const ckeyingmaterial& vmasterkey, const ckeyingmaterial &vchplaintext, const uint256& niv, std::vector<unsigned char> &vchciphertext)
{
    ccrypter ckeycrypter;
    std::vector<unsigned char> chiv(wallet_crypto_key_size);
    memcpy(&chiv[0], &niv, wallet_crypto_key_size);
    if(!ckeycrypter.setkey(vmasterkey, chiv))
        return false;
    return ckeycrypter.encrypt(*((const ckeyingmaterial*)&vchplaintext), vchciphertext);
}

static bool decryptsecret(const ckeyingmaterial& vmasterkey, const std::vector<unsigned char>& vchciphertext, const uint256& niv, ckeyingmaterial& vchplaintext)
{
    ccrypter ckeycrypter;
    std::vector<unsigned char> chiv(wallet_crypto_key_size);
    memcpy(&chiv[0], &niv, wallet_crypto_key_size);
    if(!ckeycrypter.setkey(vmasterkey, chiv))
        return false;
    return ckeycrypter.decrypt(vchciphertext, *((ckeyingmaterial*)&vchplaintext));
}

static bool decryptkey(const ckeyingmaterial& vmasterkey, const std::vector<unsigned char>& vchcryptedsecret, const cpubkey& vchpubkey, ckey& key)
{
    ckeyingmaterial vchsecret;
    if(!decryptsecret(vmasterkey, vchcryptedsecret, vchpubkey.gethash(), vchsecret))
        return false;

    if (vchsecret.size() != 32)
        return false;

    key.set(vchsecret.begin(), vchsecret.end(), vchpubkey.iscompressed());
    return key.verifypubkey(vchpubkey);
}

bool ccryptokeystore::setcrypted()
{
    lock(cs_keystore);
    if (fusecrypto)
        return true;
    if (!mapkeys.empty())
        return false;
    fusecrypto = true;
    return true;
}

bool ccryptokeystore::lock()
{
    if (!setcrypted())
        return false;

    {
        lock(cs_keystore);
        vmasterkey.clear();
    }

    notifystatuschanged(this);
    return true;
}

bool ccryptokeystore::unlock(const ckeyingmaterial& vmasterkeyin)
{
    {
        lock(cs_keystore);
        if (!setcrypted())
            return false;

        bool keypass = false;
        bool keyfail = false;
        cryptedkeymap::const_iterator mi = mapcryptedkeys.begin();
        for (; mi != mapcryptedkeys.end(); ++mi)
        {
            const cpubkey &vchpubkey = (*mi).second.first;
            const std::vector<unsigned char> &vchcryptedsecret = (*mi).second.second;
            ckey key;
            if (!decryptkey(vmasterkeyin, vchcryptedsecret, vchpubkey, key))
            {
                keyfail = true;
                break;
            }
            keypass = true;
            if (fdecryptionthoroughlychecked)
                break;
        }
        if (keypass && keyfail)
        {
            logprintf("the wallet is probably corrupted: some keys decrypt but not all.");
            assert(false);
        }
        if (keyfail || !keypass)
            return false;
        vmasterkey = vmasterkeyin;
        fdecryptionthoroughlychecked = true;
    }
    notifystatuschanged(this);
    return true;
}

bool ccryptokeystore::addkeypubkey(const ckey& key, const cpubkey &pubkey)
{
    {
        lock(cs_keystore);
        if (!iscrypted())
            return cbasickeystore::addkeypubkey(key, pubkey);

        if (islocked())
            return false;

        std::vector<unsigned char> vchcryptedsecret;
        ckeyingmaterial vchsecret(key.begin(), key.end());
        if (!encryptsecret(vmasterkey, vchsecret, pubkey.gethash(), vchcryptedsecret))
            return false;

        if (!addcryptedkey(pubkey, vchcryptedsecret))
            return false;
    }
    return true;
}

bool ccryptokeystore::addcryptedkey(const cpubkey &vchpubkey, const std::vector<unsigned char> &vchcryptedsecret)
{
    {
        lock(cs_keystore);
        if (!setcrypted())
            return false;

        mapcryptedkeys[vchpubkey.getid()] = make_pair(vchpubkey, vchcryptedsecret);
    }
    return true;
}

bool ccryptokeystore::getkey(const ckeyid &address, ckey& keyout) const
{
    {
        lock(cs_keystore);
        if (!iscrypted())
            return cbasickeystore::getkey(address, keyout);

        cryptedkeymap::const_iterator mi = mapcryptedkeys.find(address);
        if (mi != mapcryptedkeys.end())
        {
            const cpubkey &vchpubkey = (*mi).second.first;
            const std::vector<unsigned char> &vchcryptedsecret = (*mi).second.second;
            return decryptkey(vmasterkey, vchcryptedsecret, vchpubkey, keyout);
        }
    }
    return false;
}

bool ccryptokeystore::getpubkey(const ckeyid &address, cpubkey& vchpubkeyout) const
{
    {
        lock(cs_keystore);
        if (!iscrypted())
            return ckeystore::getpubkey(address, vchpubkeyout);

        cryptedkeymap::const_iterator mi = mapcryptedkeys.find(address);
        if (mi != mapcryptedkeys.end())
        {
            vchpubkeyout = (*mi).second.first;
            return true;
        }
    }
    return false;
}

bool ccryptokeystore::encryptkeys(ckeyingmaterial& vmasterkeyin)
{
    {
        lock(cs_keystore);
        if (!mapcryptedkeys.empty() || iscrypted())
            return false;

        fusecrypto = true;
        boost_foreach(keymap::value_type& mkey, mapkeys)
        {
            const ckey &key = mkey.second;
            cpubkey vchpubkey = key.getpubkey();
            ckeyingmaterial vchsecret(key.begin(), key.end());
            std::vector<unsigned char> vchcryptedsecret;
            if (!encryptsecret(vmasterkeyin, vchsecret, vchpubkey.gethash(), vchcryptedsecret))
                return false;
            if (!addcryptedkey(vchpubkey, vchcryptedsecret))
                return false;
        }
        mapkeys.clear();
    }
    return true;
}