<commit_before>// Copyright 2013 Velodyne Acoustics, Inc.
//
// 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.
/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkVelodyneHDLPositionReader.cxx

  Copyright (c) <NAME>, <NAME>, <NAME>
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.  See the above copyright notice for more information.

=========================================================================*/

#include "vtkVelodyneHDLPositionReader.h"

#include "vtkPacketFileReader.h"
#include "vtkPacketFileWriter.h"
#include "vtkVelodyneTransformInterpolator.h"

#include <vtkCellArray.h>
#include <vtkCellData.h>
#include <vtkDataArray.h>
#include <vtkDoubleArray.h>
#include <vtkFloatArray.h>
#include <vtkInformation.h>
#include <vtkInformationVector.h>
#include <vtkMath.h>
#include <vtkNew.h>
#include <vtkObjectFactory.h>
#include <vtkPointData.h>
#include <vtkPoints.h>
#include <vtkPolyData.h>
#include <vtkPolyLine.h>
#include <vtkSmartPointer.h>
#include <vtkStreamingDemandDrivenPipeline.h>
#include <vtkTransform.h>
#include <vtkTupleInterpolator.h>
#include <vtkUnsignedCharArray.h>
#include <vtkUnsignedIntArray.h>
#include <vtkUnsignedShortArray.h>

#include <vtk_libproj4.h>
#include "NMEAParser.h"

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

#include <algorithm>
#include <map>
#include <sstream>

#include <cmath>

#ifdef _MSC_VER
#include <boost/cstdint.hpp>
typedef boost::uint8_t uint8_t;
#else
#include <stdint.h>
#endif

namespace
{
struct PositionPacket
{
  // tohTimestamp is a microseconds rolling counter provided by the lidar
  // internal clock which will be adjusted using the gps data (if one is plugged)
  // tohTimestamp gives the number of microseconds ellapsed since the top of
  // the hour. The top of the hour is defined as (modulo 1 hour):
  // - without a gps plugged: the instant when the lidar was powered on
  // - with a gps: any full UTC hour (such as 12:00:00 am, UTC)
  unsigned int tohTimestamp;
  short gyro[3];
  short temp[3];
  short accelx[3];
  short accely[3];
  char sentance[73];
};
}

//-----------------------------------------------------------------------------
class vtkVelodyneHDLPositionReader::vtkInternal
{
public:
  vtkInternal()
  {
    this->Reader = 0;
    this->UTMZone = -1;
    this->Offset[0] = 0.0;
    this->Offset[1] = 0.0;
    this->Offset[2] = 0.0;
    this->CalibrationTransform->Identity();
  }

  int ProcessHDLPacket(const unsigned char* data, unsigned int bytes, PositionPacket& position);
  std::vector<std::string> ParseSentance(const std::string& sentance);

  void InterpolateGPS(
    vtkPoints* points, vtkDataArray* gpsTime, vtkDataArray* times, vtkDataArray* heading);

  vtkPacketFileReader* Reader;
  int UTMZone;
  std::string UTMString;
  double Offset[3];

  vtkNew<vtkVelodyneTransformInterpolator> Interp;
  vtkNew<vtkTransform> CalibrationTransform;
};

namespace
{
const unsigned short BIT_12_MASK = 0x0fff;
const unsigned short REMAINDER_12_MASK = 0x07ff;
const unsigned short SIGN_12_MASK = 0x0800;

const double GYRO_SCALE = 0.09766;   // deg / s
const double TEMP_SCALE = 0.1453;    // C
const double TEMP_OFFSET = 25.0;     // C
const double ACCEL_SCALE = 0.001221; // G
}

//-----------------------------------------------------------------------------
int vtkVelodyneHDLPositionReader::vtkInternal::ProcessHDLPacket(
  const unsigned char* data, unsigned int bytes, PositionPacket& position)
{
  if (bytes != 512)
  {
    // Data-Packet Specifications says that position-packets are 512 byte long.
    return 0;
  }

  for (int i = 0; i < 14; ++i)
  {
    if (data[i] != 0)
    {
      std::cerr << "unexpected data in first zeros block\n";
      return 0;
    }
  }

  for (int i = 0; i < 3; ++i)
  {
    memcpy(position.gyro + i, data + 14 + i * 8, 2);
    memcpy(position.temp + i, data + 14 + i * 8 + 2, 2);
    memcpy(position.accelx + i, data + 14 + i * 8 + 4, 2);
    memcpy(position.accely + i, data + 14 + i * 8 + 6, 2);
  }

  for (int i = 0; i < 3; ++i)
  {
    // Selector only least significant 12 bits
    position.gyro[i] &= BIT_12_MASK;
    position.temp[i] &= BIT_12_MASK;
    position.accelx[i] &= BIT_12_MASK;
    position.accely[i] &= BIT_12_MASK;

    // Perform 12 bit twos complement
    position.gyro[i] =
      -2048 * ((position.gyro[i] & SIGN_12_MASK) >> 11) + (position.gyro[i] & REMAINDER_12_MASK);
    position.temp[i] =
      -2048 * ((position.temp[i] & SIGN_12_MASK) >> 11) + (position.temp[i] & REMAINDER_12_MASK);
    position.accelx[i] = -2048 * ((position.accelx[i] & SIGN_12_MASK) >> 11) +
      (position.accelx[i] & REMAINDER_12_MASK);
    position.accely[i] = -2048 * ((position.accely[i] & SIGN_12_MASK) >> 11) +
      (position.accely[i] & REMAINDER_12_MASK);
  }

  memcpy(&position.tohTimestamp, data + 14 + 3 * 8 + 160, 4);

  std::copy(data + 14 + 8 + 8 + 8 + 160 + 4 + 4, data + 14 + 8 + 8 + 8 + 160 + 4 + 4 + 72,
    position.sentance);
  position.sentance[72] = '\0';

  return 1;
}

//-----------------------------------------------------------------------------
std::vector<std::string> vtkVelodyneHDLPositionReader::vtkInternal::ParseSentance(
  const std::string& sentance)
{
  std::stringstream sstr(sentance);
  std::string token;

  std::vector<std::string> result;

  while (std::getline(sstr, token, ','))
  {
    result.push_back(token);
  }

  return result;
}

//-----------------------------------------------------------------------------
void vtkVelodyneHDLPositionReader::SetShouldWarnOnWeirdGPSData(bool ShouldWarnOnWeirdGPSData_)
{
  this->ShouldWarnOnWeirdGPSData = ShouldWarnOnWeirdGPSData_;
}

//-----------------------------------------------------------------------------
void vtkVelodyneHDLPositionReader::SetCalibrationTransform(vtkTransform* transform)
{
  if (transform)
  {
    this->Internal->CalibrationTransform->SetMatrix(transform->GetMatrix());
  }
  else
  {
    this->Internal->CalibrationTransform->Identity();
  }
  this->Modified();
}

//-----------------------------------------------------------------------------
vtkVelodyneTransformInterpolator* vtkVelodyneHDLPositionReader::GetInterpolator()
{
  return this->Internal->Interp.GetPointer();
}

//-----------------------------------------------------------------------------
void vtkVelodyneHDLPositionReader::vtkInternal::InterpolateGPS(
  vtkPoints* points, vtkDataArray* gpsTime, vtkDataArray* times, vtkDataArray* headings)
{
  vtkNew<vtkTupleInterpolator> headingInterpolator;

  // assert(gpsTime is sorted)
  assert(points->GetNumberOfPoints() == times->GetNumberOfTuples());

  this->Interp->SetInterpolationTypeToLinear();
  this->Interp->Initialize();
  headingInterpolator->SetInterpolationTypeToLinear();
  headingInterpolator->SetNumberOfComponents(2);

  double timeOffset = 0.0;

  assert(times->GetNumberOfTuples() == gpsTime->GetNumberOfTuples());
  unsigned int lastGPS = 0;

  for (vtkIdType i = 0, k = times->GetNumberOfTuples(); i < k; ++i)
  {
    const unsigned int currGPS = gpsTime->GetTuple1(i);
    if (currGPS != lastGPS)
    {
      if (currGPS < lastGPS)
      {
        // time of day has wrapped; increment time offset
        timeOffset += 24.0 * 3600.0;
      }
      lastGPS = currGPS;

      // Compute time in seconds from decimal-encoded time
      const int hours = currGPS / 10000;
      const int minutes = (currGPS / 100) % 100;
      const int seconds = currGPS % 100;
      const double convertedtime = ((3600 * hours) + (60.0 * minutes) + seconds) + timeOffset;

      // Get position and heading
      double pos[3];
      points->GetPoint(i, pos);

      const double heading = headings->GetTuple1(i);

      // Check the input data
      bool isTranslationFinite =
        vtkMath::IsFinite(pos[0]) && vtkMath::IsFinite(pos[1]) && vtkMath::IsFinite(pos[2]);
      bool isRotationFinite = vtkMath::IsFinite(heading);

      // Compute transform
      // Here we want to compute the transform to go
      // from the solid referential frame to the world
      // georeferenced frame. Hence, given the position
      // and orientation of the GPS in the solid frame we
      // need to apply the transform from the solid frame
      // to the GPS (backward gps pose) and then the transform
      // from the GPS to the world georeferenced frame
      vtkNew<vtkTransform> transformGpsWorld, transformVehiculeWorld;
      transformGpsWorld->PostMultiply();
      if (isRotationFinite)
        transformGpsWorld->RotateZ(heading);
      else
        vtkGenericWarningMacro("Error in GPS rotation");
      if (isTranslationFinite)
        transformGpsWorld->Translate(pos);
      else
        vtkGenericWarningMacro("Error in GPS position");

      // Compute transform from vehicule to GPS
      // and then compose with the transform GPS to world
      vtkNew<vtkMatrix4x4> gpsToWorld, vehiculeToGps, vehiculeToWorld;
      this->CalibrationTransform->GetMatrix(vehiculeToGps.Get());
      transformGpsWorld->GetMatrix(gpsToWorld.Get());
      vehiculeToGps->Invert();
      vtkMatrix4x4::Multiply4x4(gpsToWorld.Get(), vehiculeToGps.Get(), vehiculeToWorld.Get());
      transformVehiculeWorld->SetMatrix(vehiculeToWorld.Get());
      transformVehiculeWorld->Modified();

      // Add the transform to the interpolator
      this->Interp->AddTransform(convertedtime, transformVehiculeWorld.GetPointer());

      // Compute heading vector for interpolation
      double ha = heading * DEG_TO_RAD;
      double hv[2] = { cos(ha), sin(ha) };
      headingInterpolator->AddTuple(convertedtime, hv);
    }
  }
}

//-----------------------------------------------------------------------------
vtkStandardNewMacro(vtkVelodyneHDLPositionReader);

//-----------------------------------------------------------------------------
vtkVelodyneHDLPositionReader::vtkVelodyneHDLPositionReader()
{
  this->Internal = new vtkInternal;
  this->SetNumberOfInputPorts(0);
  this->SetNumberOfOutputPorts(1);
}

//-----------------------------------------------------------------------------
vtkVelodyneHDLPositionReader::~vtkVelodyneHDLPositionReader()
{
  delete this->Internal;
}

//-----------------------------------------------------------------------------
const std::string& vtkVelodyneHDLPositionReader::GetFileName()
{
  return this->FileName;
}

//-----------------------------------------------------------------------------
void vtkVelodyneHDLPositionReader::SetFileName(const std::string& filename)
{
  if (filename == this->FileName)
  {
    return;
  }

  this->FileName = filename;
  this->Modified();
}

namespace
{

int LatLongToZone(double lat, double lon)
{
  double longTemp = (lon + 180) - static_cast<int>((lon + 180) / 360) * 360 - 180;

  int zone = static_cast<int>((longTemp + 180) / 6) + 1;
  if (lat >= 56.0 && lat < 64.0 && longTemp >= 3.0 && longTemp < 12.0)
  {
    zone = 32;
  }

  if (lat >= 72.0 && lat < 84)
  {
    if (longTemp >= 0.0 && longTemp < 9.0)
    {
      zone = 31;
    }
    else if (longTemp >= 9.0 && longTemp < 21.0)
    {
      zone = 33;
    }
    else if (longTemp >= 21.0 && longTemp < 33.0)
    {
      zone = 35;
    }
    else if (longTemp >= 33.0 && longTemp < 42.0)
    {
      zone = 37;
    }
  }

  return zone;
}
}

//-----------------------------------------------------------------------------
int vtkVelodyneHDLPositionReader::RequestData(
  vtkInformation* request, vtkInformationVector** inputVector, vtkInformationVector* outputVector)
{
  vtkPolyData* output = vtkPolyData::GetData(outputVector);

  if (!this->FileName.length())
  {
    vtkErrorMacro("FileName has not been set.");
    return 0;
  }

  this->Internal->UTMZone = -1;

  vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
  vtkSmartPointer<vtkCellArray> cells = vtkSmartPointer<vtkCellArray>::New();
  vtkSmartPointer<vtkPolyLine> polyLine = vtkSmartPointer<vtkPolyLine>::New();
  vtkSmartPointer<vtkIdList> polyIds = polyLine->GetPointIds();

  // Data arrays
  vtkSmartPointer<vtkDoubleArray> lats = vtkSmartPointer<vtkDoubleArray>::New();
  lats->SetName("lat");
  vtkSmartPointer<vtkDoubleArray> lons = vtkSmartPointer<vtkDoubleArray>::New();
  lons->SetName("lon");

  vtkSmartPointer<vtkDoubleArray> times = vtkSmartPointer<vtkDoubleArray>::New();
  times->SetName("time");

  vtkSmartPointer<vtkDoubleArray> gpsTime = vtkSmartPointer<vtkDoubleArray>::New();
  gpsTime->SetName("gpstime");

  typedef std::map<std::string, vtkSmartPointer<vtkDoubleArray> > VecMap;
  VecMap dataVectors;
  dataVectors.insert(std::make_pair("gyro1", vtkSmartPointer<vtkDoubleArray>::New()));
  dataVectors.insert(std::make_pair("gyro2", vtkSmartPointer<vtkDoubleArray>::New()));
  dataVectors.insert(std::make_pair("gyro3", vtkSmartPointer<vtkDoubleArray>::New()));
  dataVectors.insert(std::make_pair("temp1", vtkSmartPointer<vtkDoubleArray>::New()));
  dataVectors.insert(std::make_pair("temp2", vtkSmartPointer<vtkDoubleArray>::New()));
  dataVectors.insert(std::make_pair("temp3", vtkSmartPointer<vtkDoubleArray>::New()));
  dataVectors.insert(std::make_pair("accel1x", vtkSmartPointer<vtkDoubleArray>::New()));
  dataVectors.insert(std::make_pair("accel1y", vtkSmartPointer<vtkDoubleArray>::New()));
  dataVectors.insert(std::make_pair("accel2x", vtkSmartPointer<vtkDoubleArray>::New()));
  dataVectors.insert(std::make_pair("accel2y", vtkSmartPointer<vtkDoubleArray>::New()));
  dataVectors.insert(std::make_pair("accel3x", vtkSmartPointer<vtkDoubleArray>::New()));
  dataVectors.insert(std::make_pair("accel3y", vtkSmartPointer<vtkDoubleArray>::New()));
  dataVectors.insert(std::make_pair("heading", vtkSmartPointer<vtkDoubleArray>::New()));
  for (VecMap::iterator it = dataVectors.begin(); it != dataVectors.end(); ++it)
  {
    it->second->SetName(it->first.c_str());
  }

  points->Allocate(5000, 5000);
  cells->Allocate(5000, 5000);
  lats->Allocate(5000, 5000);
  lons->Allocate(5000, 5000);
  gpsTime->Allocate(5000, 5000);

  const unsigned char* data;
  unsigned int dataLength;
  double timeSinceStart;

  projPJ pj_utm = NULL;
  // PROJ *pj_latlong;
  // const char *latlonargs[4] = {"+proj=longlat", "+ellps=WGS84",
  //                              "+datum=WGS84",  "+no_defs" };

  // // Modern compilers and old proj4 api are at war here
  // pj_latlong = proj_init(4, const_cast<char**>(latlonargs));
  // assert(pj_latlong);

  this->Open();
  vtkIdType pointcount = 0;

  while (this->Internal->Reader->NextPacket(data, dataLength, timeSinceStart))
  {
    PositionPacket position;
    if (!this->Internal->ProcessHDLPacket(data, dataLength, position))
    {
      continue;
    }

    double x, y, z, lat, lon, heading, gpsUpdateTime;
    if (std::string(position.sentance).size() == 0)
    {
      // If there is no sentence to parse (no gps connected),
      // we use the following:
      x = 0.0;
      y = 0.0;
      z = 0.0;
      lat = 0.0;
      lon = 0.0;
      heading = 0.0;
      // the follwing value is wrong (no gps update so no update time),
      // so it should also be 0.0 (invalid)
      // but this value is kept to not risk breaking anything:
      gpsUpdateTime = position.tohTimestamp;
    }
    else
    {
      NMEAParser parser;
      NMEALocation parsedNMEA;
      if (!parser.ParseLocation(position.sentance, parsedNMEA))
      {
        vtkGenericWarningMacro("Failed to parse NMEA sentence: "
                               << position.sentance);
      }

      if (this->Internal->UTMZone < 0)
      {
        assert(!pj_utm);
        this->Internal->UTMZone = LatLongToZone(parsedNMEA.Lat, parsedNMEA.Long);
        std::stringstream utmparams;
        utmparams << "+proj=utm ";
        std::stringstream zone;
        zone << "+zone=" << this->Internal->UTMZone;
        this->Internal->UTMString = zone.str();
        // WARNING: Dont let the string stream pass out of scope until
        // we finish initialization
        utmparams << this->Internal->UTMString << " ";
        if (parsedNMEA.Lat < 0)
        {
          utmparams << "+south ";
        }

        utmparams << "+ellps=WGS84 ";
        utmparams << "+units=m ";
        utmparams << "+no_defs ";
        pj_utm = pj_init_plus(utmparams.str().c_str());
      }

      assert(pj_utm);<commit_msg>Allow longer NMEA sentences in PositionPackets<commit_after>// Copyright 2013 Velodyne Acoustics, Inc.
//
// 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.
/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkVelodyneHDLPositionReader.cxx

  Copyright (c) <NAME>, <NAME>, <NAME>
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.  See the above copyright notice for more information.

=========================================================================*/

#include "vtkVelodyneHDLPositionReader.h"

#include "vtkPacketFileReader.h"
#include "vtkPacketFileWriter.h"
#include "vtkVelodyneTransformInterpolator.h"

#include <vtkCellArray.h>
#include <vtkCellData.h>
#include <vtkDataArray.h>
#include <vtkDoubleArray.h>
#include <vtkFloatArray.h>
#include <vtkInformation.h>
#include <vtkInformationVector.h>
#include <vtkMath.h>
#include <vtkNew.h>
#include <vtkObjectFactory.h>
#include <vtkPointData.h>
#include <vtkPoints.h>
#include <vtkPolyData.h>
#include <vtkPolyLine.h>
#include <vtkSmartPointer.h>
#include <vtkStreamingDemandDrivenPipeline.h>
#include <vtkTransform.h>
#include <vtkTupleInterpolator.h>
#include <vtkUnsignedCharArray.h>
#include <vtkUnsignedIntArray.h>
#include <vtkUnsignedShortArray.h>

#include <vtk_libproj4.h>
#include "NMEAParser.h"

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

#include <algorithm>
#include <map>
#include <sstream>

#include <cmath>

#ifdef _MSC_VER
#include <boost/cstdint.hpp>
typedef boost::uint8_t uint8_t;
#else
#include <stdint.h>
#endif

namespace
{
struct PositionPacket
{
  // tohTimestamp is a microseconds rolling counter provided by the lidar
  // internal clock which will be adjusted using the gps data (if one is plugged)
  // tohTimestamp gives the number of microseconds ellapsed since the top of
  // the hour. The top of the hour is defined as (modulo 1 hour):
  // - without a gps plugged: the instant when the lidar was powered on
  // - with a gps: any full UTC hour (such as 12:00:00 am, UTC)
  unsigned int tohTimestamp;
  short gyro[3];
  short temp[3];
  short accelx[3];
  short accely[3];
  // UDP position packet is 554 bytes long with 42 bytes of header and 512 bytes
  // of payload. In the payload, 512 - (198 + 4 + 1 + 3) = 306 are available for
  // the NMEA sequence.
  // The last bytes stored in the sentence char array should be 0 (NMEA
  // sentences are not that long).
  char sentance[306];
};
}

//-----------------------------------------------------------------------------
class vtkVelodyneHDLPositionReader::vtkInternal
{
public:
  vtkInternal()
  {
    this->Reader = 0;
    this->UTMZone = -1;
    this->Offset[0] = 0.0;
    this->Offset[1] = 0.0;
    this->Offset[2] = 0.0;
    this->CalibrationTransform->Identity();
  }

  int ProcessHDLPacket(const unsigned char* data, unsigned int bytes, PositionPacket& position);
  std::vector<std::string> ParseSentance(const std::string& sentance);

  void InterpolateGPS(
    vtkPoints* points, vtkDataArray* gpsTime, vtkDataArray* times, vtkDataArray* heading);

  vtkPacketFileReader* Reader;
  int UTMZone;
  std::string UTMString;
  double Offset[3];

  vtkNew<vtkVelodyneTransformInterpolator> Interp;
  vtkNew<vtkTransform> CalibrationTransform;
};

namespace
{
const unsigned short BIT_12_MASK = 0x0fff;
const unsigned short REMAINDER_12_MASK = 0x07ff;
const unsigned short SIGN_12_MASK = 0x0800;

const double GYRO_SCALE = 0.09766;   // deg / s
const double TEMP_SCALE = 0.1453;    // C
const double TEMP_OFFSET = 25.0;     // C
const double ACCEL_SCALE = 0.001221; // G
}

//-----------------------------------------------------------------------------
int vtkVelodyneHDLPositionReader::vtkInternal::ProcessHDLPacket(
  const unsigned char* data, unsigned int bytes, PositionPacket& position)
{
  if (bytes != 512)
  {
    // Data-Packet Specifications says that position-packets are 512 byte long.
    return 0;
  }

  for (int i = 0; i < 14; ++i)
  {
    if (data[i] != 0)
    {
      std::cerr << "unexpected data in first zeros block\n";
      return 0;
    }
  }

  for (int i = 0; i < 3; ++i)
  {
    memcpy(position.gyro + i, data + 14 + i * 8, 2);
    memcpy(position.temp + i, data + 14 + i * 8 + 2, 2);
    memcpy(position.accelx + i, data + 14 + i * 8 + 4, 2);
    memcpy(position.accely + i, data + 14 + i * 8 + 6, 2);
  }

  for (int i = 0; i < 3; ++i)
  {
    // Selector only least significant 12 bits
    position.gyro[i] &= BIT_12_MASK;
    position.temp[i] &= BIT_12_MASK;
    position.accelx[i] &= BIT_12_MASK;
    position.accely[i] &= BIT_12_MASK;

    // Perform 12 bit twos complement
    position.gyro[i] =
      -2048 * ((position.gyro[i] & SIGN_12_MASK) >> 11) + (position.gyro[i] & REMAINDER_12_MASK);
    position.temp[i] =
      -2048 * ((position.temp[i] & SIGN_12_MASK) >> 11) + (position.temp[i] & REMAINDER_12_MASK);
    position.accelx[i] = -2048 * ((position.accelx[i] & SIGN_12_MASK) >> 11) +
      (position.accelx[i] & REMAINDER_12_MASK);
    position.accely[i] = -2048 * ((position.accely[i] & SIGN_12_MASK) >> 11) +
      (position.accely[i] & REMAINDER_12_MASK);
  }

  memcpy(&position.tohTimestamp, data + 14 + 3 * 8 + 160, 4);

  const int sentence_start =  14 + 8 + 8 + 8 + 160 + 4 + 4;
  std::copy(data + sentence_start,
            data + sentence_start + 306,
            position.sentance);
  // protection to terminate the string in case the sentence does not fit in the
  // 306 bytes.
  position.sentance[305] = '\0';

  return 1;
}

//-----------------------------------------------------------------------------
std::vector<std::string> vtkVelodyneHDLPositionReader::vtkInternal::ParseSentance(
  const std::string& sentance)
{
  std::stringstream sstr(sentance);
  std::string token;

  std::vector<std::string> result;

  while (std::getline(sstr, token, ','))
  {
    result.push_back(token);
  }

  return result;
}

//-----------------------------------------------------------------------------
void vtkVelodyneHDLPositionReader::SetShouldWarnOnWeirdGPSData(bool ShouldWarnOnWeirdGPSData_)
{
  this->ShouldWarnOnWeirdGPSData = ShouldWarnOnWeirdGPSData_;
}

//-----------------------------------------------------------------------------
void vtkVelodyneHDLPositionReader::SetCalibrationTransform(vtkTransform* transform)
{
  if (transform)
  {
    this->Internal->CalibrationTransform->SetMatrix(transform->GetMatrix());
  }
  else
  {
    this->Internal->CalibrationTransform->Identity();
  }
  this->Modified();
}

//-----------------------------------------------------------------------------
vtkVelodyneTransformInterpolator* vtkVelodyneHDLPositionReader::GetInterpolator()
{
  return this->Internal->Interp.GetPointer();
}

//-----------------------------------------------------------------------------
void vtkVelodyneHDLPositionReader::vtkInternal::InterpolateGPS(
  vtkPoints* points, vtkDataArray* gpsTime, vtkDataArray* times, vtkDataArray* headings)
{
  vtkNew<vtkTupleInterpolator> headingInterpolator;

  // assert(gpsTime is sorted)
  assert(points->GetNumberOfPoints() == times->GetNumberOfTuples());

  this->Interp->SetInterpolationTypeToLinear();
  this->Interp->Initialize();
  headingInterpolator->SetInterpolationTypeToLinear();
  headingInterpolator->SetNumberOfComponents(2);

  double timeOffset = 0.0;

  assert(times->GetNumberOfTuples() == gpsTime->GetNumberOfTuples());
  unsigned int lastGPS = 0;

  for (vtkIdType i = 0, k = times->GetNumberOfTuples(); i < k; ++i)
  {
    const unsigned int currGPS = gpsTime->GetTuple1(i);
    if (currGPS != lastGPS)
    {
      if (currGPS < lastGPS)
      {
        // time of day has wrapped; increment time offset
        timeOffset += 24.0 * 3600.0;
      }
      lastGPS = currGPS;

      // Compute time in seconds from decimal-encoded time
      const int hours = currGPS / 10000;
      const int minutes = (currGPS / 100) % 100;
      const int seconds = currGPS % 100;
      const double convertedtime = ((3600 * hours) + (60.0 * minutes) + seconds) + timeOffset;

      // Get position and heading
      double pos[3];
      points->GetPoint(i, pos);

      const double heading = headings->GetTuple1(i);

      // Check the input data
      bool isTranslationFinite =
        vtkMath::IsFinite(pos[0]) && vtkMath::IsFinite(pos[1]) && vtkMath::IsFinite(pos[2]);
      bool isRotationFinite = vtkMath::IsFinite(heading);

      // Compute transform
      // Here we want to compute the transform to go
      // from the solid referential frame to the world
      // georeferenced frame. Hence, given the position
      // and orientation of the GPS in the solid frame we
      // need to apply the transform from the solid frame
      // to the GPS (backward gps pose) and then the transform
      // from the GPS to the world georeferenced frame
      vtkNew<vtkTransform> transformGpsWorld, transformVehiculeWorld;
      transformGpsWorld->PostMultiply();
      if (isRotationFinite)
        transformGpsWorld->RotateZ(heading);
      else
        vtkGenericWarningMacro("Error in GPS rotation");
      if (isTranslationFinite)
        transformGpsWorld->Translate(pos);
      else
        vtkGenericWarningMacro("Error in GPS position");

      // Compute transform from vehicule to GPS
      // and then compose with the transform GPS to world
      vtkNew<vtkMatrix4x4> gpsToWorld, vehiculeToGps, vehiculeToWorld;
      this->CalibrationTransform->GetMatrix(vehiculeToGps.Get());
      transformGpsWorld->GetMatrix(gpsToWorld.Get());
      vehiculeToGps->Invert();
      vtkMatrix4x4::Multiply4x4(gpsToWorld.Get(), vehiculeToGps.Get(), vehiculeToWorld.Get());
      transformVehiculeWorld->SetMatrix(vehiculeToWorld.Get());
      transformVehiculeWorld->Modified();

      // Add the transform to the interpolator
      this->Interp->AddTransform(convertedtime, transformVehiculeWorld.GetPointer());

      // Compute heading vector for interpolation
      double ha = heading * DEG_TO_RAD;
      double hv[2] = { cos(ha), sin(ha) };
      headingInterpolator->AddTuple(convertedtime, hv);
    }
  }
}

//-----------------------------------------------------------------------------
vtkStandardNewMacro(vtkVelodyneHDLPositionReader);

//-----------------------------------------------------------------------------
vtkVelodyneHDLPositionReader::vtkVelodyneHDLPositionReader()
{
  this->Internal = new vtkInternal;
  this->SetNumberOfInputPorts(0);
  this->SetNumberOfOutputPorts(1);
}

//-----------------------------------------------------------------------------
vtkVelodyneHDLPositionReader::~vtkVelodyneHDLPositionReader()
{
  delete this->Internal;
}

//-----------------------------------------------------------------------------
const std::string& vtkVelodyneHDLPositionReader::GetFileName()
{
  return this->FileName;
}

//-----------------------------------------------------------------------------
void vtkVelodyneHDLPositionReader::SetFileName(const std::string& filename)
{
  if (filename == this->FileName)
  {
    return;
  }

  this->FileName = filename;
  this->Modified();
}

namespace
{

int LatLongToZone(double lat, double lon)
{
  double longTemp = (lon + 180) - static_cast<int>((lon + 180) / 360) * 360 - 180;

  int zone = static_cast<int>((longTemp + 180) / 6) + 1;
  if (lat >= 56.0 && lat < 64.0 && longTemp >= 3.0 && longTemp < 12.0)
  {
    zone = 32;
  }

  if (lat >= 72.0 && lat < 84)
  {
    if (longTemp >= 0.0 && longTemp < 9.0)
    {
      zone = 31;
    }
    else if (longTemp >= 9.0 && longTemp < 21.0)
    {
      zone = 33;
    }
    else if (longTemp >= 21.0 && longTemp < 33.0)
    {
      zone = 35;
    }
    else if (longTemp >= 33.0 && longTemp < 42.0)
    {
      zone = 37;
    }
  }

  return zone;
}
}

//-----------------------------------------------------------------------------
int vtkVelodyneHDLPositionReader::RequestData(
  vtkInformation* request, vtkInformationVector** inputVector, vtkInformationVector* outputVector)
{
  vtkPolyData* output = vtkPolyData::GetData(outputVector);

  if (!this->FileName.length())
  {
    vtkErrorMacro("FileName has not been set.");
    return 0;
  }

  this->Internal->UTMZone = -1;

  vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
  vtkSmartPointer<vtkCellArray> cells = vtkSmartPointer<vtkCellArray>::New();
  vtkSmartPointer<vtkPolyLine> polyLine = vtkSmartPointer<vtkPolyLine>::New();
  vtkSmartPointer<vtkIdList> polyIds = polyLine->GetPointIds();

  // Data arrays
  vtkSmartPointer<vtkDoubleArray> lats = vtkSmartPointer<vtkDoubleArray>::New();
  lats->SetName("lat");
  vtkSmartPointer<vtkDoubleArray> lons = vtkSmartPointer<vtkDoubleArray>::New();
  lons->SetName("lon");

  vtkSmartPointer<vtkDoubleArray> times = vtkSmartPointer<vtkDoubleArray>::New();
  times->SetName("time");

  vtkSmartPointer<vtkDoubleArray> gpsTime = vtkSmartPointer<vtkDoubleArray>::New();
  gpsTime->SetName("gpstime");

  typedef std::map<std::string, vtkSmartPointer<vtkDoubleArray> > VecMap;
  VecMap dataVectors;
  dataVectors.insert(std::make_pair("gyro1", vtkSmartPointer<vtkDoubleArray>::New()));
  dataVectors.insert(std::make_pair("gyro2", vtkSmartPointer<vtkDoubleArray>::New()));
  dataVectors.insert(std::make_pair("gyro3", vtkSmartPointer<vtkDoubleArray>::New()));
  dataVectors.insert(std::make_pair("temp1", vtkSmartPointer<vtkDoubleArray>::New()));
  dataVectors.insert(std::make_pair("temp2", vtkSmartPointer<vtkDoubleArray>::New()));
  dataVectors.insert(std::make_pair("temp3", vtkSmartPointer<vtkDoubleArray>::New()));
  dataVectors.insert(std::make_pair("accel1x", vtkSmartPointer<vtkDoubleArray>::New()));
  dataVectors.insert(std::make_pair("accel1y", vtkSmartPointer<vtkDoubleArray>::New()));
  dataVectors.insert(std::make_pair("accel2x", vtkSmartPointer<vtkDoubleArray>::New()));
  dataVectors.insert(std::make_pair("accel2y", vtkSmartPointer<vtkDoubleArray>::New()));
  dataVectors.insert(std::make_pair("accel3x", vtkSmartPointer<vtkDoubleArray>::New()));
  dataVectors.insert(std::make_pair("accel3y", vtkSmartPointer<vtkDoubleArray>::New()));
  dataVectors.insert(std::make_pair("heading", vtkSmartPointer<vtkDoubleArray>::New()));
  for (VecMap::iterator it = dataVectors.begin(); it != dataVectors.end(); ++it)
  {
    it->second->SetName(it->first.c_str());
  }

  points->Allocate(5000, 5000);
  cells->Allocate(5000, 5000);
  lats->Allocate(5000, 5000);
  lons->Allocate(5000, 5000);
  gpsTime->Allocate(5000, 5000);

  const unsigned char* data;
  unsigned int dataLength;
  double timeSinceStart;

  projPJ pj_utm = NULL;
  // PROJ *pj_latlong;
  // const char *latlonargs[4] = {"+proj=longlat", "+ellps=WGS84",
  //                              "+datum=WGS84",  "+no_defs" };

  // // Modern compilers and old proj4 api are at war here
  // pj_latlong = proj_init(4, const_cast<char**>(latlonargs));
  // assert(pj_latlong);

  this->Open();
  vtkIdType pointcount = 0;

  while (this->Internal->Reader->NextPacket(data, dataLength, timeSinceStart))
  {
    PositionPacket position;
    if (!this->Internal->ProcessHDLPacket(data, dataLength, position))
    {
      continue;
    }

    double x, y, z, lat, lon, heading, gpsUpdateTime;
    if (std::string(position.sentance).size() == 0)
    {
      // If there is no sentence to parse (no gps connected),
      // we use the following:
      x = 0.0;
      y = 0.0;
      z = 0.0;
      lat = 0.0;
      lon = 0.0;
      heading = 0.0;
      // the follwing value is wrong (no gps update so no update time),
      // so it should also be 0.0 (invalid)
      // but this value is kept to not risk breaking anything:
      gpsUpdateTime = position.tohTimestamp;
    }
    else
    {
      NMEAParser parser;
      NMEALocation parsedNMEA;
      std::string sentence = std::string(position.sentance);
      boost::trim_right(sentence);
      if (!parser.ParseLocation(sentence, parsedNMEA))
      {
        vtkGenericWarningMacro("Failed to parse NMEA sentence: "
                               << "<" << sentence << ">");
      }

      if (this->Internal->UTMZone < 0)
      {
        assert(!pj_utm);
        this->Internal->UTMZone = LatLongToZone(parsedNMEA.Lat, parsedNMEA.Long);
        std::stringstream utmparams;
        utmparams << "+proj=utm ";
        std::stringstream zone;
        zone << "+zone=" << this->Internal->UTMZone;
        this->Internal->UTMString = zone.str();
        // WARNING: Dont let the string stream pass out of scope until
        // we finish initialization
        utmparams << this->Internal->UTMString << " ";
        if (parsedNMEA.Lat < 0)
        {
          utmparams << "+south ";
        }

        utmparams << "+ellps=WGS84 ";
        utmparams << "+units=m ";
        utmparams << "+no_defs ";
        pj_utm = pj_init_plus(utmparams.str().c_str());
      }

      assert(pj_utm);
{'fraction_non_alphanumeric': 0.18968107233649179, 'fraction_numerical': 0.02308181187889993, 'mean_word_length': 2.8782209276271566, 'pattern_counts': {'":': 0, '<': 231, '<?xml version=': 0, '>': 353, 'https://': 0, 'lorem ipsum': 0, 'www.': 4, 'xml': 2}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 6, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3435446', 'n_tokens_mistral': 11738, 'n_tokens_neox': 10927, 'n_words': 2504}