<commit_before>/*=========================================================================

Program:   Medical Imaging & Interaction Toolkit
Module:    $RCSfile: mitkPropertyManager.cpp,v $
Language:  C++
Date:      $Date$
Version:   $Revision$

Copyright (c) German Cancer Research Center, Division of Medical and
Biological Informatics. All rights reserved.
See MITKCopyright.txt or http://www.mitk.org/copyright.html 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 notices for more information.

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

#include <ipPic/ipPicTypeMultiplex.h>
#include "ipSegmentation.h"

// only make macros available in local scope:
namespace {

// returns true if segmentation pixel at ofs is set, includes check for border pixels, i.e. border pixels do not result in returning true
#define SEGSET(ofs) (              \
((ofs)>=0 && (ofs)<maxOfs) &&          \
( *(((PicType*)seg->data) + (ofs)) != 0 ) &&  \
( (ofs)%line != 0 || (pos+1)%line != 0 ) &&    \
( (ofs+1)%line != 0 || (pos)%line != 0 )      \
)

// appends a point to result, sets finished to true if this point is the same as the first one
// allocates new momory if necessary
#define ADD_CONTOUR_POINT            \
result[numPts*2]   = xPos+xCorner[dir];      \
result[numPts*2+1] = yPos+yCorner[dir];      \
if (result[numPts*2]==result[0] && result[numPts*2+1]==result[1] && numPts>0) finished = true; \
numPts++;                    \
if (numPts==resSize) {              \
  resSize+=16+resSize/2;            \
  result = (float*)realloc( result, resSize*2*sizeof(float) ); \
  if (!result) finished = true;        \
}

}

template<typename PicType>
float* tmGetContour4N( const ipPicDescriptor *seg, int startOfs, int &numPts, int &resSize, float *result )
// returns the number of contour points (xy-pairs) in result
// optimized macros: DON'T TOUCH THE CODE (removed smiley, this is not funny but cruel for any maintainer!)
{
  numPts = 0;
  int line   = seg->n[0];
  int maxOfs = seg->n[0] * seg->n[1];

  int straight[4]  = { 1, -line, -1, line };
  int right[4]  = { line, 1, -line, -1 };
  float xMod[4]    = { 1.0, 0.0, -1.0, 0.0 };
  float yMod[4]    = { 0.0, -1.0, 0.0, 1.0 };
  float xCorner[4]  = { 1.0, 1.0, 0.0, 0.0 };
  float yCorner[4]  = { 1.0, 0.0, 0.0, 1.0 };

  int dir = 0;
  int pos = startOfs;
  float xPos = (float)(pos % line);
  float yPos = (float)(pos / line);

  while ( SEGSET( pos+right[dir] ) && dir<4 ) dir++;
  if (dir==4) return result;  // no contour pixel

  bool finished = false;
  if (result==0) {
    resSize = 2048;
    result = (float*)malloc( resSize*2*sizeof(float) );
  }

  do {
    if ( SEGSET( pos+right[dir] ) ) {
      // modify direction (turn right):
      dir = (dir-1) & 3;
      // modify position:
      pos  += straight[dir];
      xPos += xMod[dir];
      yPos += yMod[dir];
    }
    else if ( SEGSET( pos+straight[dir] ) ) {
      ADD_CONTOUR_POINT
      // modify position:
      pos += straight[dir];
      xPos += xMod[dir];
      yPos += yMod[dir];
    }
    else {
      ADD_CONTOUR_POINT
      // modify direction (turn left):
      dir = (dir+1) & 3;
    }
  } while (!finished);
  return result;
}

float* ipMITKSegmentationGetContour4N( const ipPicDescriptor *seg, int startOfs, int &numPoints, int &sizeBuffer, float *pointBuffer )
{
  float *newBuffer;
  ipPicTypeMultiplexR4( tmGetContour4N, seg, newBuffer, startOfs, numPoints, sizeBuffer, pointBuffer );
  return newBuffer;
}

template<typename PicType>
float* tmGetContour8N( const ipPicDescriptor *seg, int startOfs, int &numPts, int &resSize, float *result )
// returns the number of contour points (xy-pairs) in result
// optimized macros: DON'T TOUCH THE CODE ;-)
{
  numPts = 0;
  int line   = seg->n[0];               // width of segmentation in pixels
  int maxOfs = seg->n[0] * seg->n[1];  // memory offset of pixel just beyond bottom-right-most pixel of segmentation (not a valid offset)

  int straight[4]  = { 1, -line, -1, line };  // right, top, left, down   (memory offsets)
  int right[4]  = { line, 1, -line, -1 };     // down, right, top, left
  float xMod[4]    = { 1.0, 0.0, -1.0, 0.0 };
  float yMod[4]    = { 0.0, -1.0, 0.0, 1.0 };
  float xCorner[4]  = { 1.0, 1.0, 0.0, 0.0 };
  float yCorner[4]  = { 1.0, 0.0, 0.0, 1.0 };

  int dir = 0;
  int pos = startOfs;    // initial position, this is where the contour search starts
  float xPos = (float)(pos % line); // calculate x and y from the memory offset
  float yPos = (float)(pos / line);

  while ( SEGSET( pos+right[dir] ) && dir<4 ) dir++;
  if (dir==4) {
    // check diagonal pixels:
    dir = 0;
    while ( SEGSET( pos+right[dir]+straight[dir] ) && dir<4 ) dir++;
    if (dir==4) return result;  // no contour pixel
    // chose next suitable neighbour:
    pos  += straight[dir];
    xPos += xMod[dir];
    yPos += yMod[dir];
  }

  bool finished = false;
  if (result==0) {
    resSize = 2048;
    result = (float*)malloc( resSize*2*sizeof(float) );
  }

  // here xPos,yPos are on some pixel next to a segmentation pixel. Where is "next to"? This is defined by the value of dir and the offsets in right[dir].

  // tries to complete the contour until a point is added that is identical to the first point
  do {
    if ( SEGSET( pos+right[dir] ) ) {      // valgrind complaint: jump dependent on uninitialized value (from my current understanding this could only be some pixel value outside the image
      // modify direction (turn right):    // this if will evaluate to true during the first iteration
      dir = (dir-1) & 3;   // ok, some weird logic selects a new direction
      // modify position:
      pos  += straight[dir];   // definitions of right and straight (plus xMod and yMod) lead to a counter-clockwise movement around the contour
      xPos += xMod[dir];
      yPos += yMod[dir];
    }
    else if ( SEGSET( pos+straight[dir] ) ) {
      ADD_CONTOUR_POINT
      // modify position:
      pos += straight[dir];
      xPos += xMod[dir];
      yPos += yMod[dir];
    }
    else if ( SEGSET( pos+right[dir]+straight[dir] ) ) {   // valgrind complaint: jump dependent on uninitialized value
      ADD_CONTOUR_POINT
      // modify position:
      pos += straight[dir];
      xPos += xMod[dir];
      yPos += yMod[dir];
      // modify direction (turn right):
      dir = (dir-1) & 3;
      // modify position second time:
      pos += straight[dir];
      xPos += xMod[dir];
      yPos += yMod[dir];
    }
    else {
      ADD_CONTOUR_POINT
      // modify direction (turn left):
      dir = (dir+1) & 3;
    }
  } while (!finished);
  return result;
}

float* ipMITKSegmentationGetContour8N( const ipPicDescriptor *seg, int startOfs, int &numPoints, int &sizeBuffer, float *pointBuffer )
{
  float *newBuffer;
  ipPicTypeMultiplexR4( tmGetContour8N, seg, newBuffer, startOfs, numPoints, sizeBuffer, pointBuffer );
  return newBuffer;
}
<commit_msg>COMP: fixed warning.<commit_after>/*=========================================================================

Program:   Medical Imaging & Interaction Toolkit
Module:    $RCSfile: mitkPropertyManager.cpp,v $
Language:  C++
Date:      $Date$
Version:   $Revision$

Copyright (c) German Cancer Research Center, Division of Medical and
Biological Informatics. All rights reserved.
See MITKCopyright.txt or http://www.mitk.org/copyright.html 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 notices for more information.

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

#include <ipPic/ipPicTypeMultiplex.h>
#include "ipSegmentation.h"

// only make macros available in local scope:
namespace {

// returns true if segmentation pixel at ofs is set, includes check for border pixels, i.e. border pixels do not result in returning true
#define SEGSET(ofs) (              \
((ofs)>=0 && (ofs)<maxOfs) &&          \
( *(((PicType*)seg->data) + (ofs)) != 0 ) &&  \
( (ofs)%line != 0 || (pos+1)%line != 0 ) &&    \
( (ofs+1)%line != 0 || (pos)%line != 0 )      \
)

// appends a point to result, sets finished to true if this point is the same as the first one
// allocates new momory if necessary
#define ADD_CONTOUR_POINT            \
result[numPts*2]   = xPos+xCorner[dir];      \
result[numPts*2+1] = yPos+yCorner[dir];      \
if (result[numPts*2]==result[0] && result[numPts*2+1]==result[1] && numPts>0) finished = true; \
numPts++;                    \
if (numPts==resSize) {              \
  resSize+=16+resSize/2;            \
  result = (float*)realloc( result, resSize*2*sizeof(float) ); \
  if (!result) finished = true;        \
}

}

template<typename PicType>
float* tmGetContour4N( const ipPicDescriptor *seg, int startOfs, int &numPts, int &resSize, float *result )
// returns the number of contour points (xy-pairs) in result
// optimized macros: DON'T TOUCH THE CODE (removed smiley, this is not funny but cruel for any maintainer!)
{
  numPts = 0;
  int line   = seg->n[0];
  int maxOfs = seg->n[0] * seg->n[1];

  int straight[4]  = { 1, -line, -1, line };
  int right[4]  = { line, 1, -line, -1 };
  float xMod[4]    = { 1.0, 0.0, -1.0, 0.0 };
  float yMod[4]    = { 0.0, -1.0, 0.0, 1.0 };
  float xCorner[4]  = { 1.0, 1.0, 0.0, 0.0 };
  float yCorner[4]  = { 1.0, 0.0, 0.0, 1.0 };

  int dir = 0;
  int pos = startOfs;
  float xPos = (float)(pos % line);
  float yPos = (float)(pos / line);

  while ( SEGSET( pos+right[dir] ) && dir<4 ) dir++;
  if (dir==4) return result;  // no contour pixel

  bool finished = false;
  if (result==0) {
    resSize = 2048;
    result = (float*)malloc( resSize*2*sizeof(float) );
  }

  do {
    if ( SEGSET( pos+right[dir] ) ) {
      // modify direction (turn right):
      dir = (dir-1) & 3;
      // modify position:
      pos  += straight[dir];
      xPos += xMod[dir];
      yPos += yMod[dir];
    }
    else if ( SEGSET( pos+straight[dir] ) ) {
      ADD_CONTOUR_POINT
      // modify position:
      pos += straight[dir];
      xPos += xMod[dir];
      yPos += yMod[dir];
    }
    else {
      ADD_CONTOUR_POINT
      // modify direction (turn left):
      dir = (dir+1) & 3;
    }
  } while (!finished);
  return result;
}

float* ipMITKSegmentationGetContour4N( const ipPicDescriptor *seg, int startOfs, int &numPoints, int &sizeBuffer, float *pointBuffer )
{
  float *newBuffer = NULL;
  ipPicTypeMultiplexR4( tmGetContour4N, seg, newBuffer, startOfs, numPoints, sizeBuffer, pointBuffer );
  return newBuffer;
}

template<typename PicType>
float* tmGetContour8N( const ipPicDescriptor *seg, int startOfs, int &numPts, int &resSize, float *result )
// returns the number of contour points (xy-pairs) in result
// optimized macros: DON'T TOUCH THE CODE ;-)
{
  numPts = 0;
  int line   = seg->n[0];               // width of segmentation in pixels
  int maxOfs = seg->n[0] * seg->n[1];  // memory offset of pixel just beyond bottom-right-most pixel of segmentation (not a valid offset)

  int straight[4]  = { 1, -line, -1, line };  // right, top, left, down   (memory offsets)
  int right[4]  = { line, 1, -line, -1 };     // down, right, top, left
  float xMod[4]    = { 1.0, 0.0, -1.0, 0.0 };
  float yMod[4]    = { 0.0, -1.0, 0.0, 1.0 };
  float xCorner[4]  = { 1.0, 1.0, 0.0, 0.0 };
  float yCorner[4]  = { 1.0, 0.0, 0.0, 1.0 };

  int dir = 0;
  int pos = startOfs;    // initial position, this is where the contour search starts
  float xPos = (float)(pos % line); // calculate x and y from the memory offset
  float yPos = (float)(pos / line);

  while ( SEGSET( pos+right[dir] ) && dir<4 ) dir++;
  if (dir==4) {
    // check diagonal pixels:
    dir = 0;
    while ( SEGSET( pos+right[dir]+straight[dir] ) && dir<4 ) dir++;
    if (dir==4) return result;  // no contour pixel
    // chose next suitable neighbour:
    pos  += straight[dir];
    xPos += xMod[dir];
    yPos += yMod[dir];
  }

  bool finished = false;
  if (result==0) {
    resSize = 2048;
    result = (float*)malloc( resSize*2*sizeof(float) );
  }

  // here xPos,yPos are on some pixel next to a segmentation pixel. Where is "next to"? This is defined by the value of dir and the offsets in right[dir].

  // tries to complete the contour until a point is added that is identical to the first point
  do {
    if ( SEGSET( pos+right[dir] ) ) {      // valgrind complaint: jump dependent on uninitialized value (from my current understanding this could only be some pixel value outside the image
      // modify direction (turn right):    // this if will evaluate to true during the first iteration
      dir = (dir-1) & 3;   // ok, some weird logic selects a new direction
      // modify position:
      pos  += straight[dir];   // definitions of right and straight (plus xMod and yMod) lead to a counter-clockwise movement around the contour
      xPos += xMod[dir];
      yPos += yMod[dir];
    }
    else if ( SEGSET( pos+straight[dir] ) ) {
      ADD_CONTOUR_POINT
      // modify position:
      pos += straight[dir];
      xPos += xMod[dir];
      yPos += yMod[dir];
    }
    else if ( SEGSET( pos+right[dir]+straight[dir] ) ) {   // valgrind complaint: jump dependent on uninitialized value
      ADD_CONTOUR_POINT
      // modify position:
      pos += straight[dir];
      xPos += xMod[dir];
      yPos += yMod[dir];
      // modify direction (turn right):
      dir = (dir-1) & 3;
      // modify position second time:
      pos += straight[dir];
      xPos += xMod[dir];
      yPos += yMod[dir];
    }
    else {
      ADD_CONTOUR_POINT
      // modify direction (turn left):
      dir = (dir+1) & 3;
    }
  } while (!finished);
  return result;
}

float* ipMITKSegmentationGetContour8N( const ipPicDescriptor *seg, int startOfs, int &numPoints, int &sizeBuffer, float *pointBuffer )
{
  float *newBuffer = NULL;
  ipPicTypeMultiplexR4( tmGetContour8N, seg, newBuffer, startOfs, numPoints, sizeBuffer, pointBuffer );
  return newBuffer;
}
<|endoftext|>


<commit_before>/* This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/. */

#include <mpd/client.h>
#include "consumer.hh"
#include "globals.hh"

using namespace boost::filesystem;
using namespace std;

#include <fstream>
Consumer::Consumer(string throwaway)
    : _playlist_file_(std::move(throwaway))
    , _write_to_file_(true) {

    std::ifstream file(_playlist_file_);

    if (file) {
        string temp;
        while (getline(file, temp)) {
            _playlist_lines_.insert(temp);
        }
    }
}

void Consumer::_sorted_insert_song_(const std::string& str) {
    bool inserted = _playlist_lines_.insert(str).second;
    if (NONO && inserted) {
        printf("Insert `%s` into playlist `%s`.\n", str.c_str(),
               _playlist_file_.c_str());
    }
}

Consumer::~Consumer() {
    if (_write_to_file_) {
        if (!NONO) {
            std::ofstream file(_playlist_file_);
            if (file) {
                for (const auto& s : _playlist_lines_) {
                    file << s << '\n';
                }
                if (USE_MPD) {
                    _run_mpd_();
                }
            } else {
                throw std::runtime_error("Problem opening file " +
                                            _playlist_file_);
            }
        } else if (USE_MPD) {
            printf("Will clear the mpd playlist, update the mpd "
                   "database, and load %s.\n",
                   path(_playlist_file_)
                       .filename()
                       .replace_extension()
                       .c_str());
        }
    }
}

#include <taglib/fileref.h>
void Consumer::_apply_tags_(const path& path, const string& artist,
                            const string& title) {
    using namespace TagLib;
    bool changed = false;

    FileRef file(path.c_str());
    Tag* tags = file.tag();

    if (!tags) {
        fprintf(stderr, "Music file `%s` has no tags!\n", path.c_str());
        return;
    }

    if (tags->artist() != artist) {
        if (NONO) {
            printf("  Artist: `%s` -> `%s`\n",
                   tags->artist().toCString(), artist.c_str());
        } else {
            changed = true;
            tags->setArtist(artist);
        }
    }

    if (tags->title() != title) {
        if (NONO) {
            printf("  Title: `%s` -> `%s`\n",
                   tags->title().toCString(), title.c_str());
        } else {
            changed = true;
            tags->setTitle(title);
        }
    }

    if (changed) {
        file.save();
    }
}
<commit_msg>Reduce scope of the playlist file to inside the validity if statement.<commit_after>/* This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/. */

#include <mpd/client.h>
#include "consumer.hh"
#include "globals.hh"

using namespace boost::filesystem;
using namespace std;

#include <fstream>
Consumer::Consumer(string throwaway)
    : _playlist_file_(std::move(throwaway))
    , _write_to_file_(true) {

    std::ifstream file(_playlist_file_);

    if (file) {
        string temp;
        while (getline(file, temp)) {
            _playlist_lines_.insert(temp);
        }
    }
}

void Consumer::_sorted_insert_song_(const std::string& str) {
    bool inserted = _playlist_lines_.insert(str).second;
    if (NONO && inserted) {
        printf("Insert `%s` into playlist `%s`.\n", str.c_str(),
               _playlist_file_.c_str());
    }
}

Consumer::~Consumer() {
    if (_write_to_file_) {
        if (!NONO) {
            if (auto file = std::ofstream(_playlist_file_)) {
                for (const auto& s : _playlist_lines_) {
                    file << s << '\n';
                }
                if (USE_MPD) {
                    _run_mpd_();
                }
            } else {
                throw std::runtime_error("Problem opening file " +
                                            _playlist_file_);
            }
        } else if (USE_MPD) {
            printf("Will clear the mpd playlist, update the mpd "
                   "database, and load %s.\n",
                   path(_playlist_file_)
                       .filename()
                       .replace_extension()
                       .c_str());
        }
    }
}

#include <taglib/fileref.h>
void Consumer::_apply_tags_(const path& path, const string& artist,
                            const string& title) {
    using namespace TagLib;
    bool changed = false;

    FileRef file(path.c_str());
    Tag* tags = file.tag();

    if (!tags) {
        fprintf(stderr, "Music file `%s` has no tags!\n", path.c_str());
        return;
    }

    if (tags->artist() != artist) {
        if (NONO) {
            printf("  Artist: `%s` -> `%s`\n",
                   tags->artist().toCString(), artist.c_str());
        } else {
            changed = true;
            tags->setArtist(artist);
        }
    }

    if (tags->title() != title) {
        if (NONO) {
            printf("  Title: `%s` -> `%s`\n",
                   tags->title().toCString(), title.c_str());
        } else {
            changed = true;
            tags->setTitle(title);
        }
    }

    if (changed) {
        file.save();
    }
}
<|endoftext|>


<commit_before>/***************************************************************************
    copyright            : (C) 2002 - 2008 by Scott Wheeler
    email                : wheeler@kde.org
 ***************************************************************************/

/***************************************************************************
 *   This library is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU Lesser General Public License version   *
 *   2.1 as published by the Free Software Foundation.                     *
 *                                                                         *
 *   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     *
 *   Lesser General Public License for more details.                       *
 *                                                                         *
 *   You should have received a copy of the GNU Lesser General Public      *
 *   License along with this library; if not, write to the Free Software   *
 *   Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA         *
 *   02110-1301  USA                                                       *
 *                                                                         *
 *   Alternatively, this file is available under the Mozilla Public        *
 *   License Version 1.1.  You may obtain a copy of the License at         *
 *   http://www.mozilla.org/MPL/                                           *
 ***************************************************************************/

#include "tfile.h"
#include "tstring.h"
#include "tdebug.h"

#include <stdio.h>
#include <string.h>
#include <sys/stat.h>
#include <iostream>

#ifdef _WIN32
# include <wchar.h>
# include <windows.h>
# include <io.h>
# define ftruncate _chsize
#else
# include <unistd.h>
#endif

#include <stdlib.h>

#ifndef R_OK
# define R_OK 4
#endif
#ifndef W_OK
# define W_OK 2
#endif

using namespace TagLib;

#ifdef _WIN32

typedef FileName FileNameHandle;

#else

struct FileNameHandle : public std::string
{
  FileNameHandle(FileName name) : std::string(name) {}
  operator FileName () const { return c_str(); }
};

#endif

class File::FilePrivate
{
public:
  FilePrivate(FileName fileName);

  FILE *file;

  FileNameHandle name;

  bool readOnly;
  bool valid;
  ulong size;
  static const uint bufferSize = 1024;
};

File::FilePrivate::FilePrivate(FileName fileName) :
  file(0),
  name(fileName),
  readOnly(true),
  valid(true),
  size(0)
{
  // First try with read / write mode, if that fails, fall back to read only.

#ifdef _WIN32

  if(wcslen((const wchar_t *) fileName) > 0) {

    file = _wfopen(name, L"rb+");

    if(file)
      readOnly = false;
    else
      file = _wfopen(name, L"rb");

    if(file)
      return;

  }

#endif

  file = fopen(name, "rb+");

  if(file)
    readOnly = false;
  else
    file = fopen(name, "rb");

  if(!file)
  {
    debug("Could not open file " + String((const char *) name));
    std::cout<<"Could not open file"<<name<<std::endl;
  }
}

////////////////////////////////////////////////////////////////////////////////
// public members
////////////////////////////////////////////////////////////////////////////////

File::File(FileName file)
{
  d = new FilePrivate(file);
}

File::~File()
{
  if(d->file)
    fclose(d->file);
  delete d;
}

FileName File::name() const
{
  return d->name;
}

ByteVector File::readBlock(ulong length)
{
  if(!d->file) {
    debug("File::readBlock() -- Invalid File");
    return ByteVector::null;
  }

  if(length == 0)
    return ByteVector::null;

  if(length > FilePrivate::bufferSize &&
     length > ulong(File::length()))
  {
    length = File::length();
  }

  ByteVector v(static_cast<uint>(length));
  const int count = fread(v.data(), sizeof(char), length, d->file);
  v.resize(count);
  return v;
}

void File::writeBlock(const ByteVector &data)
{
  if(!d->file)
    return;

  if(d->readOnly) {
    debug("File::writeBlock() -- attempted to write to a file that is not writable");
    return;
  }

  fwrite(data.data(), sizeof(char), data.size(), d->file);
}

long File::find(const ByteVector &pattern, long fromOffset, const ByteVector &before)
{
  if(!d->file || pattern.size() > d->bufferSize)
      return -1;

  // The position in the file that the current buffer starts at.

  long bufferOffset = fromOffset;
  ByteVector buffer;

  // These variables are used to keep track of a partial match that happens at
  // the end of a buffer.

  int previousPartialMatch = -1;
  int beforePreviousPartialMatch = -1;

  // Save the location of the current read pointer.  We will restore the
  // position using seek() before all returns.

  long originalPosition = tell();

  // Start the search at the offset.

  seek(fromOffset);

  // This loop is the crux of the find method.  There are three cases that we
  // want to account for:
  //
  // (1) The previously searched buffer contained a partial match of the search
  // pattern and we want to see if the next one starts with the remainder of
  // that pattern.
  //
  // (2) The search pattern is wholly contained within the current buffer.
  //
  // (3) The current buffer ends with a partial match of the pattern.  We will
  // note this for use in the next itteration, where we will check for the rest
  // of the pattern.
  //
  // All three of these are done in two steps.  First we check for the pattern
  // and do things appropriately if a match (or partial match) is found.  We
  // then check for "before".  The order is important because it gives priority
  // to "real" matches.

  for(buffer = readBlock(d->bufferSize); buffer.size() > 0; buffer = readBlock(d->bufferSize)) {

    // (1) previous partial match

    if(previousPartialMatch >= 0 && int(d->bufferSize) > previousPartialMatch) {
      const int patternOffset = (d->bufferSize - previousPartialMatch);
      if(buffer.containsAt(pattern, 0, patternOffset)) {
        seek(originalPosition);
        return bufferOffset - d->bufferSize + previousPartialMatch;
      }
    }

    if(!before.isNull() && beforePreviousPartialMatch >= 0 && int(d->bufferSize) > beforePreviousPartialMatch) {
      const int beforeOffset = (d->bufferSize - beforePreviousPartialMatch);
      if(buffer.containsAt(before, 0, beforeOffset)) {
        seek(originalPosition);
        return -1;
      }
    }

    // (2) pattern contained in current buffer

    long location = buffer.find(pattern);
    if(location >= 0) {
      seek(originalPosition);
      return bufferOffset + location;
    }

    if(!before.isNull() && buffer.find(before) >= 0) {
      seek(originalPosition);
      return -1;
    }

    // (3) partial match

    previousPartialMatch = buffer.endsWithPartialMatch(pattern);

    if(!before.isNull())
      beforePreviousPartialMatch = buffer.endsWithPartialMatch(before);

    bufferOffset += d->bufferSize;
  }

  // Since we hit the end of the file, reset the status before continuing.

  clear();

  seek(originalPosition);

  return -1;
}

long File::rfind(const ByteVector &pattern, long fromOffset, const ByteVector &before)
{
  if(!d->file || pattern.size() > d->bufferSize)
      return -1;

  // The position in the file that the current buffer starts at.

  ByteVector buffer;

  // These variables are used to keep track of a partial match that happens at
  // the end of a buffer.

  /*
  int previousPartialMatch = -1;
  int beforePreviousPartialMatch = -1;
  */

  // Save the location of the current read pointer.  We will restore the
  // position using seek() before all returns.

  long originalPosition = tell();

  // Start the search at the offset.

  long bufferOffset;
  if(fromOffset == 0) {
    seek(-1 * int(d->bufferSize), End);
    bufferOffset = tell();
  }
  else {
    seek(fromOffset + -1 * int(d->bufferSize), Beginning);
    bufferOffset = tell();
  }

  // See the notes in find() for an explanation of this algorithm.

  for(buffer = readBlock(d->bufferSize); buffer.size() > 0; buffer = readBlock(d->bufferSize)) {

    // TODO: (1) previous partial match

    // (2) pattern contained in current buffer

    long location = buffer.rfind(pattern);
    if(location >= 0) {
      seek(originalPosition);
      return bufferOffset + location;
    }

    if(!before.isNull() && buffer.find(before) >= 0) {
      seek(originalPosition);
      return -1;
    }

    // TODO: (3) partial match

    bufferOffset -= d->bufferSize;
    seek(bufferOffset);
  }

  // Since we hit the end of the file, reset the status before continuing.

  clear();

  seek(originalPosition);

  return -1;
}

void File::insert(const ByteVector &data, ulong start, ulong replace)
{
  if(!d->file)
    return;

  if(data.size() == replace) {
    seek(start);
    writeBlock(data);
    return;
  }
  else if(data.size() < replace) {
      seek(start);
      writeBlock(data);
      removeBlock(start + data.size(), replace - data.size());
      return;
  }

  // Woohoo!  Faster (about 20%) than id3lib at last.  I had to get hardcore
  // and avoid TagLib's high level API for rendering just copying parts of
  // the file that don't contain tag data.
  //
  // Now I'll explain the steps in this ugliness:

  // First, make sure that we're working with a buffer that is longer than
  // the *differnce* in the tag sizes.  We want to avoid overwriting parts
  // that aren't yet in memory, so this is necessary.

  ulong bufferLength = bufferSize();

  while(data.size() - replace > bufferLength)
    bufferLength += bufferSize();

  // Set where to start the reading and writing.

  long readPosition = start + replace;
  long writePosition = start;

  ByteVector buffer;
  ByteVector aboutToOverwrite(static_cast<uint>(bufferLength));

  // This is basically a special case of the loop below.  Here we're just
  // doing the same steps as below, but since we aren't using the same buffer
  // size -- instead we're using the tag size -- this has to be handled as a
  // special case.  We're also using File::writeBlock() just for the tag.
  // That's a bit slower than using char *'s so, we're only doing it here.

  seek(readPosition);
  int bytesRead = fread(aboutToOverwrite.data(), sizeof(char), bufferLength, d->file);
  readPosition += bufferLength;

  seek(writePosition);
  writeBlock(data);
  writePosition += data.size();

  buffer = aboutToOverwrite;

  // In case we've already reached the end of file...

  buffer.resize(bytesRead);

  // Ok, here's the main loop.  We want to loop until the read fails, which
  // means that we hit the end of the file.

  while(!buffer.isEmpty()) {

    // Seek to the current read position and read the data that we're about
    // to overwrite.  Appropriately increment the readPosition.

    seek(readPosition);
    bytesRead = fread(aboutToOverwrite.data(), sizeof(char), bufferLength, d->file);
    aboutToOverwrite.resize(bytesRead);
    readPosition += bufferLength;

    // Check to see if we just read the last block.  We need to call clear()
    // if we did so that the last write succeeds.

    if(ulong(bytesRead) < bufferLength)
      clear();

    // Seek to the write position and write our buffer.  Increment the
    // writePosition.

    seek(writePosition);
    fwrite(buffer.data(), sizeof(char), buffer.size(), d->file);
    writePosition += buffer.size();

    // Make the current buffer the data that we read in the beginning.

    buffer = aboutToOverwrite;

    // Again, we need this for the last write.  We don't want to write garbage
    // at the end of our file, so we need to set the buffer size to the amount
    // that we actually read.

    bufferLength = bytesRead;
  }
}

void File::removeBlock(ulong start, ulong length)
{
  if(!d->file)
    return;

  ulong bufferLength = bufferSize();

  long readPosition = start + length;
  long writePosition = start;

  ByteVector buffer(static_cast<uint>(bufferLength));

  ulong bytesRead = 1;

  while(bytesRead != 0) {
    seek(readPosition);
    bytesRead = fread(buffer.data(), sizeof(char), bufferLength, d->file);
    readPosition += bytesRead;

    // Check to see if we just read the last block.  We need to call clear()
    // if we did so that the last write succeeds.

    if(bytesRead < bufferLength)
      clear();

    seek(writePosition);
    fwrite(buffer.data(), sizeof(char), bytesRead, d->file);
    writePosition += bytesRead;
  }
  truncate(writePosition);
}

bool File::readOnly() const
{
  return d->readOnly;
}

bool File::isReadable(const char *file)
{
  return access(file, R_OK) == 0;
}

bool File::isOpen() const
{
  return (d->file != NULL);
}

bool File::isValid() const
{
  return isOpen() && d->valid;
}

void File::seek(long offset, Position p)
{
  if(!d->file) {
    debug("File::seek() -- trying to seek in a file that isn't opened.");
    return;
  }

  switch(p) {
  case Beginning:
    fseek(d->file, offset, SEEK_SET);
    break;
  case Current:
    fseek(d->file, offset, SEEK_CUR);
    break;
  case End:
    fseek(d->file, offset, SEEK_END);
    break;
  }
}

void File::clear()
{
  clearerr(d->file);
}

long File::tell() const
{
  return ftell(d->file);
}

long File::length()
{
  // Do some caching in case we do multiple calls.

  if(d->size > 0)
    return d->size;

  if(!d->file)
    return 0;

  long curpos = tell();

  seek(0, End);
  long endpos = tell();

  seek(curpos, Beginning);

  d->size = endpos;
  return endpos;
}

bool File::isWritable(const char *file)
{
  return access(file, W_OK) == 0;
}

////////////////////////////////////////////////////////////////////////////////
// protected members
////////////////////////////////////////////////////////////////////////////////

void File::setValid(bool valid)
{
  d->valid = valid;
}

void File::truncate(long length)
{
  ftruncate(fileno(d->file), length);
}

TagLib::uint File::bufferSize()
{
  return FilePrivate::bufferSize;
}
<commit_msg>ups, revert my accidentally commited debug output<commit_after>/***************************************************************************
    copyright            : (C) 2002 - 2008 by Scott Wheeler
    email                : wheeler@kde.org
 ***************************************************************************/

/***************************************************************************
 *   This library is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU Lesser General Public License version   *
 *   2.1 as published by the Free Software Foundation.                     *
 *                                                                         *
 *   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     *
 *   Lesser General Public License for more details.                       *
 *                                                                         *
 *   You should have received a copy of the GNU Lesser General Public      *
 *   License along with this library; if not, write to the Free Software   *
 *   Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA         *
 *   02110-1301  USA                                                       *
 *                                                                         *
 *   Alternatively, this file is available under the Mozilla Public        *
 *   License Version 1.1.  You may obtain a copy of the License at         *
 *   http://www.mozilla.org/MPL/                                           *
 ***************************************************************************/

#include "tfile.h"
#include "tstring.h"
#include "tdebug.h"

#include <stdio.h>
#include <string.h>
#include <sys/stat.h>

#ifdef _WIN32
# include <wchar.h>
# include <windows.h>
# include <io.h>
# define ftruncate _chsize
#else
# include <unistd.h>
#endif

#include <stdlib.h>

#ifndef R_OK
# define R_OK 4
#endif
#ifndef W_OK
# define W_OK 2
#endif

using namespace TagLib;

#ifdef _WIN32

typedef FileName FileNameHandle;

#else

struct FileNameHandle : public std::string
{
  FileNameHandle(FileName name) : std::string(name) {}
  operator FileName () const { return c_str(); }
};

#endif

class File::FilePrivate
{
public:
  FilePrivate(FileName fileName);

  FILE *file;

  FileNameHandle name;

  bool readOnly;
  bool valid;
  ulong size;
  static const uint bufferSize = 1024;
};

File::FilePrivate::FilePrivate(FileName fileName) :
  file(0),
  name(fileName),
  readOnly(true),
  valid(true),
  size(0)
{
  // First try with read / write mode, if that fails, fall back to read only.

#ifdef _WIN32

  if(wcslen((const wchar_t *) fileName) > 0) {

    file = _wfopen(name, L"rb+");

    if(file)
      readOnly = false;
    else
      file = _wfopen(name, L"rb");

    if(file)
      return;

  }

#endif

  file = fopen(name, "rb+");

  if(file)
    readOnly = false;
  else
    file = fopen(name, "rb");

  if(!file)
  {
    debug("Could not open file " + String((const char *) name));
  }
}

////////////////////////////////////////////////////////////////////////////////
// public members
////////////////////////////////////////////////////////////////////////////////

File::File(FileName file)
{
  d = new FilePrivate(file);
}

File::~File()
{
  if(d->file)
    fclose(d->file);
  delete d;
}

FileName File::name() const
{
  return d->name;
}

ByteVector File::readBlock(ulong length)
{
  if(!d->file) {
    debug("File::readBlock() -- Invalid File");
    return ByteVector::null;
  }

  if(length == 0)
    return ByteVector::null;

  if(length > FilePrivate::bufferSize &&
     length > ulong(File::length()))
  {
    length = File::length();
  }

  ByteVector v(static_cast<uint>(length));
  const int count = fread(v.data(), sizeof(char), length, d->file);
  v.resize(count);
  return v;
}

void File::writeBlock(const ByteVector &data)
{
  if(!d->file)
    return;

  if(d->readOnly) {
    debug("File::writeBlock() -- attempted to write to a file that is not writable");
    return;
  }

  fwrite(data.data(), sizeof(char), data.size(), d->file);
}

long File::find(const ByteVector &pattern, long fromOffset, const ByteVector &before)
{
  if(!d->file || pattern.size() > d->bufferSize)
      return -1;

  // The position in the file that the current buffer starts at.

  long bufferOffset = fromOffset;
  ByteVector buffer;

  // These variables are used to keep track of a partial match that happens at
  // the end of a buffer.

  int previousPartialMatch = -1;
  int beforePreviousPartialMatch = -1;

  // Save the location of the current read pointer.  We will restore the
  // position using seek() before all returns.

  long originalPosition = tell();

  // Start the search at the offset.

  seek(fromOffset);

  // This loop is the crux of the find method.  There are three cases that we
  // want to account for:
  //
  // (1) The previously searched buffer contained a partial match of the search
  // pattern and we want to see if the next one starts with the remainder of
  // that pattern.
  //
  // (2) The search pattern is wholly contained within the current buffer.
  //
  // (3) The current buffer ends with a partial match of the pattern.  We will
  // note this for use in the next itteration, where we will check for the rest
  // of the pattern.
  //
  // All three of these are done in two steps.  First we check for the pattern
  // and do things appropriately if a match (or partial match) is found.  We
  // then check for "before".  The order is important because it gives priority
  // to "real" matches.

  for(buffer = readBlock(d->bufferSize); buffer.size() > 0; buffer = readBlock(d->bufferSize)) {

    // (1) previous partial match

    if(previousPartialMatch >= 0 && int(d->bufferSize) > previousPartialMatch) {
      const int patternOffset = (d->bufferSize - previousPartialMatch);
      if(buffer.containsAt(pattern, 0, patternOffset)) {
        seek(originalPosition);
        return bufferOffset - d->bufferSize + previousPartialMatch;
      }
    }

    if(!before.isNull() && beforePreviousPartialMatch >= 0 && int(d->bufferSize) > beforePreviousPartialMatch) {
      const int beforeOffset = (d->bufferSize - beforePreviousPartialMatch);
      if(buffer.containsAt(before, 0, beforeOffset)) {
        seek(originalPosition);
        return -1;
      }
    }

    // (2) pattern contained in current buffer

    long location = buffer.find(pattern);
    if(location >= 0) {
      seek(originalPosition);
      return bufferOffset + location;
    }

    if(!before.isNull() && buffer.find(before) >= 0) {
      seek(originalPosition);
      return -1;
    }

    // (3) partial match

    previousPartialMatch = buffer.endsWithPartialMatch(pattern);

    if(!before.isNull())
      beforePreviousPartialMatch = buffer.endsWithPartialMatch(before);

    bufferOffset += d->bufferSize;
  }

  // Since we hit the end of the file, reset the status before continuing.

  clear();

  seek(originalPosition);

  return -1;
}

long File::rfind(const ByteVector &pattern, long fromOffset, const ByteVector &before)
{
  if(!d->file || pattern.size() > d->bufferSize)
      return -1;

  // The position in the file that the current buffer starts at.

  ByteVector buffer;

  // These variables are used to keep track of a partial match that happens at
  // the end of a buffer.

  /*
  int previousPartialMatch = -1;
  int beforePreviousPartialMatch = -1;
  */

  // Save the location of the current read pointer.  We will restore the
  // position using seek() before all returns.

  long originalPosition = tell();

  // Start the search at the offset.

  long bufferOffset;
  if(fromOffset == 0) {
    seek(-1 * int(d->bufferSize), End);
    bufferOffset = tell();
  }
  else {
    seek(fromOffset + -1 * int(d->bufferSize), Beginning);
    bufferOffset = tell();
  }

  // See the notes in find() for an explanation of this algorithm.

  for(buffer = readBlock(d->bufferSize); buffer.size() > 0; buffer = readBlock(d->bufferSize)) {

    // TODO: (1) previous partial match

    // (2) pattern contained in current buffer

    long location = buffer.rfind(pattern);
    if(location >= 0) {
      seek(originalPosition);
      return bufferOffset + location;
    }

    if(!before.isNull() && buffer.find(before) >= 0) {
      seek(originalPosition);
      return -1;
    }

    // TODO: (3) partial match

    bufferOffset -= d->bufferSize;
    seek(bufferOffset);
  }

  // Since we hit the end of the file, reset the status before continuing.

  clear();

  seek(originalPosition);

  return -1;
}

void File::insert(const ByteVector &data, ulong start, ulong replace)
{
  if(!d->file)
    return;

  if(data.size() == replace) {
    seek(start);
    writeBlock(data);
    return;
  }
  else if(data.size() < replace) {
      seek(start);
      writeBlock(data);
      removeBlock(start + data.size(), replace - data.size());
      return;
  }

  // Woohoo!  Faster (about 20%) than id3lib at last.  I had to get hardcore
  // and avoid TagLib's high level API for rendering just copying parts of
  // the file that don't contain tag data.
  //
  // Now I'll explain the steps in this ugliness:

  // First, make sure that we're working with a buffer that is longer than
  // the *differnce* in the tag sizes.  We want to avoid overwriting parts
  // that aren't yet in memory, so this is necessary.

  ulong bufferLength = bufferSize();

  while(data.size() - replace > bufferLength)
    bufferLength += bufferSize();

  // Set where to start the reading and writing.

  long readPosition = start + replace;
  long writePosition = start;

  ByteVector buffer;
  ByteVector aboutToOverwrite(static_cast<uint>(bufferLength));

  // This is basically a special case of the loop below.  Here we're just
  // doing the same steps as below, but since we aren't using the same buffer
  // size -- instead we're using the tag size -- this has to be handled as a
  // special case.  We're also using File::writeBlock() just for the tag.
  // That's a bit slower than using char *'s so, we're only doing it here.

  seek(readPosition);
  int bytesRead = fread(aboutToOverwrite.data(), sizeof(char), bufferLength, d->file);
  readPosition += bufferLength;

  seek(writePosition);
  writeBlock(data);
  writePosition += data.size();

  buffer = aboutToOverwrite;

  // In case we've already reached the end of file...

  buffer.resize(bytesRead);

  // Ok, here's the main loop.  We want to loop until the read fails, which
  // means that we hit the end of the file.

  while(!buffer.isEmpty()) {

    // Seek to the current read position and read the data that we're about
    // to overwrite.  Appropriately increment the readPosition.

    seek(readPosition);
    bytesRead = fread(aboutToOverwrite.data(), sizeof(char), bufferLength, d->file);
    aboutToOverwrite.resize(bytesRead);
    readPosition += bufferLength;

    // Check to see if we just read the last block.  We need to call clear()
    // if we did so that the last write succeeds.

    if(ulong(bytesRead) < bufferLength)
      clear();

    // Seek to the write position and write our buffer.  Increment the
    // writePosition.

    seek(writePosition);
    fwrite(buffer.data(), sizeof(char), buffer.size(), d->file);
    writePosition += buffer.size();

    // Make the current buffer the data that we read in the beginning.

    buffer = aboutToOverwrite;

    // Again, we need this for the last write.  We don't want to write garbage
    // at the end of our file, so we need to set the buffer size to the amount
    // that we actually read.

    bufferLength = bytesRead;
  }
}

void File::removeBlock(ulong start, ulong length)
{
  if(!d->file)
    return;

  ulong bufferLength = bufferSize();

  long readPosition = start + length;
  long writePosition = start;

  ByteVector buffer(static_cast<uint>(bufferLength));

  ulong bytesRead = 1;

  while(bytesRead != 0) {
    seek(readPosition);
    bytesRead = fread(buffer.data(), sizeof(char), bufferLength, d->file);
    readPosition += bytesRead;

    // Check to see if we just read the last block.  We need to call clear()
    // if we did so that the last write succeeds.

    if(bytesRead < bufferLength)
      clear();

    seek(writePosition);
    fwrite(buffer.data(), sizeof(char), bytesRead, d->file);
    writePosition += bytesRead;
  }
  truncate(writePosition);
}

bool File::readOnly() const
{
  return d->readOnly;
}

bool File::isReadable(const char *file)
{
  return access(file, R_OK) == 0;
}

bool File::isOpen() const
{
  return (d->file != NULL);
}

bool File::isValid() const
{
  return isOpen() && d->valid;
}

void File::seek(long offset, Position p)
{
  if(!d->file) {
    debug("File::seek() -- trying to seek in a file that isn't opened.");
    return;
  }

  switch(p) {
  case Beginning:
    fseek(d->file, offset, SEEK_SET);
    break;
  case Current:
    fseek(d->file, offset, SEEK_CUR);
    break;
  case End:
    fseek(d->file, offset, SEEK_END);
    break;
  }
}

void File::clear()
{
  clearerr(d->file);
}

long File::tell() const
{
  return ftell(d->file);
}

long File::length()
{
  // Do some caching in case we do multiple calls.

  if(d->size > 0)
    return d->size;

  if(!d->file)
    return 0;

  long curpos = tell();

  seek(0, End);
  long endpos = tell();

  seek(curpos, Beginning);

  d->size = endpos;
  return endpos;
}

bool File::isWritable(const char *file)
{
  return access(file, W_OK) == 0;
}

////////////////////////////////////////////////////////////////////////////////
// protected members
////////////////////////////////////////////////////////////////////////////////

void File::setValid(bool valid)
{
  d->valid = valid;
}

void File::truncate(long length)
{
  ftruncate(fileno(d->file), length);
}

TagLib::uint File::bufferSize()
{
  return FilePrivate::bufferSize;
}
<|endoftext|>


<commit_before>#include "calculate.hpp"
#include <sstream>
#include <stack>

using namespace std;

multimap<string, oper_t> opers;
multimap<string, func_t, funcname_compare> funcs;

/// pop element from stack and return it
/// std::stack<T>::pop() is stupid and returns void
template<typename T>
T pop(stack<T> &s)
{
	T val = s.top();
	s.pop();
	return val;
}

/// insert binary operator specified by oit into Shunting-Yard
void insert_binaryoper(postfix_t &out, stack<token_t> &s, decltype(opers)::iterator oit)
{
	while (!s.empty())
	{
		bool found = false; // stack top element is operator
		int tprec; // prec of stack top element
		auto range = opers.equal_range(s.top().first);
		for (auto oit2 = range.first; oit2 != range.second; ++oit2)
		{
			if (s.top().second == (oit2->second.unary ? 1 : 2)) // find the correct arity version of the operator
			{
				tprec = oit2->second.prec;
				found = true;
				break;
			}
		}

		if ((found && ((!oit->second.right && oit->second.prec == tprec) || (oit->second.prec < tprec))) || (!found && funcs.find(s.top().first) != funcs.end()))
			out.push_back(pop(s));
		else
			break;
	}
	s.push(token_t(oit->first, 2));
}

/// insert implicit multiplication at current state
void insert_implicitmult(postfix_t &out, stack<token_t> &s)
{
	auto range = opers.equal_range("*");
	auto oit = range.first;
	for (; oit != range.second; ++oit)
	{
		if (oit->second.unary == false)
			break;
	}

	if (oit != range.second) // if binary multiplication operator exists
		insert_binaryoper(out, s, oit);
}

/// convert infix string into postfix token list
postfix_t infix2postfix(string in)
{
	postfix_t out;
	stack<token_t> s;

	token_t lasttok;
	for (auto it = in.cbegin(); it != in.cend();)
	{
		const unsigned int i = it - in.cbegin(); // index of current character for parse_error purposes

		static const string spaces = " \t\r\n";
		if (spaces.find(*it) != string::npos)
		{
			++it;
			continue;
		}

		/*cout << string(it, in.cend()) << endl;
		cout << lasttok.first << "/" << lasttok.second << endl;
		if (!s.empty())
			cout << s.top().first << "/" << s.top().second << endl;*/

		// try to parse number
		static const string numbers = "0123456789.";
		auto it2 = it;
		for (; it2 != in.cend() && numbers.find(*it2) != string::npos; ++it2); // TODO: find_first_not_of
		if (it2 != it)
		{
			if (lasttok.first == ")" || (opers.find(lasttok.first) == opers.end() && funcs.find(lasttok.first) != funcs.end()) || lasttok.second == -1)
				throw parse_error("Missing operator", i);

			out.push_back(lasttok = token_t(string(it, it2), -1));
			it = it2;
			continue;
		}

		// try to parse operator
		auto lastoper = opers.find(lasttok.first);
		bool lunary = lasttok.first == "" || lasttok.first == "(" || lasttok.first == "," || (lastoper != opers.end() && !(lastoper->second.unary && lastoper->second.right)); // true if operator at current location would be left unary
		/*cout << unary << endl;
		cout << endl;*/

		auto oit = opers.begin();
		for (; oit != opers.end(); ++oit)
		{
			if (equal(oit->first.begin(), oit->first.end(), it) && (oit->second.unary == lunary || (oit->second.unary && oit->second.right)))
				break;
		}
		if (oit != opers.end())
		{
			if (lunary)
			{
				s.push(lasttok = token_t(oit->first, 1));
			}
			else if (oit->second.unary && oit->second.right) // right unary operator
			{
				out.push_back(lasttok = token_t(oit->first, 1)); // needs binaryop insertion?
			}
			else
			{
				insert_binaryoper(out, s, oit);
				lasttok = token_t(oit->first, 2);
			}
			it += oit->first.size();
			continue;
		}

		// try to parse function
		auto fit = funcs.begin();
		for (; fit != funcs.end(); ++fit)
		{
			if (opers.find(fit->first) == opers.end() && equal(fit->first.begin(), fit->first.end(), it))
				break;
		}
		if (fit != funcs.end())
		{
			if (lasttok.first == ")" || funcs.find(lasttok.first) != funcs.end())
				throw parse_error("Missing operator", i);
			else if (lasttok.second == -1)
				insert_implicitmult(out, s);

			s.push(lasttok = token_t(fit->first, 0));
			it += fit->first.size();
			continue;
		}

		// try to parse function argument separator
		if (*it == ',')
		{
			if (lasttok.first == "(" || lasttok.first == ",")
				throw parse_error("Missing argument", i);

			bool found = false;
			while (!s.empty())
			{
				token_t tok = s.top();

				if (tok.first == "(")
				{
					found = true;
					break;
				}
				else
				{
					out.push_back(tok);
					s.pop();
				}
			}

			if (!found)
				throw parse_error("Found ',' not inside function arguments", i);

			s.top().second++; // increase number of arguments in current parenthesis
			lasttok = token_t(",", 0);
			++it;
			continue;
		}

		if (*it == '(')
		{
			if (lasttok.second == -1 || lasttok.first == ")")
				insert_implicitmult(out, s);

			s.push(lasttok = token_t("(", 1));
			++it;
			continue;
		}

		if (*it == ')')
		{
			if (lasttok.first == "(" || lasttok.first == ",")
				throw parse_error("Missing argument", i);

			bool found = false;
			while (!s.empty())
			{
				token_t tok = s.top();
				if (tok.first == "(")
				{
					found = true;
					break;
				}
				else
				{
					out.push_back(tok);
					s.pop();
				}
			}

			if (!found)
				throw parse_error("Found excess '('", i);

			token_t tok = pop(s); // pop '('

			if (!s.empty() && opers.find(s.top().first) == opers.end() && funcs.find(s.top().first) != funcs.end()) // if parenthesis part of function arguments
				out.push_back(token_t(pop(s).first, tok.second));

			lasttok = token_t(")", 0);
			++it;
			continue;
		}

		throw parse_error("Unknown token found", i);
	}

	while (!s.empty())
	{
		token_t tok = pop(s);
		if (tok.first == "(")
			throw parse_error("Found unclosed '('", in.size());
		out.push_back(tok);
	}

	return out;
}

/// evaluate postfix expression
num_t evalpostfix(postfix_t in)
{
	stack<num_t> s;
	for (token_t &tok : in)
	{
		if (tok.second == -1) // number
			s.push(stod(tok.first));
		else
		{
			if (s.size() < tok.second)
				throw runtime_error("Not enough arguments (have " + to_string(s.size()) + ") for function '" + tok.first + "' (want " + to_string(tok.second) + ")");
			else
			{
				args_t v;
				for (int i = 0; i < tok.second; i++)
					v.insert(v.begin(), pop(s)); // pop elements for function arguments in reverse order

				auto range = funcs.equal_range(tok.first);
				return_t ret(false, 0);
				for (auto it = range.first; it != range.second; ++it)
				{
					ret = it->second(v);
					if (ret.first) // find a function that can evaluate given parameters
						break;
				}

				if (ret.first)
					s.push(ret.second);
				else
				{
					ostringstream args; // stringstream because to_string adds trailing zeroes
					for (auto vit = v.begin(); vit != v.end(); ++vit) // construct exception argument list
					{
						args << *vit;
						if ((vit + 1) != v.end())
							args << ", ";
					}
					throw runtime_error("Unacceptable arguments (" + args.str() + ") for function '" + tok.first + "'");
				}
			}
		}
	}

	if (s.size() == 1)
		return s.top();
	else
		throw runtime_error("No single result found");
}
<commit_msg>find_oper for easy operator finding, handle right unary operators correctly after constants and higher prec operators, allow functions after unary operators<commit_after>#include "calculate.hpp"
#include <sstream>
#include <stack>

using namespace std;

multimap<string, oper_t> opers;
multimap<string, func_t, funcname_compare> funcs;

/// pop element from stack and return it
/// std::stack<T>::pop() is stupid and returns void
template<typename T>
T pop(stack<T> &s)
{
	T val = s.top();
	s.pop();
	return val;
}

/// insert binary operator specified by oit into Shunting-Yard
void insert_binaryoper(postfix_t &out, stack<token_t> &s, decltype(opers)::iterator oit)
{
	while (!s.empty())
	{
		bool found = false; // stack top element is operator
		int tprec; // prec of stack top element
		auto range = opers.equal_range(s.top().first);
		for (auto oit2 = range.first; oit2 != range.second; ++oit2)
		{
			if (s.top().second == (oit2->second.unary ? 1 : 2)) // find the correct arity version of the operator
			{
				tprec = oit2->second.prec;
				found = true;
				break;
			}
		}

		if ((found && ((!oit->second.right && oit->second.prec == tprec) || (oit->second.prec < tprec))) || (!found && funcs.find(s.top().first) != funcs.end()))
			out.push_back(pop(s));
		else
			break;
	}
	s.push(token_t(oit->first, 2));
}

/// find operator with specific string and arity
decltype(opers)::iterator find_oper(const string &str, bool unary)
{
	auto range = opers.equal_range(str);
	auto oit = range.first;
	for (; oit != range.second; ++oit)
	{
		if (oit->second.unary == unary)
			break;
	}
	return oit == range.second ? opers.end() : oit;
}

/// insert implicit multiplication at current state
void insert_implicitmult(postfix_t &out, stack<token_t> &s)
{
	auto oit = find_oper("*", false);
	if (oit != opers.end()) // if binary multiplication operator exists
		insert_binaryoper(out, s, oit);
}

/// convert infix string into postfix token list
postfix_t infix2postfix(string in)
{
	postfix_t out;
	stack<token_t> s;

	token_t lasttok;
	for (auto it = in.cbegin(); it != in.cend();)
	{
		const unsigned int i = it - in.cbegin(); // index of current character for parse_error purposes

		static const string spaces = " \t\r\n";
		if (spaces.find(*it) != string::npos)
		{
			++it;
			continue;
		}

		/*cout << string(it, in.cend()) << endl;
		cout << lasttok.first << "/" << lasttok.second << endl;
		if (!s.empty())
			cout << s.top().first << "/" << s.top().second << endl;*/

		// try to parse number
		static const string numbers = "0123456789.";
		auto it2 = it;
		for (; it2 != in.cend() && numbers.find(*it2) != string::npos; ++it2); // TODO: find_first_not_of
		if (it2 != it)
		{
			if (lasttok.first == ")" || (opers.find(lasttok.first) == opers.end() && funcs.find(lasttok.first) != funcs.end()) || lasttok.second == -1)
				throw parse_error("Missing operator", i);

			out.push_back(lasttok = token_t(string(it, it2), -1));
			it = it2;
			continue;
		}

		// try to parse operator
		auto lastoper = opers.find(lasttok.first);
		bool lunary = lasttok.first == "" || lasttok.first == "(" || lasttok.first == "," || (lastoper != opers.end() && !(lastoper->second.unary && lastoper->second.right)); // true if operator at current location would be left unary
		/*cout << unary << endl;
		cout << endl;*/

		auto oit = opers.begin();
		for (; oit != opers.end(); ++oit)
		{
			if (equal(oit->first.begin(), oit->first.end(), it) && (oit->second.unary == lunary || (oit->second.unary && oit->second.right)))
				break;
		}
		if (oit != opers.end())
		{
			if (lunary)
			{
				s.push(lasttok = token_t(oit->first, 1));
			}
			else if (oit->second.unary && oit->second.right) // right unary operator
			{
				// allow right unary operators to be used on constants and apply higher prec functions before
				while (!s.empty())
				{
					token_t tok = s.top();

					auto oit2 = find_oper(tok.first, true);
					if ((oit2 != opers.end() && oit2->second.prec > oit->second.prec) || (oit2 == opers.end() && funcs.find(tok.first) != funcs.end()))
						out.push_back(pop(s));
					else
						break;
				}
				out.push_back(lasttok = token_t(oit->first, 1)); // needs stack popping before?
			}
			else
			{
				insert_binaryoper(out, s, oit);
				lasttok = token_t(oit->first, 2);
			}
			it += oit->first.size();
			continue;
		}

		// try to parse function
		auto fit = funcs.begin();
		for (; fit != funcs.end(); ++fit)
		{
			if (opers.find(fit->first) == opers.end() && equal(fit->first.begin(), fit->first.end(), it))
				break;
		}
		if (fit != funcs.end())
		{
			if (lasttok.first == ")" || (opers.find(lasttok.first) == opers.end() && funcs.find(lasttok.first) != funcs.end()))
				throw parse_error("Missing operator", i);
			else if (lasttok.second == -1)
				insert_implicitmult(out, s);

			s.push(lasttok = token_t(fit->first, 0));
			it += fit->first.size();
			continue;
		}

		// try to parse function argument separator
		if (*it == ',')
		{
			if (lasttok.first == "(" || lasttok.first == ",")
				throw parse_error("Missing argument", i);

			bool found = false;
			while (!s.empty())
			{
				token_t tok = s.top();

				if (tok.first == "(")
				{
					found = true;
					break;
				}
				else
				{
					out.push_back(tok);
					s.pop();
				}
			}

			if (!found)
				throw parse_error("Found ',' not inside function arguments", i);

			s.top().second++; // increase number of arguments in current parenthesis
			lasttok = token_t(",", 0);
			++it;
			continue;
		}

		if (*it == '(')
		{
			if (lasttok.second == -1 || lasttok.first == ")")
				insert_implicitmult(out, s);

			s.push(lasttok = token_t("(", 1));
			++it;
			continue;
		}

		if (*it == ')')
		{
			if (lasttok.first == "(" || lasttok.first == ",")
				throw parse_error("Missing argument", i);

			bool found = false;
			while (!s.empty())
			{
				token_t tok = s.top();
				if (tok.first == "(")
				{
					found = true;
					break;
				}
				else
				{
					out.push_back(tok);
					s.pop();
				}
			}

			if (!found)
				throw parse_error("Found excess '('", i);

			token_t tok = pop(s); // pop '('

			if (!s.empty() && opers.find(s.top().first) == opers.end() && funcs.find(s.top().first) != funcs.end()) // if parenthesis part of function arguments
				out.push_back(token_t(pop(s).first, tok.second));

			lasttok = token_t(")", 0);
			++it;
			continue;
		}

		throw parse_error("Unknown token found", i);
	}

	while (!s.empty())
	{
		token_t tok = pop(s);
		if (tok.first == "(")
			throw parse_error("Found unclosed '('", in.size());
		out.push_back(tok);
	}

	return out;
}

/// evaluate postfix expression
num_t evalpostfix(postfix_t in)
{
	stack<num_t> s;
	for (token_t &tok : in)
	{
		if (tok.second == -1) // number
			s.push(stod(tok.first));
		else
		{
			if (s.size() < tok.second)
				throw runtime_error("Not enough arguments (have " + to_string(s.size()) + ") for function '" + tok.first + "' (want " + to_string(tok.second) + ")");
			else
			{
				args_t v;
				for (int i = 0; i < tok.second; i++)
					v.insert(v.begin(), pop(s)); // pop elements for function arguments in reverse order

				auto range = funcs.equal_range(tok.first);
				return_t ret(false, 0);
				for (auto it = range.first; it != range.second; ++it)
				{
					ret = it->second(v);
					if (ret.first) // find a function that can evaluate given parameters
						break;
				}

				if (ret.first)
					s.push(ret.second);
				else
				{
					ostringstream args; // stringstream because to_string adds trailing zeroes
					for (auto vit = v.begin(); vit != v.end(); ++vit) // construct exception argument list
					{
						args << *vit;
						if ((vit + 1) != v.end())
							args << ", ";
					}
					throw runtime_error("Unacceptable arguments (" + args.str() + ") for function '" + tok.first + "'");
				}
			}
		}
	}

	if (s.size() == 1)
		return s.top();
	else
		throw runtime_error("No single result found");
}
<|endoftext|>


<commit_before>#include "constants.h"

#include <cv_bridge/cv_bridge.h>
#include <dynamic_reconfigure/server.h>
#include <image_transport/camera_publisher.h>
#include <image_transport/image_transport.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>

#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <velodyne_msgs/VelodynePacket.h>
#include <velodyne_msgs/VelodyneScan.h>
#include <velodyne_puck/VelodynePuckConfig.h>

namespace velodyne_puck {

using namespace sensor_msgs;
using namespace velodyne_msgs;

using PointT = pcl::PointXYZI;
using CloudT = pcl::PointCloud<PointT>;

/// Convert image and camera_info to point cloud
CloudT ToCloud(const ImageConstPtr& image_msg, const CameraInfo& cinfo_msg,
               bool organized);

/// Used for indexing into packet and image, (NOISE not used now)
enum Index { RANGE = 0, INTENSITY = 1, AZIMUTH = 2, NOISE = 3 };

class Decoder {
 public:
  /// Number of channels for image data
  static constexpr int kChannels = 2;  // (range [m], intensity)

  explicit Decoder(const ros::NodeHandle& pnh);

  Decoder(const Decoder&) = delete;
  Decoder operator=(const Decoder&) = delete;

  void PacketCb(const VelodynePacketConstPtr& packet_msg);
  void ConfigCb(VelodynePuckConfig& config, int level);

 private:
  /// All of these uses laser index from velodyne which is interleaved

  /// 9.3.1.3 Data Point
  /// A data point is a measurement by one laser channel of a relection of a
  /// laser pulse
  struct DataPoint {
    uint16_t distance;
    uint8_t reflectivity;
  } __attribute__((packed));
  static_assert(sizeof(DataPoint) == 3, "sizeof(DataPoint) != 3");

  /// 9.3.1.1 Firing Sequence
  /// A firing sequence occurs when all the lasers in a sensor are fired. There
  /// are 16 firings per cycle for VLP-16
  struct FiringSequence {
    DataPoint points[kFiringsPerSequence];  // 16
  } __attribute__((packed));
  static_assert(sizeof(FiringSequence) == 48, "sizeof(FiringSequence) != 48");

  /// 9.3.1.4 Azimuth
  /// A two-byte azimuth value (alpha) appears after the flag bytes at the
  /// beginning of each data block
  ///
  /// 9.3.1.5 Data Block
  /// The information from 2 firing sequences of 16 lasers is contained in each
  /// data block. Each packet contains the data from 24 firing sequences in 12
  /// data blocks.
  struct DataBlock {
    uint16_t flag;
    uint16_t azimuth;                              // [0, 35999]
    FiringSequence sequences[kSequencesPerBlock];  // 2
  } __attribute__((packed));
  static_assert(sizeof(DataBlock) == 100, "sizeof(DataBlock) != 100");

  struct Packet {
    DataBlock blocks[kBlocksPerPacket];  // 12
    /// The four-byte time stamp is a 32-bit unsigned integer marking the moment
    /// of the first data point in the first firing sequcne of the first data
    /// block. The time stamp’s value is the number of microseconds elapsed
    /// since the top of the hour.
    uint32_t stamp;
    uint8_t factory[2];
  } __attribute__((packed));
  static_assert(sizeof(Packet) == sizeof(velodyne_msgs::VelodynePacket().data),
                "sizeof(Packet) != 1206");

  void DecodeAndFill(const Packet* const packet_buf);

 private:
  bool CheckFactoryBytes(const Packet* const packet);
  void Reset();

  // ROS related parameters
  std::string frame_id_;
  ros::NodeHandle pnh_;
  image_transport::ImageTransport it_;
  ros::Subscriber packet_sub_;
  ros::Publisher cloud_pub_;
  image_transport::Publisher intensity_pub_, range_pub_;
  image_transport::CameraPublisher camera_pub_;
  dynamic_reconfigure::Server<VelodynePuckConfig> cfg_server_;
  VelodynePuckConfig config_;

  // cached
  cv::Mat image_;
  std::vector<double> azimuths_;
  std::vector<uint64_t> timestamps_;
  int curr_col_{0};
  std::vector<double> elevations_;
};

Decoder::Decoder(const ros::NodeHandle& pnh)
    : pnh_(pnh), it_(pnh), cfg_server_(pnh) {
  pnh_.param<std::string>("frame_id", frame_id_, "velodyne");
  ROS_INFO("Velodyne frame_id: %s", frame_id_.c_str());
  cfg_server_.setCallback(boost::bind(&Decoder::ConfigCb, this, _1, _2));

  // Pre-compute elevations
  for (int i = 0; i < kFiringsPerSequence; ++i) {
    elevations_.push_back(kMaxElevation - i * kDeltaElevation);
  }
}

bool Decoder::CheckFactoryBytes(const Packet* const packet_buf) {
  // Check return mode and product id, for now just die
  const auto return_mode = packet_buf->factory[0];

  if (!(return_mode == 55 || return_mode == 56)) {
    ROS_ERROR(
        "return mode must be Strongest (55) or Last Return (56), "
        "instead got (%u)",
        return_mode);
    return false;
  }

  const auto product_id = packet_buf->factory[1];
  if (product_id != 34) {
    ROS_ERROR("product id must be VLP-16 or Puck Lite (34), instead got (%u)",
              product_id);
    return false;
  }

  return true;
}

void Decoder::DecodeAndFill(const Packet* const packet_buf) {
  if (!CheckFactoryBytes(packet_buf)) {
    ros::shutdown();
  }

  // transform
  //            ^ x_l
  //            | -> /
  //            | a /
  //            |  /
  //            | /
  // <----------o
  // y_l

  // For each data block, 12 total
  for (int iblk = 0; iblk < kBlocksPerPacket; ++iblk) {
    const auto& block = packet_buf->blocks[iblk];
    auto raw_azimuth = block.azimuth;         // nominal azimuth [0,35999]
    auto azimuth = Raw2Azimuth(raw_azimuth);  // nominal azimuth [0, 2pi)

    ROS_WARN_STREAM_COND(raw_azimuth > kMaxRawAzimuth,
                         "Invalid raw azimuth: " << raw_azimuth);
    ROS_WARN_COND(block.flag != UPPER_BANK, "Invalid block %d", iblk);

    float azimuth_gap{0};
    if (iblk == kBlocksPerPacket - 1) {
      // Last block, 12th
      const auto& prev_block = packet_buf->blocks[iblk - 1];
      const auto prev_azimuth = Raw2Azimuth(prev_block.azimuth);
      azimuth_gap = azimuth - prev_azimuth;
    } else {
      // First 11 blocks
      const auto& next_block = packet_buf->blocks[iblk + 1];
      const auto next_azimuth = Raw2Azimuth(next_block.azimuth);
      azimuth_gap = next_azimuth - azimuth;
    }

    // Adjust for azimuth rollover from 2pi to 0
    if (azimuth_gap < 0) azimuth_gap += kTau;
    const auto half_azimuth_gap = azimuth_gap / 2;

    // for each firing sequence in the data block, 2
    for (int iseq = 0; iseq < kSequencesPerBlock; ++iseq, ++curr_col_) {
      const auto col = iblk * 2 + iseq;
      const auto& seq = block.sequences[iseq];

      // for each laser beam, 16
      for (int lid = 0; lid < kFiringsPerSequence; ++lid) {
        const auto& point = seq.points[lid];
        auto& v = image_.at<cv::Vec3f>(LaserId2Row(lid), curr_col_);
        v[RANGE] = point.distance * kDistanceResolution;
        v[INTENSITY] = point.reflectivity;
        const auto offset = lid * kSingleFiringRatio * half_azimuth_gap +
                            iseq * half_azimuth_gap;
        v[AZIMUTH] = azimuth + offset;
      }

      timestamps_[curr_col_] = col * kFiringCycleNs;
      azimuths_[curr_col_] = Raw2Azimuth(block.azimuth);
    }
  }
}

void Decoder::PacketCb(const VelodynePacketConstPtr& packet_msg) {
  const auto start = ros::Time::now();

  const auto* packet_buf =
      reinterpret_cast<const Packet*>(&(packet_msg->data[0]));
  DecodeAndFill(packet_buf);

  if (curr_col_ < config_.image_width) {
    return;
  }

  std_msgs::Header header;
  header.frame_id = frame_id_;
  header.stamp.fromNSec(packet_msg->stamp.toNSec());

  const ImagePtr image_msg =
      cv_bridge::CvImage(header, "32FC3", image_).toImageMsg();

  // Fill in camera info
  const CameraInfoPtr cinfo_msg(new CameraInfo);
  cinfo_msg->header = header;
  cinfo_msg->height = image_msg->height;
  cinfo_msg->width = image_msg->width;
  cinfo_msg->distortion_model = "VLP16";
  cinfo_msg->P[0] = kFiringCycleNs;  // delta time between two measurements

  // D = [altitude, azimuth]
  cinfo_msg->D = elevations_;
  cinfo_msg->D.insert(cinfo_msg->D.end(), azimuths_.begin(), azimuths_.end());

  // Publish on demand
  if (camera_pub_.getNumSubscribers() > 0) {
    camera_pub_.publish(image_msg, cinfo_msg);
  }

  if (cloud_pub_.getNumSubscribers() > 0) {
    cloud_pub_.publish(ToCloud(image_msg, *cinfo_msg, config_.organized));
  }

  if (range_pub_.getNumSubscribers() > 0 ||
      intensity_pub_.getNumSubscribers() > 0) {
    // Publish range and intensity separately
    cv::Mat sep[3];
    cv::split(image_, sep);

    cv::Mat range = sep[RANGE];
    // should be 2, use 3 for more contrast
    range.convertTo(range, CV_8UC1, 3.0);
    range_pub_.publish(
        cv_bridge::CvImage(header, image_encodings::MONO8, range).toImageMsg());

    // use 300 for more contrast
    cv::Mat intensity = sep[INTENSITY];
    double a, b;
    cv::minMaxIdx(intensity, &a, &b);
    intensity.convertTo(intensity, CV_8UC1, 255 / (b - a), 255 * a / (a - b));
    intensity_pub_.publish(
        cv_bridge::CvImage(header, image_encodings::MONO8, intensity)
            .toImageMsg());
  }

  // Don't forget to reset
  Reset();
  ROS_DEBUG("Time: %f", (ros::Time::now() - start).toSec());
}

void Decoder::ConfigCb(VelodynePuckConfig& config, int level) {
  config.min_range = std::min(config.min_range, config.max_range);

  if (config.full_sweep) {
    config.full_sweep = false;
    ROS_WARN("Full sweep mode not supported. Use the following image width");
    ROS_WARN("5 rpm - 3600, 10 rpm - 1800, 20 rpm - 900");
  }

  config.image_width /= kSequencesPerPacket;
  config.image_width *= kSequencesPerPacket;

  ROS_INFO(
      "Reconfigure Request: min_range: %f, max_range: %f, image_width: %d, "
      "organized: %s, full_sweep: %s",
      config.min_range, config.max_range, config.image_width,
      config.organized ? "True" : "False",
      config.full_sweep ? "True" : "False");

  config_ = config;
  Reset();

  if (level < 0) {
    ROS_INFO("Initialize ROS subscriber/publisher...");
    camera_pub_ = it_.advertiseCamera("image", 10);
    cloud_pub_ = pnh_.advertise<PointCloud2>("cloud", 10);
    intensity_pub_ = it_.advertise("intensity", 1);
    range_pub_ = it_.advertise("range", 1);

    packet_sub_ =
        pnh_.subscribe<VelodynePacket>("packet", 256, &Decoder::PacketCb, this);
    ROS_INFO("Decoder initialized");
  }
}

void Decoder::Reset() {
  curr_col_ = 0;
  image_ = cv::Mat(kFiringsPerSequence, config_.image_width, CV_32FC3,
                   cv::Scalar(kNaNF));
  azimuths_.clear();
  azimuths_.resize(config_.image_width, kNaND);
  timestamps_.clear();
  timestamps_.resize(config_.image_width, 0);
}

CloudT ToCloud(const ImageConstPtr& image_msg, const CameraInfo& cinfo_msg,
               bool organized) {
  CloudT cloud;
  const auto image = cv_bridge::toCvShare(image_msg)->image;
  const auto& elevations = cinfo_msg.D;  // might be unsafe

  cloud.header = pcl_conversions::toPCL(image_msg->header);
  cloud.reserve(image.total());

  for (int r = 0; r < image.rows; ++r) {
    const auto* const row_ptr = image.ptr<cv::Vec3f>(r);
    // Because image row 0 is the highest laser point
    const auto phi = elevations[r];
    const auto cos_phi = std::cos(phi);
    const auto sin_phi = std::sin(phi);

    for (int c = 0; c < image.cols; ++c) {
      const cv::Vec3f& data = row_ptr[c];

      PointT p;
      if (std::isnan(data[RANGE])) {
        if (organized) {
          p.x = p.y = p.z = p.intensity = kNaNF;
          cloud.points.push_back(p);
        }
      } else {
        const auto d = data[RANGE];
        const auto theta = data[AZIMUTH];
        const auto x = d * cos_phi * std::cos(theta);
        const auto y = d * cos_phi * std::sin(theta);
        const auto z = d * sin_phi;

        p.x = x;
        p.y = -y;
        p.z = z;
        p.intensity = data[INTENSITY];

        cloud.points.push_back(p);
      }
    }
  }

  if (organized) {
    cloud.width = image.cols;
    cloud.height = image.rows;
  } else {
    cloud.width = cloud.size();
    cloud.height = 1;
  }

  return cloud;
}

}  // namespace velodyne_puck

int main(int argc, char** argv) {
  ros::init(argc, argv, "velodyne_puck_decoder");
  ros::NodeHandle pnh("~");

  velodyne_puck::Decoder node(pnh);
  ros::spin();
}
<commit_msg>ahh, a bug on timestamps and azimuths<commit_after>#include "constants.h"

#include <cv_bridge/cv_bridge.h>
#include <dynamic_reconfigure/server.h>
#include <image_transport/camera_publisher.h>
#include <image_transport/image_transport.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>

#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <velodyne_msgs/VelodynePacket.h>
#include <velodyne_msgs/VelodyneScan.h>
#include <velodyne_puck/VelodynePuckConfig.h>

namespace velodyne_puck {

using namespace sensor_msgs;
using namespace velodyne_msgs;

using PointT = pcl::PointXYZI;
using CloudT = pcl::PointCloud<PointT>;

/// Convert image and camera_info to point cloud
CloudT ToCloud(const ImageConstPtr& image_msg, const CameraInfo& cinfo_msg,
               bool organized);

/// Used for indexing into packet and image, (NOISE not used now)
enum Index { RANGE = 0, INTENSITY = 1, AZIMUTH = 2, NOISE = 3 };

class Decoder {
 public:
  /// Number of channels for image data
  static constexpr int kChannels = 2;  // (range [m], intensity)

  explicit Decoder(const ros::NodeHandle& pnh);

  Decoder(const Decoder&) = delete;
  Decoder operator=(const Decoder&) = delete;

  void PacketCb(const VelodynePacketConstPtr& packet_msg);
  void ConfigCb(VelodynePuckConfig& config, int level);

 private:
  /// All of these uses laser index from velodyne which is interleaved

  /// 9.3.1.3 Data Point
  /// A data point is a measurement by one laser channel of a relection of a
  /// laser pulse
  struct DataPoint {
    uint16_t distance;
    uint8_t reflectivity;
  } __attribute__((packed));
  static_assert(sizeof(DataPoint) == 3, "sizeof(DataPoint) != 3");

  /// 9.3.1.1 Firing Sequence
  /// A firing sequence occurs when all the lasers in a sensor are fired. There
  /// are 16 firings per cycle for VLP-16
  struct FiringSequence {
    DataPoint points[kFiringsPerSequence];  // 16
  } __attribute__((packed));
  static_assert(sizeof(FiringSequence) == 48, "sizeof(FiringSequence) != 48");

  /// 9.3.1.4 Azimuth
  /// A two-byte azimuth value (alpha) appears after the flag bytes at the
  /// beginning of each data block
  ///
  /// 9.3.1.5 Data Block
  /// The information from 2 firing sequences of 16 lasers is contained in each
  /// data block. Each packet contains the data from 24 firing sequences in 12
  /// data blocks.
  struct DataBlock {
    uint16_t flag;
    uint16_t azimuth;                              // [0, 35999]
    FiringSequence sequences[kSequencesPerBlock];  // 2
  } __attribute__((packed));
  static_assert(sizeof(DataBlock) == 100, "sizeof(DataBlock) != 100");

  struct Packet {
    DataBlock blocks[kBlocksPerPacket];  // 12
    /// The four-byte time stamp is a 32-bit unsigned integer marking the moment
    /// of the first data point in the first firing sequcne of the first data
    /// block. The time stamp’s value is the number of microseconds elapsed
    /// since the top of the hour.
    uint32_t stamp;
    uint8_t factory[2];
  } __attribute__((packed));
  static_assert(sizeof(Packet) == sizeof(velodyne_msgs::VelodynePacket().data),
                "sizeof(Packet) != 1206");

  void DecodeAndFill(const Packet* const packet_buf, uint64_t time);

 private:
  bool CheckFactoryBytes(const Packet* const packet);
  void Reset();

  // ROS related parameters
  std::string frame_id_;
  ros::NodeHandle pnh_;
  image_transport::ImageTransport it_;
  ros::Subscriber packet_sub_;
  ros::Publisher cloud_pub_;
  image_transport::Publisher intensity_pub_, range_pub_;
  image_transport::CameraPublisher camera_pub_;
  dynamic_reconfigure::Server<VelodynePuckConfig> cfg_server_;
  VelodynePuckConfig config_;

  // cached
  cv::Mat image_;
  std::vector<double> azimuths_;
  std::vector<uint64_t> timestamps_;
  int curr_col_{0};
  std::vector<double> elevations_;
};

Decoder::Decoder(const ros::NodeHandle& pnh)
    : pnh_(pnh), it_(pnh), cfg_server_(pnh) {
  pnh_.param<std::string>("frame_id", frame_id_, "velodyne");
  ROS_INFO("Velodyne frame_id: %s", frame_id_.c_str());
  cfg_server_.setCallback(boost::bind(&Decoder::ConfigCb, this, _1, _2));

  // Pre-compute elevations
  for (int i = 0; i < kFiringsPerSequence; ++i) {
    elevations_.push_back(kMaxElevation - i * kDeltaElevation);
  }
}

bool Decoder::CheckFactoryBytes(const Packet* const packet_buf) {
  // Check return mode and product id, for now just die
  const auto return_mode = packet_buf->factory[0];

  if (!(return_mode == 55 || return_mode == 56)) {
    ROS_ERROR(
        "return mode must be Strongest (55) or Last Return (56), "
        "instead got (%u)",
        return_mode);
    return false;
  }

  const auto product_id = packet_buf->factory[1];
  if (product_id != 34) {
    ROS_ERROR("product id must be VLP-16 or Puck Lite (34), instead got (%u)",
              product_id);
    return false;
  }

  return true;
}

void Decoder::DecodeAndFill(const Packet* const packet_buf, uint64_t time) {
  if (!CheckFactoryBytes(packet_buf)) {
    ros::shutdown();
  }

  // transform
  //            ^ x_l
  //            | -> /
  //            | a /
  //            |  /
  //            | /
  // <----------o
  // y_l

  // For each data block, 12 total
  for (int iblk = 0; iblk < kBlocksPerPacket; ++iblk) {
    const auto& block = packet_buf->blocks[iblk];
    const auto raw_azimuth = block.azimuth;         // nominal azimuth [0,35999]
    const auto azimuth = Raw2Azimuth(raw_azimuth);  // nominal azimuth [0, 2pi)

    ROS_WARN_STREAM_COND(raw_azimuth > kMaxRawAzimuth,
                         "Invalid raw azimuth: " << raw_azimuth);
    ROS_WARN_COND(block.flag != UPPER_BANK, "Invalid block %d", iblk);

    float azimuth_gap{0};
    if (iblk == kBlocksPerPacket - 1) {
      // Last block, 12th
      const auto& prev_block = packet_buf->blocks[iblk - 1];
      const auto prev_azimuth = Raw2Azimuth(prev_block.azimuth);
      azimuth_gap = azimuth - prev_azimuth;
    } else {
      // First 11 blocks
      const auto& next_block = packet_buf->blocks[iblk + 1];
      const auto next_azimuth = Raw2Azimuth(next_block.azimuth);
      azimuth_gap = next_azimuth - azimuth;
    }

    // Adjust for azimuth rollover from 2pi to 0
    if (azimuth_gap < 0) azimuth_gap += kTau;
    const auto half_azimuth_gap = azimuth_gap / 2;

    // for each firing sequence in the data block, 2
    for (int iseq = 0; iseq < kSequencesPerBlock; ++iseq, ++curr_col_) {
      const auto col = iblk * 2 + iseq;
      timestamps_[curr_col_] = time + col * kFiringCycleNs;
      azimuths_[curr_col_] = azimuth + half_azimuth_gap * iseq;

      const auto& seq = block.sequences[iseq];
      // for each laser beam, 16
      for (int lid = 0; lid < kFiringsPerSequence; ++lid) {
        const auto& point = seq.points[lid];
        auto& v = image_.at<cv::Vec3f>(LaserId2Row(lid), curr_col_);
        v[RANGE] = point.distance * kDistanceResolution;
        v[INTENSITY] = point.reflectivity;
        const auto offset = lid * kSingleFiringRatio * half_azimuth_gap +
                            iseq * half_azimuth_gap;
        v[AZIMUTH] = azimuth + offset;
      }
    }
  }
}

void Decoder::PacketCb(const VelodynePacketConstPtr& packet_msg) {
  const auto start = ros::Time::now();

  const auto* packet_buf =
      reinterpret_cast<const Packet*>(&(packet_msg->data[0]));
  DecodeAndFill(packet_buf, packet_msg->stamp.toNSec());

  if (curr_col_ < config_.image_width) {
    return;
  }

  std_msgs::Header header;
  header.frame_id = frame_id_;
  header.stamp.fromNSec(timestamps_.front());

  const ImagePtr image_msg =
      cv_bridge::CvImage(header, "32FC3", image_).toImageMsg();

  // Fill in camera info
  const CameraInfoPtr cinfo_msg(new CameraInfo);
  cinfo_msg->header = header;
  cinfo_msg->height = image_msg->height;
  cinfo_msg->width = image_msg->width;
  cinfo_msg->distortion_model = "VLP16";
  cinfo_msg->P[0] = kFiringCycleNs;  // delta time between two measurements

  // D = [altitude, azimuth]
  cinfo_msg->D = elevations_;
  cinfo_msg->D.insert(cinfo_msg->D.end(), azimuths_.begin(), azimuths_.end());

  // Publish on demand
  if (camera_pub_.getNumSubscribers() > 0) {
    camera_pub_.publish(image_msg, cinfo_msg);
  }

  if (cloud_pub_.getNumSubscribers() > 0) {
    cloud_pub_.publish(ToCloud(image_msg, *cinfo_msg, config_.organized));
  }

  if (range_pub_.getNumSubscribers() > 0 ||
      intensity_pub_.getNumSubscribers() > 0) {
    // Publish range and intensity separately
    cv::Mat sep[3];
    cv::split(image_, sep);

    cv::Mat range = sep[RANGE];
    // should be 2, use 3 for more contrast
    range.convertTo(range, CV_8UC1, 3.0);
    range_pub_.publish(
        cv_bridge::CvImage(header, image_encodings::MONO8, range).toImageMsg());

    // use 300 for more contrast
    cv::Mat intensity = sep[INTENSITY];
    double a, b;
    cv::minMaxIdx(intensity, &a, &b);
    intensity.convertTo(intensity, CV_8UC1, 255 / (b - a), 255 * a / (a - b));
    intensity_pub_.publish(
        cv_bridge::CvImage(header, image_encodings::MONO8, intensity)
            .toImageMsg());
  }

  // Don't forget to reset
  Reset();
  ROS_DEBUG("Time: %f", (ros::Time::now() - start).toSec());
}

void Decoder::ConfigCb(VelodynePuckConfig& config, int level) {
  config.min_range = std::min(config.min_range, config.max_range);

  if (config.full_sweep) {
    config.full_sweep = false;
    ROS_WARN("Full sweep mode not supported. Use the following image width");
    ROS_WARN("5 rpm - 3600, 10 rpm - 1800, 20 rpm - 900");
  }

  config.image_width /= kSequencesPerPacket;
  config.image_width *= kSequencesPerPacket;

  ROS_INFO(
      "Reconfigure Request: min_range: %f, max_range: %f, image_width: %d, "
      "organized: %s, full_sweep: %s",
      config.min_range, config.max_range, config.image_width,
      config.organized ? "True" : "False",
      config.full_sweep ? "True" : "False");

  config_ = config;
  Reset();

  if (level < 0) {
    ROS_INFO("Initialize ROS subscriber/publisher...");
    camera_pub_ = it_.advertiseCamera("image", 10);
    cloud_pub_ = pnh_.advertise<PointCloud2>("cloud", 10);
    intensity_pub_ = it_.advertise("intensity", 1);
    range_pub_ = it_.advertise("range", 1);

    packet_sub_ =
        pnh_.subscribe<VelodynePacket>("packet", 256, &Decoder::PacketCb, this);
    ROS_INFO("Decoder initialized");
  }
}

void Decoder::Reset() {
  curr_col_ = 0;
  image_ = cv::Mat(kFiringsPerSequence, config_.image_width, CV_32FC3,
                   cv::Scalar(kNaNF));
  azimuths_.clear();
  azimuths_.resize(config_.image_width, kNaND);
  timestamps_.clear();
  timestamps_.resize(config_.image_width, 0);
}

CloudT ToCloud(const ImageConstPtr& image_msg, const CameraInfo& cinfo_msg,
               bool organized) {
  CloudT cloud;
  const auto image = cv_bridge::toCvShare(image_msg)->image;
  const auto& elevations = cinfo_msg.D;  // might be unsafe

  cloud.header = pcl_conversions::toPCL(image_msg->header);
  cloud.reserve(image.total());

  for (int r = 0; r < image.rows; ++r) {
    const auto* const row_ptr = image.ptr<cv::Vec3f>(r);
    // Because image row 0 is the highest laser point
    const auto phi = elevations[r];
    const auto cos_phi = std::cos(phi);
    const auto sin_phi = std::sin(phi);

    for (int c = 0; c < image.cols; ++c) {
      const cv::Vec3f& data = row_ptr[c];

      PointT p;
      if (std::isnan(data[RANGE])) {
        if (organized) {
          p.x = p.y = p.z = p.intensity = kNaNF;
          cloud.points.push_back(p);
        }
      } else {
        const auto d = data[RANGE];
        const auto theta = data[AZIMUTH];
        const auto x = d * cos_phi * std::cos(theta);
        const auto y = d * cos_phi * std::sin(theta);
        const auto z = d * sin_phi;

        p.x = x;
        p.y = -y;
        p.z = z;
        p.intensity = data[INTENSITY];

        cloud.points.push_back(p);
      }
    }
  }

  if (organized) {
    cloud.width = image.cols;
    cloud.height = image.rows;
  } else {
    cloud.width = cloud.size();
    cloud.height = 1;
  }

  return cloud;
}

}  // namespace velodyne_puck

int main(int argc, char** argv) {
  ros::init(argc, argv, "velodyne_puck_decoder");
  ros::NodeHandle pnh("~");

  velodyne_puck::Decoder node(pnh);
  ros::spin();
}
<|endoftext|>