/***************************************************************************
 *   Copyright (c) 2011 Jürgen Riegel <juergen.riegel@web.de>              *
 *                                                                         *
 *   This file is part of the FreeCAD CAx development system.              *
 *                                                                         *
 *   This library is free software; you can redistribute it and/or         *
 *   modify it under the terms of the GNU Library General Public           *
 *   License as published by the Free Software Foundation; either          *
 *   version 2 of the License, or (at your option) any later version.      *
 *                                                                         *
 *   This library  is distributed in the hope that it will be useful,      *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU Library General Public License for more details.                  *
 *                                                                         *
 *   You should have received a copy of the GNU Library General Public     *
 *   License along with this library; see the file COPYING.LIB. If not,    *
 *   write to the Free Software Foundation, Inc., 59 Temple Place,         *
 *   Suite 330, Boston, MA  02111-1307, USA                                *
 *                                                                         *
 ***************************************************************************/

#include "PreCompiled.h"
#ifndef _PreComp_
# ifdef FC_OS_LINUX
#  include <unistd.h>
# endif
# include <memory>
# include <sstream>

# include <boost/lexical_cast.hpp>
# include <boost/regex.hpp>
# include <boost/algorithm/string.hpp>
# include <boost/math/special_functions/fpclassify.hpp> // needed for compilation on some systems
#endif

#include <Base/Console.h>
#include <Base/Converter.h>
#include <Base/Exception.h>
#include <Base/FileInfo.h>
#include <Base/Sequencer.h>
#include <Base/Stream.h>

#include "PointsAlgos.h"
#include <E57Format.h>


using namespace Points;

void PointsAlgos::Load(PointKernel &points, const char *FileName)
{
    Base::FileInfo File(FileName);

    // checking on the file
    if (!File.isReadable())
        throw Base::FileException("File to load not existing or not readable", FileName);

    if (File.hasExtension("asc"))
        LoadAscii(points,FileName);
    else
        throw Base::RuntimeError("Unknown ending");
}

void PointsAlgos::LoadAscii(PointKernel &points, const char *FileName)
{
    boost::regex rx("^\\s*([-+]?[0-9]*)\\.?([0-9]+([eE][-+]?[0-9]+)?)"
                     "\\s+([-+]?[0-9]*)\\.?([0-9]+([eE][-+]?[0-9]+)?)"
                     "\\s+([-+]?[0-9]*)\\.?([0-9]+([eE][-+]?[0-9]+)?)\\s*$");
    //boost::regex rx("(\\b[0-9]+\\.([0-9]+\\b)?|\\.[0-9]+\\b)");
    //boost::regex rx("^\\s*(-?[0-9]*)\\.([0-9]+)\\s+(-?[0-9]*)\\.([0-9]+)\\s+(-?[0-9]*)\\.([0-9]+)\\s*$");
    boost::cmatch what;

    Base::Vector3d pt;
    int LineCnt=0;
    std::string line;
    Base::FileInfo fi(FileName);

    Base::ifstream tmp_str(fi, std::ios::in);

    // estimating size
    while (std::getline(tmp_str,line))
        LineCnt++;

    // resize the PointKernel
    points.resize(LineCnt);

    Base::SequencerLauncher seq( "Loading points...", LineCnt );

    // again to the beginning
    Base::ifstream file(fi, std::ios::in);
    LineCnt = 0;

    try {
        // read file
        while (std::getline(file, line)) {
            if (boost::regex_match(line.c_str(), what, rx)) {
                pt.x = std::atof(what[1].first);
                pt.y = std::atof(what[4].first);
                pt.z = std::atof(what[7].first);

                points.setPoint(LineCnt,pt);
                seq.next();
                LineCnt++;
            }
        }
    }
    catch (...) {
        points.clear();
        throw Base::BadFormatError("Reading in points failed.");
    }

    // now remove the last points from the kernel
    // Note: first we allocate memory corresponding to the number of lines (points and comments)
    //       and read in the file twice. But then the size of the kernel is too high
    if (LineCnt < (int)points.size())
        points.erase(LineCnt, points.size());
}

// ----------------------------------------------------------------------------

Reader::Reader()
{
    width = 0;
    height = 0;
}

Reader::~Reader()
{
}

void Reader::clear()
{
    intensity.clear();
    colors.clear();
    normals.clear();
}

const PointKernel& Reader::getPoints() const
{
    return points;
}

bool Reader::hasProperties() const
{
    return (hasIntensities() || hasColors() || hasNormals());
}

const std::vector<float>& Reader::getIntensities() const
{
    return intensity;
}

bool Reader::hasIntensities() const
{
    return (!intensity.empty());
}

const std::vector<App::Color>& Reader::getColors() const
{
    return colors;
}

bool Reader::hasColors() const
{
    return (!colors.empty());
}

const std::vector<Base::Vector3f>& Reader::getNormals() const
{
    return normals;
}

bool Reader::hasNormals() const
{
    return (!normals.empty());
}

bool Reader::isStructured() const
{
    return (width > 1 && height > 1);
}

int Reader::getWidth() const
{
    return width;
}

int Reader::getHeight() const
{
    return height;
}

// ----------------------------------------------------------------------------

AscReader::AscReader()
{
}

AscReader::~AscReader()
{
}

void AscReader::read(const std::string& filename)
{
    points.load(filename.c_str());
}

// ----------------------------------------------------------------------------

namespace Points {
class Converter {
public:
    Converter() = default;
    virtual ~Converter() = default;
    virtual std::string toString(double) const = 0;
    virtual double toDouble(Base::InputStream&) const = 0;
    virtual int getSizeOf() const = 0;

private:
    Converter(const Converter&) = delete;
    Converter(Converter&&) = delete;
    Converter& operator= (const Converter&) = delete;
    Converter& operator= (Converter&&) = delete;
};
template <typename T>
class ConverterT : public Converter {
public:
    std::string toString(double f) const override {
        T c = static_cast<T>(f);
        std::ostringstream oss;
        oss.precision(7);
        oss.setf(std::ostringstream::showpoint);
        oss << c;
        return oss.str();
    }
    double toDouble(Base::InputStream& str) const override {
        T c;
        str >> c;
        return static_cast<double>(c);
    }
    int getSizeOf() const override {
        return sizeof(T);
    }
};

using ConverterPtr = std::shared_ptr<Converter>;

class DataStreambuf : public std::streambuf
{
public:
    explicit DataStreambuf(const std::vector<char>& data) : _buffer(data) {
        _beg = 0;
        _end = data.size();
        _cur = 0;
    }
    ~DataStreambuf() override {
    }

protected:
    int_type uflow() override {
        if (_cur == _end)
            return traits_type::eof();

        return static_cast<DataStreambuf::int_type>(_buffer[_cur++]) & 0x000000ff;
    }
    int_type underflow() override {
        if (_cur == _end)
            return traits_type::eof();

        return static_cast<DataStreambuf::int_type>(_buffer[_cur]) & 0x000000ff;
    }
    int_type pbackfail(int_type ch) override {
        if (_cur == _beg || (ch != traits_type::eof() && ch != _buffer[_cur-1]))
            return traits_type::eof();

        return static_cast<DataStreambuf::int_type>(_buffer[--_cur]) & 0x000000ff;
    }
    std::streamsize showmanyc() override {
        return _end - _cur;
    }
    pos_type seekoff(std::streambuf::off_type off,
        std::ios_base::seekdir way,
        std::ios_base::openmode =
            std::ios::in | std::ios::out) override {
        int p_pos=-1;
        if (way == std::ios_base::beg)
            p_pos = _beg;
        else if (way == std::ios_base::end)
            p_pos = _end;
        else if (way == std::ios_base::cur)
            p_pos = _cur;

        if (p_pos > _end)
            return traits_type::eof();

        if (((p_pos + off) > _end) || ((p_pos + off) < _beg))
            return traits_type::eof();

        _cur = p_pos+ off;

        return ((p_pos+off) - _beg);
    }
    pos_type seekpos(std::streambuf::pos_type pos,
        std::ios_base::openmode which =
        std::ios::in | std::ios::out) override {
        (void)which;
        return seekoff(pos, std::ios_base::beg);
    }

private:
    DataStreambuf(const DataStreambuf&) = delete;
    DataStreambuf(DataStreambuf&&) = delete;
    DataStreambuf& operator=(const DataStreambuf&) = delete;
    DataStreambuf& operator=(DataStreambuf&&) = delete;

private:
    const std::vector<char>& _buffer;
    int _beg, _end, _cur;
};

//Taken from https://github.com/PointCloudLibrary/pcl/blob/master/io/src/lzf.cpp
unsigned int
lzfDecompress (const void *const in_data,  unsigned int in_len,
                    void             *out_data, unsigned int out_len)
{
  unsigned char const *ip = static_cast<const unsigned char *> (in_data);
  unsigned char       *op = static_cast<unsigned char *> (out_data);
  unsigned char const *const in_end  = ip + in_len;
  unsigned char       *const out_end = op + out_len;

  do
  {
    unsigned int ctrl = *ip++;

    // Literal run
    if (ctrl < (1 << 5))
    {
      ctrl++;

      if (op + ctrl > out_end)
      {
        errno = E2BIG;
        return (0);
      }

      // Check for overflow
      if (ip + ctrl > in_end)
      {
        errno = EINVAL;
        return (0);
      }
      switch (ctrl)
      {
      case 32: *op++ = *ip++;
          /* FALLTHRU */
      case 31: *op++ = *ip++;
          /* FALLTHRU */
      case 30: *op++ = *ip++;
          /* FALLTHRU */
      case 29: *op++ = *ip++;
          /* FALLTHRU */
      case 28: *op++ = *ip++;
          /* FALLTHRU */
      case 27: *op++ = *ip++;
          /* FALLTHRU */
      case 26: *op++ = *ip++;
          /* FALLTHRU */
      case 25: *op++ = *ip++;
          /* FALLTHRU */
      case 24: *op++ = *ip++;
          /* FALLTHRU */
      case 23: *op++ = *ip++;
          /* FALLTHRU */
      case 22: *op++ = *ip++;
          /* FALLTHRU */
      case 21: *op++ = *ip++;
          /* FALLTHRU */
      case 20: *op++ = *ip++;
          /* FALLTHRU */
      case 19: *op++ = *ip++;
          /* FALLTHRU */
      case 18: *op++ = *ip++;
          /* FALLTHRU */
      case 17: *op++ = *ip++;
          /* FALLTHRU */
      case 16: *op++ = *ip++;
          /* FALLTHRU */
      case 15: *op++ = *ip++;
          /* FALLTHRU */
      case 14: *op++ = *ip++;
          /* FALLTHRU */
      case 13: *op++ = *ip++;
          /* FALLTHRU */
      case 12: *op++ = *ip++;
          /* FALLTHRU */
      case 11: *op++ = *ip++;
          /* FALLTHRU */
      case 10: *op++ = *ip++;
          /* FALLTHRU */
      case  9: *op++ = *ip++;
          /* FALLTHRU */
      case  8: *op++ = *ip++;
          /* FALLTHRU */
      case  7: *op++ = *ip++;
          /* FALLTHRU */
      case  6: *op++ = *ip++;
          /* FALLTHRU */
      case  5: *op++ = *ip++;
          /* FALLTHRU */
      case  4: *op++ = *ip++;
          /* FALLTHRU */
      case  3: *op++ = *ip++;
          /* FALLTHRU */
      case  2: *op++ = *ip++;
          /* FALLTHRU */
      case  1: *op++ = *ip++;
      }
    }
    // Back reference
    else
    {
      unsigned int len = ctrl >> 5;

      unsigned char *ref = op - ((ctrl & 0x1f) << 8) - 1;

      // Check for overflow
      if (ip >= in_end)
      {
        errno = EINVAL;
        return (0);
      }
      if (len == 7)
      {
        len += *ip++;
        // Check for overflow
        if (ip >= in_end)
        {
          errno = EINVAL;
          return (0);
        }
      }
      ref -= *ip++;

      if (op + len + 2 > out_end)
      {
        errno = E2BIG;
        return (0);
      }

      if (ref < static_cast<unsigned char *> (out_data))
      {
        errno = EINVAL;
        return (0);
      }

      switch (len)
      {
        default:
        {
          len += 2;

          if (op >= ref + len)
          {
            // Disjunct areas
            memcpy (op, ref, len);
            op += len;
          }
          else
          {
            // Overlapping, use byte by byte copying
            do
              *op++ = *ref++;
            while (--len);
          }

          break;
        }
        case 9: *op++ = *ref++;
          /* FALLTHRU */
        case 8: *op++ = *ref++;
          /* FALLTHRU */
        case 7: *op++ = *ref++;
          /* FALLTHRU */
        case 6: *op++ = *ref++;
          /* FALLTHRU */
        case 5: *op++ = *ref++;
          /* FALLTHRU */
        case 4: *op++ = *ref++;
          /* FALLTHRU */
        case 3: *op++ = *ref++;
          /* FALLTHRU */
        case 2: *op++ = *ref++;
          /* FALLTHRU */
        case 1: *op++ = *ref++;
          /* FALLTHRU */
        case 0: *op++ = *ref++; // two octets more
                *op++ = *ref++;
      }
    }
  }
  while (ip < in_end);

  return (static_cast<unsigned int> (op - static_cast<unsigned char*> (out_data)));
}
}

PlyReader::PlyReader()
{
}

PlyReader::~PlyReader()
{
}

void PlyReader::read(const std::string& filename)
{
    clear();
    this->width = 1;
    this->height = 0;

    Base::FileInfo fi(filename);
    Base::ifstream inp(fi, std::ios::in | std::ios::binary);

    std::string format;
    std::vector<std::string> fields;
    std::vector<std::string> types;
    std::vector<int> sizes;
    std::size_t offset = 0;
    std::size_t numPoints = readHeader(inp, format, offset, fields, types, sizes);

    Eigen::MatrixXd data(numPoints, fields.size());
    if (format == "ascii") {
        readAscii(inp, offset, data);
    }
    else if (format == "binary_little_endian") {
        readBinary(false, inp, offset, types, sizes, data);
    }
    else if (format == "binary_big_endian") {
        readBinary(true, inp, offset, types, sizes, data);
    }

    std::vector<std::string>::iterator it;
    std::size_t max_size = std::numeric_limits<std::size_t>::max();

    // x field
    std::size_t x = max_size;
    it = std::find(fields.begin(), fields.end(), "x");
    if (it != fields.end())
        x = std::distance(fields.begin(), it);

    // y field
    std::size_t y = max_size;
    it = std::find(fields.begin(), fields.end(), "y");
    if (it != fields.end())
        y = std::distance(fields.begin(), it);

    // z field
    std::size_t z = max_size;
    it = std::find(fields.begin(), fields.end(), "z");
    if (it != fields.end())
        z = std::distance(fields.begin(), it);

    // normal x field
    std::size_t normal_x = max_size;
    it = std::find(fields.begin(), fields.end(), "normal_x");
    if (it == fields.end())
        it = std::find(fields.begin(), fields.end(), "nx");
    if (it != fields.end())
        normal_x = std::distance(fields.begin(), it);

    // normal y field
    std::size_t normal_y = max_size;
    it = std::find(fields.begin(), fields.end(), "normal_y");
    if (it == fields.end())
        it = std::find(fields.begin(), fields.end(), "ny");
    if (it != fields.end())
        normal_y = std::distance(fields.begin(), it);

    // normal z field
    std::size_t normal_z = max_size;
    it = std::find(fields.begin(), fields.end(), "normal_z");
    if (it == fields.end())
        it = std::find(fields.begin(), fields.end(), "nz");
    if (it != fields.end())
        normal_z = std::distance(fields.begin(), it);

    // intensity field
    std::size_t greyvalue = max_size;
    it = std::find(fields.begin(), fields.end(), "intensity");
    if (it != fields.end())
        greyvalue = std::distance(fields.begin(), it);

    // rgb(a) field
    std::size_t red = max_size, green = max_size, blue = max_size, alpha = max_size;
    it = std::find(fields.begin(), fields.end(), "red");
    if (it != fields.end())
        red = std::distance(fields.begin(), it);

    it = std::find(fields.begin(), fields.end(), "green");
    if (it != fields.end())
        green = std::distance(fields.begin(), it);

    it = std::find(fields.begin(), fields.end(), "blue");
    if (it != fields.end())
        blue = std::distance(fields.begin(), it);

    it = std::find(fields.begin(), fields.end(), "alpha");
    if (it != fields.end())
        alpha = std::distance(fields.begin(), it);

    // transfer the data
    bool hasData = (x != max_size && y != max_size && z != max_size);
    bool hasNormal = (normal_x != max_size && normal_y != max_size && normal_z != max_size);
    bool hasIntensity = (greyvalue != max_size);
    bool hasColor = (red != max_size && green != max_size && blue != max_size);

    if (hasData) {
        points.reserve(numPoints);
        for (std::size_t i=0; i<numPoints; i++) {
            points.push_back(Base::Vector3d(data(i,x),data(i,y),data(i,z)));
        }
    }

    if (hasData && hasNormal) {
        normals.reserve(numPoints);
        for (std::size_t i=0; i<numPoints; i++) {
            normals.emplace_back(data(i,normal_x),data(i,normal_y),data(i,normal_z));
        }
    }

    if (hasData && hasIntensity) {
        intensity.reserve(numPoints);
        for (std::size_t i=0; i<numPoints; i++) {
            intensity.push_back(data(i,greyvalue));
        }
    }

    if (hasData && hasColor) {
        colors.reserve(numPoints);
        float a = 1.0;
        if (types[red] == "uchar") {
            for (std::size_t i=0; i<numPoints; i++) {
                float r = data(i, red);
                float g = data(i, green);
                float b = data(i, blue);
                if (alpha != max_size)
                    a = data(i, alpha);
                colors.emplace_back(static_cast<float>(r)/255.0f,
                                    static_cast<float>(g)/255.0f,
                                    static_cast<float>(b)/255.0f,
                                    static_cast<float>(a)/255.0f);
            }
        }
        else if (types[red] == "float") {
            for (std::size_t i=0; i<numPoints; i++) {
                float r = data(i, red);
                float g = data(i, green);
                float b = data(i, blue);
                if (alpha != max_size)
                    a = data(i, alpha);
                colors.emplace_back(r, g, b, a);
            }
        }
    }
}

std::size_t PlyReader::readHeader(std::istream& in,
                                  std::string& format,
                                  std::size_t& offset,
                                  std::vector<std::string>& fields,
                                  std::vector<std::string>& types,
                                  std::vector<int>& sizes)
{
    std::string line, element;
    std::vector<std::string> list;
    std::size_t numPoints = 0;
    // a pair of numbers of elements and the total size of the properties
    std::vector<std::pair<std::size_t, std::size_t> > count_props;

    // read in the first three characters
    char ply[3];
    in.read(ply, 3);
    in.ignore(1);
    if (!in || (ply[0] != 'p') || (ply[1] != 'l') || (ply[2] != 'y'))
        throw Base::BadFormatError("Not a ply file"); // wrong header

    while (std::getline(in, line)) {
        if (line.empty())
            continue;

        // since the file is loaded in binary mode we may get the CR at the end
        boost::trim(line);
        boost::split(list, line, boost::is_any_of ("\t\r "), boost::token_compress_on);

        std::istringstream str(line);
        str.imbue(std::locale::classic());

        std::string kw;
        str >> kw;
        if (kw == "format") {
            if (list.size() != 3) {
                throw Base::BadFormatError("Not a valid ply file");
            }

            std::string format_string = list[1];
            std::string version = list[2];

            if (format_string == "ascii") {
                format = format_string;
            }
            else if (format_string == "binary_big_endian") {
                format = format_string;
            }
            else if (format_string == "binary_little_endian") {
                format = format_string;
            }
            else {
                // wrong format version
                throw Base::BadFormatError("Wrong format version");
            }
            if (version != "1.0") {
                // wrong version
                throw Base::BadFormatError("Wrong version number");
            }
        }
        else if (kw == "element") {
            if (list.size() != 3) {
                throw Base::BadFormatError("Not a valid ply file");
            }

            std::string name = list[1];
            std::size_t count = boost::lexical_cast<std::size_t>(list[2]);
            if (name == "vertex") {
                element = name;
                numPoints = count;
            }
            else {
                // if another element than 'vertex' comes first then calculate the offset
                if (numPoints == 0) {
                    count_props.emplace_back(count, 0);
                }
                else {
                    // this happens for elements coming after 'vertex'
                    element.clear();
                }
            }
        }
        else if (kw == "property") {
            if (list.size() < 3) {
                throw Base::BadFormatError("Not a valid ply file");
            }

            std::string name = list.back();
            std::list<std::string> number;
            if (list[1] == "list") {
                number.insert(number.end(), list.begin(), list.end());
                number.pop_front(); // property
                number.pop_front(); // list
                number.pop_back();
            }
            else {
                number.push_back(list[1]);
            }

            for (std::list<std::string>::iterator it = number.begin(); it != number.end(); ++it) {
                int size = 0;
                if (*it == "char" || *it == "int8") {
                    size = 1;
                }
                else if (*it == "uchar" || *it == "uint8") {
                    size = 1;
                }
                else if (*it == "short" || *it == "int16") {
                    size = 2;
                }
                else if (*it == "ushort" || *it == "uint16") {
                    size = 2;
                }
                else if (*it == "int" || *it == "int32") {
                    size = 4;
                }
                else if (*it == "uint" || *it == "uint32") {
                    size = 4;
                }
                else if (*it == "float" || *it == "float32") {
                    size = 4;
                }
                else if (*it == "double" || *it == "float64") {
                    size = 8;
                }
                else {
                    // no valid number type
                    throw Base::BadFormatError("Not a valid number type");
                }

                if (element == "vertex") {
                    // store the property name and type
                    fields.push_back(name);
                    types.push_back(*it);
                    sizes.push_back(size);
                }
                else if (!count_props.empty()) {
                    count_props.back().second += size;
                }
            }
        }
        else if (kw == "end_header") {
            break;
        }
    }

    if (fields.size() != sizes.size() ||
        fields.size() != types.size()) {
        throw Base::BadFormatError("");
    }

    offset = 0;
    if (format == "ascii") {
        // just sum up the number of lines to ignore
        std::vector<std::pair<std::size_t, std::size_t> >::iterator it;
        for (it = count_props.begin(); it != count_props.end(); ++it) {
            offset += it->first;
        }
    }
    else {
        std::vector<std::pair<std::size_t, std::size_t> >::iterator it;
        for (it = count_props.begin(); it != count_props.end(); ++it) {
            offset += it->first * it->second;
        }
    }

    return numPoints;
}

void PlyReader::readAscii(std::istream& inp, std::size_t offset, Eigen::MatrixXd& data)
{
    std::string line;
    std::size_t row = 0;
    std::size_t numPoints = data.rows();
    std::size_t numFields = data.cols();
    std::vector<std::string> list;
    while (std::getline(inp, line) && row < numPoints) {
        if (line.empty())
            continue;

        if (offset > 0) {
            offset--;
            continue;
        }

        // since the file is loaded in binary mode we may get the CR at the end
        boost::trim(line);
        boost::split(list, line, boost::is_any_of ("\t\r "), boost::token_compress_on);

        std::istringstream str(line);

        for (std::size_t col = 0; col < list.size() && col < numFields; col++) {
            double value = boost::lexical_cast<double>(list[col]);
            data(row, col) = value;
        }

        ++row;
    }
}

void PlyReader::readBinary(bool swapByteOrder,
                           std::istream& inp,
                           std::size_t offset,
                           const std::vector<std::string>& types,
                           const std::vector<int>& sizes,
                           Eigen::MatrixXd& data)
{
    std::size_t numPoints = data.rows();
    std::size_t numFields = data.cols();

    int neededSize = 0;
    ConverterPtr convert_float32(new ConverterT<float>);
    ConverterPtr convert_float64(new ConverterT<double>);
    ConverterPtr convert_int8(new ConverterT<int8_t>);
    ConverterPtr convert_uint8(new ConverterT<uint8_t>);
    ConverterPtr convert_int16(new ConverterT<int16_t>);
    ConverterPtr convert_uint16(new ConverterT<uint16_t>);
    ConverterPtr convert_int32(new ConverterT<int32_t>);
    ConverterPtr convert_uint32(new ConverterT<uint32_t>);

    std::vector<ConverterPtr> converters;
    for (std::size_t j=0; j<numFields; j++) {
        std::string t = types[j];
        switch (sizes[j]) {
        case 1:
            if (t == "char" || t == "int8")
                converters.push_back(convert_int8);
            else if (t == "uchar" || t == "uint8")
                converters.push_back(convert_uint8);
            else
                throw Base::BadFormatError("Unexpected type");
            break;
        case 2:
            if (t == "short" || t == "int16")
                converters.push_back(convert_int16);
            else if (t == "ushort" || t == "uint16")
                converters.push_back(convert_uint16);
            else
                throw Base::BadFormatError("Unexpected type");
            break;
        case 4:
            if (t == "int" || t == "int32")
                converters.push_back(convert_int32);
            else if (t == "uint" || t == "uint32")
                converters.push_back(convert_uint32);
            else if (t == "float" || t == "float32")
                converters.push_back(convert_float32);
            else
                throw Base::BadFormatError("Unexpected type");
            break;
        case 8:
            if (t == "double" || t == "float64")
                converters.push_back(convert_float64);
            else
                throw Base::BadFormatError("Unexpected type");
            break;
        default:
            throw Base::BadFormatError("Unexpected type");
        }

        neededSize += converters.back()->getSizeOf();
    }

    std::streamoff ulSize = 0;
    std::streamoff ulCurr = 0;
    std::streambuf* buf = inp.rdbuf();
    if (buf) {
        ulCurr = buf->pubseekoff(static_cast<std::streamoff>(offset), std::ios::cur, std::ios::in);
        ulSize = buf->pubseekoff(0, std::ios::end, std::ios::in);
        buf->pubseekoff(ulCurr, std::ios::beg, std::ios::in);
        if (ulCurr + neededSize*static_cast<std::streamoff>(numPoints) > ulSize)
            throw Base::BadFormatError("File expects too many elements");
    }

    Base::InputStream str(inp);
    str.setByteOrder(swapByteOrder ? Base::Stream::BigEndian : Base::Stream::LittleEndian);
    for (std::size_t i=0; i<numPoints; i++) {
        for (std::size_t j=0; j<numFields; j++) {
            double value = converters[j]->toDouble(str);
            data(i, j) = value;
        }
    }
}

// ----------------------------------------------------------------------------

PcdReader::PcdReader()
{
}

PcdReader::~PcdReader()
{
}

void PcdReader::read(const std::string& filename)
{
    clear();
    this->width = -1;
    this->height = -1;

    Base::FileInfo fi(filename);
    Base::ifstream inp(fi, std::ios::in | std::ios::binary);

    std::string format;
    std::vector<std::string> fields;
    std::vector<std::string> types;
    std::vector<int> sizes;
    std::size_t numPoints = readHeader(inp, format, fields, types, sizes);

    Eigen::MatrixXd data(numPoints, fields.size());
    if (format == "ascii") {
        readAscii(inp, data);
    }
    else if (format == "binary") {
        readBinary(false, inp, types, sizes, data);
    }
    else if (format == "binary_compressed") {
        unsigned int c, u;
        Base::InputStream str(inp);
        str >> c >> u;

        std::vector<char> compressed(c);
        inp.read(&compressed[0], c);
        std::vector<char> uncompressed(u);
        if (lzfDecompress(&compressed[0], c, &uncompressed[0], u) == u) {
            DataStreambuf ibuf(uncompressed);
            std::istream istr(nullptr);
            istr.rdbuf(&ibuf);
            readBinary(true, istr, types, sizes, data);
        }
        else {
            throw Base::BadFormatError("Failed to decompress binary data");
        }
    }

    std::vector<std::string>::iterator it;
    std::size_t max_size = std::numeric_limits<std::size_t>::max();

    // x field
    std::size_t x = max_size;
    it = std::find(fields.begin(), fields.end(), "x");
    if (it != fields.end())
        x = std::distance(fields.begin(), it);

    // y field
    std::size_t y = max_size;
    it = std::find(fields.begin(), fields.end(), "y");
    if (it != fields.end())
        y = std::distance(fields.begin(), it);

    // z field
    std::size_t z = max_size;
    it = std::find(fields.begin(), fields.end(), "z");
    if (it != fields.end())
        z = std::distance(fields.begin(), it);

    // normal x field
    std::size_t normal_x = max_size;
    it = std::find(fields.begin(), fields.end(), "normal_x");
    if (it == fields.end())
        it = std::find(fields.begin(), fields.end(), "nx");
    if (it != fields.end())
        normal_x = std::distance(fields.begin(), it);

    // normal y field
    std::size_t normal_y = max_size;
    it = std::find(fields.begin(), fields.end(), "normal_y");
    if (it == fields.end())
        it = std::find(fields.begin(), fields.end(), "ny");
    if (it != fields.end())
        normal_y = std::distance(fields.begin(), it);

    // normal z field
    std::size_t normal_z = max_size;
    it = std::find(fields.begin(), fields.end(), "normal_z");
    if (it == fields.end())
        it = std::find(fields.begin(), fields.end(), "nz");
    if (it != fields.end())
        normal_z = std::distance(fields.begin(), it);

    // intensity field
    std::size_t greyvalue = max_size;
    it = std::find(fields.begin(), fields.end(), "intensity");
    if (it != fields.end())
        greyvalue = std::distance(fields.begin(), it);

    // rgb(a) field
    std::size_t rgba = max_size;
    it = std::find(fields.begin(), fields.end(), "rgb");
    if (it == fields.end())
        it = std::find(fields.begin(), fields.end(), "rgba");
    if (it != fields.end())
        rgba = std::distance(fields.begin(), it);

    // transfer the data
    bool hasData = (x != max_size && y != max_size && z != max_size);
    bool hasNormal = (normal_x != max_size && normal_y != max_size && normal_z != max_size);
    bool hasIntensity = (greyvalue != max_size);
    bool hasColor = (rgba != max_size);

    if (hasData) {
        points.reserve(numPoints);
        for (std::size_t i=0; i<numPoints; i++) {
            points.push_back(Base::Vector3d(data(i,x),data(i,y),data(i,z)));
        }
    }

    if (hasData && hasNormal) {
        normals.reserve(numPoints);
        for (std::size_t i=0; i<numPoints; i++) {
            normals.emplace_back(data(i,normal_x),data(i,normal_y),data(i,normal_z));
        }
    }

    if (hasData && hasIntensity) {
        intensity.reserve(numPoints);
        for (std::size_t i=0; i<numPoints; i++) {
            intensity.push_back(data(i,greyvalue));
        }
    }

    if (hasData && hasColor) {
        colors.reserve(numPoints);
        if (types[rgba] == "U") {
            for (std::size_t i=0; i<numPoints; i++) {
                uint32_t packed = static_cast<uint32_t>(data(i,rgba));
                uint32_t a = (packed >> 24) & 0xff;
                uint32_t r = (packed >> 16) & 0xff;
                uint32_t g = (packed >> 8) & 0xff;
                uint32_t b = packed & 0xff;
                colors.emplace_back(static_cast<float>(r)/255.0f,
                                    static_cast<float>(g)/255.0f,
                                    static_cast<float>(b)/255.0f,
                                    static_cast<float>(a)/255.0f);
            }
        }
        else if (types[rgba] == "F") {
            static_assert(sizeof(float) == sizeof(uint32_t), "float and uint32_t have different sizes");
            for (std::size_t i=0; i<numPoints; i++) {
                float f = static_cast<float>(data(i,rgba));
                uint32_t packed;
                std::memcpy(&packed, &f, sizeof(packed));
                uint32_t a = (packed >> 24) & 0xff;
                uint32_t r = (packed >> 16) & 0xff;
                uint32_t g = (packed >> 8) & 0xff;
                uint32_t b = packed & 0xff;
                colors.emplace_back(static_cast<float>(r)/255.0f,
                                    static_cast<float>(g)/255.0f,
                                    static_cast<float>(b)/255.0f,
                                    static_cast<float>(a)/255.0f);
            }
        }
    }
}

std::size_t PcdReader::readHeader(std::istream& in,
                                  std::string& format,
                                  std::vector<std::string>& fields,
                                  std::vector<std::string>& types,
                                  std::vector<int>& sizes)
{
    std::string line;
    std::vector<std::string> counts;
    std::vector<std::string> list;
    std::size_t points = 0;

    while (std::getline(in, line)) {
        if (line.empty())
            continue;

        // since the file is loaded in binary mode we may get the CR at the end
        boost::trim(line);
        boost::split(list, line, boost::is_any_of ("\t\r "), boost::token_compress_on);

        std::istringstream str(line);
        str.imbue(std::locale::classic());

        std::string kw;
        str >> kw;
        if (kw == "FIELDS") {
            for (std::size_t i=1; i<list.size(); i++) {
                fields.push_back(list[i]);
            }
        }
        else if (kw == "SIZE") {
            for (std::size_t i=1; i<list.size(); i++) {
                sizes.push_back(boost::lexical_cast<int>(list[i]));
            }
        }
        else if (kw == "TYPE") {
            for (std::size_t i=1; i<list.size(); i++) {
                types.push_back(list[i]);
            }
        }
        else if (kw == "COUNT") {
            for (std::size_t i=1; i<list.size(); i++) {
                counts.push_back(list[i]);
            }
        }
        else if (kw == "WIDTH") {
            str >> std::ws >> this->width;
        }
        else if (kw == "HEIGHT") {
            str >> std::ws >> this->height;
        }
        else if (kw == "POINTS") {
            str >> std::ws >> points;
        }
        else if (kw == "DATA") {
            str >> std::ws >> format;
            break;
        }
    }

    std::size_t w = static_cast<std::size_t>(this->width);
    std::size_t h = static_cast<std::size_t>(this->height);
    std::size_t size = w * h;
    if (fields.size() != sizes.size() ||
        fields.size() != types.size() ||
        fields.size() != counts.size() ||
        points != size) {
        throw Base::BadFormatError("");
    }

    return points;
}

void PcdReader::readAscii(std::istream& inp, Eigen::MatrixXd& data)
{
    std::string line;
    std::size_t row = 0;
    std::size_t numPoints = data.rows();
    std::size_t numFields = data.cols();
    std::vector<std::string> list;
    while (std::getline(inp, line) && row < numPoints) {
        if (line.empty())
            continue;

        // since the file is loaded in binary mode we may get the CR at the end
        boost::trim(line);
        boost::split(list, line, boost::is_any_of ("\t\r "), boost::token_compress_on);

        std::istringstream str(line);

        for (std::size_t col = 0; col < list.size() && col < numFields; col++) {
            double value = boost::lexical_cast<double>(list[col]);
            data(row, col) = value;
        }

        ++row;
    }
}

void PcdReader::readBinary(bool transpose,
                           std::istream& inp,
                           const std::vector<std::string>& types,
                           const std::vector<int>& sizes,
                           Eigen::MatrixXd& data)
{
    std::size_t numPoints = data.rows();
    std::size_t numFields = data.cols();

    int neededSize = 0;
    ConverterPtr convert_float32(new ConverterT<float>);
    ConverterPtr convert_float64(new ConverterT<double>);
    ConverterPtr convert_int8(new ConverterT<int8_t>);
    ConverterPtr convert_uint8(new ConverterT<uint8_t>);
    ConverterPtr convert_int16(new ConverterT<int16_t>);
    ConverterPtr convert_uint16(new ConverterT<uint16_t>);
    ConverterPtr convert_int32(new ConverterT<int32_t>);
    ConverterPtr convert_uint32(new ConverterT<uint32_t>);

    std::vector<ConverterPtr> converters;
    for (std::size_t j=0; j<numFields; j++) {
        char t = types[j][0];
        switch (sizes[j]) {
        case 1:
            if (t == 'I')
                converters.push_back(convert_int8);
            else if (t == 'U')
                converters.push_back(convert_uint8);
            else
                throw Base::BadFormatError("Unexpected type");
            break;
        case 2:
            if (t == 'I')
                converters.push_back(convert_int16);
            else if (t == 'U')
                converters.push_back(convert_uint16);
            else
                throw Base::BadFormatError("Unexpected type");
            break;
        case 4:
            if (t == 'I')
                converters.push_back(convert_int32);
            else if (t == 'U')
                converters.push_back(convert_uint32);
            else if (t == 'F')
                converters.push_back(convert_float32);
            else
                throw Base::BadFormatError("Unexpected type");
            break;
        case 8:
            if (t == 'F')
                converters.push_back(convert_float64);
            else
                throw Base::BadFormatError("Unexpected type");
            break;
        default:
            throw Base::BadFormatError("Unexpected type");
        }

        neededSize += converters.back()->getSizeOf();
    }

    std::streamoff ulSize = 0;
    std::streamoff ulCurr = 0;
    std::streambuf* buf = inp.rdbuf();
    if (buf) {
        ulCurr = buf->pubseekoff(0, std::ios::cur, std::ios::in);
        ulSize = buf->pubseekoff(0, std::ios::end, std::ios::in);
        buf->pubseekoff(ulCurr, std::ios::beg, std::ios::in);
        if (ulCurr + neededSize*static_cast<std::streamoff>(numPoints) > ulSize)
            throw Base::BadFormatError("File expects too many elements");
    }

    Base::InputStream str(inp);
    if (transpose) {
        for (std::size_t j=0; j<numFields; j++) {
            for (std::size_t i=0; i<numPoints; i++) {
                double value = converters[j]->toDouble(str);
                data(i, j) = value;
            }
        }
    }
    else {
        for (std::size_t i=0; i<numPoints; i++) {
            for (std::size_t j=0; j<numFields; j++) {
                double value = converters[j]->toDouble(str);
                data(i, j) = value;
            }
        }
    }
}

// ----------------------------------------------------------------------------

E57Reader::E57Reader(const bool& Color, const bool& State, const float& Distance)
{
    useColor = Color;
    checkState = State;
    minDistance = Distance;
}

E57Reader::~E57Reader()
{
}

void E57Reader::read(const std::string& filename)
{
    try {
        // read file
        e57::ImageFile imfi(filename, "r");
        e57::StructureNode root = imfi.root();
        if (root.isDefined("data3D")) {
            e57::VectorNode data3D(root.get("data3D"));
            for (int child = 0; child < data3D.childCount(); ++child) {
                e57::StructureNode            scan_data(data3D.get(child));
                e57::CompressedVectorNode     cvn(scan_data.get("points"));
                e57::StructureNode            prototype(cvn.prototype());
                // create buffers for the compressed vector reader
                const size_t buf_size = 1024;
                double xyz[buf_size * 3];
                double intensity[buf_size];
                int64_t state[buf_size];
                unsigned rgb[buf_size * 3];
                int64_t nil[buf_size];

                // check the channels which are needed
                unsigned ptr_xyz[3];
                unsigned ptr_rgb[3];
                bool inty = false;
                bool inv_state = false;
                unsigned cnt_xyz = 0;
                unsigned cnt_rgb = 0;
                std::vector<e57::SourceDestBuffer> sdb;
                for (int i = 0; i < prototype.childCount(); ++i) {
                    e57::Node n(prototype.get(i));
                    if ((n.type() == e57::E57_FLOAT) || (n.type() == e57::E57_SCALED_INTEGER)) {
                        if (n.elementName() == "cartesianX") {
                            ptr_xyz[0] = cnt_xyz++;
                            sdb.emplace_back(
                                    imfi
                                    , n.elementName()
                                    , &(xyz[0])
                                    , buf_size
                                    , true
                                    , true

                            );
                        }
                        else if (n.elementName() == "cartesianY") {
                            ptr_xyz[1] = cnt_xyz++;
                            sdb.emplace_back(
                                    imfi
                                    , n.elementName()
                                    , &(xyz[buf_size])
                                    , buf_size
                                    , true
                                    , true

                            );
                        }
                        else if (n.elementName() == "cartesianZ") {
                            ptr_xyz[2] = cnt_xyz++;
                            sdb.emplace_back(
                                    imfi
                                    , n.elementName()
                                    , &(xyz[2 * buf_size])
                                    , buf_size
                                    , true
                                    , true

                            );
                        }
                        else if (n.elementName() == "intensity") {
                            inty = true;
                            sdb.emplace_back(
                                    imfi
                                    , n.elementName()
                                    , &(intensity[0])
                                    , buf_size
                                    , true
                                    , true

                            );
                        }
                        else {
                            sdb.emplace_back(
                                    imfi
                                    , n.elementName()
                                    , &(nil[0])
                                    , buf_size
                                    , true
                                    , true

                            );
                        }

                    }
                    else if (n.type() == e57::E57_INTEGER) {
                        if (n.elementName() == "colorRed") {
                            ptr_rgb[0] = cnt_rgb++;
                            sdb.emplace_back(
                                    imfi
                                    , n.elementName()
                                    , &(rgb[0])
                                    , buf_size
                                    , true
                                    , true

                            );
                        }
                        else if (n.elementName() == "colorGreen") {
                            ptr_rgb[1] = cnt_rgb++;
                            sdb.emplace_back(
                                    imfi
                                    , n.elementName()
                                    , &(rgb[buf_size])
                                    , buf_size
                                    , true
                                    , true

                            );
                        }
                        else if (n.elementName() == "colorBlue") {
                            ptr_rgb[2] = cnt_rgb++;
                            sdb.emplace_back(
                                    imfi
                                    , n.elementName()
                                    , &(rgb[2 * buf_size])
                                    , buf_size
                                    , true
                                    , true

                            );
                        }
                        else if (n.elementName() == "cartesianInvalidState") {
                            inv_state = true;
                            sdb.emplace_back(
                                    imfi
                                    , n.elementName()
                                    , &(state[0])
                                    , buf_size
                                    , true
                                    , true

                            );
                        }
                        else {
                            sdb.emplace_back(
                                    imfi
                                    , n.elementName()
                                    , &(nil[0])
                                    , buf_size
                                    , true
                                    , true

                            );
                        }
                    }
                }

                // read the data

                if (cnt_xyz == 3) {
                    unsigned count;
                    unsigned cnt_pts = 0;
                    Base::Vector3d pt, last;
                    e57::CompressedVectorReader cvr(cvn.reader(sdb));
                    bool hasColor = (cnt_rgb == 3) && useColor;
                    bool hasState = inv_state && checkState;
                    bool filter = false;

                    while ((count = cvr.read())) {
                        for (size_t i = 0; i < count; ++i) {
                            filter = false;
                            if (hasState) {
                                if (state[i] != 0) { filter = true; }
                            }
                            pt.x = xyz[ptr_xyz[0] * buf_size + i];
                            pt.y = xyz[ptr_xyz[1] * buf_size + i];
                            pt.z = xyz[ptr_xyz[2] * buf_size + i];
                            if ((!filter) && (cnt_pts > 0)) {
                                if (Base::Distance(last, pt) < minDistance) {
                                    filter = true;
                                }
                            }
                            if (!filter) {
                                cnt_pts++;
                                points.push_back(pt);
                                last = pt;
                                if (hasColor) {
                                    App::Color c;
                                    c.r = static_cast<float>(rgb[ptr_rgb[0] * buf_size + i]) / 255.0f;
                                    c.g = static_cast<float>(rgb[ptr_rgb[1] * buf_size + i]) / 255.0f;
                                    c.b = static_cast<float>(rgb[ptr_rgb[2] * buf_size + i]) / 255.0f;
                                    if (inty) { c.a = intensity[i]; }
                                    colors.push_back(c);
                                }
                            }
                        }
                    }
                }
                else {
                    Base::Console().Error("Missing channels xyz.");
                }
            }
        }
    }
    catch (...) {
        points.clear();
        throw Base::BadFormatError("E57");
    }
}

// ----------------------------------------------------------------------------

Writer::Writer(const PointKernel& p) : points(p)
{
    width = p.size();
    height = 1;
}

Writer::~Writer()
{
}

void Writer::setIntensities(const std::vector<float>& i)
{
    intensity = i;
}

void Writer::setColors(const std::vector<App::Color>& c)
{
    colors = c;
}

void Writer::setNormals(const std::vector<Base::Vector3f>& n)
{
    normals = n;
}

void Writer::setWidth(int w)
{
    width = w;
}

void Writer::setHeight(int h)
{
    height = h;
}

void Writer::setPlacement(const Base::Placement& p)
{
    placement = p;
}

// ----------------------------------------------------------------------------

AscWriter::AscWriter(const PointKernel& p) : Writer(p)
{
}

AscWriter::~AscWriter()
{
}

void AscWriter::write(const std::string& filename)
{
    if (placement.isIdentity()) {
        points.save(filename.c_str());
    }
    else {
        PointKernel copy = points;
        copy.transformGeometry(placement.toMatrix());
        copy.save(filename.c_str());
    }
}

// ----------------------------------------------------------------------------

PlyWriter::PlyWriter(const PointKernel& p) : Writer(p)
{
}

PlyWriter::~PlyWriter()
{
}

void PlyWriter::write(const std::string& filename)
{
    std::list<std::string> properties;
    properties.emplace_back("float x");
    properties.emplace_back("float y");
    properties.emplace_back("float z");

    ConverterPtr convert_float(new ConverterT<float>);
    ConverterPtr convert_uint(new ConverterT<uint32_t>);

    std::vector<ConverterPtr> converters;
    converters.push_back(convert_float);
    converters.push_back(convert_float);
    converters.push_back(convert_float);

    bool hasIntensity = (intensity.size() == points.size());
    bool hasColors = (colors.size() == points.size());
    bool hasNormals = (normals.size() == points.size());

    if (hasNormals) {
        properties.emplace_back("float nx");
        properties.emplace_back("float ny");
        properties.emplace_back("float nz");
        converters.push_back(convert_float);
        converters.push_back(convert_float);
        converters.push_back(convert_float);
    }

    if (hasColors) {
        properties.emplace_back("uchar red");
        properties.emplace_back("uchar green");
        properties.emplace_back("uchar blue");
        properties.emplace_back("uchar alpha");
        converters.push_back(convert_uint);
        converters.push_back(convert_uint);
        converters.push_back(convert_uint);
        converters.push_back(convert_uint);
    }

    if (hasIntensity) {
        properties.emplace_back("float intensity");
        converters.push_back(convert_float);
    }

    std::size_t numPoints = points.size();
    std::size_t numValid = 0;
    const std::vector<Base::Vector3f>& pts = points.getBasicPoints();
    for (std::size_t i=0; i<numPoints; i++) {
        const Base::Vector3f& p = pts[i];
        if (!boost::math::isnan(p.x) &&
            !boost::math::isnan(p.y) &&
            !boost::math::isnan(p.z))
            numValid++;
    }

    Eigen::MatrixXf data(numPoints, properties.size());

    if (placement.isIdentity()) {
        for (std::size_t i=0; i<numPoints; i++) {
            data(i,0) = pts[i].x;
            data(i,1) = pts[i].y;
            data(i,2) = pts[i].z;
        }
    }
    else {
        Base::Vector3d tmp;
        for (std::size_t i=0; i<numPoints; i++) {
            tmp = Base::convertTo<Base::Vector3d>(pts[i]);
            placement.multVec(tmp, tmp);
            data(i,0) = static_cast<float>(tmp.x);
            data(i,1) = static_cast<float>(tmp.y);
            data(i,2) = static_cast<float>(tmp.z);
        }
    }

    std::size_t col = 3;
    if (hasNormals) {
        int col0 = col;
        int col1 = col+1;
        int col2 = col+2;
        Base::Rotation rot = placement.getRotation();
        if (rot.isIdentity()) {
            for (std::size_t i=0; i<numPoints; i++) {
                data(i,col0) = normals[i].x;
                data(i,col1) = normals[i].y;
                data(i,col2) = normals[i].z;
            }
        }
        else {
            Base::Vector3d tmp;
            for (std::size_t i=0; i<numPoints; i++) {
                tmp = Base::convertTo<Base::Vector3d>(normals[i]);
                rot.multVec(tmp, tmp);
                data(i,col0) = static_cast<float>(tmp.x);
                data(i,col1) = static_cast<float>(tmp.y);
                data(i,col2) = static_cast<float>(tmp.z);
            }
        }
        col += 3;
    }

    if (hasColors) {
        int col0 = col;
        int col1 = col+1;
        int col2 = col+2;
        int col3 = col+3;
        for (std::size_t i=0; i<numPoints; i++) {
            App::Color c = colors[i];
            data(i,col0) = (c.r*255.0f + 0.5f);
            data(i,col1) = (c.g*255.0f + 0.5f);
            data(i,col2) = (c.b*255.0f + 0.5f);
            data(i,col3) = (c.a*255.0f + 0.5f);
        }
        col += 4;
    }

    if (hasIntensity) {
        for (std::size_t i=0; i<numPoints; i++) {
            data(i,col) = intensity[i];
        }
        col += 1;
    }

    Base::ofstream out(filename, std::ios::out);
    out << "ply" << std::endl
        << "format ascii 1.0" << std::endl
        << "comment FreeCAD generated" << std::endl;
    out << "element vertex " << numValid << std::endl;

    // the properties
    for (std::list<std::string>::iterator it = properties.begin(); it != properties.end(); ++it)
        out << "property " << *it << std::endl;
    out << "end_header" << std::endl;

    for (std::size_t r=0; r<numPoints; r++) {
        if (boost::math::isnan(data(r,0)))
            continue;
        if (boost::math::isnan(data(r,1)))
            continue;
        if (boost::math::isnan(data(r,2)))
            continue;
        for (std::size_t c=0; c<col; c++) {
            float value = data(r,c);
            out << converters[c]->toString(value) << " ";
        }
        out << std::endl;
    }
}

// ----------------------------------------------------------------------------

PcdWriter::PcdWriter(const PointKernel& p) : Writer(p)
{
}

PcdWriter::~PcdWriter()
{
}

void PcdWriter::write(const std::string& filename)
{
    std::list<std::string> fields;
    fields.emplace_back("x");
    fields.emplace_back("y");
    fields.emplace_back("z");

    std::list<std::string> types;
    types.emplace_back("F");
    types.emplace_back("F");
    types.emplace_back("F");

    ConverterPtr convert_float(new ConverterT<float>);
    ConverterPtr convert_uint(new ConverterT<uint32_t>);

    std::vector<ConverterPtr> converters;
    converters.push_back(convert_float);
    converters.push_back(convert_float);
    converters.push_back(convert_float);

    bool hasIntensity = (intensity.size() == points.size());
    bool hasColors = (colors.size() == points.size());
    bool hasNormals = (normals.size() == points.size());

    if (hasNormals) {
        fields.emplace_back("normal_x");
        fields.emplace_back("normal_y");
        fields.emplace_back("normal_z");
        types.emplace_back("F");
        types.emplace_back("F");
        types.emplace_back("F");
        converters.push_back(convert_float);
        converters.push_back(convert_float);
        converters.push_back(convert_float);
    }

    if (hasColors) {
        fields.emplace_back("rgba");
        types.emplace_back("U");
        converters.push_back(convert_uint);
    }

    if (hasIntensity) {
        fields.emplace_back("intensity");
        types.emplace_back("F");
        converters.push_back(convert_float);
    }

    std::size_t numPoints = points.size();
    const std::vector<Base::Vector3f>& pts = points.getBasicPoints();

    Eigen::MatrixXd data(numPoints, fields.size());

    if (placement.isIdentity()) {
        for (std::size_t i=0; i<numPoints; i++) {
            data(i,0) = pts[i].x;
            data(i,1) = pts[i].y;
            data(i,2) = pts[i].z;
        }
    }
    else {
        Base::Vector3d tmp;
        for (std::size_t i=0; i<numPoints; i++) {
            tmp = Base::convertTo<Base::Vector3d>(pts[i]);
            placement.multVec(tmp, tmp);
            data(i,0) = static_cast<float>(tmp.x);
            data(i,1) = static_cast<float>(tmp.y);
            data(i,2) = static_cast<float>(tmp.z);
        }
    }

    std::size_t col = 3;
    if (hasNormals) {
        int col0 = col;
        int col1 = col+1;
        int col2 = col+2;
        Base::Rotation rot = placement.getRotation();
        if (rot.isIdentity()) {
            for (std::size_t i=0; i<numPoints; i++) {
                data(i,col0) = normals[i].x;
                data(i,col1) = normals[i].y;
                data(i,col2) = normals[i].z;
            }
        }
        else {
            Base::Vector3d tmp;
            for (std::size_t i=0; i<numPoints; i++) {
                tmp = Base::convertTo<Base::Vector3d>(normals[i]);
                rot.multVec(tmp, tmp);
                data(i,col0) = static_cast<float>(tmp.x);
                data(i,col1) = static_cast<float>(tmp.y);
                data(i,col2) = static_cast<float>(tmp.z);
            }
        }
        col += 3;
    }

    if (hasColors) {
        for (std::size_t i=0; i<numPoints; i++) {
            App::Color c = colors[i];
            // http://docs.pointclouds.org/1.3.0/structpcl_1_1_r_g_b.html
            uint32_t packed = static_cast<uint32_t>(c.a*255.0f + 0.5f) << 24 |
                              static_cast<uint32_t>(c.r*255.0f + 0.5f) << 16 |
                              static_cast<uint32_t>(c.g*255.0f + 0.5f) << 8  |
                              static_cast<uint32_t>(c.b*255.0f + 0.5f);

            data(i,col) = packed;
        }
        col += 1;
    }

    if (hasIntensity) {
        for (std::size_t i=0; i<numPoints; i++) {
            data(i,col) = intensity[i];
        }
        col += 1;
    }

    std::size_t numFields = fields.size();
    Base::ofstream out(filename, std::ios::out);
    out << "# .PCD v0.7 - Point Cloud Data file format" << std::endl
        << "VERSION 0.7" << std::endl;

    // the fields
    out << "FIELDS";
    for (std::list<std::string>::iterator it = fields.begin(); it != fields.end(); ++it)
        out << " " << *it;
    out << std::endl;

    // the sizes
    out << "SIZE";
    for (std::size_t i=0; i<numFields; i++)
        out << " 4";
    out << std::endl;

    // the types
    out << "TYPE";
    for (std::list<std::string>::iterator it = types.begin(); it != types.end(); ++it)
        out << " " << *it;
    out << std::endl;

    // the count
    out << "COUNT";
    for (std::size_t i=0; i<numFields; i++)
        out << " 1";
    out << std::endl;

    out << "WIDTH " << width << std::endl;
    out << "HEIGHT " << height << std::endl;

    Base::Placement plm;
    Base::Vector3d p = plm.getPosition();
    Base::Rotation o = plm.getRotation();
    double x,y,z,w; o.getValue(x,y,z,w);
    out << "VIEWPOINT " << p.x << " " << p.y << " " << p.z
        << " " << w << " " << x << " " << y << " " << z << std::endl;

    out << "POINTS " << numPoints << std::endl
        << "DATA ascii" << std::endl;

    for (std::size_t r=0; r<numPoints; r++) {
        for (std::size_t c=0; c<col; c++) {
            double value = data(r,c);
            if (boost::math::isnan(value))
                out << "nan ";
            else
                out << converters[c]->toString(value) << " ";
        }
        out << std::endl;
    }
}