#include "vtkTemporalTransformsReader.h"
#include "vtkTemporalTransformsWriter.h"

#include <iomanip>
#include <iostream>
#include <stdio.h>

#include <vtkPolyData.h>
#include <vtkTesting.h>

#include "TestHelpers.h"

const double epsilon = 1e-6;

bool file_exists(const std::string path)
{
  std::ifstream infile(path);
  return infile.good();
}

bool check_mm04_orbslam2_no_loop_closure(std::string path)
{
  auto reader = vtkSmartPointer<vtkTemporalTransformsReader>::New();
  reader->SetFileName(path.c_str());
  reader->Update();
  vtkPolyData* read = reader->GetOutput();

  bool allgood = true;
  allgood &= read->GetNumberOfCells() == 1;
  allgood &= read->GetNumberOfPoints() == 242;
  if (!allgood)
  {
    // no need to go further (else we can now ask for point 241)
    return 0;
  }

  vtkDoubleArray* points = vtkDoubleArray::SafeDownCast(read->GetPoints()->GetData());
  allgood &= points != nullptr;
  double* lastPointXYZ = points->GetTuple3(241);
  const double referenceLastPointXYZ[3] = { 0.1678862, -0.0361966, -0.0566818 };
  allgood &= compare(lastPointXYZ, referenceLastPointXYZ, 3, epsilon);

  vtkDoubleArray* time = vtkDoubleArray::SafeDownCast(read->GetPointData()->GetArray("Time"));
  allgood &= time != nullptr;
  double lastTime = time->GetTuple1(241);
  const double referenceTime = 136.457;
  allgood &= compare(&lastTime, &referenceTime, 1, epsilon);

  vtkDoubleArray* orientation =
    vtkDoubleArray::SafeDownCast(read->GetPointData()->GetArray("Orientation(AxisAngle)"));
  allgood &= orientation != nullptr;
  double* lastOrientation = orientation->GetTuple4(241);
  const double referenceLastOrientation[4] = {
    0.711088177611299, 0.335114389446281, 0.618103510463650, 0.022307328124517
  };
  allgood &= compare(lastOrientation, referenceLastOrientation, 4, epsilon);

  return allgood;
}

bool read_write_trajectory(std::string to_read, std::string to_write)
{
  auto reader = vtkSmartPointer<vtkTemporalTransformsReader>::New();
  reader->SetFileName(to_read.c_str());
  reader->Update();

  auto writer = vtkSmartPointer<vtkTemporalTransformsWriter>::New();
  writer->SetInputConnection(0, reader->GetOutputPort());
  writer->SetFileName(to_write.c_str());
  writer->SetInputArrayToProcess(0, 0, 0, 0, "Orientation(AxisAngle)");
  writer->Update();
  return 1;
}

int TestTemporalTransformsReaderWriter(int argc, char* argv[])
{
  // Load the initial dataset:
  vtkNew<vtkTesting> testing;
  testing->AddArguments(argc, argv);

  std::string dataRoot = testing->GetDataRoot();
  std::string tmpDirectory = testing->GetTempDirectory();
  std::string referenceTrajectory = dataRoot + "/trajectories/mm04/orbslam2-no-loop-closure.csv";
  std::string temporaryFile =
    tmpDirectory + "/TestTemporalTransformsReaderWriter-orbslam2-no-loop-closure.csv";

  // First, check that the reader works:
  if (!check_mm04_orbslam2_no_loop_closure(referenceTrajectory))
  {
    std::cout << "Reading trajectory using vtkTemporalTransformsReader"
              << " does not seem to work" << std::endl;
    return 1;
  }

  // Then prepare to test the writer
  if (file_exists(temporaryFile))
  {
    const int result = std::remove(temporaryFile.c_str());
    if (result != 0)
    {
      std::cout << "Failed to delete existing file: " << temporaryFile << " cannot test writer"
                << std::endl;
      return 1;
    }
  }

  // Then test the writer
  read_write_trajectory(referenceTrajectory, temporaryFile);
  if (!file_exists(temporaryFile))
  {
    std::cout << "Failed to produce file using writer: " << temporaryFile << std::endl;
    return 1;
  }

  if (!check_mm04_orbslam2_no_loop_closure(temporaryFile))
  {
    std::cout << "Reading file written using vtkTemporalTransformsWriter"
                 " does not seem to work"
              << std::endl;
    return 1;
  }

  const int result = std::remove(temporaryFile.c_str());
  if (result != 0)
  {
    std::cout << "Failed to cleanup by deleting existing file: " << temporaryFile << std::endl;
    return 1;
  }

  return 0;
}
