/******************************************************************************
 *
 * Project:  GDAL
 * Purpose:  GDAL Wrapper for image matching via correlation algorithm.
 * Author:   Frank Warmerdam, warmerdam@pobox.com
 * Author:   Andrew Migal, migal.drew@gmail.com
 *
 ******************************************************************************
 * Copyright (c) 2012, Frank Warmerdam
 *
 * SPDX-License-Identifier: MIT
 ****************************************************************************/

#include "gdal_alg.h"
#include "gdal_simplesurf.h"

//! @cond Doxygen_Suppress

//! @endcond

// TODO(schwehr): What?  This below: "0,001"

/**
 * @file
 * @author Andrew Migal migal.drew@gmail.com
 * @brief Algorithms for searching corresponding points on images.
 * @details This implementation is  based on an simplified version
 * of SURF algorithm (Speeded Up Robust Features).
 * Provides capability for detection feature points
 * and finding equal points on different images.
 * As original, this realization is scale invariant, but sensitive to rotation.
 * Images should have similar rotation angles (maximum difference is up to 10-15
 * degrees), otherwise algorithm produces incorrect and very unstable results.
 */

/**
 * Detect feature points on provided image. Please carefully read documentation
 * below.
 *
 * @param poDataset Image on which feature points will be detected
 * @param panBands Array of 3 raster bands numbers, for Red, Green, Blue bands
 * (in that order)
 * @param nOctaveStart Number of bottom octave. Octave numbers starts from one.
 * This value directly and strongly affects to amount of recognized points
 * @param nOctaveEnd Number of top octave. Should be equal or greater than
 * octaveStart
 * @param dfThreshold Value from 0 to 1. Threshold for feature point
 * recognition.  Number of detected points is larger if threshold is lower
 *
 * @see GDALFeaturePoint, GDALSimpleSURF class for details.
 *
 * @note Every octave finds points in specific size. For small images
 * use small octave numbers, for high resolution - large.
 * For 1024x1024 images it's normal to use any octave numbers from range 1-6.
 * (for example, octave start - 1, octave end - 3, or octave start - 2, octave
 * end - 2.)
 * For larger images, try 1-10 range or even higher.
 * Pay attention that number of detected point decreases quickly per octave for
 * particular image. Algorithm finds more points in case of small octave number.
 * If method detects nothing, reduce octave start value.
 * In addition, if many feature points are required (the largest possible
 * amount), use the lowest octave start value (1) and wide octave range.
 *
 * @note Typical threshold's value is 0,001. It's pretty good for all images.
 * But this value depends on image's nature and may be various in each
 * particular case.
 * For example, value can be 0,002 or 0,005.
 * Notice that number of detected points is larger if threshold is lower.
 * But with high threshold feature points will be better - "stronger", more
 * "unique" and distinctive.
 *
 * Feel free to experiment with parameters, because character, robustness and
 * number of points entirely depend on provided range of octaves and threshold.
 *
 * NOTICE that every octave requires time to compute. Use a little range
 * or only one octave, if execution time is significant.
 *
 * @return CE_None or CE_Failure if error occurs.
 */

static std::vector<GDALFeaturePoint> *
GatherFeaturePoints(GDALDataset *poDataset, int *panBands, int nOctaveStart,
                    int nOctaveEnd, double dfThreshold)
{
    if (poDataset == nullptr)
    {
        CPLError(CE_Failure, CPLE_AppDefined, "GDALDataset isn't specified");
        return nullptr;
    }

    if (panBands == nullptr)
    {
        CPLError(CE_Failure, CPLE_AppDefined, "Raster bands are not specified");
        return nullptr;
    }

    if (nOctaveStart <= 0 || nOctaveEnd < 0 || nOctaveStart > nOctaveEnd)
    {
        CPLError(CE_Failure, CPLE_AppDefined, "Octave numbers are invalid");
        return nullptr;
    }

    if (dfThreshold < 0)
    {
        CPLError(CE_Failure, CPLE_AppDefined,
                 "Threshold have to be greater than zero");
        return nullptr;
    }

    GDALRasterBand *poRstRedBand = poDataset->GetRasterBand(panBands[0]);
    GDALRasterBand *poRstGreenBand = poDataset->GetRasterBand(panBands[1]);
    GDALRasterBand *poRstBlueBand = poDataset->GetRasterBand(panBands[2]);

    const int nWidth = poRstRedBand->GetXSize();
    const int nHeight = poRstRedBand->GetYSize();

    if (nWidth == 0 || nHeight == 0)
    {
        CPLError(CE_Failure, CPLE_AppDefined,
                 "Must have non-zero width and height.");
        return nullptr;
    }

    // Allocate memory for grayscale image.
    double **padfImg = new double *[nHeight];
    for (int i = 0;;)
    {
        padfImg[i] = new double[nWidth];
        for (int j = 0; j < nWidth; ++j)
            padfImg[i][j] = 0.0;
        ++i;
        if (i == nHeight)
            break;
    }

    // Create grayscale image.
    GDALSimpleSURF::ConvertRGBToLuminosity(poRstRedBand, poRstGreenBand,
                                           poRstBlueBand, nWidth, nHeight,
                                           padfImg, nHeight, nWidth);

    // Prepare integral image.
    GDALIntegralImage *poImg = new GDALIntegralImage();
    poImg->Initialize(const_cast<const double **>(padfImg), nHeight, nWidth);

    // Get feature points.
    GDALSimpleSURF *poSurf = new GDALSimpleSURF(nOctaveStart, nOctaveEnd);

    std::vector<GDALFeaturePoint> *poCollection =
        poSurf->ExtractFeaturePoints(poImg, dfThreshold);

    // Clean up.
    delete poImg;
    delete poSurf;

    for (int i = 0; i < nHeight; ++i)
        delete[] padfImg[i];

    delete[] padfImg;

    return poCollection;
}

/************************************************************************/
/*                     GDALComputeMatchingPoints()                      */
/************************************************************************/

/** GDALComputeMatchingPoints. TODO document */
GDAL_GCP CPL_DLL *GDALComputeMatchingPoints(GDALDatasetH hFirstImage,
                                            GDALDatasetH hSecondImage,
                                            char **papszOptions,
                                            int *pnGCPCount)
{
    *pnGCPCount = 0;

    /* -------------------------------------------------------------------- */
    /*      Override default algorithm parameters.                          */
    /* -------------------------------------------------------------------- */
    int nOctaveStart, nOctaveEnd;
    double dfSURFThreshold;

    nOctaveStart =
        atoi(CSLFetchNameValueDef(papszOptions, "OCTAVE_START", "2"));
    nOctaveEnd = atoi(CSLFetchNameValueDef(papszOptions, "OCTAVE_END", "2"));

    dfSURFThreshold =
        CPLAtof(CSLFetchNameValueDef(papszOptions, "SURF_THRESHOLD", "0.001"));
    const double dfMatchingThreshold = CPLAtof(
        CSLFetchNameValueDef(papszOptions, "MATCHING_THRESHOLD", "0.015"));

    /* -------------------------------------------------------------------- */
    /*      Identify the bands to use.  For now we are effectively          */
    /*      limited to using RGB input so if we have one band only treat    */
    /*      it as red=green=blue=band 1.  Disallow non eightbit imagery.    */
    /* -------------------------------------------------------------------- */
    int anBandMap1[3] = {1, 1, 1};
    if (GDALGetRasterCount(hFirstImage) >= 3)
    {
        anBandMap1[1] = 2;
        anBandMap1[2] = 3;
    }

    int anBandMap2[3] = {1, 1, 1};
    if (GDALGetRasterCount(hSecondImage) >= 3)
    {
        anBandMap2[1] = 2;
        anBandMap2[2] = 3;
    }

    /* -------------------------------------------------------------------- */
    /*      Collect reference points on each image.                         */
    /* -------------------------------------------------------------------- */
    std::vector<GDALFeaturePoint> *poFPCollection1 =
        GatherFeaturePoints(GDALDataset::FromHandle(hFirstImage), anBandMap1,
                            nOctaveStart, nOctaveEnd, dfSURFThreshold);
    if (poFPCollection1 == nullptr)
        return nullptr;

    std::vector<GDALFeaturePoint> *poFPCollection2 =
        GatherFeaturePoints(GDALDataset::FromHandle(hSecondImage), anBandMap2,
                            nOctaveStart, nOctaveEnd, dfSURFThreshold);

    if (poFPCollection2 == nullptr)
    {
        delete poFPCollection1;
        return nullptr;
    }

    /* -------------------------------------------------------------------- */
    /*      Try to find corresponding locations.                            */
    /* -------------------------------------------------------------------- */
    std::vector<GDALFeaturePoint *> oMatchPairs;

    if (CE_None != GDALSimpleSURF::MatchFeaturePoints(
                       &oMatchPairs, poFPCollection1, poFPCollection2,
                       dfMatchingThreshold))
    {
        delete poFPCollection1;
        delete poFPCollection2;
        return nullptr;
    }

    *pnGCPCount = static_cast<int>(oMatchPairs.size()) / 2;

    /* -------------------------------------------------------------------- */
    /*      Translate these into GCPs - but with the output coordinate      */
    /*      system being pixel/line on the second image.                    */
    /* -------------------------------------------------------------------- */
    GDAL_GCP *pasGCPList =
        static_cast<GDAL_GCP *>(CPLCalloc(*pnGCPCount, sizeof(GDAL_GCP)));

    GDALInitGCPs(*pnGCPCount, pasGCPList);

    for (int i = 0; i < *pnGCPCount; i++)
    {
        GDALFeaturePoint *poPoint1 = oMatchPairs[i * 2];
        GDALFeaturePoint *poPoint2 = oMatchPairs[i * 2 + 1];

        pasGCPList[i].dfGCPPixel = poPoint1->GetX() + 0.5;
        pasGCPList[i].dfGCPLine = poPoint1->GetY() + 0.5;

        pasGCPList[i].dfGCPX = poPoint2->GetX() + 0.5;
        pasGCPList[i].dfGCPY = poPoint2->GetY() + 0.5;
        pasGCPList[i].dfGCPZ = 0.0;
    }

    // Cleanup the feature point lists.
    delete poFPCollection1;
    delete poFPCollection2;

    /* -------------------------------------------------------------------- */
    /*      Optionally transform into the georef coordinates of the         */
    /*      output image.                                                   */
    /* -------------------------------------------------------------------- */
    const bool bGeorefOutput =
        CPLTestBool(CSLFetchNameValueDef(papszOptions, "OUTPUT_GEOREF", "NO"));

    if (bGeorefOutput)
    {
        double adfGeoTransform[6] = {};

        GDALGetGeoTransform(hSecondImage, adfGeoTransform);

        for (int i = 0; i < *pnGCPCount; i++)
        {
            GDALApplyGeoTransform(adfGeoTransform, pasGCPList[i].dfGCPX,
                                  pasGCPList[i].dfGCPY, &(pasGCPList[i].dfGCPX),
                                  &(pasGCPList[i].dfGCPY));
        }
    }

    return pasGCPList;
}