#include "vtkExemplePacketInterpreter.h"
#include "PacketFormat.h"
#include "vtkHelper.h"

#include <vtkPoints.h>
#include <vtkPointData.h>
#include <vtkDoubleArray.h>
#include <vtkTransform.h>

#include <bitset>
#include <boost/property_tree/xml_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/foreach.hpp>

#include <math.h>
#include <fenv.h>

#include <vtkDelimitedTextReader.h>
#include <chrono>

using std::chrono::high_resolution_clock;
using std::chrono::duration_cast;
using std::chrono::duration;
using std::chrono::milliseconds;


//! @todo this method are actually usefull for every Interpreter and should go to the top
template<typename T>
vtkSmartPointer<T> vtkExemplePacketInterpreter::CreateDataArray(bool isAdvanced,
                                                              const char* name,
                                                              vtkIdType vtkNotUsed(np),
                                                              vtkIdType prereserved_np,
                                                              vtkPolyData* pd)
{
  if (isAdvanced && !this->EnableAdvancedArrays)
  {
    return nullptr;
  }
  vtkSmartPointer<T> array = vtkSmartPointer<T>::New();
  array->SetNumberOfValues(prereserved_np);
  array->SetName(name);
  if (pd)
  {
    pd->GetPointData()->AddArray(array);
  }

  return array;
}

template<typename T, typename U>
void TrySetValue(T& array, int pos, U value)
{
  if (array != nullptr)
  {
    array->SetValue(pos,   value);
  }
}

//-----------------------------------------------------------------------------
vtkStandardNewMacro(vtkExemplePacketInterpreter)

//-----------------------------------------------------------------------------
vtkExemplePacketInterpreter::vtkExemplePacketInterpreter()
{
}

//-----------------------------------------------------------------------------
vtkExemplePacketInterpreter::~vtkExemplePacketInterpreter()
{
}

//-----------------------------------------------------------------------------
void vtkExemplePacketInterpreter::LoadCalibration(const std::string& filename)
{
  if (filename.empty())
  {
    this->IsCalibrated = false;
    return;
  }

  // Load the CSV file.
  // 2nd Column = Azimuth (Horizontal) Correction
  // 3rd Column = Elevation (Vertical) Correction

  vtkNew<vtkDelimitedTextReader> reader;
  reader->SetFileName(filename.c_str());
  reader->DetectNumericColumnsOn();
  reader->SetHaveHeaders(true);
  reader->SetFieldDelimiterCharacters(",");
  reader->SetStringDelimiter('"');
  reader->Update();

 
}

void vtkExemplePacketInterpreter::ProcessPacket(unsigned char const* data,
                                              unsigned int dataLength)
{
  auto start = high_resolution_clock::now();
  if (!this->IsLidarPacket(data, dataLength))
  {
    return;
  }
}

//-----------------------------------------------------------------------------
bool vtkExemplePacketInterpreter::IsLidarPacket(unsigned char const * vtkNotUsed(data),
                                              unsigned int vtkNotUsed(dataLength))
{
  return true;
}

//-----------------------------------------------------------------------------
vtkSmartPointer<vtkPolyData> vtkExemplePacketInterpreter::CreateNewEmptyFrame(vtkIdType numberOfPoints,
                                                                            vtkIdType vtkNotUsed(prereservedNumberOfPoints))
{
  vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New();
  return polyData;
}

//-----------------------------------------------------------------------------
bool vtkExemplePacketInterpreter::PreProcessPacket(unsigned char const* data,
  unsigned int vtkNotUsed(dataLength), fpos_t filePosition, double packetNetworkTime,
  std::vector<FrameInformation>* frameCatalog)
{
  bool isNewFrame = false;

  return isNewFrame;
}

//-----------------------------------------------------------------------------
std::string vtkExemplePacketInterpreter::GetSensorInformation(bool vtkNotUsed(shortVersion))
{
  return "Exemple Sensor";
}

