#include "itkBSplineScatteredDataPointSetToImageFilter.h"
#include "itkPointSet.h"
#include "itkImage.h"
#include "itkVectorImage.h"
#include "itkImageFileReader.h"
#include "itkImageFileWriter.h"

int main(int argc, char* argv[])
{
    const unsigned int ImageDimension = 3;

    using VectorType = itk::Vector<float, ImageDimension>;
    using InputImageType = itk::Image<VectorType, ImageDimension>;
    using InputPointType = InputImageType::PointType;
    using OutputImageType = itk::Image<VectorType, ImageDimension + 1>;
    using PointSetType = itk::PointSet<VectorType, ImageDimension + 1>;
    using OutputPointType = PointSetType::PointType;
    using ReaderType = itk::ImageFileReader<InputImageType>;
    using WriterType = itk::ImageFileWriter<OutputImageType>;

    PointSetType::Pointer pointSet = PointSetType::New();
    unsigned long pointId = 0;
    InputImageType::PointType origin;
    InputImageType::SpacingType spacing;
    InputImageType::SizeType size;

    try
    {
        for (int i = 1; i < argc; ++i)
        {
            ReaderType::Pointer reader = ReaderType::New();
            std::cout << "Reading file " << argv[i] << std::endl;
            reader->SetFileName(argv[i]);
            reader->Update();
            using IteratorType = itk::ImageRegionConstIterator<InputImageType>;
            const InputImageType * image = reader->GetOutput();
            IteratorType it(image, image->GetBufferedRegion());
            it.GoToBegin();
            InputPointType inPoint;
            OutputPointType outPoint;
            int iCount = 0;
            while (!it.IsAtEnd())
            {
                inPoint = image->GetPixel(it.GetIndex());
                for (int j = 0; j < ImageDimension; ++j)
                    outPoint[j] = inPoint[i];
                outPoint[ImageDimension] = i - 1;
                pointSet->SetPoint(pointId, outPoint);
                // Transfer the pixel data to the value associated with the point.
                pointSet->SetPointData(pointId, it.Get());
                ++it;
                ++pointId;
                ++iCount;
            }
            std::cout << "Number of points in " << argv[i] << " = " << iCount <<
                std::endl;
            std::cout << "Total number of points = " <<
                pointSet->GetNumberOfPoints() << std::endl;
            origin = image->GetOrigin();
            spacing = image->GetSpacing();
            size = image->GetLargestPossibleRegion().GetSize();
        }

        typedef itk::BSplineScatteredDataPointSetToImageFilter < PointSetType,
            OutputImageType > SplineFilterType;
        SplineFilterType::Pointer splineFilter = SplineFilterType::New();

        int splineorder = 3; // complexity of the spline

        SplineFilterType::ArrayType ncontrol;
        ncontrol.Fill(splineorder + 1);
        SplineFilterType::ArrayType closedim;
        closedim.Fill(0);

        OutputImageType::PointType parametricDomainOrigin;
        OutputImageType::SpacingType parametricDomainSpacing;
        OutputImageType::SizeType parametricDomainSize;
        for (int i = 0; i < ImageDimension; ++i)
        {
            parametricDomainOrigin[i] = origin[i];
            parametricDomainSpacing[i] = spacing[i];
            parametricDomainSize[i] = size[i];
        }
        parametricDomainOrigin[ImageDimension] = 0;
        parametricDomainSize[ImageDimension] = argc - 2;
        parametricDomainSpacing[ImageDimension] = 10.0;

        splineFilter->SetGenerateOutputImage(true); // the only reason to turn this off
        // is if one only wants to use the control point lattice for further processing
        splineFilter->SetInput(pointSet);
        splineFilter->SetSplineOrder(splineorder);
        splineFilter->SetNumberOfControlPoints(ncontrol);
        splineFilter->SetNumberOfLevels(3);
        splineFilter->SetCloseDimension(closedim);
        splineFilter->SetSize(parametricDomainSize);
        splineFilter->SetSpacing(parametricDomainSpacing);
        splineFilter->SetOrigin(parametricDomainOrigin);
        std::cout << "Before update spline filter" << std::endl;
        splineFilter->Update();
        std::cout << "After update spline filter" << std::endl;

        WriterType::Pointer writer = WriterType::New();
        writer->SetInput(splineFilter->GetOutput());
        writer->SetFileName("C:/a/DVF/Output.mha");
        writer->Update();
        std::cout << "After write image filter" << std::endl;
    }
    catch (itk::ExceptionObject & err)
    {
        std::cout << "ExceptionObject caught !" << std::endl;
        std::cout << err << std::endl;
        return EXIT_FAILURE;
    }

    return EXIT_SUCCESS;
};