#include "kfeaturelbp.h"
#include "kpicinfo.h"
#include "common.h"

#include "gdal_priv.h"
#include "cpl_conv.h" // for CPLMalloc()

#include <boost/dynamic_bitset.hpp>
#include <boost/math/special_functions/sin_pi.hpp>
#include <boost/math/special_functions/cos_pi.hpp>

#include <QCoreApplication>
#include <QFile>
#include <QDebug>

#include <map>
#include <utility>
#include <limits>

// test the extend function
//    KFeatureLBP *t= new KFeatureLBP();
//    float a[20]={1,2,3,4,
//             2,3,4,5,
//             3,4,5,6,
//             4,5,6,7,
//             5,6,7,8};
//    float *b = new float(110);
//    t->replicateExtend(a,b,10,11);
//    for(int i=0;i<11;++i)
//    {
//        for(int j=0;j<10;++j)
//        {
//            std::cout<<int(b[10*i+j])<<" ";
//        }
//        std::cout<<"\r\n";
//    }
//    std::cout<<"end"<<std::endl;
//    delete b;
KFeatureLBP::~KFeatureLBP()
{
    CPLFree(m_pHistogram);

    for(int bandIndex = 0;bandIndex<m_bandNum;++bandIndex){
        //qDebug()<<m_extDataBuff[bandIndex];
        CPLFree(m_extDataBuff[bandIndex]);
    }
    CPLFree(m_extDataBuff);
}

KFeatureLBP::KFeatureLBP(GDALDataset *piDataset, GDALDataset *poDataset, int sampleNum, int kernelRadius, bool improved)
    : m_piDataset(piDataset),
      m_poDataset(poDataset),
      m_kernelRadius(kernelRadius),
      m_sampleNum(sampleNum),
      m_beImproved(improved),
      m_fileName(""),
      refTable(NULL)
{

}

bool KFeatureLBP::calLBP(float *inBuff, GByte *outBuff, int width, int height,bool toBeNormarlized, KProgressBar * pProgressBar)
{
    assert(NULL != outBuff);
    assert(NULL != inBuff);

    boost::dynamic_bitset<> tempOne(m_sampleNum,1);
    boost::dynamic_bitset<> tempZero(m_sampleNum,0);

    GUIntBig maxValue = get2Power(m_sampleNum);

    for(int iYPos = m_kernelRadius,iYDes=0;iYPos<height-m_kernelRadius;++iYPos,++iYDes)
    {
        for(int iXPos = m_kernelRadius,iXDes=0;iXPos<width-m_kernelRadius;++iXPos,++iXDes)
        {
            boost::dynamic_bitset<> tempbin(m_sampleNum,0);
            // calculate anticlockwise
            for(int nsCnt=0;nsCnt<m_sampleNum;++nsCnt)
            {
                float fxAxis = iXPos + m_kernelRadius*boost::math::cos_pi(2.*nsCnt/m_sampleNum);
                float fyAxis = iYPos - m_kernelRadius*boost::math::sin_pi(2.*nsCnt/m_sampleNum);
                int ixAxis = static_cast<int>(fxAxis);
                int iyAxis = static_cast<int>(fyAxis);
                fxAxis = fxAxis-ixAxis;
                fyAxis = fyAxis-iyAxis;
                /** bilinear interpolation algorithm
                 * y x-------
                 * |(0,0)
                 * |  |a   b|
                 * |  | x,y | --> gray(x,y)=(1-x)*[(1-y)*a + y*c] + x*[(1-y)*b + y*d]
                 * |  |c   d|
                 *
                 *                  |f(0,0) f(1,0)||1-y|
                 * gray(x,y)=[1-x,x]|             ||   |
                 *                  |f(0,1) f(1,1)|| y |
                 */
                float tempValue = (1-fxAxis)*((1-fyAxis)*inBuff[iyAxis*width+ixAxis]
                        +fyAxis*inBuff[(iyAxis+1)*width+ixAxis])
                        +fxAxis*((1-fyAxis)*inBuff[iyAxis*width+ixAxis+1]
                        +fyAxis*inBuff[(iyAxis+1)*width+ixAxis+1]);

                tempbin <<= 1;
                tempbin |= tempValue>inBuff[iYPos*width+iXPos]?tempOne:tempZero;
            }
            if(NULL != pProgressBar) pProgressBar->autoUpdate();
            if(toBeNormarlized==false)
            {
                if(m_beImproved) outBuff[iYDes*(width-2*m_kernelRadius)+iXDes]=refTable[tempbin.to_ulong()];
                else outBuff[iYDes*(width-2*m_kernelRadius)+iXDes]=tempbin.to_ulong()*255./maxValue;
            }
            else{
                if(m_beImproved) outBuff[iYDes*(width-2*m_kernelRadius)+iXDes]=refTable[tempbin.to_ulong()]*255./(m_sampleNum+2.);
                // here we suppose that the traditional lbp is normarlized anyway
                else outBuff[iYDes*(width-2*m_kernelRadius)+iXDes]=tempbin.to_ulong()*255./maxValue;
            }

//            tempbin=tempZero;
//            tempbin <<= 1;
//            tempbin |= inBuff[iYPos*width+iXPos+1]>inBuff[iYPos*width+iXPos]?tempOne:tempZero;
//            tempbin <<= 1;
//            tempbin |= inBuff[(iYPos-1)*width+iXPos+1]>inBuff[iYPos*width+iXPos]?tempOne:tempZero;
//            tempbin <<= 1;
//            tempbin |= inBuff[(iYPos-1)*width+iXPos]>inBuff[iYPos*width+iXPos]?tempOne:tempZero;
//            tempbin <<= 1;
//            tempbin |= inBuff[(iYPos-1)*width+iXPos-1]>inBuff[iYPos*width+iXPos]?tempOne:tempZero;
//            tempbin <<= 1;
//            tempbin |= inBuff[iYPos*width+iXPos-1]>inBuff[iYPos*width+iXPos]?tempOne:tempZero;
//            tempbin <<= 1;
//            tempbin |= inBuff[(iYPos+1)*width+iXPos-1]>inBuff[iYPos*width+iXPos]?tempOne:tempZero;
//            tempbin <<= 1;
//            tempbin |= inBuff[(iYPos+1)*width+iXPos]>inBuff[iYPos*width+iXPos]?tempOne:tempZero;
//            tempbin <<= 1;
//            tempbin |= inBuff[(iYPos+1)*width+iXPos+1]>inBuff[iYPos*width+iXPos]?tempOne:tempZero;
//            outBuff[iYDes*(width-2*m_kernelRadius)+iXDes]=tempbin.to_ulong();
        }
    }

    return true;
}

bool KFeatureLBP::run(Kapok::K_BorderTypes type,bool toBeNormarlized)
{
    int bandNum = KPicInfo::getInstance()->getBandNum();
    int nXSize = KPicInfo::getInstance()->getWidth();
    int nYSize = KPicInfo::getInstance()->getHeight();

    //GDALRasterBand * piBand = NULL;
    GDALRasterBand * poBand = NULL;
    //float *pafData = NULL;
    GByte *pafOutData = NULL;
    int err_code = 1 + bandNum;
    if(externDataSet(type)) err_code -= 1;

    if(m_beImproved){
        // create the reference table
        GUIntBig maxIndex = get2Power(m_sampleNum);
        GUIntBig minValue = 0;
        KProgressBar progressBar("Calculating LBP refTable",maxIndex,80);
        K_PROGRESS_START(progressBar);
        for(GUIntBig index = 0;index < maxIndex;++index)
        {
            // Achieving Rotation Invariance
            minValue = index;
            // leave the template args to be empty means determined by the compiler
            boost::dynamic_bitset<> bin(m_sampleNum, index);
            for(int pos = 0;pos<m_sampleNum;++pos){
                boost::dynamic_bitset<> tempbin=(bin>>pos)|(bin<<m_sampleNum-pos);
                if(minValue>tempbin.to_ulong()){
                    minValue = tempbin.to_ulong();
                }
            }
            // confirm the "Uniform" Patterns
            boost::dynamic_bitset<> temp(m_sampleNum,minValue);
            GByte cycleSum = abs(temp[0] - temp[m_sampleNum - 1]);
            // the default value is set to be the num of bits which are set
            refTable[index] = bin.count();
            for(GByte pos = 0;pos < m_sampleNum - 1;++pos)
            {
                cycleSum += abs(temp[pos]-temp[pos+1]);
                if(cycleSum > 2){
                    refTable[index] = m_sampleNum+1;
                    break;
                }
            }
            progressBar.autoUpdate();
        }
        K_PROGRESS_END(progressBar);
    }

    KProgressBar progressBar("Calculating LBP feature",bandNum*nXSize*nYSize,80);
    K_PROGRESS_START(progressBar);
    // get the LBP feature
    pafOutData = (GByte *) CPLMalloc(sizeof(GByte)*nXSize*nYSize);
    //pafData = (float *) CPLMalloc(sizeof(float)*(nXSize + 2*m_kernelRadius + 1)*(nYSize + 2*m_kernelRadius + 1));
    for(int index = 0; index < bandNum; ++index)
    {
        //piBand = m_piDataset->GetRasterBand(index + 1);
        poBand = m_poDataset->GetRasterBand(index + 1);

//        piBand->RasterIO( GF_Read, 0, 0, nXSize + 2*m_kernelRadius + 1, nYSize + 2*m_kernelRadius + 1
//                          , pafData, nXSize + 2*m_kernelRadius + 1, nYSize + 2*m_kernelRadius + 1
//                          , GDT_Float32, 0, 0 );

        if(calLBP(m_extDataBuff[index],pafOutData,nXSize + 2*m_kernelRadius,nYSize + 2*m_kernelRadius,toBeNormarlized,&progressBar)) err_code -= 1;

        poBand->RasterIO( GF_Write, 0, 0, nXSize , nYSize, pafOutData
                          , nXSize, nYSize, GDT_Byte, 0, 0 );
        poBand->FlushCache();

    }
    K_PROGRESS_END(progressBar);

    GDALClose(m_poDataset);

    std::map<int,long> histMap;
    int maxValue = (std::numeric_limits<int>::min)();

    if(!m_beImproved || toBeNormarlized) maxValue=256;
    else maxValue = m_sampleNum+2;

    for(int index = 0;index<maxValue;++index) histMap[index]=0;
    for(long index = 0;index<nXSize*nYSize;++index){ histMap[pafOutData[index]]++; }
    m_pHistogram = (long *) CPLMalloc(sizeof(long)*maxValue+sizeof(long));
    m_pHistogram[0]=maxValue;
    // key start at 0
    for(std::map<int,long>::iterator it = histMap.begin();it != histMap.end();++it){
        m_pHistogram[it->first+1]=it->second;
    }

    // release
    if(NULL != refTable) CPLFree(refTable);
    //CPLFree(pafData);
    CPLFree(pafOutData);
//    char ** filelist;// =m_piDataset->GetFileList();
//    GDALClose(m_piDataset);

//    // just fetch the first one
//    QString temp(*filelist);
//    CSLDestroy (filelist);
//    QFile file(temp);
//    if(!file.remove()) std::cout<<"KFeatureLBP:remove the temp file failed!"<<std::endl;
//    //qDebug()<<"close";

    return (0 == err_code);
}

GDALDataset *KFeatureLBP::build(QString fileName)
{
    m_fileName = fileName;
    if(KPicInfo::dataAttach(m_piDataset)) KPicInfo::getInstance()->build();
    //qDebug()<<"failed";qDebug()<<KPicInfo::dataAttach(m_piDataset);
    int bandNum = KPicInfo::getInstance()->getBandNum();
    int nXSize = KPicInfo::getInstance()->getWidth();
    int nYSize = KPicInfo::getInstance()->getHeight();

    if(m_sampleNum > 24){
        std::cout<<"KFeatureLBP:Sample points cannot more than 24!"<<std::endl;
        exit( 1 );
    }

    bool beSame = K_CheckDataSetEqu(m_piDataset,m_poDataset);

    if(!beSame){
        QString tempName=QCoreApplication::applicationDirPath()+"/tempImg%%KFeatureLBP";
        //QString tempInputName=QCoreApplication::applicationDirPath()+"/tempExtImg%%KFeatureLBP"+KPicInfo::getInstance()->getFileExtName();
        //QString tempName="D:/tempImg%%KFeatureLBP";
        //QString tempInputName="D:/tempExtImg%%KFeatureLBP"+KPicInfo::getInstance()->getFileExtName();
        if(!m_fileName.isEmpty()){ tempName = m_fileName; }

        const char *pszFormat = m_piDataset->GetDriverName();
        GDALDriver *poDriver = GetGDALDriverManager()->GetDriverByName(pszFormat);
        if( poDriver == NULL )
        {
            std::cout<<"KFeatureLBP:GetGDALDriverManager failed!"<<std::endl;
            exit( 1 );
        }
        if( CSLFetchBoolean( poDriver->GetMetadata(), GDAL_DCAP_CREATE, FALSE ) )
        {
            //qDebug( "KFeatureLBP:Driver %s supports Create() method.", pszFormat );
            m_realExtName = KPicInfo::getInstance()->getFileExtName();
        }
        else
        {
            poDriver = GetGDALDriverManager()->GetDriverByName("BMP");
            m_realExtName = ".bmp";
        }
        tempName += m_realExtName;
        if(NULL != m_poDataset) GDALClose(m_poDataset);
        // allocate the output Dataset
        m_poDataset = poDriver->Create(tempName.toUtf8().data(),nXSize,nYSize
                                       ,bandNum,GDT_Byte,0);
        // store the previous handle
        //m_piOrgDataset = m_piDataset;
        // +1 is to guarantee the success of bilinear interpolation algorithm
        // build the new handle to store the extended image
        //qDebug()<<"failed";qDebug()<<KPicInfo::getInstance()->getType();
        m_extDataBuff = (float **) CPLMalloc(sizeof(float *)*bandNum);
        m_bandNum = bandNum;
        for(int bandIndex = 0;bandIndex<bandNum;++bandIndex){
            m_extDataBuff[bandIndex] = (float *) CPLMalloc(sizeof(float)*(nXSize+2*m_kernelRadius + 1)*(nYSize+2*m_kernelRadius + 1));
        }
//        m_piDataset = poDriver->Create(tempInputName.toUtf8().data()
//                                       ,nXSize+2*m_kernelRadius + 1,nYSize+2*m_kernelRadius + 1
//                                       ,bandNum,KPicInfo::getInstance()->getType(),0);

    }else{
        std::cout<<"KFeatureLBP:the input and output cannot be same!"<<std::endl;
        m_poDataset = NULL;
    }

    assert(NULL != m_poDataset);

    if(NULL != refTable) CPLFree(refTable);
    if(m_beImproved) refTable = (unsigned char *) CPLMalloc(sizeof(unsigned char)*get2Power(m_sampleNum));
    else refTable=NULL;

    return m_poDataset;
}

bool KFeatureLBP::constExtend(float * inBuff, float * outBuff, int width, int height, float defaultValue)
{
    assert(NULL != outBuff);
    assert(NULL != inBuff);

    // fill the outbuff with the default value
    for(GIntBig index = 0;index < (width + 1) * (height + 1);++index)
    {
        outBuff[index] = defaultValue;
    }
    // copy the source to the outbuff
    for(int iYDes = m_kernelRadius,iYSrc = 0;iYDes < height - m_kernelRadius;++iYDes,++iYSrc)
    {
        for(int iXDes = m_kernelRadius,iXSrc = 0;iXDes < width - m_kernelRadius;++iXDes,++iXSrc)
        {
            outBuff[iYDes*width+iXDes] = inBuff[iYSrc*(width-2*m_kernelRadius)+iXSrc];
        }
    }
    return true;
}

bool KFeatureLBP::reflectExtend(float * inBuff, float * outBuff, int width, int height)
{
    assert(NULL != outBuff);
    assert(NULL != inBuff);

    if(width < 3 * m_kernelRadius || height < 3 * m_kernelRadius)
    {
        std::cout<<"KFeatureLBP:each line of the image cannot be shorter than KernelSize!"<<std::endl;
        exit( 1 );
    }
    // copy the source to the outbuff
    for(int iYDes = m_kernelRadius,iYSrc = 0;iYDes < height - m_kernelRadius;++iYDes,++iYSrc)
    {
        for(int iXDes = m_kernelRadius,iXSrc = 0;iXDes < width - m_kernelRadius;++iXDes,++iXSrc)
        {
            outBuff[iYDes*width+iXDes] = inBuff[iYSrc*(width-2*m_kernelRadius)+iXSrc];
        }
    }

    // fill horizontal
    for(int iYDes = m_kernelRadius;iYDes < height - m_kernelRadius;++iYDes)
    {
        // fill the head of each line
        int iXDes = m_kernelRadius-1,iXSrc = m_kernelRadius+1;
        for(;iXDes >= 0 && iXSrc < width - m_kernelRadius;--iXDes,++iXSrc)
        {
            outBuff[iYDes*width+iXDes] = outBuff[iYDes*width+iXSrc];
        }
        for(;iXDes >= 0;--iXDes)
        {
            outBuff[iYDes*width+iXDes] = outBuff[iYDes*width+iXSrc-1];
        }
        // fill the end of each line
        iXDes = width - m_kernelRadius,iXSrc = width - m_kernelRadius - 2;
        for(;iXDes < width + 1&& iXSrc >= m_kernelRadius;++iXDes,--iXSrc)
        {
            outBuff[iYDes*width+iXDes] = outBuff[iYDes*width+iXSrc];
        }
        for(;iXDes < width + 1;++iXDes)
        {
            outBuff[iYDes*width+iXDes] = outBuff[iYDes*width+iXSrc+1];
        }
    }

    // fill vertical -- think rotate 90 degrees
    for(int iYDes = m_kernelRadius;iYDes < width - m_kernelRadius;++iYDes)
    {
        // fill the head of each line
        int iXDes = m_kernelRadius-1,iXSrc = m_kernelRadius+1;
        for(;iXDes >= 0 && iXSrc < height - m_kernelRadius;--iXDes,++iXSrc)
        {
            outBuff[iXDes*width+iYDes] = outBuff[iXSrc*width+iYDes];
        }
        for(;iXDes >= 0;--iXDes)
        {
            outBuff[iXDes*width+iYDes] = outBuff[(iXSrc-1)*width+iYDes];
        }
        // fill the end of each line
        iXDes = height - m_kernelRadius,iXSrc = height - m_kernelRadius - 2;
        for(;iXDes < height + 1 && iXSrc >= m_kernelRadius;++iXDes,--iXSrc)
        {
            outBuff[iXDes*width+iYDes] = outBuff[iXSrc*width+iYDes];
        }
        for(;iXDes < height + 1;++iXDes)
        {
            outBuff[iXDes*width+iYDes] = outBuff[(iXSrc+1)*width+iYDes];
        }
    }

    // fill four corners, please find the axis of symmetry carefully
    for(int iYDes = 0;iYDes < m_kernelRadius;++iYDes)
    {
        for(int iXDes = 0;iXDes < m_kernelRadius;++iXDes)
        {
            outBuff[iYDes*width+iXDes] = (outBuff[(2*m_kernelRadius-iYDes)*width+iXDes]+outBuff[iYDes*width+2*m_kernelRadius-iXDes])/2;
        }
    }
    for(int iYDes = 0;iYDes < m_kernelRadius;++iYDes)
    {
        for(int iXDes = width-m_kernelRadius;iXDes < width + 1;++iXDes)
        {
            outBuff[iYDes*width+iXDes] = (outBuff[(2*m_kernelRadius-iYDes)*width+iXDes]+outBuff[iYDes*width+2*(width-m_kernelRadius-1)-iXDes])/2;
        }
    }
    for(int iYDes = height - m_kernelRadius;iYDes < height + 1;++iYDes)
    {
        for(int iXDes = 0;iXDes < m_kernelRadius;++iXDes)
        {
            outBuff[iYDes*width+iXDes] = (outBuff[(2*(height-m_kernelRadius-1)-iYDes)*width+iXDes]+outBuff[iYDes*width+2*m_kernelRadius-iXDes])/2;
        }
    }
    for(int iYDes = height - m_kernelRadius;iYDes < height + 1;++iYDes)
    {
        for(int iXDes = width - m_kernelRadius;iXDes < width + 1;++iXDes)
        {
            outBuff[iYDes*width+iXDes] = (outBuff[(2*(height-m_kernelRadius-1)-iYDes)*width+iXDes]+outBuff[iYDes*width+2*(width-m_kernelRadius-1)-iXDes])/2;
        }
    }
    return true;
}

bool KFeatureLBP::replicateExtend(float * inBuff, float * outBuff, int width, int height)
{
    assert(NULL != outBuff);
    assert(NULL != inBuff);

    if(width < 3 * m_kernelRadius || height < 3 * m_kernelRadius)
    {
        std::cout<<"KFeatureLBP:each line of the image cannot be shorter than KernelSize!"<<std::endl;
        exit( 1 );
    }
    // copy the source to the outbuff
    for(int iYDes = m_kernelRadius,iYSrc = 0;iYDes < height - m_kernelRadius;++iYDes,++iYSrc)
    {
        for(int iXDes = m_kernelRadius,iXSrc = 0;iXDes < width - m_kernelRadius;++iXDes,++iXSrc)
        {
            outBuff[iYDes*width+iXDes] = inBuff[iYSrc*(width-2*m_kernelRadius)+iXSrc];
        }
    }

    // fill horizontal
    for(int iYDes = m_kernelRadius;iYDes < height - m_kernelRadius;++iYDes)
    {
        // fill the head of each line
        int iXDes = m_kernelRadius-1,iXSrc = m_kernelRadius;
        for(;iXDes >= 0 && iXSrc < width - m_kernelRadius;--iXDes)
        {
            outBuff[iYDes*width+iXDes] = outBuff[iYDes*width+iXSrc];
        }
        for(;iXDes >= 0;--iXDes)
        {
            outBuff[iYDes*width+iXDes] = outBuff[iYDes*width+iXSrc];
        }
        // fill the end of each line
        iXDes = width - m_kernelRadius,iXSrc = width - m_kernelRadius - 1;
        for(;iXDes < width + 1 && iXSrc >= m_kernelRadius;++iXDes)
        {
            outBuff[iYDes*width+iXDes] = outBuff[iYDes*width+iXSrc];
        }
        for(;iXDes < width + 1;++iXDes)
        {
            outBuff[iYDes*width+iXDes] = outBuff[iYDes*width+iXSrc];
        }
    }

    // fill vertical -- think rotate 90 degrees
    for(int iYDes = m_kernelRadius;iYDes < width - m_kernelRadius;++iYDes)
    {
        // fill the head of each line
        int iXDes = m_kernelRadius-1,iXSrc = m_kernelRadius;
        for(;iXDes >= 0 && iXSrc < height - m_kernelRadius;--iXDes)
        {
            outBuff[iXDes*width+iYDes] = outBuff[iXSrc*width+iYDes];
        }
        for(;iXDes >= 0;--iXDes)
        {
            outBuff[iXDes*width+iYDes] = outBuff[iXSrc*width+iYDes];
        }
        // fill the end of each line
        iXDes = height - m_kernelRadius,iXSrc = height - m_kernelRadius - 1;
        for(;iXDes < height + 1 && iXSrc >= m_kernelRadius;++iXDes)
        {
            outBuff[iXDes*width+iYDes] = outBuff[iXSrc*width+iYDes];
        }
        for(;iXDes < height + 1;++iXDes)
        {
            outBuff[iXDes*width+iYDes] = outBuff[iXSrc*width+iYDes];
        }
    }

    // fill four corners, please find the axis of symmetry carefully
    for(int iYDes = 0;iYDes < m_kernelRadius;++iYDes)
    {
        for(int iXDes = 0;iXDes < m_kernelRadius;++iXDes)
        {
            outBuff[iYDes*width+iXDes] = outBuff[m_kernelRadius*width+m_kernelRadius];
        }
    }
    for(int iYDes = 0;iYDes < m_kernelRadius;++iYDes)
    {
        for(int iXDes = width-m_kernelRadius;iXDes < width + 1;++iXDes)
        {
            outBuff[iYDes*width+iXDes] = outBuff[m_kernelRadius*width+width-m_kernelRadius-1];
        }
    }
    for(int iYDes = height - m_kernelRadius;iYDes < height + 1;++iYDes)
    {
        for(int iXDes = 0;iXDes < m_kernelRadius;++iXDes)
        {
            outBuff[iYDes*width+iXDes] = outBuff[(height-m_kernelRadius-1)*width+m_kernelRadius];
        }
    }
    for(int iYDes = height - m_kernelRadius;iYDes < height + 1;++iYDes)
    {
        for(int iXDes = width - m_kernelRadius;iXDes < width + 1;++iXDes)
        {
            outBuff[iYDes*width+iXDes] = outBuff[(height-m_kernelRadius-1)*width+width-m_kernelRadius-1];
        }
    }
    return true;
}

bool KFeatureLBP::externDataSet(Kapok::K_BorderTypes type,float defaultValue)
{
    int bandNum = KPicInfo::getInstance()->getBandNum();
    int nXSize = KPicInfo::getInstance()->getWidth();
    int nYSize = KPicInfo::getInstance()->getHeight();
    unsigned int err = bandNum;
    GDALRasterBand * piBand = NULL;
    //GDALRasterBand * poBand = NULL;
    float *pafData = NULL;
    //float *pafOutData = NULL;
    pafData = (float *) CPLMalloc(sizeof(float)*nXSize*nYSize);
    //pafOutData = (float *) CPLMalloc(sizeof(float)*(nXSize + 2*m_kernelRadius + 1)*(nYSize + 2*m_kernelRadius + 1));
    for(int index = 0; index < bandNum; ++index)
    {
        piBand = m_piDataset->GetRasterBand(index + 1);
        //poBand = m_piDataset->GetRasterBand(index + 1);

        piBand->RasterIO( GF_Read, 0, 0, nXSize, nYSize, pafData, nXSize, nYSize, GDT_Float32, 0, 0 );
        switch(type)
        {
        case Kapok::Border_Constant:
            err -= constExtend(pafData, m_extDataBuff[index], nXSize + 2*m_kernelRadius, nYSize + 2*m_kernelRadius, defaultValue);
            break;
        case Kapok::Border_Replicate:
            err -= replicateExtend(pafData, m_extDataBuff[index], nXSize + 2*m_kernelRadius, nYSize + 2*m_kernelRadius);
            break;
        case Kapok::Border_Reflect:
        default:
            err -= reflectExtend(pafData, m_extDataBuff[index], nXSize + 2*m_kernelRadius, nYSize + 2*m_kernelRadius);
            break;
        }

//        poBand->RasterIO( GF_Write, 0, 0, nXSize + 2*m_kernelRadius + 1
//                          , nYSize + 2*m_kernelRadius + 1, pafOutData
//                          , nXSize + 2*m_kernelRadius + 1, nYSize + 2*m_kernelRadius + 1, GDT_Float32, 0, 0 );
//        poBand->FlushCache();
    }
    CPLFree(pafData);
    //CPLFree(pafOutData);

    return !err;
}

QString KFeatureLBP::getSVMString(int start)
{
    int size = m_pHistogram[0];
    QString temp("");
    if(NULL==m_pHistogram) exit(1);
//    for(int index= start;index<start+size;++index){
//        temp+=QString("%1:%%2 ").arg(index).arg(index-start+1);
//    }
    for(int index = 1;index<size+1;++index){
        temp+=QString("%1:%2 ").arg(index-1+start).arg(m_pHistogram[index]);
        //temp=QString(temp).arg(m_pHistogram[index]);
        //qDebug()<<temp;
    }
    return temp;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2019 Mikhail Komarov <nemo@nil.foundation>
//
// MIT License
//
// 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 CRYPTO3_STREAM_SALSA20_POLICY_HPP
#define CRYPTO3_STREAM_SALSA20_POLICY_HPP

#include <boost/endian/conversion.hpp>

#include <nil/crypto3/detail/inline_variable.hpp>

#include <nil/crypto3/stream/detail/basic_functions.hpp>

#define SALSA20_QUARTER_ROUND(x1, x2, x3, x4) \
    do {                                      \
        x2 ^= policy_type::rotl<7>(x1 + x4);  \
        x3 ^= policy_type::rotl<9>(x2 + x1);  \
        x4 ^= policy_type::rotl<13>(x3 + x2); \
        x1 ^= policy_type::rotl<18>(x4 + x3); \
    } while (0)

namespace nil {
    namespace crypto3 {
        namespace stream {
            namespace detail {
                template<std::size_t IVBits, std::size_t KeyBits, std::size_t Rounds>
                struct salsa20_policy : public basic_functions<32> {
                    typedef basic_functions<32> policy_type;

                    typedef typename policy_type::byte_type byte_type;
                    typedef typename policy_type::word_type word_type;

                    constexpr static const std::size_t rounds = Rounds;
                    BOOST_STATIC_ASSERT(Rounds % 2 == 0);

                    constexpr static const std::size_t value_bits = CHAR_BIT;
                    typedef byte_type value_type;

                    constexpr static const std::size_t block_size = 64;
                    constexpr static const std::size_t block_bits = block_size * value_bits;
                    typedef std::array<byte_type, block_size> block_type;

                    constexpr static const std::size_t min_key_bits = 16 * CHAR_BIT;
                    constexpr static const std::size_t max_key_bits = 32 * CHAR_BIT;
                    constexpr static const std::size_t key_bits = KeyBits;
                    constexpr static const std::size_t key_size = key_bits / CHAR_BIT;
                    BOOST_STATIC_ASSERT(min_key_bits <= KeyBits <= max_key_bits);
                    BOOST_STATIC_ASSERT(key_size % 16 == 0);
                    typedef std::array<byte_type, key_size> key_type;

                    constexpr static const std::size_t key_schedule_size = 16;
                    constexpr static const std::size_t key_schedule_bits = key_schedule_size * word_bits;
                    typedef std::array<word_type, key_schedule_size> key_schedule_type;

                    constexpr static const std::size_t round_constants_size = 4;
                    typedef std::array<word_type, round_constants_size> round_constants_type;

                    CRYPTO3_INLINE_VARIABLE(round_constants_type, tau,
                                            ({0x61707865, 0x3120646e, 0x79622d36, 0x6b206574}));
                    CRYPTO3_INLINE_VARIABLE(round_constants_type, sigma,
                                            ({0x61707865, 0x3320646e, 0x79622d32, 0x6b206574}));

                    constexpr static const std::size_t iv_bits = IVBits;
                    constexpr static const std::size_t iv_size = IVBits / CHAR_BIT;
                    typedef std::array<byte_type, iv_size> iv_type;

                    static void hsalsa20(word_type output[8], const key_schedule_type input) {
                        word_type x00 = input[0], x01 = input[1], x02 = input[2], x03 = input[3], x04 = input[4],
                                  x05 = input[5], x06 = input[6], x07 = input[7], x08 = input[8], x09 = input[9],
                                  x10 = input[10], x11 = input[11], x12 = input[12], x13 = input[13], x14 = input[14],
                                  x15 = input[15];

                        for (size_t i = 0; i != rounds / 2; ++i) {
                            SALSA20_QUARTER_ROUND(x00, x04, x08, x12);
                            SALSA20_QUARTER_ROUND(x05, x09, x13, x01);
                            SALSA20_QUARTER_ROUND(x10, x14, x02, x06);
                            SALSA20_QUARTER_ROUND(x15, x03, x07, x11);

                            SALSA20_QUARTER_ROUND(x00, x01, x02, x03);
                            SALSA20_QUARTER_ROUND(x05, x06, x07, x04);
                            SALSA20_QUARTER_ROUND(x10, x11, x08, x09);
                            SALSA20_QUARTER_ROUND(x15, x12, x13, x14);
                        }

                        output[0] = x00;
                        output[1] = x05;
                        output[2] = x10;
                        output[3] = x15;
                        output[4] = x06;
                        output[5] = x07;
                        output[6] = x08;
                        output[7] = x09;
                    }

                    static void salsa_core(block_type &block, const key_schedule_type &input) {
                        word_type x00 = input[0], x01 = input[1], x02 = input[2], x03 = input[3], x04 = input[4],
                                  x05 = input[5], x06 = input[6], x07 = input[7], x08 = input[8], x09 = input[9],
                                  x10 = input[10], x11 = input[11], x12 = input[12], x13 = input[13], x14 = input[14],
                                  x15 = input[15];

                        for (size_t i = 0; i != rounds / 2; ++i) {
                            SALSA20_QUARTER_ROUND(x00, x04, x08, x12);
                            SALSA20_QUARTER_ROUND(x05, x09, x13, x01);
                            SALSA20_QUARTER_ROUND(x10, x14, x02, x06);
                            SALSA20_QUARTER_ROUND(x15, x03, x07, x11);

                            SALSA20_QUARTER_ROUND(x00, x01, x02, x03);
                            SALSA20_QUARTER_ROUND(x05, x06, x07, x04);
                            SALSA20_QUARTER_ROUND(x10, x11, x08, x09);
                            SALSA20_QUARTER_ROUND(x15, x12, x13, x14);
                        }

                        boost::endian::store_little_u32(x00 + input[0], block[4 * 0]);
                        boost::endian::store_little_u32(x01 + input[1], block[4 * 1]);
                        boost::endian::store_little_u32(x02 + input[2], block[4 * 2]);
                        boost::endian::store_little_u32(x03 + input[3], block[4 * 3]);
                        boost::endian::store_little_u32(x04 + input[4], block[4 * 4]);
                        boost::endian::store_little_u32(x05 + input[5], block[4 * 5]);
                        boost::endian::store_little_u32(x06 + input[6], block[4 * 6]);
                        boost::endian::store_little_u32(x07 + input[7], block[4 * 7]);
                        boost::endian::store_little_u32(x08 + input[8], block[4 * 8]);
                        boost::endian::store_little_u32(x09 + input[9], block[4 * 9]);
                        boost::endian::store_little_u32(x10 + input[10], block[4 * 10]);
                        boost::endian::store_little_u32(x11 + input[11], block[4 * 11]);
                        boost::endian::store_little_u32(x12 + input[12], block[4 * 12]);
                        boost::endian::store_little_u32(x13 + input[13], block[4 * 13]);
                        boost::endian::store_little_u32(x14 + input[14], block[4 * 14]);
                        boost::endian::store_little_u32(x15 + input[15], block[4 * 15]);
                    }
                };
            }    // namespace detail
        }        // namespace stream
    }            // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_SALSA20_POLICY_HPP


/**
 * ****************************************************************************
 * Copyright (c) 2015, Robert Lukierski.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.
 *
 * Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * Neither the name of the copyright holder nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * ****************************************************************************
 * N-Sphere.
 * ****************************************************************************
 */

#ifndef VISIONCORE_TYPES_HYPERSPHERE_HPP
#define VISIONCORE_TYPES_HYPERSPHERE_HPP

#include <VisionCore/Platform.hpp>

#include <Eigen/Dense>

// https://en.wikipedia.org/wiki/N-sphere

namespace vc
{

namespace types
{

template<typename _Scalar, int _Dimension = 0> class Hypersphere;

template <typename T> using CircleT = Hypersphere<T,1>;
template <typename T> using SphereT = Hypersphere<T,2>;

namespace internal
{

template<typename T, int Dimension>
struct helper_surface_area;

template<typename T, int Dimension>
struct helper_volume
{
    EIGEN_DEVICE_FUNC static inline T calc(T radius)
    {
        return (radius / T(Dimension)) * helper_surface_area<T,Dimension-1>::calc(radius);
    }
};

template<typename T, int Dimension>
struct helper_surface_area
{
    EIGEN_DEVICE_FUNC static inline T calc(T radius)
    {
        return T(2.0 * M_PI) * radius * helper_volume<T,Dimension-1>::calc(radius);
    }
};

template<typename T>
struct helper_surface_area<T,0>
{
    EIGEN_DEVICE_FUNC static inline T calc(T radius) { return T(2.0); }
};

template<typename T>
struct helper_volume<T,0>
{
    EIGEN_DEVICE_FUNC static inline T calc(T radius) { return T(1.0); }
};

}

}

}

namespace Eigen
{
    namespace internal
    {
        template<typename _Scalar, int _Dimension>
        struct traits<vc::types::Hypersphere<_Scalar,_Dimension> >
        {
            static constexpr int Dimension = _Dimension;
            typedef _Scalar Scalar;
            typedef Matrix<Scalar,_Dimension+1,1> CoeffType;
        };

        template<typename _Scalar, int _Dimension, int _Options>
        struct traits<Map<vc::types::Hypersphere<_Scalar,_Dimension>, _Options> >
            : traits<vc::types::Hypersphere<_Scalar, _Dimension> >
        {
            static constexpr int Dimension = _Dimension;
            typedef _Scalar Scalar;
            typedef Map<Matrix<Scalar,_Dimension+1,1>, _Options> CoeffType;
        };

        template<typename _Scalar, int _Dimension, int _Options>
        struct traits<Map<const vc::types::Hypersphere<_Scalar,_Dimension>, _Options> >
            : traits<const vc::types::Hypersphere<_Scalar, _Dimension> >
        {
            static constexpr int Dimension = _Dimension;
            typedef _Scalar Scalar;
            typedef Map<const Matrix<Scalar,_Dimension+1,1>, _Options> CoeffType;
        };

    }
}

namespace vc
{

namespace types
{

template<typename Derived>
class HypersphereBase
{
public:
    static constexpr int Dimension = Eigen::internal::traits<Derived>::Dimension;
    typedef typename Eigen::internal::traits<Derived>::Scalar Scalar;
    typedef typename Eigen::internal::traits<Derived>::CoeffType CoeffType;
    typedef Eigen::Matrix<Scalar,Dimension+1,1> VectorT;

    static inline constexpr int dimension() { return Dimension; }

    template<typename NewScalarType>
    EIGEN_DEVICE_FUNC inline Hypersphere<NewScalarType,Dimension> cast() const
    {
        return Hypersphere<NewScalarType,Dimension>(coeff().template cast<NewScalarType>(),
                                                    (NewScalarType)radius());
    }

    EIGEN_DEVICE_FUNC CoeffType& coeff()
    {
        return static_cast<Derived*>(this)->coeff_nonconst();
    }

    EIGEN_DEVICE_FUNC const CoeffType& coeff() const
    {
        return static_cast<const Derived*>(this)->coeff_const();
    }

    EIGEN_DEVICE_FUNC Scalar& radius()
    {
        return static_cast<Derived*>(this)->radius_nonconst();
    }

    EIGEN_DEVICE_FUNC const Scalar& radius() const
    {
        return static_cast<const Derived*>(this)->radius_const();
    }

    template<typename OtherDerived>
    EIGEN_DEVICE_FUNC inline HypersphereBase<Derived>& operator=(const HypersphereBase<OtherDerived>& other)
    {
        coeff() = other.coeff();
        return *this;
    }

    EIGEN_DEVICE_FUNC inline Scalar volume()
    {
        using Eigen::numext::pow;
        return (pow(Scalar(M_PI), Scalar(Dimension+1)/Scalar(2.0)) / (tgamma(Scalar(Dimension+1)/Scalar(2.0) + Scalar(1.0)))) * pow(radius(), (Scalar)(Dimension+1) );
    }

    template<int OtherDimension>
    EIGEN_DEVICE_FUNC inline Scalar volume()
    {
        return internal::helper_volume<Scalar,OtherDimension+1>::calc(radius());
    }

    EIGEN_DEVICE_FUNC inline Scalar surfaceArea()
    {
        return internal::helper_surface_area<Scalar,Dimension>::calc(radius());
    }

    template<int OtherDimension>
    EIGEN_DEVICE_FUNC inline Scalar surfaceArea()
    {
        return internal::helper_surface_area<Scalar,OtherDimension>::calc(radius());
    }

    EIGEN_DEVICE_FUNC inline Scalar operator()(const VectorT& x) const
    {
        return evaluate(x);
    }

    EIGEN_DEVICE_FUNC inline Scalar evaluate(const VectorT& x) const
    {
        Scalar sum = Scalar(0.0);

        for(int i = 0 ; i < Dimension + 1 ; ++i)
        {
            const Scalar term = coeff()(i) - x(i);
            sum += (term * term);
        }

        return sqrt(sum);
    }

    EIGEN_DEVICE_FUNC inline bool isPointInside(const VectorT& x) const
    {
        return evaluate(x) < radius();
    }

    EIGEN_DEVICE_FUNC inline void setZero()
    {
        coeff().setZero();
        radius() = Scalar(0.0);
    }

#ifdef VISIONCORE_ENABLE_CEREAL
    template<typename Archive>
    void load(Archive & archive, std::uint32_t const version)
    {
        archive(cereal::make_nvp("Coefficients", coeff()));
        archive(cereal::make_nvp("Radius", radius()));
    }

    template<typename Archive>
    void save(Archive & archive, std::uint32_t const version) const
    {
        archive(cereal::make_nvp("Coefficients", coeff()));
        archive(cereal::make_nvp("Radius", radius()));
    }
#endif // VISIONCORE_ENABLE_CEREAL
};

/**
 * Generic Hypersphere.
 */
template<typename _Scalar, int _Dimension>
class Hypersphere : public HypersphereBase<Hypersphere<_Scalar,_Dimension>>
{
    typedef HypersphereBase<Hypersphere<_Scalar,_Dimension>> Base;
public:
    static constexpr int Dimension = Eigen::internal::traits<Hypersphere>::Dimension;
    typedef typename Eigen::internal::traits<Hypersphere>::Scalar Scalar;
    typedef typename Eigen::internal::traits<Hypersphere>::CoeffType CoeffType;

    friend class vc::types::HypersphereBase<Hypersphere<_Scalar,_Dimension>>;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    //EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Hypersphere)
    using Base::operator=;

    EIGEN_DEVICE_FUNC inline Hypersphere()
    {
    }

    EIGEN_DEVICE_FUNC inline Hypersphere(const Hypersphere<_Scalar,_Dimension>& other) : coeff_(other.coeff()), radius_(other.radius())
    {
    }

    template<typename OtherDerived>
    EIGEN_DEVICE_FUNC inline Hypersphere(const HypersphereBase<OtherDerived>& other) : coeff_(other.coeff()), radius_(other.radius())
    {
    }

    EIGEN_DEVICE_FUNC inline Hypersphere(const CoeffType& coeff, const Scalar& r) : coeff_(coeff), radius_(r)
    {
    }

    EIGEN_DEVICE_FUNC inline ~Hypersphere()
    {
    }

protected:
    EIGEN_DEVICE_FUNC inline const CoeffType& coeff_const() const { return coeff_; }
    EIGEN_DEVICE_FUNC inline CoeffType& coeff_nonconst() { return coeff_; }

    EIGEN_DEVICE_FUNC inline const Scalar& radius_const() const { return radius_; }
    EIGEN_DEVICE_FUNC inline Scalar& radius_nonconst() { return radius_; }

    CoeffType coeff_;
    Scalar radius_;
};

template<typename _Scalar, int _Dimension>
inline std::ostream& operator<<(std::ostream& os, const Hypersphere<_Scalar,_Dimension>& p)
{
    os << _Dimension << "-Sphere(r = " << p.radius() << ", " << p.coeff() << ")";
    return os;
}

}

}

namespace Eigen
{
/**
 * Specialisation of Eigen::Map for Hypersphere.
 */
template<typename _Scalar, int _Dimension, int _Options>
class Map<vc::types::Hypersphere<_Scalar,_Dimension>, _Options>
    : public vc::types::HypersphereBase<Map<vc::types::Hypersphere<_Scalar,_Dimension>, _Options> >
{
    typedef vc::types::HypersphereBase<Map<vc::types::Hypersphere<_Scalar,_Dimension>, _Options> > Base;

public:
    static constexpr int Dimension = Eigen::internal::traits<Map>::Dimension;
    typedef typename Eigen::internal::traits<Map>::Scalar Scalar;
    typedef typename Eigen::internal::traits<Map>::CoeffType CoeffType;

    friend class vc::types::HypersphereBase<Map<vc::types::Hypersphere<_Scalar,_Dimension>, _Options> >;

    EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)

    EIGEN_DEVICE_FUNC inline Map(Scalar* coeffs) : coeff_(coeffs), radius_(coeffs + Dimension + 1)
    {
    }

protected:
    EIGEN_DEVICE_FUNC inline const CoeffType& coeff_const() const { return coeff_; }
    EIGEN_DEVICE_FUNC inline CoeffType& coeff_nonconst() { return coeff_; }

    EIGEN_DEVICE_FUNC inline const Scalar& radius_const() const { return *radius_; }
    EIGEN_DEVICE_FUNC inline Scalar& radius_nonconst() { return *radius_; }

    CoeffType coeff_;
    Scalar* radius_;
};

/**
 * Specialisation of Eigen::Map for const Hypersphere.
 */
template<typename _Scalar, int _Dimension, int _Options>
class Map<const vc::types::Hypersphere<_Scalar,_Dimension>, _Options>
    : public vc::types::HypersphereBase<Map<const vc::types::Hypersphere<_Scalar,_Dimension>, _Options> >
{
    typedef vc::types::HypersphereBase<Map<const vc::types::Hypersphere<_Scalar,_Dimension>, _Options> > Base;

public:
    static constexpr int Dimension = Eigen::internal::traits<Map>::Dimension;
    typedef typename Eigen::internal::traits<Map>::Scalar Scalar;
    typedef typename Eigen::internal::traits<Map>::CoeffType CoeffType;

    friend class vc::types::HypersphereBase<Map<const vc::types::Hypersphere<_Scalar,_Dimension>, _Options> >;

    EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)

    EIGEN_DEVICE_FUNC inline Map(const Scalar* coeffs) : coeff_(coeffs), radius_(coeffs + Dimension + 1)
    {
    }

protected:
    EIGEN_DEVICE_FUNC inline const CoeffType& coeff_const() const { return coeff_; }
    EIGEN_DEVICE_FUNC inline const Scalar& radius_const() const { return *radius_; }

    const CoeffType coeff_;
    const Scalar* radius_;
};

}

#endif // VISIONCORE_TYPES_HYPERSPHERE_HPP


#include "engine.hpp"

#include <algorithm>
#include <numeric>
#include <cmath>
#include <functional> // for std::cref (pass reference of VectorField to odeint)
#include <boost/numeric/odeint/stepper/runge_kutta_cash_karp54.hpp>
#include <boost/numeric/odeint/integrate/integrate_adaptive.hpp>
#include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
#include <boost/numeric/odeint/stepper/generation.hpp> // make_controlled

#include "variable.hpp"
#include "sde.hpp" // SDE integrator

namespace odeint = boost::numeric::odeint;

typedef odeint::runge_kutta_cash_karp54<RealVec> real_vec_error_stepper_type;

constexpr bool VERBOSE = false; // TODO: better way to handle message printing

const double Engine::dt_max = 1.0;
const double Engine::dt_init = 0.001;
const double Engine::h_init = 0.01; // TODO: can be relaxed with higher-order solvers!

Engine::Engine(const State & s0, const std::vector<Transition*> transitions,
    const Parameters & par) : s0(s0), h(h_init), dt(dt_init), vf(s0, transitions, par) {
  // duplicate transitions
  replace_transitions(transitions); // copy transitions to this->transitions
}

Engine::Engine(const Engine & e) : s0(e.s0), h(e.h), dt(e.dt), vf(e.vf) {
  // copy constructor: duplicate transitions
  replace_transitions(e.transitions);
}

Engine & Engine::operator=(const Engine & e) { // copy assignment constructor
  if ( this != &e ) {
    s0 = e.s0;
    h = e.h;
    dt = e.dt;
    vf = e.vf;
    replace_transitions(e.transitions);
  } // else do nothing
  return *this;
}

void Engine::reset(const State & s0, const Parameters & par) {
  this->s0 = s0;
  // FIXME: reset algorithmic parameters??
  // FIXME: update VectorField?
}

State Engine::evolve_gillespie(State s, double tmax, const Parameters & p, Rng & rng) {
  while ( true ) {
    // choose the next event
    std::vector<double> rates(transitions.size(), 0.0);
    std::transform(transitions.begin(), transitions.end(), rates.begin(),
        [&] (Transition* x) -> double {return x->rate(s, p);});
    double lambda = std::accumulate(rates.begin(), rates.end(), 0.0);
    // check that lambda > 0 (otherwise jump to tmax)
    if ( lambda == 0 ) {
      s.t() = tmax;
      break;
    } // else, lambda > 0
    double time_increment = rng.exponential(lambda);
    // check that the next event takes place before tmax
    if ( std::nextafter(s.t() + time_increment, tmax) >= tmax ) {
      s.t() = tmax;
      break;
    } // else, apply the transition and incerment the time with dt

    Transition* trans = choose_random_transition(transitions, rates, lambda, rng);
    if ( trans == nullptr ) {
      throw std::logic_error("unable to randomly choose transition" + RIGHT_HERE);
    }
    s = trans->apply(s);
    s.t() += time_increment; // manually increase the time
    // TODO: make dt optional argument for Transition::apply?
  }
  return s;
}

State Engine::evolve_tauleap(State s, double tmax, const Parameters & p, Rng & rng) {
  while ( std::nextafter(s.t(), tmax) < tmax ) {
    // choose the next event
    std::vector<double> rates(transitions.size(), 0.0);
    std::transform(transitions.begin(), transitions.end(), rates.begin(),
        [&] (Transition* x) -> double {return x->rate(s, p);});
    double lambda = std::accumulate(rates.begin(), rates.end(), 0.0);
    // sample number of events
    int n = rng.poisson(lambda * dt);
    // do n transitions
    for ( int i = 0; i < n; ++i ) {
      Transition* trans = choose_random_transition(transitions, rates, lambda, rng);
      if ( trans == nullptr ) {
        throw std::logic_error("unable to randomly choose transition" + RIGHT_HERE);
      }
      s = trans->apply(s);
      // re-compute the rates..
      std::transform(transitions.begin(), transitions.end(), rates.begin(),
          [&] (Transition* x) -> double {return x->rate(s, p);});
      lambda = std::accumulate(rates.begin(), rates.end(), 0.0);
    }
    s.t() += dt; // manually increase the time
    // adjust the timestep
    if ( lambda > 0 ) {
      dt = std::min({dt_max, 1/lambda, tmax - s.t()}); // TODO: smoothen, use intermediate rates
    } else {
      dt = std::min(dt_max, tmax - s.t());
    }
  }
  return s;
}

State Engine::evolve_ode(State s, double tmax, const Parameters & p) {
  // make sure that all variables of the state are continuous
  for ( auto & var : s ) {
    var.make_continuous(); // make_continous makes the entire vector continuous
  }
  // initialize a vectorfield with the fully continous state
  vf.update(s, p);
  // integrate the system of ODEs
  if ( tmax > s.t() ) {
    std::tie(s, std::ignore) = integrate(tmax - s.t(), vf, s, h);
  }
  return s;
}

State Engine::evolve_sde(State s, double tmax, const Parameters & p, Rng & rng) {
  // make sure that all variables of the state are continuous
  for ( auto & var : s ) {
    var.make_continuous(); // make_continous makes the entire vector continuous
  }
  // initialize a vectorfield with the fully continous state
  vf.update(s, p);
  // integrate the system of SDEs
  if ( tmax > s.t() ) {
    std::tie(s, std::ignore) = sde_integrate(tmax - s.t(), vf, s, h, rng);
  }
  return s;
}

State Engine::evolve_hybrid(State s, double tmax,
    const Parameters & p, Rng & rng, bool sde) {
  // use THRESHOLD to define a backwards-compatible switcher
  VarTypeSwitcher switcher = [](const State & s, const Parameters & p) -> std::vector<bool> {
    std::vector<bool> ought_discrete(s.flat_size());
    for ( size_t i = 0; i < s.flat_size(); ++i ) {
      const Variable & var = s[i];
      if ( var.is_discrete )
        // keep discrete if below threshold + window/2
        ought_discrete[i] = (var.disc_value < THRESHOLD + WINDOW/2);
      else { // continuous
        // make discrete if below threshold - window/2
        ought_discrete[i] = (var.cont_value < THRESHOLD - WINDOW/2);
      }
    }
    return ought_discrete;
  };
  // now use the general method
  return evolve_hybrid(s, tmax, p, switcher, rng, sde);
}

State Engine::evolve_hybrid(State s, double tmax,
    const Parameters & p, const VarTypeSwitcher & switcher, Rng & rng, bool sde) {
  // make sure that the timestep "fits"
  while ( std::nextafter(s.t(), tmax) < tmax ) {
    /* test if we need to switch Variables from discrete
     * to continuous and vice versa. Do this first to
     * avoid problems with the initially defined state.
     */
    std::vector<bool> ought_discrete = switcher(s, p);
    for ( size_t i = 0; i < s.flat_size(); ++i ) {
      Variable & var = s[i];
      if ( var.is_discrete && !ought_discrete[i] ) {
        var.make_continuous();
        // print message for debugging etc.
        if ( VERBOSE ) {
          std::cout << "# switching from discrete to continuous" << std::endl;
        }
      }
      if ( !var.is_discrete && ought_discrete[i] ) {
        if ( VERBOSE && var.value() < 0 ) {
          std::cerr << "# WARNING: negative var detected: " << var << std::endl;
        }
        var.make_discrete();
        // be careful with the initial timestep...
        dt = std::min(dt, dt_init);
        // print message for debugging etc.
        if ( VERBOSE ) {
          std::cout << "# switching from continuous to discrete" << std::endl;
        }
      }
    }
    // re-assign indices whenever the signature of s has changed
    vf.update(s, p);
    // take a good time step based on instantaneous transition rate
    double lambda = vf.totalTransitionRate();
    if ( lambda > 0 ) {
      dt = std::min(dt_max, 1/lambda);
    } else {
      dt = dt_max;
    }
    double time_increment = std::min(dt, tmax - s.t());
    // integrate the deterministic variables and the loads
    std::map<Transition*, double> loads;
    if ( sde ) {
      // h is passed by ref and modified (TODO adaptive SDE integrator)
      std::tie(s, loads) = sde_integrate(time_increment, vf, s, h, rng);
    } else { // use ODE integrator
      // h is passed by ref and modified
      std::tie(s, loads) = integrate(time_increment, vf, s, h);
    }
    auto op = [] (double c, std::pair<Transition*, double> l) {return c + l.second;};
    double Lambda = std::accumulate(loads.begin(), loads.end(), 0.0, op);
    // sample number of events (0 if Lambda = 0)
    int n = (Lambda > 0 ? rng.poisson(Lambda) : 0); // Lambda = lambda * dt

    // do n transitions
    for ( int i = 0; i < n; ++i ) {
      if ( i > 0 ) {
        // re-compute the rates in order to apply another transition
        for ( auto & [trans, load] : loads ) {
          load = trans->rate(s, p) * dt; // approximate load by rate * dt
        }
        Lambda = std::accumulate(loads.begin(), loads.end(), 0.0, op);
        if ( Lambda == 0 ) {
          // print message for debugging etc.
          if ( VERBOSE ) {
            std::cerr << "# WARNING: sampling more than one event is not possible"
                      << RIGHT_HERE << std::endl;
          }
          break; // break for ( i = 0; i < n; ++i )
        }
      }
      // sample a random transition
      Transition* trans = choose_random_transition(loads, Lambda, rng);
      if ( trans == nullptr ) {
        throw std::logic_error("unable to choose random transition" + RIGHT_HERE);
      } // else..
      State s_prime = trans->apply(s);
      if ( s_prime.isNonNegative() ) {
        s = s_prime;
      } else {
        throw std::logic_error("about to do an illegal transition" + RIGHT_HERE);
      }
    } // for i = 0 ... n-1
  }
  return s;
}

/** Auxiliary integrate function is a wrapper around the Boost odeint methods.
 *
 * The State is "encoded" into a single vector, that can be handled by
 * odeint. After integration, the vector is decoded back into the State
 * object.
 */
std::pair<State, std::map<Transition*, double>>
    integrate(double dt, const VectorField & vf, const State & s, double & h) {
  // TODO: verify that s and vf are compatible
  auto controlled_stepper = odeint::make_controlled(1e-6, 1e-6, real_vec_error_stepper_type());
  // TODO: make the stepper member of engine
  RealVec y = vf.encode(s);

  double t_start = s.t();
  double t_end = s.t() + dt;
  double t_old = s.t(); // used to keep track of the timestep
  double h0 = h; // the initially proposed stepsize
  // observer is used to get h before final step to t_end
  auto observer = [&] (const RealVec & y, double t_new) {
    if ( t_old < t_new && t_new < t_end ) {
      h = t_new - t_old;
      t_old = t_new;
    }
  };
  size_t steps = odeint::integrate_adaptive(controlled_stepper,
      std::cref(vf), y, t_start, t_end, h0,
      observer);
  (void) steps; // TODO: do something with steps??

  State u = vf.decodeState(s.t()+dt, y);
  auto loads = vf.decodeLoads(s.t()+dt, y);
  return std::make_pair(u, loads);
}

/** Use an SDE integrator to evolve the State forward in time.
 */
std::pair<State, std::map<Transition*, double>>
    sde_integrate(double dt, const VectorField & vf, const State & s, double & h, Rng & rng) {
  // define the EM stepper
  EulerMaruyamaStepper stepper(rng); // rng is passed by ref
  // TODO: verify that s and vf are compatible
  RealVec y = vf.encode(s);

  double t_start = s.t();
  double t_end = s.t() + dt;
  double h0 = std::min(dt, h);
  size_t steps = odeint::integrate_const(stepper, std::cref(vf),
      y, t_start, t_end, h0);
  (void) steps; // TODO: do something with steps??

  State u = vf.decodeState(t_end, y);
  auto loads = vf.decodeLoads(t_end, y);
  return std::make_pair(u, loads);
}

Transition* choose_random_transition(const std::vector<Transition*> & transitions,
    const std::vector<double> & rates, double lambda, Rng & rng) {
  if ( lambda <= 0 ) {
    std::cerr << "# WARNING: lambda not positive: " << lambda << RIGHT_HERE << std::endl;
    return nullptr;
  }
  double u = 0.0;
  try {
    u = rng.uniform(0, lambda);
  } catch ( const std::exception & ex ) {
    std::cout << ex.what() << RIGHT_HERE << std::endl;
    throw ex;
  }
  auto transit = transitions.begin();
  for ( auto r : rates ) {
    u -= r;
    if ( u < 0 ) {
      break;
    } // else...
    ++transit;
  }
  if ( transit != transitions.end() ) {
    return *transit;
  } else {
    return nullptr; // signals failure
  }
}

Transition* choose_random_transition(const std::map<Transition*, double> & loads,
    double Lambda, Rng & rng) {
  if ( Lambda <= 0 ) {
    std::cerr << "# WARNING: lambda not positive: " << Lambda << RIGHT_HERE << std::endl;
    return nullptr;
  }
  double u = 0.0;
  try { // FIXME: Lambda can be nan!!
    u = rng.uniform(0, Lambda);
  } catch ( const std::exception & ex ) {
    std::cout << ex.what() << RIGHT_HERE << std::endl;
    throw ex;
  }
  //double u = rng.uniform(0, Lambda);
  for ( auto & load : loads ) {
    u -= load.second;
    if ( u < 0 ) {
      return load.first;
    }
  } // for load in loads
  return nullptr; // signals failure
}


/*********************************************************************************
 *  OKVIS - Open Keyframe-based Visual-Inertial SLAM
 *  Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 *
 *   * Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright notice,
 *     this list of conditions and the following disclaimer in the documentation
 *     and/or other materials provided with the distribution.
 *   * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of
 *     its contributors may be used to endorse or promote products derived from
 *     this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: Feb 3, 2015
 *      Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
 *********************************************************************************/

/**
 * @file implementation/RadialTangentialDistortion.hpp
 * @brief Header implementation file for the RadialTangentialDistortion class.
 * @author Stefan Leutenegger
 */

#include <Eigen/LU>
#include <iostream>

/// \brief vio Main namespace of this package.
namespace vio {
/// \brief cameras Namespace for camera-related functionality.
namespace cameras {

// The default constructor with all zero ki
RadialTangentialDistortion::RadialTangentialDistortion()
    : k1_(0.0), k2_(0.0), p1_(0.0), p2_(0.0) {
  parameters_.setZero();
}

// Constructor initialising ki
RadialTangentialDistortion::RadialTangentialDistortion(float k1, float k2,
                                                       float p1, float p2) {
  parameters_[0] = k1;
  parameters_[1] = k2;
  parameters_[2] = p1;
  parameters_[3] = p2;
  k1_ = k1;
  k2_ = k2;
  p1_ = p1;
  p2_ = p2;
}

bool RadialTangentialDistortion::setParameters(
    const Eigen::VectorXd& parameters) {
  if (parameters.cols() != NumDistortionIntrinsics) {
    return false;
  }
  parameters_ = parameters.cast<float>();
  k1_ = parameters[0];
  k2_ = parameters[1];
  p1_ = parameters[2];
  p2_ = parameters[3];
  return true;
}

bool RadialTangentialDistortion::distort(
    const Eigen::Vector2d& pointUndistorted,
    Eigen::Vector2d* pointDistorted) const {
  // just compute the distorted point
  const float u0 = pointUndistorted[0];
  const float u1 = pointUndistorted[1];
  const float mx_u = u0 * u0;
  const float my_u = u1 * u1;
  const float mxy_u = u0 * u1;
  const float rho_u = mx_u + my_u;
  const float rad_dist_u = k1_ * rho_u + k2_ * rho_u * rho_u;
  (*pointDistorted)[0] =
      u0 + u0 * rad_dist_u + 2.0 * p1_ * mxy_u + p2_ * (rho_u + 2.0 * mx_u);
  (*pointDistorted)[1] =
      u1 + u1 * rad_dist_u + 2.0 * p2_ * mxy_u + p1_ * (rho_u + 2.0 * my_u);
  return true;
}
bool RadialTangentialDistortion::distort(
    const Eigen::Vector2f& pointUndistorted,
    Eigen::Vector2f* pointDistorted) const {
  // just compute the distorted point
  const float u0 = pointUndistorted[0];
  const float u1 = pointUndistorted[1];
  const float mx_u = u0 * u0;
  const float my_u = u1 * u1;
  const float mxy_u = u0 * u1;
  const float rho_u = mx_u + my_u;
  const float rad_dist_u = k1_ * rho_u + k2_ * rho_u * rho_u;
  (*pointDistorted)[0] =
      u0 + u0 * rad_dist_u + 2.0 * p1_ * mxy_u + p2_ * (rho_u + 2.0 * mx_u);
  (*pointDistorted)[1] =
      u1 + u1 * rad_dist_u + 2.0 * p2_ * mxy_u + p1_ * (rho_u + 2.0 * my_u);
  return true;
}

bool RadialTangentialDistortion::distort(
    const Eigen::Vector2d& pointUndistorted, Eigen::Vector2d* pointDistorted,
    Eigen::Matrix2d* pointJacobian, Eigen::Matrix2Xd* parameterJacobian) const {
  // first compute the distorted point
  const float u0 = pointUndistorted[0];
  const float u1 = pointUndistorted[1];
  const float mx_u = u0 * u0;
  const float my_u = u1 * u1;
  const float mxy_u = u0 * u1;
  const float rho_u = mx_u + my_u;
  const float rad_dist_u = k1_ * rho_u + k2_ * rho_u * rho_u;
  (*pointDistorted)[0] =
      u0 + u0 * rad_dist_u + 2.0 * p1_ * mxy_u + p2_ * (rho_u + 2.0 * mx_u);
  (*pointDistorted)[1] =
      u1 + u1 * rad_dist_u + 2.0 * p2_ * mxy_u + p1_ * (rho_u + 2.0 * my_u);

  // next the Jacobian w.r.t. changes on the undistorted point
  Eigen::Matrix2d& J = *pointJacobian;
  J(0, 0) = 1 + rad_dist_u + k1_ * 2.0 * mx_u + k2_ * rho_u * 4 * mx_u +
            2.0 * p1_ * u1 + 6 * p2_ * u0;
  J(1, 0) = k1_ * 2.0 * u0 * u1 + k2_ * 4 * rho_u * u0 * u1 + p1_ * 2.0 * u0 +
            2.0 * p2_ * u1;
  J(0, 1) = J(1, 0);
  J(1, 1) = 1 + rad_dist_u + k1_ * 2.0 * my_u + k2_ * rho_u * 4 * my_u +
            6 * p1_ * u1 + 2.0 * p2_ * u0;

  if (parameterJacobian) {
    // the Jacobian w.r.t. intrinsics parameters
    Eigen::Matrix2Xd& J2 = *parameterJacobian;
    J2.resize(2, NumDistortionIntrinsics);
    const float r2 = rho_u;
    const float r4 = r2 * r2;

    //[ u0*(u0^2 + u1^2), u0*(u0^2 + u1^2)^2,       2*u0*u1, 3*u0^2 + u1^2]
    //[ u1*(u0^2 + u1^2), u1*(u0^2 + u1^2)^2, u0^2 + 3*u1^2,       2*u0*u1]

    J2(0, 0) = u0 * r2;
    J2(0, 1) = u0 * r4;
    J2(0, 2) = 2.0 * u0 * u1;
    J2(0, 3) = r2 + 2.0 * u0 * u0;

    J2(1, 0) = u1 * r2;
    J2(1, 1) = u1 * r4;
    J2(1, 2) = r2 + 2.0 * u1 * u1;
    J2(1, 3) = 2.0 * u0 * u1;
  }
  return true;
}
bool RadialTangentialDistortion::distort(
    const Eigen::Vector2f& pointUndistorted, Eigen::Vector2f* pointDistorted,
    Eigen::Matrix2f* pointJacobian, Eigen::Matrix2Xf* parameterJacobian) const {
  // first compute the distorted point
  const float u0 = pointUndistorted[0];
  const float u1 = pointUndistorted[1];
  const float mx_u = u0 * u0;
  const float my_u = u1 * u1;
  const float mxy_u = u0 * u1;
  const float rho_u = mx_u + my_u;
  const float rad_dist_u = k1_ * rho_u + k2_ * rho_u * rho_u;
  (*pointDistorted)[0] =
      u0 + u0 * rad_dist_u + 2.0 * p1_ * mxy_u + p2_ * (rho_u + 2.0 * mx_u);
  (*pointDistorted)[1] =
      u1 + u1 * rad_dist_u + 2.0 * p2_ * mxy_u + p1_ * (rho_u + 2.0 * my_u);

  // next the Jacobian w.r.t. changes on the undistorted point
  Eigen::Matrix2f& J = *pointJacobian;
  J(0, 0) = 1 + rad_dist_u + k1_ * 2.0 * mx_u + k2_ * rho_u * 4 * mx_u +
            2.0 * p1_ * u1 + 6 * p2_ * u0;
  J(1, 0) = k1_ * 2.0 * u0 * u1 + k2_ * 4 * rho_u * u0 * u1 + p1_ * 2.0 * u0 +
            2.0 * p2_ * u1;
  J(0, 1) = J(1, 0);
  J(1, 1) = 1 + rad_dist_u + k1_ * 2.0 * my_u + k2_ * rho_u * 4 * my_u +
            6 * p1_ * u1 + 2.0 * p2_ * u0;

  if (parameterJacobian) {
    // the Jacobian w.r.t. intrinsics parameters
    Eigen::Matrix2Xf& J2 = *parameterJacobian;
    J2.resize(2, NumDistortionIntrinsics);
    const float r2 = rho_u;
    const float r4 = r2 * r2;

    //[ u0*(u0^2 + u1^2), u0*(u0^2 + u1^2)^2,       2*u0*u1, 3*u0^2 + u1^2]
    //[ u1*(u0^2 + u1^2), u1*(u0^2 + u1^2)^2, u0^2 + 3*u1^2,       2*u0*u1]

    J2(0, 0) = u0 * r2;
    J2(0, 1) = u0 * r4;
    J2(0, 2) = 2.0 * u0 * u1;
    J2(0, 3) = r2 + 2.0 * u0 * u0;

    J2(1, 0) = u1 * r2;
    J2(1, 1) = u1 * r4;
    J2(1, 2) = r2 + 2.0 * u1 * u1;
    J2(1, 3) = 2.0 * u0 * u1;
  }
  return true;
}

bool RadialTangentialDistortion::distortWithExternalParameters(
    const Eigen::Vector2d& pointUndistorted, const Eigen::VectorXd& parameters,
    Eigen::Vector2d* pointDistorted, Eigen::Matrix2d* pointJacobian,
    Eigen::Matrix2Xd* parameterJacobian) const {
  const float k1 = parameters[0];
  const float k2 = parameters[1];
  const float p1 = parameters[2];
  const float p2 = parameters[3];
  // first compute the distorted point
  const float u0 = pointUndistorted[0];
  const float u1 = pointUndistorted[1];
  const float mx_u = u0 * u0;
  const float my_u = u1 * u1;
  const float mxy_u = u0 * u1;
  const float rho_u = mx_u + my_u;
  const float rad_dist_u = k1 * rho_u + k2 * rho_u * rho_u;
  (*pointDistorted)[0] =
      u0 + u0 * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho_u + 2.0 * mx_u);
  (*pointDistorted)[1] =
      u1 + u1 * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho_u + 2.0 * my_u);

  // next the Jacobian w.r.t. changes on the undistorted point
  Eigen::Matrix2d& J = *pointJacobian;
  J(0, 0) = 1 + rad_dist_u + k1 * 2.0 * mx_u + k2 * rho_u * 4 * mx_u +
            2.0 * p1 * u1 + 6 * p2 * u0;
  J(1, 0) = k1 * 2.0 * u0 * u1 + k2 * 4 * rho_u * u0 * u1 + p1 * 2.0 * u0 +
            2.0 * p2 * u1;
  J(0, 1) = J(1, 0);
  J(1, 1) = 1 + rad_dist_u + k1 * 2.0 * my_u + k2 * rho_u * 4 * my_u +
            6 * p1 * u1 + 2.0 * p2 * u0;

  if (parameterJacobian) {
    // the Jacobian w.r.t. intrinsics parameters
    Eigen::Matrix2Xd& J2 = *parameterJacobian;
    J2.resize(2, NumDistortionIntrinsics);
    const float r2 = rho_u;
    const float r4 = r2 * r2;

    //[ u0*(u0^2 + u1^2), u0*(u0^2 + u1^2)^2,       2*u0*u1, 3*u0^2 + u1^2]
    //[ u1*(u0^2 + u1^2), u1*(u0^2 + u1^2)^2, u0^2 + 3*u1^2,       2*u0*u1]

    J2(0, 0) = u0 * r2;
    J2(0, 1) = u0 * r4;
    J2(0, 2) = 2.0 * u0 * u1;
    J2(0, 3) = r2 + 2.0 * u0 * u0;

    J2(1, 0) = u1 * r2;
    J2(1, 1) = u1 * r4;
    J2(1, 2) = r2 + 2.0 * u1 * u1;
    J2(1, 3) = 2.0 * u0 * u1;
  }
  return true;
}
bool RadialTangentialDistortion::undistort(
    const Eigen::Vector2d& pointDistorted,
    Eigen::Vector2d* pointUndistorted) const {
  // this is expensive: we solve with Gauss-Newton...
  Eigen::Vector2d x_bar = pointDistorted;  // initialise at distorted point
  const int n = 5;                         // just 5 iterations max.
  Eigen::Matrix2d E;                       // error Jacobian

  bool success = false;
  for (int i = 0; i < n; i++) {
    Eigen::Vector2d x_tmp;

    distort(x_bar, &x_tmp, &E);

    Eigen::Vector2d e(pointDistorted - x_tmp);
    Eigen::Matrix2d E2 = (E.transpose() * E);
    Eigen::Vector2d du = E2.inverse() * E.transpose() * e;

    x_bar += du;

    const double chi2 = e.dot(e);
    if (chi2 < 1e-4) {
      success = true;
    }
    if (chi2 < 1e-15) {
      success = true;
      break;
    }
  }
  *pointUndistorted = x_bar;

  if (!success) {
    // std::cout<<(E.transpose() * E)<<std::endl;
  }
  return success;
}

bool RadialTangentialDistortion::undistort(
    const Eigen::Vector2f& pointDistorted,
    Eigen::Vector2f* pointUndistorted) const {
  // this is expensive: we solve with Gauss-Newton...
  Eigen::Vector2f x_bar = pointDistorted;  // initialise at distorted point
  const int n = 5;                         // just 5 iterations max.
  Eigen::Matrix2f E;                       // error Jacobian

  bool success = false;
  for (int i = 0; i < n; i++) {
    Eigen::Vector2f x_tmp;

    distort(x_bar, &x_tmp, &E);

    Eigen::Vector2f e(pointDistorted - x_tmp);
    Eigen::Matrix2f E2 = (E.transpose() * E);
    Eigen::Vector2f du = E2.inverse() * E.transpose() * e;

    x_bar += du;

    const double chi2 = e.dot(e);
    if (chi2 < 1e-4) {
      success = true;
    }
    if (chi2 < 1e-15) {
      success = true;
      break;
    }
  }
  *pointUndistorted = x_bar;

  if (!success) {
    // std::cout<<(E.transpose() * E)<<std::endl;
  }
  return success;
}

bool RadialTangentialDistortion::undistort(
    const Eigen::Vector2d& pointDistorted, Eigen::Vector2d* pointUndistorted,
    Eigen::Matrix2d* pointJacobian) const {
  // this is expensive: we solve with Gauss-Newton...
  Eigen::Vector2d x_bar = pointDistorted;  // initialise at distorted point
  const int n = 5;                         // just 5 iterations max.
  Eigen::Matrix2d E;                       // error Jacobian

  bool success = false;
  for (int i = 0; i < n; i++) {
    Eigen::Vector2d x_tmp;

    distort(x_bar, &x_tmp, &E);

    Eigen::Vector2d e(pointDistorted - x_tmp);
    Eigen::Vector2d dx = (E.transpose() * E).inverse() * E.transpose() * e;

    x_bar += dx;

    const double chi2 = e.dot(e);
    if (chi2 < 1e-4) {
      success = true;
    }
    if (chi2 < 1e-15) {
      success = true;
      break;
    }
  }
  *pointUndistorted = x_bar;

  // the Jacobian of the inverse map is simply the inverse Jacobian.
  *pointJacobian = E.inverse();

  return success;
}

bool RadialTangentialDistortion::undistort(
    const Eigen::Vector2f& pointDistorted, Eigen::Vector2f* pointUndistorted,
    Eigen::Matrix2f* pointJacobian) const {
  Eigen::Vector2f x_bar = pointDistorted;  // initialize at distorted point
  const int n = 5;                         // Max 5 iterations
  Eigen::Matrix2f E;                       // error Jacobian

  bool success = false;
  for (int i = 0; i < n; ++i) {
    Eigen::Vector2f x_tmp;
    distort(x_bar, &x_tmp, &E);

    Eigen::Vector2f e(pointDistorted - x_tmp);
    Eigen::Vector2f dx = (E.transpose() * E).inverse() * E.transpose() * e;

    x_bar += dx;

    const float chi2 = e.dot(e);
    if (chi2 < 1e-4) {
      success = true;
    }
    if (chi2 < 1e-8) {
      success = true;
      break;
    }
  }
  *pointUndistorted = x_bar;

  // the Jacobian of the inverse map is simply the inverse Jacobian.
  *pointJacobian = E.inverse();

  return success;
}

}  // namespace cameras
}  // namespace vio


#include <mandoline/mesh3.hpp>
#include <mtao/cmdline_parser.hpp>
#include <mtao/logging/logger.hpp>

#include <Eigen/Dense>
using namespace mandoline;

template <typename Matrix, typename Vector>
struct SparseLDLT
{
    typedef typename Matrix::Scalar Scalar;
    SparseLDLT() {}
    SparseLDLT(const Matrix & A)
    {
        // L=tril(A);
        L=A.template triangularView<Eigen::StrictlyLower>();//Don't copy the diagonal
        for(int i=0; i<L.rows(); ++i)
        {
            if(L.coeff(i,i)!=0)
                L.coeffRef(i,i)=0;
        }
        Dinv=D=A.diagonal();

        // for k=1:size(L,2)
        for(int k=0; k<A.rows(); ++k)//k is the column that we're infecting the remaining columns with
        {//L(:,k)

            //Solidify the current column values
            //==================================
            if(D(k)==0) continue;
            if(Dinv(k)<0.25*D(k))//If D has shrunk too much since it started
                Dinv(k)=1/D(k);
            else
                Dinv(k)=1/Dinv(k);
            L.innerVector(k) *= Dinv(k);

            //Add k terms to all of the following columns
            //===========================================
            for(typename Matrix::InnerIterator it(L,k); it; ++it)// -L(i,k)*D(k)*L(j,k)
            {
                int j = it.row();//j>k
                if(j<=k) continue;
                Scalar missing=0;
                Scalar multiplier=it.value();//L(j,k)*D(k)

                typename Matrix::InnerIterator k_it(L,k);
                typename Matrix::InnerIterator j_it(L,j);
                //move down teh column of L(:,k) to collect missing elements in the match with A(:,j)
                //i=k_it.row()

                while (k_it && k_it.row()<j){//L(i,k)
                    while(j_it)//L(i,j) occasionally
                    {
                        if(j_it.row() < k_it.row())
                            ++j_it;
                        else if(j_it.row() == k_it.row())//L(i,k) are L(i,j) are nonzero
                            break;
                        else
                        {
                            missing += k_it.value();//L(i,k) will fill something not in L(i,j)
                            break;
                        }
                    }
                    ++k_it;
                }

                if(k_it && j_it.row() == j)
                {
                    Dinv(j) -= it.value() * multiplier;
                }

                typename Matrix::InnerIterator j_it2(L,j);
                while(k_it && j_it2)
                {
                    if(j_it2.row() < k_it.row())
                        ++j_it2;
                    else if(j_it2.row() == k_it.row())//L(i,k) and L(i,j) are both nonzero, -=L(i,k)*L(j,k)*D(k)
                    {
                        j_it2.valueRef() -= multiplier * k_it.value() ;//k_it.value()=L(i,k)
                        ++j_it2;
                        ++k_it;
                    }
                    else
                    {
                        missing+=k_it.value();
                        ++k_it;
                    }
                }

                while(k_it)
                {
                    missing+=k_it.value();
                    ++k_it;
                }
                Dinv(j)-=0.97*missing*multiplier;
            }
        }

        /*
           std::cout << L << std::endl;
           */

    }
    void solve(const Vector & b, Vector & x)
    {
        x = L.template triangularView<Eigen::UnitLower>().solve(b);
        x.noalias() = x.cwiseProduct(Dinv);//safe beacuse it's a dot
        L.transpose().template triangularView<Eigen::UnitUpper>().solveInPlace(x);
    }
    Matrix getA()
    {
        Matrix
                A = L.template triangularView<Eigen::UnitLower>();
        A = A * D.asDiagonal();
        A = A * L.template triangularView<Eigen::UnitLower>().transpose();

        return A;
    }
private:
    Matrix L;
    Vector D,Dinv;
};

template <typename MatrixType, typename VectorType, typename Preconditioner>
struct PreconditionedConjugateGradient
{
    typedef MatrixType Matrix;
    typedef VectorType Vector;
    typedef typename Vector::Scalar Scalar;
    PreconditionedConjugateGradient(const Matrix & A): A(A)
    {
        precond = Preconditioner(A);
    }
    auto solve(const Vector& b) {
        auto x = b.eval();
        x.setZero();
        Vector r = b-A*x;
        Vector z;
        precond.solve(r,z);
        Vector p = z;
        Vector Ap = A*p;
        Scalar rdz = r.dot(z);
        Scalar alpha, beta;
        auto error = [&]() { return r.template lpNorm<Eigen::Infinity>(); };

        uint iterations = 0;
        while(++iterations < 10 &&
                error() > epsilon)
        {
            alpha = (rdz)/(p.dot(Ap));
            x+=alpha * p;
            r-=alpha * Ap;
            precond.solve(r,z);
            beta=1/rdz;
            rdz = r.dot(z);
            beta*=rdz;
            p=z+beta*p;
            Ap=A*p;
        }
        return x;
    }
private:
    const Matrix & A;
    Preconditioner precond;

    Scalar epsilon = 1e-5;

};

template <typename Matrix, typename Vector>
auto ldlt_pcg_solver(const Matrix & A, const Vector& b)
{
    return PreconditionedConjugateGradient<Matrix,Vector, SparseLDLT<Matrix, mtao::Vector<typename Vector::Scalar, Vector::RowsAtCompileTime>>>(A);
    //auto solver = IterativeLinearSolver<PreconditionedConjugateGradientCapsule<Matrix,Vector, Preconditioner> >(A.rows(), 1e-5);
}
template <typename Matrix, typename Vector>
auto ldlt_pcg_solve(const Matrix & A, const Vector & b)
{
    auto solver = ldlt_pcg_solver(A,b);
    //auto solver = IterativeLinearSolver<PreconditionedConjugateGradientCapsule<Matrix,Vector, Preconditioner> >(A.rows(), 1e-5);
    return solver.solve(b);
}

using namespace mtao::logging;

double max_eigenvalue(const Eigen::SparseMatrix<double>& A) {
    mtao::VecXd xold = mtao::VecXd::Random(A.rows()).normalized();
    mtao::VecXd x = (A * xold).normalized();
    while((x - xold).norm() > 1e-5) {
        xold = x;
        x = (A * xold).normalized();
        std::cout << "Max:" << (x-xold).norm() << std::endl;
    }
    return (A*x).norm();
}

double min_eigenvalue(const Eigen::SparseMatrix<double>& A) {
    mtao::VecXd xold = mtao::VecXd::Random(A.rows()).normalized();
    std::cout << "Preparing solver" << std::endl;
    auto solver = ldlt_pcg_solver(A,xold);
    std::cout << "Done" << std::endl;

    mtao::VecXd x = (solver.solve(xold)).normalized();
    while((x - xold).norm() > 1e-5) {
        xold = x;
        x = (solver.solve(xold)).normalized();
        std::cout << "Min:" << (x-xold).norm() << std::endl;
    }
    return (A*x).norm();
}

int main(int argc, char * argv[]) {

    auto&& log = make_logger("profiler",mtao::logging::Level::All);
    mtao::CommandLineParser clp;
    clp.parse(argc, argv);

    if(clp.args().size() < 1) {
        fatal() << "No input mesh filename!";
        return {};
    }

    std::string input_cutmesh = clp.arg(0);

    CutCellMesh<3> ccm = CutCellMesh<3>::from_proto(input_cutmesh);

    auto B = ccm.boundary();
    mtao::VecXd DM = ccm.mesh_face_mask();
    B = DM.asDiagonal() * B;
    DM = mtao::VecXd::Ones(ccm.cell_size());
    for(auto&& [i,c]: mtao::iterator::enumerate(ccm.cells())) {
    }
    B = B * DM.asDiagonal();

    mtao::VecXd DH2 = ccm.dual_hodge2();
    Eigen::SparseMatrix<double> L = B.transpose() * DH2.asDiagonal() * B;

    double M = max_eigenvalue(L);
    double m = min_eigenvalue(L);

    std::cout << m << " < " << M  << std::endl;
    std::cout << "condition number: " << (M / m) << std::endl;
}