/*
 * Copyright (C) 2005-2017 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 "otbSubPixelDisparityImageFilter.h"
#include "otbImageFileReader.h"
#include "otbImageFileWriter.h"
#include "otbStandardWriterWatcher.h"
#include "otbImage.h"


  const unsigned int Dimension = 2;
  typedef float                                                          PixelType;
  typedef otb::Image<PixelType, Dimension>                               FloatImageType;



  typedef otb::Functor::NCCBlockMatching<FloatImageType,FloatImageType> NCCBlockMatchingFunctorType;

  typedef otb::SubPixelDisparityImageFilter<FloatImageType,
                                               FloatImageType,
                                               FloatImageType,
                                               FloatImageType,
                                               NCCBlockMatchingFunctorType> NCCSubPixelDisparityFilterType;

  typedef otb::Functor::SSDBlockMatching<FloatImageType,FloatImageType> SSDBlockMatchingFunctorType;
  typedef otb::SubPixelDisparityImageFilter<FloatImageType,
                                                FloatImageType,
                                                FloatImageType,
                                                FloatImageType,
                                                SSDBlockMatchingFunctorType> SSDSubPixelDisparityFilterType;


  typedef otb::Functor::LPBlockMatching<FloatImageType,FloatImageType> LPBlockMatchingFunctorType;
  typedef otb::SubPixelDisparityImageFilter<FloatImageType,
                                                  FloatImageType,
                                                  FloatImageType,
                                                  FloatImageType,
                                                  LPBlockMatchingFunctorType> LPSubPixelDisparityFilterType;


  typedef otb::ImageFileReader<FloatImageType>               ReaderType;
  typedef otb::ImageFileWriter<FloatImageType>      WriterType;

int otbSubPixelDisparityImageFilterNew(int itkNotUsed(argc), char * itkNotUsed(argv) [])
{

  // Instantiation
  SSDSubPixelDisparityFilterType::Pointer filter = SSDSubPixelDisparityFilterType::New();

   return EXIT_SUCCESS;
}

template<class TImage,class TFunctorType> int ProcessAndWriteOutput(otb::SubPixelDisparityImageFilter<TImage,
                                                       TImage,TImage,TImage,TFunctorType> * subPixFilter,int argc,char* argv[])
{

  ReaderType::Pointer leftReader = ReaderType::New();
  leftReader->SetFileName(argv[1]);

  ReaderType::Pointer rightReader = ReaderType::New();
  rightReader->SetFileName(argv[2]);

  ReaderType::Pointer bmhdispReader = ReaderType::New();
  bmhdispReader->SetFileName(argv[3]);
  // ReaderType::Pointer bmvdispReader = ReaderType::New();
  //          bmvdispReader->SetFileName(argv[3]);

  ReaderType::Pointer metricReader = ReaderType::New();
  metricReader->SetFileName(argv[4]);

  //  subPixFilter->SetVerticalDisparityInput(bmvdispReader->GetVerticalDisparityOutput());


  subPixFilter->SetLeftInput(leftReader->GetOutput());
  subPixFilter->SetRightInput(rightReader->GetOutput());

  subPixFilter->SetHorizontalDisparityInput(bmhdispReader->GetOutput());
  subPixFilter->SetRadius(atoi(argv[10]));

  subPixFilter->SetMinimumHorizontalDisparity(atoi(argv[11]));
  subPixFilter->SetMaximumHorizontalDisparity(atoi(argv[12]));

  subPixFilter->SetMinimumVerticalDisparity(atoi(argv[13]));
  subPixFilter->SetMaximumVerticalDisparity(atoi(argv[14]));

  subPixFilter->SetMetricInput(metricReader->GetOutput());

  ReaderType::Pointer leftMaskReader = ReaderType::New();
  ReaderType::Pointer rightMaskReader = ReaderType::New();
  if (argc > 15)
    {
    leftMaskReader->SetFileName(argv[15]);
    subPixFilter->SetLeftMaskInput(leftMaskReader->GetOutput());
    }

  if (argc > 16)
    {
    rightMaskReader->SetFileName(argv[16]);
    subPixFilter->SetRightMaskInput(rightMaskReader->GetOutput());
    }

  switch (atoi(argv[9]))
    {
    case 1:
      subPixFilter->SetRefineMethod(NCCSubPixelDisparityFilterType::PARABOLIC);
      break;
    case 2:
      subPixFilter->SetRefineMethod(NCCSubPixelDisparityFilterType::TRIANGULAR);
      break;
    case 3:
      subPixFilter->SetRefineMethod(NCCSubPixelDisparityFilterType::DICHOTOMY);
      break;
    default:
      std::cout << "wrong pixel mode" << std::endl;
      return EXIT_FAILURE;
      break;
    }
  otb::StandardWriterWatcher watcher(subPixFilter, "Sub-pixel refinement");
  // writers
  // horizontal disparity writer
  WriterType::Pointer hdispWriter = WriterType::New();
  hdispWriter->SetInput(subPixFilter->GetHorizontalDisparityOutput());
  hdispWriter->SetFileName(argv[5]);
  otb::StandardWriterWatcher watcher1(hdispWriter, subPixFilter, "Computing disparity ");

  hdispWriter->Update();

  // vertical disparity writer
  WriterType::Pointer vdispWriter = WriterType::New();
  vdispWriter->SetInput(subPixFilter->GetVerticalDisparityOutput());
  vdispWriter->SetFileName(argv[6]);

  vdispWriter->Update();

  // metric writer
  // vertical disparity writer
  WriterType::Pointer metricWriter = WriterType::New();
  metricWriter->SetInput(subPixFilter->GetMetricOutput());
  metricWriter->SetFileName(argv[7]);

  metricWriter->Update();

  return EXIT_SUCCESS;
}


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

  if ((argc < 15) || (argc > 17))
    {
    std::cerr << "Usage: " << argv[0]
        << " leftinput_fname rightinput_fname blockmatchinginput_fname blockmatchingmetricinput_fname ";
    std::cerr << "hdispoutput_fname vdispoutput_fname metricoutput_fname " << std::endl;
    std::cerr << "metric (0=SSD, 1=NCC, 2=LP pseudo norm) ";
    std::cerr << "subpixelmode (1=parabolic,2=triangular,3=dichotomy)" << std::endl;
    std::cerr << "radius minhdisp maxhdisp minvdisp maxvdisp " << std::endl;
    std::cerr << "(leftmaskinput_fname) (rightmaskinput_fname)" << std::endl;
    return EXIT_FAILURE;
    }

  SSDSubPixelDisparityFilterType::Pointer SSDSubPixFilter = SSDSubPixelDisparityFilterType::New();
  NCCSubPixelDisparityFilterType::Pointer NCCSubPixFilter = NCCSubPixelDisparityFilterType::New();
  LPSubPixelDisparityFilterType::Pointer LPSubPixFilter = LPSubPixelDisparityFilterType::New();

  // Instantiation
  switch (atoi(argv[8]))
    {
    case 0:
      SSDSubPixFilter->MinimizeOn();
      return ProcessAndWriteOutput<FloatImageType, SSDBlockMatchingFunctorType> (SSDSubPixFilter, argc, argv);
      break;
    case 1:
      NCCSubPixFilter->MinimizeOff();
      return ProcessAndWriteOutput<FloatImageType, NCCBlockMatchingFunctorType> (NCCSubPixFilter, argc, argv);
      break;
    case 2:
      LPSubPixFilter->MinimizeOff();
      return ProcessAndWriteOutput<FloatImageType, LPBlockMatchingFunctorType> (LPSubPixFilter, argc, argv);
      break;
    default:
      std::cout << "wrong norm type  (0=SSD, 1=NCC, 2=LP pseudo norm)" << std::endl;
      return EXIT_FAILURE;
      break;
    }

  return EXIT_FAILURE;
}
