/*
 * Copyright (C) 2005-2020 Centre National d'Etudes Spatiales (CNES)
 *
 * This file is part of Orfeo Toolbox
 *
 *     https://www.orfeo-toolbox.org/
 *
 * 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 "otbWrapperApplicationFactory.h"

// Elevation handler
#include "otbWrapperElevationParametersHandler.h"

#ifdef OTB_USE_SIFTFAST
#include "otbSiftFastImageFilter.h"
#else
#include "otbImageToSIFTKeyPointSetFilter.h"
#endif
#include "otbImageToSURFKeyPointSetFilter.h"
#include "otbKeyPointSetsMatchingFilter.h"
#include "otbMultiToMonoChannelExtractROI.h"
#include "otbKeyPointSetsMatchingFilter.h"

#include "otbGenericRSTransform.h"
#include "otbOGRDataSourceWrapper.h"
#include "ogrsf_frmts.h"

namespace otb
{
namespace Wrapper
{

class HomologousPointsExtraction : public Application
{
public:
  /** Standard class typedefs. */
  typedef HomologousPointsExtraction    Self;
  typedef Application                   Superclass;
  typedef itk::SmartPointer<Self>       Pointer;
  typedef itk::SmartPointer<const Self> ConstPointer;

  typedef FloatVectorImageType::PixelType         RealVectorType;
  typedef FloatVectorImageType::InternalPixelType ValueType;
  typedef itk::PointSet<RealVectorType, 2> PointSetType;

// This is ugly ...
#ifdef OTB_USE_SIFTFAST
  typedef SiftFastImageFilter<FloatImageType, PointSetType> SiftFilterType;
#else
  typedef ImageToSIFTKeyPointSetFilter<FloatImageType, PointSetType> SiftFilterType;
#endif

  typedef ImageToSURFKeyPointSetFilter<FloatImageType, PointSetType> SurfFilterType;

  typedef itk::Statistics::EuclideanDistanceMetric<RealVectorType> DistanceType;
  typedef otb::KeyPointSetsMatchingFilter<PointSetType, DistanceType> MatchingFilterType;

  typedef MatchingFilterType::LandmarkListType LandmarkListType;
  typedef PointSetType::PointType              PointType;
  typedef otb::MultiToMonoChannelExtractROI<ValueType, ValueType> ExtractChannelFilterType;
  typedef otb::GenericRSTransform<> RSTransformType;

  /** Standard macro */
  itkNewMacro(Self);

  itkTypeMacro(HomologousPointsExtraction, otb::Wrapper::Application);

private:
  void DoInit() override
  {
    SetName("HomologousPointsExtraction");
    SetDescription("Compute homologous points between images using keypoints");
    SetDocLongDescription(
        "This application computes homologous points between images using keypoints. "
        " SIFT or SURF keypoints can be used and the band on which keypoints are computed can be set independently for both images."
        " The application offers two modes:"
        " the first is the full mode where keypoints are extracted from the full extent of both images"
        " (please note that in this mode large image file are not supported). "
        "The second mode, called geobins, allows one to set-up spatial binning to get fewer points"
        " spread across the entire image. "
        "In this mode, the corresponding spatial bin in the second image is estimated using geographical"
        " transform or sensor modelling, and is padded according to the user defined precision. Last, in"
        " both modes the application can filter matches whose colocalisation in first image exceed this precision. "
        "The elevation parameters are to deal more precisely with sensor modelling in case of sensor geometry data. "
        "The outvector option allows creating a vector file with segments corresponding to the localisation error between the matches."
        " It can be useful to assess the precision of a registration for instance."
        " The vector file is always reprojected to EPSG:4326 so that it can be displayed in a GIS."
        " This is done via reprojection or by applying the image sensor models.");
    // Documentation
    SetDocLimitations("Full mode does not handle large images.");
    SetDocSeeAlso("RefineSensorModel");
    SetDocAuthors("OTB-Team");

    AddDocTag(Tags::FeatureExtraction);

    AddParameter(ParameterType_InputImage, "in1", "Input Image 1");
    SetParameterDescription("in1", " First input image");

    AddParameter(ParameterType_Int, "band1", "Input band 1");
    SetParameterDescription("band1", "Index of the band from input image 1 to use for keypoints extraction");
    SetMinimumParameterIntValue("band1", 1);
    SetDefaultParameterInt("band1", 1);

    AddParameter(ParameterType_InputImage, "in2", "Input Image 2");
    SetParameterDescription("in2", " Second input image");

    AddParameter(ParameterType_Int, "band2", "Input band 2");
    SetParameterDescription("band2", "Index of the band from input image 1 to use for keypoints extraction");
    SetMinimumParameterIntValue("band2", 1);
    SetDefaultParameterInt("band2", 1);

    AddParameter(ParameterType_OutputFilename, "out", "Output file with tie points");
    SetParameterDescription("out", "File containing the list of tie points");

    AddParameter(ParameterType_OutputFilename, "outvector", "Output vector file with tie points");
    SetParameterDescription("outvector", "File containing segments representing matches ");
    MandatoryOff("outvector");
    DisableParameter("outvector");

    AddParameter(ParameterType_Choice, "algorithm", "Keypoints detection algorithm");
    SetParameterDescription("algorithm", "Choice of the detection algorithm to use");

    AddChoice("algorithm.surf", "SURF algorithm");
    AddChoice("algorithm.sift", "SIFT algorithm");

    AddParameter(ParameterType_Float, "threshold", "Distance threshold for matching");
    SetParameterDescription("threshold", "The distance threshold for matching.");
    SetMinimumParameterFloatValue("threshold", 0.0);
    SetDefaultParameterFloat("threshold", 0.6);

    AddParameter(ParameterType_Bool, "backmatching", "Use back-matching to filter matches");
    SetParameterDescription("backmatching", "If set to true, matches should be consistent in both ways.");

    AddParameter(ParameterType_Choice, "mode", "Keypoints search mode");

    AddChoice("mode.full", "Extract and match all keypoints (no streaming)");
    SetParameterDescription("mode.full", "Extract and match all keypoints, loading both images entirely into memory");

    AddChoice("mode.geobins", "Search keypoints in small spatial bins regularly spread across first image");
    SetParameterDescription("mode.geobins",
                            "This method retrieves a set of tie points regularly spread across image 1. Corresponding bins in image 2 are retrieved using "
                            "sensor and geographical information if available. The first bin position takes into account the margin parameter. Bins are "
                            "cropped to the largest image region shrunk by the margin parameter for both in1 and in2 images.");

    AddParameter(ParameterType_Int, "mode.geobins.binsize", "Size of bin");
    SetParameterDescription("mode.geobins.binsize", "Radius of the spatial bin in pixels");
    SetDefaultParameterInt("mode.geobins.binsize", 256);
    SetMinimumParameterIntValue("mode.geobins.binsize", 1);

    AddParameter(ParameterType_Int, "mode.geobins.binsizey", "Size of bin (y direction)");
    SetParameterDescription("mode.geobins.binsizey", "Radius of the spatial bin in pixels (y direction). If not set, the mode.geobins.binsize value is used.");
    SetMinimumParameterIntValue("mode.geobins.binsizey", 1);
    MandatoryOff("mode.geobins.binsizey");

    AddParameter(ParameterType_Int, "mode.geobins.binstep", "Steps between bins");
    SetParameterDescription("mode.geobins.binstep", "Steps between bins in pixels");
    SetDefaultParameterInt("mode.geobins.binstep", 256);
    SetMinimumParameterIntValue("mode.geobins.binstep", 1);

    AddParameter(ParameterType_Int, "mode.geobins.binstepy", "Steps between bins (y direction)");
    SetParameterDescription("mode.geobins.binstepy", "Steps between bins in pixels (y direction). If not set, the mode.geobins.binstep value is used.");
    SetMinimumParameterIntValue("mode.geobins.binstepy", 1);
    MandatoryOff("mode.geobins.binstepy");

    AddParameter(ParameterType_Int, "mode.geobins.margin", "Margin from image border to start/end bins (in pixels)");
    SetParameterDescription("mode.geobins.margin", "Margin from image border to start/end bins (in pixels)");
    SetMinimumParameterIntValue("mode.geobins.margin", 0);
    SetDefaultParameterInt("mode.geobins.margin", 10);

    AddParameter(ParameterType_Float, "precision", "Estimated precision of the colocalisation function (in pixels)");
    SetParameterDescription("precision", "Estimated precision of the colocalisation function in pixels");
    SetDefaultParameterFloat("precision", 0.);

    AddParameter(ParameterType_Bool, "mfilter", "Filter points according to geographical or sensor based colocalisation");
    SetParameterDescription("mfilter",
                            "If enabled, this option allows one to filter matches according to colocalisation from sensor or geographical information, using "
                            "the given tolerancy expressed in pixels");

    AddParameter(ParameterType_Bool, "2wgs84", "If enabled, points from second image will be exported in WGS84");

    // Elevation
    ElevationParametersHandler::AddElevationParameters(this, "elev");

    // Doc example parameter settings
    SetDocExampleParameterValue("in1", "sensor_stereo_left.tif");
    SetDocExampleParameterValue("in2", "sensor_stereo_right.tif");
    SetDocExampleParameterValue("mode", "full");
    SetDocExampleParameterValue("out", "homologous.txt");

    SetOfficialDocLink();
  }

  void DoUpdateParameters() override
  {
  }

  void Match(FloatImageType* im1, FloatImageType* im2, RSTransformType* rsTransform, RSTransformType* rsTransform1ToWGS84, RSTransformType* rsTransform2ToWGS84,
             std::ofstream& file, OGRMultiLineString* mls = nullptr)
  {
    MatchingFilterType::Pointer matchingFilter = MatchingFilterType::New();

    if (GetParameterString("algorithm") == "sift")
    {
      otbAppLogINFO("Using SIFT points");
      SiftFilterType::Pointer sift1 = SiftFilterType::New();
      sift1->SetInput(im1);

      SiftFilterType::Pointer sift2 = SiftFilterType::New();
      sift2->SetInput(im2);

      sift1->Update();

      otbAppLogINFO("Found " << sift1->GetOutput()->GetNumberOfPoints() << " sift points in image 1.");

      sift2->Update();

      otbAppLogINFO("Found " << sift2->GetOutput()->GetNumberOfPoints() << " sift points in image 2.");

      matchingFilter->SetInput1(sift1->GetOutput());
      matchingFilter->SetInput2(sift2->GetOutput());
    }
    else if (GetParameterString("algorithm") == "surf")
    {
      SurfFilterType::Pointer surf1 = SurfFilterType::New();
      surf1->SetInput(im1);

      SurfFilterType::Pointer surf2 = SurfFilterType::New();
      surf2->SetInput(im2);

      otbAppLogINFO("Doing update");
      surf1->Update();

      otbAppLogINFO("Found " << surf1->GetOutput()->GetNumberOfPoints() << " surf points in image 1.");

      surf2->Update();

      otbAppLogINFO("Found " << surf2->GetOutput()->GetNumberOfPoints() << " surf points in image 2.");

      matchingFilter->SetInput1(surf1->GetOutput());
      matchingFilter->SetInput2(surf2->GetOutput());
      matchingFilter->SetDistanceThreshold(GetParameterFloat("threshold"));
      matchingFilter->SetUseBackMatching(GetParameterInt("backmatching"));
    }

    try
    {

      matchingFilter->Update();

      LandmarkListType::Pointer landmarks = matchingFilter->GetOutput();

      otbAppLogINFO("Found " << landmarks->Size() << " homologous points.");

      unsigned int discarded = 0;

      for (LandmarkListType::Iterator it = landmarks->Begin(); it != landmarks->End(); ++it)
      {
        PointType point1 = it.Get()->GetPoint1();
        PointType point2 = it.Get()->GetPoint2();

        double    error = 0;
        PointType pprime1, pprime2;

        bool filtered = false;

        if (GetParameterInt("mfilter"))
        {
          pprime1 = rsTransform->TransformPoint(point1);
          error   = std::sqrt((point2[0] - pprime1[0]) * (point2[0] - pprime1[0]) + (point2[1] - pprime1[1]) * (point2[1] - pprime1[1]));

          if (error > GetParameterFloat("precision") * std::sqrt(std::abs(im2->GetSignedSpacing()[0] * im2->GetSignedSpacing()[1])))
          {
            filtered = true;
          }
        }

        if (!filtered)
        {
          if (GetParameterInt("2wgs84"))
          {
            pprime2 = rsTransform2ToWGS84->TransformPoint(point2);

            file << point1[0] << "\t" << point1[1] << "\t" << pprime2[0] << "\t" << pprime2[1] << std::endl;
          }
          else
          {
            file << point1[0] << "\t" << point1[1] << "\t" << point2[0] << "\t" << point2[1] << std::endl;
          }

          if (mls)
          {
            pprime1 = rsTransform1ToWGS84->TransformPoint(point1);
            pprime2 = rsTransform2ToWGS84->TransformPoint(point2);

            OGRLineString ls;
            ls.addPoint(pprime1[0], pprime1[1]);
            ls.addPoint(pprime2[0], pprime2[1]);
            mls->addGeometry(&ls);
          }
        }
        else
        {
          ++discarded;
        }
      }
      otbAppLogINFO(<< discarded << " points discarded");
    }
    catch (itk::ExceptionObject&)
    {
      // silent catch
    }
  }


  void DoExecute() override
  {
    OGRMultiLineString mls;

    FloatVectorImageType::Pointer image1 = this->GetParameterImage("in1");
    FloatVectorImageType::Pointer image2 = this->GetParameterImage("in2");

    // Setting up RS Transform
    RSTransformType::Pointer rsTransform = RSTransformType::New();
    rsTransform->SetInputKeywordList(image1->GetImageKeywordlist());
    rsTransform->SetInputProjectionRef(image1->GetProjectionRef());
    rsTransform->SetOutputKeywordList(image2->GetImageKeywordlist());
    rsTransform->SetOutputProjectionRef(image2->GetProjectionRef());

    RSTransformType::Pointer rsTransform1ToWGS84 = RSTransformType::New();
    rsTransform1ToWGS84->SetInputKeywordList(image1->GetImageKeywordlist());
    rsTransform1ToWGS84->SetInputProjectionRef(image1->GetProjectionRef());

    RSTransformType::Pointer rsTransform2ToWGS84 = RSTransformType::New();
    rsTransform2ToWGS84->SetInputKeywordList(image2->GetImageKeywordlist());
    rsTransform2ToWGS84->SetInputProjectionRef(image2->GetProjectionRef());

    // Setting up output file
    std::ofstream file;
    file.open(GetParameterString("out"));
    file << std::fixed;
    file.precision(12);

    // Setting up channel extractors
    ExtractChannelFilterType::Pointer extractChannel1 = ExtractChannelFilterType::New();
    extractChannel1->SetInput(image1);
    extractChannel1->SetChannel(GetParameterInt("band1"));

    ExtractChannelFilterType::Pointer extractChannel2 = ExtractChannelFilterType::New();
    extractChannel2->SetInput(image2);
    extractChannel2->SetChannel(GetParameterInt("band2"));

    // Setup the DEM Handler
    otb::Wrapper::ElevationParametersHandler::SetupDEMHandlerFromElevationParameters(this, "elev");

    rsTransform->InstantiateTransform();
    rsTransform1ToWGS84->InstantiateTransform();
    rsTransform2ToWGS84->InstantiateTransform();


    if (GetParameterString("mode") == "full")
    {
      // Launch detection on whole images
      Match(extractChannel1->GetOutput(), extractChannel2->GetOutput(), rsTransform, rsTransform1ToWGS84, rsTransform2ToWGS84, file, &mls);
    }
    else if (GetParameterString("mode") == "geobins")
    {
      // Compute binning on first image
      FloatImageType::SizeType size       = image1->GetLargestPossibleRegion().GetSize();
      unsigned int             bin_size_x = GetParameterInt("mode.geobins.binsize");
      unsigned int             bin_size_y = bin_size_x;

      unsigned int image_border_margin = GetParameterInt("mode.geobins.margin");

      if (IsParameterEnabled("mode.geobins.binsizey"))
      {
        bin_size_y = GetParameterInt("mode.geobins.binsizey");
      }

      unsigned int bin_step_x = GetParameterInt("mode.geobins.binstep");
      unsigned int bin_step_y = bin_step_x;

      if (IsParameterEnabled("mode.geobins.binstepy"))
      {
        bin_step_y = GetParameterInt("mode.geobins.binstepy");
      }

      unsigned int nb_bins_x = static_cast<unsigned int>(std::ceil(static_cast<float>(size[0] - 2 * image_border_margin) / (bin_size_x + bin_step_x)));
      unsigned int nb_bins_y = static_cast<unsigned int>(std::ceil(static_cast<float>(size[1] - 2 * image_border_margin) / (bin_size_y + bin_step_y)));

      for (unsigned int i = 0; i < nb_bins_x; ++i)
      {
        for (unsigned int j = 0; j < nb_bins_y; ++j)
        {
          unsigned int startx = image_border_margin + i * (bin_size_x + bin_step_x);
          unsigned int starty = image_border_margin + j * (bin_size_y + bin_step_y);


          FloatImageType::SizeType   size1;
          FloatImageType::IndexType  index1;
          FloatImageType::RegionType region1;

          index1[0] = startx;
          index1[1] = starty;
          size1[0]  = bin_size_x;
          size1[1]  = bin_size_y;

          region1.SetIndex(index1);
          region1.SetSize(size1);

          FloatImageType::RegionType largestRegion = image1->GetLargestPossibleRegion();

          largestRegion.ShrinkByRadius(image_border_margin);
          region1.Crop(largestRegion);

          otbAppLogINFO("(" << i + 1 << "/" << nb_bins_x << ", " << j + 1 << "/" << nb_bins_y << ") Considering region1: " << region1.GetIndex() << ", "
                            << region1.GetSize());


          extractChannel1->SetExtractionRegion(region1);


          // We need to find the corresponding region in image 2
          itk::ContinuousIndex<double, 2> ul_i, ur_i, lr_i, ll_i;
          FloatImageType::PointType ul1, ur1, ll1, lr1, p1, p2, p3, p4;
          itk::ContinuousIndex<double, 2> i1, i2, i3, i4, i_min, i_max;

          ul_i[0] = static_cast<double>(startx) - 0.5;
          ul_i[1] = static_cast<double>(starty) - 0.5;
          ur_i    = ul_i;
          lr_i    = ul_i;
          ll_i    = ul_i;
          ur_i[0] += static_cast<double>(bin_size_x);
          lr_i[0] += static_cast<double>(bin_size_x);
          lr_i[1] += static_cast<double>(bin_size_y);
          ll_i[1] += static_cast<double>(bin_size_y);

          image1->TransformContinuousIndexToPhysicalPoint(ul_i, ul1);
          image1->TransformContinuousIndexToPhysicalPoint(ur_i, ur1);
          image1->TransformContinuousIndexToPhysicalPoint(lr_i, lr1);
          image1->TransformContinuousIndexToPhysicalPoint(ll_i, ll1);

          p1 = rsTransform->TransformPoint(ul1);
          p2 = rsTransform->TransformPoint(ur1);
          p3 = rsTransform->TransformPoint(lr1);
          p4 = rsTransform->TransformPoint(ll1);

          image2->TransformPhysicalPointToContinuousIndex(p1, i1);
          image2->TransformPhysicalPointToContinuousIndex(p2, i2);
          image2->TransformPhysicalPointToContinuousIndex(p3, i3);
          image2->TransformPhysicalPointToContinuousIndex(p4, i4);

          i_min[0] = std::min(std::min(i1[0], i2[0]), std::min(i3[0], i4[0]));
          i_min[1] = std::min(std::min(i1[1], i2[1]), std::min(i3[1], i4[1]));

          i_max[0] = std::max(std::max(i1[0], i2[0]), std::max(i3[0], i4[0]));
          i_max[1] = std::max(std::max(i1[1], i2[1]), std::max(i3[1], i4[1]));

          FloatImageType::IndexType index2;
          FloatImageType::SizeType  size2;

          index2[0] = std::floor(i_min[0]);
          index2[1] = std::floor(i_min[1]);

          size2[0] = std::ceil(i_max[0] - i_min[0]);
          size2[1] = std::ceil(i_max[1] - i_min[1]);

          FloatImageType::RegionType region2;
          region2.SetIndex(index2);
          region2.SetSize(size2);
          region2.PadByRadius(static_cast<unsigned int>(GetParameterInt("precision")));

          FloatImageType::RegionType largestRegion2 = image2->GetLargestPossibleRegion();
          largestRegion2.ShrinkByRadius(image_border_margin);

          if (region2.Crop(largestRegion2))
          {
            otbAppLogINFO("Corresponding region2 is " << region2.GetIndex() << ", " << region2.GetSize());

            extractChannel2->SetExtractionRegion(region2);

            Match(extractChannel1->GetOutput(), extractChannel2->GetOutput(), rsTransform, rsTransform1ToWGS84, rsTransform2ToWGS84, file, &mls);
          }
          else
          {
            otbAppLogINFO("Corresponding region2 is out of range: " << region2.GetIndex() << ", " << region2.GetSize());
          }
        }
      }
    }
    file.close();

    if (IsParameterEnabled("outvector"))
    {
      // Create the datasource (for matches export)
      otb::ogr::Layer               layer(nullptr, false);
      otb::ogr::DataSource::Pointer ogrDS;

      ogrDS = otb::ogr::DataSource::New(GetParameterString("outvector"), otb::ogr::DataSource::Modes::Overwrite);
      std::string projref =
          "GEOGCS[\"WGS 84\",DATUM[\"WGS_1984\",SPHEROID[\"WGS "
          "84\",6378137,298.257223563,AUTHORITY[\"EPSG\",\"7030\"]],AUTHORITY[\"EPSG\",\"6326\"]],PRIMEM[\"Greenwich\",0,AUTHORITY[\"EPSG\",\"8901\"]],UNIT["
          "\"degree\",0.01745329251994328,AUTHORITY[\"EPSG\",\"9122\"]],AUTHORITY[\"EPSG\",\"4326\"]]";
      OGRSpatialReference oSRS(projref.c_str());

      // and create the layer
      layer                = ogrDS->CreateLayer("matches", &oSRS, wkbMultiLineString);
      OGRFeatureDefn& defn = layer.GetLayerDefn();
      ogr::Feature    feature(defn);

      feature.SetGeometry(&mls);
      layer.CreateFeature(feature);
    }
  }
};
}
}


OTB_APPLICATION_EXPORT(otb::Wrapper::HomologousPointsExtraction)
