<commit_before>/* This file is part of Strigi Desktop Search
 *
 * Copyright (C) 2006 Jos van den Oever <jos@vandenoever.info>
 *
 * 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., 51 Franklin Street, Fifth Floor,
 * Boston, MA 02110-1301, USA.
 */
#include "jstreamsconfig.h"
#include "rpminputstream.h"
#include "cpioinputstream.h"
#include "gzipinputstream.h"
#include "bz2inputstream.h"
#include "subinputstream.h"

#include <list>

using namespace jstreams;
using namespace std;

class RpmInputStream::RpmHeaderInfo {
};
/**
 * RPM files as specified here:
 * http://www.freestandards.org/spec/refspecs/LSB_1.3.0/gLSB/gLSB/swinstall.html
 * and here:
 * http://www.rpm.org/max-rpm-snapshot/s1-rpm-file-format-rpm-file-format.html
 **/
bool
RpmInputStream::checkHeader(const char* data, int32_t datasize) {
    static const char unsigned magic[] = {0xed,0xab,0xee,0xdb,0x03,0x00};
    if (datasize < 6) return false;
    // this check could be more strict
    bool ok = memcmp(data, magic, 6) == 0;
    return ok;
}
RpmInputStream::RpmInputStream(StreamBase<char>* input)
        : SubStreamProvider(input), headerinfo(0) {
    uncompressionStream = 0;
    cpio = 0;

    // skip the header
    const char* b;
    // lead:96 bytes
    if (input->read(b, 96, 96) != 96) {
        status = Error;
        error = "File is too small to be an RPM file.";
        return;
    }

    // read signature
    const unsigned char headmagic[] = {0x8e,0xad,0xe8,0x01};
    if (input->read(b, 16, 16) != 16 || memcmp(b, headmagic, 4)!=0) {
        error = "error in signature\n";
        status = Error;
        return;
    }
    int32_t nindex = read4bytes((const unsigned char*)(b+8));
    int32_t hsize = read4bytes((const unsigned char*)(b+12));
    int32_t sz = nindex*16+hsize;
    if (sz%8) {
        sz+=8-sz%8;
    }
    input->read(b, sz, sz);

    // read header
    if (input->read(b, 16, 16) != 16 || memcmp(b, headmagic, 4)!=0) {
        error = "error in header\n";
        status = Error;
        return;
    }
    nindex = read4bytes((const unsigned char*)(b+8));
    hsize = read4bytes((const unsigned char*)(b+12));
    int32_t size = nindex*16+hsize;
    if (input->read(b, size, size) != size) {
        error = "could not read header\n";
        status = Error;
        return;
    }
    for (int32_t i=0; i<nindex; ++i) {
        const unsigned char* e = (const unsigned char*)b+i*16;
        int32_t tag = read4bytes(e);
        int32_t type = read4bytes(e+4);
        int32_t offset = read4bytes(e+8);
        if (offset < 0 || offset >= hsize) {
            // error: invalid offset
        }
        int32_t count = read4bytes(e+12);
        int32_t end = hsize;
        if (i < nindex-1) {
            end = read4bytes(e+8+16);
        }
        if (end < offset) end = offset;
        if (end > hsize) end = hsize;
// TODO actually put the data into the objects so the analyzers can use them
/*        if (type == 6) {
            string s(b+nindex*16+offset, end-offset);
            printf("%s\n", s.c_str());
        } else if (type == 8 || type == 9) {
            list<string> v;
            // TODO handle string arrays
        }
        printf("%i\t%i\t%i\t%i\n", tag, type, offset, count);*/
    }

    int64_t pos = input->getPosition();
    if (input->read(b, 16, 16) != 16) {
        error =  "could not read payload\n";
        status = Error;
        return;
    }
    input->reset(pos);
    if (BZ2InputStream::checkHeader(b, 16)) {
        uncompressionStream = new BZ2InputStream(input);
    } else {
        uncompressionStream = new GZipInputStream(input);
    }
    if (uncompressionStream->getStatus() == Error) {
        error = uncompressionStream->getError();
        status = Error;
        return;
    }
    cpio = new CpioInputStream(uncompressionStream);
    status = cpio->getStatus();
}
RpmInputStream::~RpmInputStream() {
    if (uncompressionStream) {
        delete uncompressionStream;
    }
    if (cpio) {
        delete cpio;
    }
    if (headerinfo) {
        delete headerinfo;
    }
}
StreamBase<char>*
RpmInputStream::nextEntry() {
    if (status) return 0;
    StreamBase<char>* entry = cpio->nextEntry();
    status = cpio->getStatus();
    if (status == Error) {
        error = cpio->getError();
    }
    return entry;
}
int32_t
RpmInputStream::read4bytes(const unsigned char *b) {
    return (b[0]<<24) + (b[1]<<16) + (b[2]<<8) + b[3];
}
<commit_msg>Fix regression caused by split of rpm and cpio streams.<commit_after>/* This file is part of Strigi Desktop Search
 *
 * Copyright (C) 2006 Jos van den Oever <jos@vandenoever.info>
 *
 * 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., 51 Franklin Street, Fifth Floor,
 * Boston, MA 02110-1301, USA.
 */
#include "jstreamsconfig.h"
#include "rpminputstream.h"
#include "cpioinputstream.h"
#include "gzipinputstream.h"
#include "bz2inputstream.h"
#include "subinputstream.h"

#include <list>

using namespace jstreams;
using namespace std;

class RpmInputStream::RpmHeaderInfo {
};
/**
 * RPM files as specified here:
 * http://www.freestandards.org/spec/refspecs/LSB_1.3.0/gLSB/gLSB/swinstall.html
 * and here:
 * http://www.rpm.org/max-rpm-snapshot/s1-rpm-file-format-rpm-file-format.html
 **/
bool
RpmInputStream::checkHeader(const char* data, int32_t datasize) {
    static const char unsigned magic[] = {0xed,0xab,0xee,0xdb,0x03,0x00};
    if (datasize < 6) return false;
    // this check could be more strict
    bool ok = memcmp(data, magic, 6) == 0;
    return ok;
}
RpmInputStream::RpmInputStream(StreamBase<char>* input)
        : SubStreamProvider(input), headerinfo(0) {
    uncompressionStream = 0;
    cpio = 0;

    // skip the header
    const char* b;
    // lead:96 bytes
    if (input->read(b, 96, 96) != 96) {
        status = Error;
        error = "File is too small to be an RPM file.";
        return;
    }

    // read signature
    const unsigned char headmagic[] = {0x8e,0xad,0xe8,0x01};
    if (input->read(b, 16, 16) != 16 || memcmp(b, headmagic, 4)!=0) {
        error = "error in signature\n";
        status = Error;
        return;
    }
    int32_t nindex = read4bytes((const unsigned char*)(b+8));
    int32_t hsize = read4bytes((const unsigned char*)(b+12));
    int32_t sz = nindex*16+hsize;
    if (sz%8) {
        sz+=8-sz%8;
    }
    input->read(b, sz, sz);

    // read header
    if (input->read(b, 16, 16) != 16 || memcmp(b, headmagic, 4)!=0) {
        error = "error in header\n";
        status = Error;
        return;
    }
    nindex = read4bytes((const unsigned char*)(b+8));
    hsize = read4bytes((const unsigned char*)(b+12));
    int32_t size = nindex*16+hsize;
    if (input->read(b, size, size) != size) {
        error = "could not read header\n";
        status = Error;
        return;
    }
    for (int32_t i=0; i<nindex; ++i) {
        const unsigned char* e = (const unsigned char*)b+i*16;
        int32_t tag = read4bytes(e);
        int32_t type = read4bytes(e+4);
        int32_t offset = read4bytes(e+8);
        if (offset < 0 || offset >= hsize) {
            // error: invalid offset
        }
        int32_t count = read4bytes(e+12);
        int32_t end = hsize;
        if (i < nindex-1) {
            end = read4bytes(e+8+16);
        }
        if (end < offset) end = offset;
        if (end > hsize) end = hsize;
// TODO actually put the data into the objects so the analyzers can use them
/*        if (type == 6) {
            string s(b+nindex*16+offset, end-offset);
            printf("%s\n", s.c_str());
        } else if (type == 8 || type == 9) {
            list<string> v;
            // TODO handle string arrays
        }
        printf("%i\t%i\t%i\t%i\n", tag, type, offset, count);*/
    }

    int64_t pos = input->getPosition();
    if (input->read(b, 16, 16) != 16) {
        error =  "could not read payload\n";
        status = Error;
        return;
    }
    input->reset(pos);
    if (BZ2InputStream::checkHeader(b, 16)) {
        uncompressionStream = new BZ2InputStream(input);
    } else {
        uncompressionStream = new GZipInputStream(input);
    }
    if (uncompressionStream->getStatus() == Error) {
        error = uncompressionStream->getError();
        status = Error;
        return;
    }
    cpio = new CpioInputStream(uncompressionStream);
    status = cpio->getStatus();
}
RpmInputStream::~RpmInputStream() {
    if (uncompressionStream) {
        delete uncompressionStream;
    }
    if (cpio) {
        delete cpio;
    }
    if (headerinfo) {
        delete headerinfo;
    }
}
StreamBase<char>*
RpmInputStream::nextEntry() {
    if (status) return 0;
    StreamBase<char>* entry = cpio->nextEntry();
    status = cpio->getStatus();
    if (status == Ok) {
        entryinfo = cpio->getEntryInfo();
    } else if (status == Error) {
        error = cpio->getError();
    }
    return entry;
}
int32_t
RpmInputStream::read4bytes(const unsigned char *b) {
    return (b[0]<<24) + (b[1]<<16) + (b[2]<<8) + b[3];
}
<|endoftext|>


<commit_before>/*
 * This material contains unpublished, proprietary software of
 * Entropic Research Laboratory, Inc. Any reproduction, distribution,
 * or publication of this work must be authorized in writing by Entropic
 * Research Laboratory, Inc., and must bear the notice:
 *
 *    "Copyright (c) 1990-1996 Entropic Research Laboratory, Inc.
 *                   All rights reserved"
 *
 * The copyright notice above does not evidence any actual or intended
 * publication of this source code.
 *
 * Written by:  Derek Lin
 * Checked by:
 * Revised by:  David Talkin
 *
 * Brief description:  Estimates F0 using normalized cross correlation and
 *   dynamic programming.
 *
 */

#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <malloc.h>
#include <limits.h>
#include <stdexcept>
#include <sstream>
#include <vector>

#include "f0.h"

int	    debug_level = 0;

// ----------------------------------------
// Externs
extern "C" {
int init_dp_f0(double freq, F0_params *par, long *buffsize, long *sdstep);
int dp_f0(float *fdata, int buff_size, int sdstep, double freq, F0_params *par,
          float **f0p_pt, float **vuvp_pt, float **rms_speech_pt,
          float **acpkp_pt, int *vecsize, int last_time);
}

struct f0_params;

namespace GetF0 {

// EXCEPTIONS

#define CREATE_ERROR(_Name, _Base)                                   \
  class _Name : public _Base {                                       \
  public:                                                            \
    explicit _Name(const std::string &what_arg) : _Base(what_arg) {} \
  };

CREATE_ERROR(RuntimeError, std::runtime_error);
CREATE_ERROR(LogicError, std::logic_error);
CREATE_ERROR(ParameterError, RuntimeError);
CREATE_ERROR(ProcessingError, RuntimeError);

#undef CREATE_ERROR

#define THROW_ERROR(condition, exception, s) \
  do {                                       \
    if (condition) {                         \
      std::stringstream ss;                  \
      ss << s;                               \
      throw exception(ss.str());             \
    }                                        \
  } while (0);

class GetF0 {
public:

  typedef double SampleFrequency;
  typedef int DebugLevel;

  GetF0(SampleFrequency sampleFrequency, DebugLevel debugLevel = 0);

  void resetParameters();

  /// @brief Some consistency checks on parameter values. Throws
  /// ParameterError if there's something wrong.
  void checkParameters();

  void init();
  void run();

  // ----------------------------------------
  // Getters/setters

  float& paramCandThresh()   { return m_par.cand_thresh;    } // only correlation peaks above this are considered
  float& paramLagWeight()    { return m_par.lag_weight;     } // degree to which shorter lags are weighted
  float& paramFreqWeight()   { return m_par.freq_weight;    } // weighting given to F0 trajectory smoothness
  float& paramTransCost()    { return m_par.trans_cost;     } // fixed cost for a voicing-state transition
  float& paramTransAmp()     { return m_par.trans_amp;      } // amplitude-change-modulated VUV trans. cost
  float& paramTransSpec()    { return m_par.trans_spec;     } // spectral-change-modulated VUV trans. cost
  float& paramVoiceBias()    { return m_par.voice_bias;     } // fixed bias towards the voiced hypothesis
  float& paramDoubleCost()   { return m_par.double_cost;    } // cost for octave F0 jumps
  float& paramMeanF0()       { return m_par.mean_f0;        } // talker-specific mean F0 (Hz)
  float& paramMeanF0Weight() { return m_par.mean_f0_weight; } // weight to be given to deviations from mean F0
  float& paramMinF0()        { return m_par.min_f0;         } // min. F0 to search for (Hz)
  float& paramMaxF0()        { return m_par.max_f0;         } // max. F0 to search for (Hz)
  float& paramFrameStep()    { return m_par.frame_step;     } // inter-frame-interval (sec)
  float& paramWindDur()      { return m_par.wind_dur;       } // duration of correlation window (sec)
  int  & paramNCands()       { return m_par.n_cands;        } // max. # of F0 cands. to consider at each frame
  int  & paramConditioning() { return m_par.conditioning;   } // Specify optional signal pre-conditioning.

  SampleFrequency &sampleFrequency() { return m_sampleFrequency; };
  DebugLevel &debugLevel() { return m_debugLevel; };

protected:

  /// @brief Provide a `buffer` we can read `num_records` samples
  /// from, returning how many samples we can read. Returning less
  /// than requested samples is a termination condition.
  ///
  /// `buffer` is not guaranteed to not be written to. (TODO: check to
  /// see if buffer can be written to.)
  virtual long read_samples(float **buffer, long num_records) { return 0; }

  /// @brief Like `read_samples`, but read `step` samples from
  /// previous buffer.
  virtual long read_samples_overlap(float **buffer, long num_records, long step)
  {
    return 0;
  }

  virtual void writeOutput(float *f0p, float *vuvp, float *rms_speech,
                           float *acpkp, int vecsize)
  {
  }

private:

  f0_params m_par;
  SampleFrequency m_sampleFrequency;
  DebugLevel m_debugLevel;

  bool m_initialized;

  long m_buffSize;
  long m_sdstep;

};

GetF0::GetF0(SampleFrequency sampleFrequency, DebugLevel debugLevel)
    : m_sampleFrequency(sampleFrequency),
      m_debugLevel(debugLevel),
      m_initialized(false),
      m_buffSize(0),
      m_sdstep(0)
{
  resetParameters();
}

void GetF0::resetParameters()
{
  m_par.cand_thresh = 0.3;
  m_par.lag_weight = 0.3;
  m_par.freq_weight = 0.02;
  m_par.trans_cost = 0.005;
  m_par.trans_amp = 0.5;
  m_par.trans_spec = 0.5;
  m_par.voice_bias = 0.0;
  m_par.double_cost = 0.35;
  m_par.min_f0 = 50;
  m_par.max_f0 = 550;
  m_par.frame_step = 0.01;
  m_par.wind_dur = 0.0075;
  m_par.n_cands = 20;
  m_par.mean_f0 = 200;        /* unused */
  m_par.mean_f0_weight = 0.0; /* unused */
  m_par.conditioning = 0;     /*unused */
}

void GetF0::init()
{
  checkParameters();

  /*SW: Removed range restricter, but this may be interesting:
    if (total_samps < ((par->frame_step * 2.0) + par->wind_dur) * sf), then
      input range too small*/

  // double output_starts = m_par.wind_dur/2.0;
  /* Average delay due to loc. of ref. window center. */
  //   SW: I think this is the time delay until output actually
  //       starts. In other words, we'll have some dropped frames.

  // double frame_rate = 1.0 / m_par.frame_step;

  /* Initialize variables in get_f0.c; allocate data structures;
   * determine length and overlap of input frames to read.
   *
   * sw: Looks like init_dp_f0 never returns errors via rcode, but put
   * under assertion.
   */
  THROW_ERROR(init_dp_f0(m_sampleFrequency, &m_par, &m_buffSize, &m_sdstep) ||
                  m_buffSize > INT_MAX || m_sdstep > INT_MAX,
              LogicError, "problem in init_dp_f0().");

  m_initialized = true;
}

void GetF0::run()
{
  THROW_ERROR(!m_initialized, LogicError, "Not initialized");

  float* fdata = nullptr;
  float *f0p, *vuvp, *rms_speech, *acpkp;
  int done;
  int i, vecsize;

  long actsize = read_samples(&fdata, m_buffSize);

  while (true) {

    done = (actsize < m_buffSize);

    THROW_ERROR(dp_f0(fdata, (int)actsize, (int)m_sdstep, m_sampleFrequency,
                      &m_par, &f0p, &vuvp, &rms_speech, &acpkp, &vecsize, done),
                ProcessingError, "problem in dp_f0().");

    writeOutput(f0p, vuvp, rms_speech, acpkp, vecsize);

    if (done)
      break;

    actsize = read_samples_overlap(&fdata, m_buffSize, m_sdstep);

  }
}

void GetF0::checkParameters()
{
  std::vector<std::string> errors;

  if ((m_par.cand_thresh < 0.01) || (m_par.cand_thresh > 0.99)) {
    errors.push_back("cand_thresh parameter must be between [0.01, 0.99].");
  }
  if ((m_par.wind_dur > .1) || (m_par.wind_dur < .0001)) {
    errors.push_back("wind_dur parameter must be between [0.0001, 0.1].");
  }
  if ((m_par.n_cands > 100) || (m_par.n_cands < 3)) {
    errors.push_back("n_cands parameter must be between [3,100].");
  }
  if ((m_par.max_f0 <= m_par.min_f0) ||
      (m_par.max_f0 >= (m_sampleFrequency / 2.0)) ||
      (m_par.min_f0 < (m_sampleFrequency / 10000.0))) {
    errors.push_back(
        "min(max)_f0 parameter inconsistent with sampling frequency.");
  }
  double dstep =
      ((double)((int)(0.5 + (m_sampleFrequency * m_par.frame_step)))) /
      m_sampleFrequency;
  if (dstep != m_par.frame_step) {
    if (debug_level)
      Fprintf(stderr,
              "Frame step set to %f to exactly match signal sample rate.\n",
              dstep);
    m_par.frame_step = dstep;
  }
  if ((m_par.frame_step > 0.1) ||
      (m_par.frame_step < (1.0 / m_sampleFrequency))) {
    errors.push_back(
        "frame_step parameter must be between [1/sampling rate, "
        "0.1].");
  }

  if (!errors.empty()) {
    std::stringstream ss;
    bool first = true;
    for (auto &error : errors) {
      if (!first) ss << " ";
      ss << error;
    }

    THROW_ERROR(true, ParameterError, ss.str());
  }
}

} // namespace GetF0
<commit_msg>StreamBufferSize and StreamOverlapSize<commit_after>/*
 * This material contains unpublished, proprietary software of
 * Entropic Research Laboratory, Inc. Any reproduction, distribution,
 * or publication of this work must be authorized in writing by Entropic
 * Research Laboratory, Inc., and must bear the notice:
 *
 *    "Copyright (c) 1990-1996 Entropic Research Laboratory, Inc.
 *                   All rights reserved"
 *
 * The copyright notice above does not evidence any actual or intended
 * publication of this source code.
 *
 * Written by:  Derek Lin
 * Checked by:
 * Revised by:  David Talkin
 *
 * Brief description:  Estimates F0 using normalized cross correlation and
 *   dynamic programming.
 *
 */

#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <malloc.h>
#include <limits.h>
#include <stdexcept>
#include <sstream>
#include <vector>

#include "f0.h"

int	    debug_level = 0;

// ----------------------------------------
// Externs
extern "C" {
int init_dp_f0(double freq, F0_params *par, long *buffsize, long *sdstep);
int dp_f0(float *fdata, int buff_size, int sdstep, double freq, F0_params *par,
          float **f0p_pt, float **vuvp_pt, float **rms_speech_pt,
          float **acpkp_pt, int *vecsize, int last_time);
}

struct f0_params;

namespace GetF0 {

// EXCEPTIONS

#define CREATE_ERROR(_Name, _Base)                                   \
  class _Name : public _Base {                                       \
  public:                                                            \
    explicit _Name(const std::string &what_arg) : _Base(what_arg) {} \
  };

CREATE_ERROR(RuntimeError, std::runtime_error);
CREATE_ERROR(LogicError, std::logic_error);
CREATE_ERROR(ParameterError, RuntimeError);
CREATE_ERROR(ProcessingError, RuntimeError);

#undef CREATE_ERROR

#define THROW_ERROR(condition, exception, s) \
  do {                                       \
    if (condition) {                         \
      std::stringstream ss;                  \
      ss << s;                               \
      throw exception(ss.str());             \
    }                                        \
  } while (0);

class GetF0 {
public:

  typedef double SampleFrequency;
  typedef int DebugLevel;

  GetF0(SampleFrequency sampleFrequency, DebugLevel debugLevel = 0);

  void resetParameters();

  /// @brief Some consistency checks on parameter values. Throws
  /// ParameterError if there's something wrong.
  void checkParameters();

  void init();
  void run();

  // ----------------------------------------
  // Calculations available after init
  long streamBufferSize() const;
  long streamOverlapSize() const;

  // ----------------------------------------
  // Getters/setters

  float& paramCandThresh()   { return m_par.cand_thresh;    } // only correlation peaks above this are considered
  float& paramLagWeight()    { return m_par.lag_weight;     } // degree to which shorter lags are weighted
  float& paramFreqWeight()   { return m_par.freq_weight;    } // weighting given to F0 trajectory smoothness
  float& paramTransCost()    { return m_par.trans_cost;     } // fixed cost for a voicing-state transition
  float& paramTransAmp()     { return m_par.trans_amp;      } // amplitude-change-modulated VUV trans. cost
  float& paramTransSpec()    { return m_par.trans_spec;     } // spectral-change-modulated VUV trans. cost
  float& paramVoiceBias()    { return m_par.voice_bias;     } // fixed bias towards the voiced hypothesis
  float& paramDoubleCost()   { return m_par.double_cost;    } // cost for octave F0 jumps
  float& paramMeanF0()       { return m_par.mean_f0;        } // talker-specific mean F0 (Hz)
  float& paramMeanF0Weight() { return m_par.mean_f0_weight; } // weight to be given to deviations from mean F0
  float& paramMinF0()        { return m_par.min_f0;         } // min. F0 to search for (Hz)
  float& paramMaxF0()        { return m_par.max_f0;         } // max. F0 to search for (Hz)
  float& paramFrameStep()    { return m_par.frame_step;     } // inter-frame-interval (sec)
  float& paramWindDur()      { return m_par.wind_dur;       } // duration of correlation window (sec)
  int  & paramNCands()       { return m_par.n_cands;        } // max. # of F0 cands. to consider at each frame
  int  & paramConditioning() { return m_par.conditioning;   } // Specify optional signal pre-conditioning.

  SampleFrequency &sampleFrequency() { return m_sampleFrequency; };
  DebugLevel &debugLevel() { return m_debugLevel; };

protected:

  /// @brief Provide a `buffer` we can read `num_records` samples
  /// from, returning how many samples we can read. Returning less
  /// than requested samples is a termination condition.
  ///
  /// `buffer` is not guaranteed to not be written to. (TODO: check to
  /// see if buffer can be written to.)
  virtual long read_samples(float **buffer, long num_records) { return 0; }

  /// @brief Like `read_samples`, but read `step` samples from
  /// previous buffer.
  virtual long read_samples_overlap(float **buffer, long num_records, long step)
  {
    return 0;
  }

  virtual void writeOutput(float *f0p, float *vuvp, float *rms_speech,
                           float *acpkp, int vecsize)
  {
  }

private:

  f0_params m_par;
  SampleFrequency m_sampleFrequency;
  DebugLevel m_debugLevel;

  bool m_initialized;

  long m_streamBufferSize;
  long m_streamOverlapSize;

};

GetF0::GetF0(SampleFrequency sampleFrequency, DebugLevel debugLevel)
    : m_sampleFrequency(sampleFrequency),
      m_debugLevel(debugLevel),
      m_initialized(false),
      m_streamBufferSize(0),
      m_streamOverlapSize(0)
{
  resetParameters();
}

void GetF0::resetParameters()
{
  m_par.cand_thresh = 0.3;
  m_par.lag_weight = 0.3;
  m_par.freq_weight = 0.02;
  m_par.trans_cost = 0.005;
  m_par.trans_amp = 0.5;
  m_par.trans_spec = 0.5;
  m_par.voice_bias = 0.0;
  m_par.double_cost = 0.35;
  m_par.min_f0 = 50;
  m_par.max_f0 = 550;
  m_par.frame_step = 0.01;
  m_par.wind_dur = 0.0075;
  m_par.n_cands = 20;
  m_par.mean_f0 = 200;        /* unused */
  m_par.mean_f0_weight = 0.0; /* unused */
  m_par.conditioning = 0;     /*unused */
}

void GetF0::init()
{
  checkParameters();

  /*SW: Removed range restricter, but this may be interesting:
    if (total_samps < ((par->frame_step * 2.0) + par->wind_dur) * sf), then
      input range too small*/

  // double output_starts = m_par.wind_dur/2.0;
  /* Average delay due to loc. of ref. window center. */
  //   SW: I think this is the time delay until output actually
  //       starts. In other words, we'll have some dropped frames.

  // double frame_rate = 1.0 / m_par.frame_step;

  /* Initialize variables in get_f0.c; allocate data structures;
   * determine length and overlap of input frames to read.
   *
   * sw: Looks like init_dp_f0 never returns errors via rcode, but put
   * under assertion.
   */
  THROW_ERROR(init_dp_f0(m_sampleFrequency, &m_par, &m_streamBufferSize,
                         &m_streamOverlapSize) ||
                  m_streamBufferSize > INT_MAX || m_streamOverlapSize > INT_MAX,
              LogicError, "problem in init_dp_f0().");

  m_initialized = true;
}

void GetF0::run()
{
  THROW_ERROR(!m_initialized, LogicError, "Not initialized");

  float *fdata = nullptr;
  float *f0p, *vuvp, *rms_speech, *acpkp;
  int done;
  int i, vecsize;

  long actsize = read_samples(&fdata, m_streamBufferSize);

  while (true) {
    done = (actsize < m_streamBufferSize);

    THROW_ERROR(
        dp_f0(fdata, (int)actsize, (int)m_streamOverlapSize, m_sampleFrequency,
              &m_par, &f0p, &vuvp, &rms_speech, &acpkp, &vecsize, done),
        ProcessingError, "problem in dp_f0().");

    writeOutput(f0p, vuvp, rms_speech, acpkp, vecsize);

    if (done) break;

    actsize =
        read_samples_overlap(&fdata, m_streamBufferSize, m_streamOverlapSize);
  }
}

void GetF0::checkParameters()
{
  std::vector<std::string> errors;

  if ((m_par.cand_thresh < 0.01) || (m_par.cand_thresh > 0.99)) {
    errors.push_back("cand_thresh parameter must be between [0.01, 0.99].");
  }
  if ((m_par.wind_dur > .1) || (m_par.wind_dur < .0001)) {
    errors.push_back("wind_dur parameter must be between [0.0001, 0.1].");
  }
  if ((m_par.n_cands > 100) || (m_par.n_cands < 3)) {
    errors.push_back("n_cands parameter must be between [3,100].");
  }
  if ((m_par.max_f0 <= m_par.min_f0) ||
      (m_par.max_f0 >= (m_sampleFrequency / 2.0)) ||
      (m_par.min_f0 < (m_sampleFrequency / 10000.0))) {
    errors.push_back(
        "min(max)_f0 parameter inconsistent with sampling frequency.");
  }
  double dstep =
      ((double)((int)(0.5 + (m_sampleFrequency * m_par.frame_step)))) /
      m_sampleFrequency;
  if (dstep != m_par.frame_step) {
    if (debug_level)
      Fprintf(stderr,
              "Frame step set to %f to exactly match signal sample rate.\n",
              dstep);
    m_par.frame_step = dstep;
  }
  if ((m_par.frame_step > 0.1) ||
      (m_par.frame_step < (1.0 / m_sampleFrequency))) {
    errors.push_back(
        "frame_step parameter must be between [1/sampling rate, "
        "0.1].");
  }

  if (!errors.empty()) {
    std::stringstream ss;
    bool first = true;
    for (auto &error : errors) {
      if (!first) ss << " ";
      ss << error;
    }

    THROW_ERROR(true, ParameterError, ss.str());
  }
}

long GetF0::streamBufferSize() const
{
  THROW_ERROR(!m_initialized, LogicError, "Not initialized");
  return m_streamBufferSize;
}

long GetF0::streamOverlapSize() const
{
  THROW_ERROR(!m_initialized, LogicError, "Not initialized");
  return m_streamOverlapSize;
}

} // namespace GetF0
<|endoftext|>


<commit_before>/*
 * Licensed to the Apache Software Foundation (ASF) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * The ASF licenses this file to You under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with
 * the License.  You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "URIPool.h"

#include <memory>
#include <decaf/util/Random.h>
#include <decaf/lang/System.h>

using namespace activemq;
using namespace activemq::transport;
using namespace activemq::transport::failover;
using namespace decaf;
using namespace decaf::net;
using namespace decaf::util;
using namespace decaf::lang;
using namespace decaf::lang::exceptions;

////////////////////////////////////////////////////////////////////////////////
URIPool::URIPool() : uriPool(), randomize( false ) {
}

////////////////////////////////////////////////////////////////////////////////
URIPool::URIPool(const decaf::util::List<URI>& uris) : uriPool(), randomize( false ) {
    this->uriPool.copy( uris );
}

////////////////////////////////////////////////////////////////////////////////
URIPool::~URIPool() {
}

////////////////////////////////////////////////////////////////////////////////
URI URIPool::getURI() {

    synchronized(&uriPool) {
        if( !uriPool.isEmpty() ) {

            int index = 0;  // Take the first one in the list unless random is on.

            if( isRandomize() ) {
                Random rand;
                rand.setSeed( decaf::lang::System::currentTimeMillis() );
                index = rand.nextInt( (int)uriPool.size() );
            }

            return uriPool.removeAt( index );
        }
    }

    throw NoSuchElementException(
        __FILE__, __LINE__, "URI Pool is currently empty." );
}

////////////////////////////////////////////////////////////////////////////////
bool URIPool::addURI(const URI& uri) {

    synchronized(&uriPool) {
        if (!uriPool.contains(uri)) {
            uriPool.add(uri);
            return true;
        }
    }

    return false;
}

////////////////////////////////////////////////////////////////////////////////
bool URIPool::addURIs(const LinkedList<URI>& uris) {

    bool result = false;

    synchronized(&uriPool) {
        std::auto_ptr<Iterator<URI> > iter(uris.iterator());

        while (iter->hasNext()) {
            URI uri = iter->next();

            if (!uriPool.contains(uri)) {
                uriPool.add(uri);
                result = true;
            }
        }
    }

    return result;
}

////////////////////////////////////////////////////////////////////////////////
bool URIPool::removeURI(const URI& uri) {

    synchronized(&uriPool) {
        if (uriPool.contains(uri)) {
            uriPool.remove(uri);
            return true;
        }
    }

    return false;
}

////////////////////////////////////////////////////////////////////////////////
bool URIPool::contains(const decaf::net::URI& uri) const {
    synchronized(&uriPool) {
        return uriPool.contains(uri);
    }
}

////////////////////////////////////////////////////////////////////////////////
bool URIPool::isPriority(const decaf::net::URI& uri) const {
    synchronized(&uriPool) {
        if (uriPool.isEmpty()) {
            return false;
        }

        return uriPool.getFirst().equals(uri);
    }
}

////////////////////////////////////////////////////////////////////////////////
void URIPool::clear() {
    synchronized(&uriPool) {
        this->uriPool.clear();
    }
}
<commit_msg>clear a couple warnings<commit_after>/*
 * Licensed to the Apache Software Foundation (ASF) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * The ASF licenses this file to You under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with
 * the License.  You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "URIPool.h"

#include <memory>
#include <decaf/util/Random.h>
#include <decaf/lang/System.h>

using namespace activemq;
using namespace activemq::transport;
using namespace activemq::transport::failover;
using namespace decaf;
using namespace decaf::net;
using namespace decaf::util;
using namespace decaf::lang;
using namespace decaf::lang::exceptions;

////////////////////////////////////////////////////////////////////////////////
URIPool::URIPool() : uriPool(), randomize( false ) {
}

////////////////////////////////////////////////////////////////////////////////
URIPool::URIPool(const decaf::util::List<URI>& uris) : uriPool(), randomize( false ) {
    this->uriPool.copy( uris );
}

////////////////////////////////////////////////////////////////////////////////
URIPool::~URIPool() {
}

////////////////////////////////////////////////////////////////////////////////
URI URIPool::getURI() {

    synchronized(&uriPool) {
        if( !uriPool.isEmpty() ) {

            int index = 0;  // Take the first one in the list unless random is on.

            if( isRandomize() ) {
                Random rand;
                rand.setSeed( decaf::lang::System::currentTimeMillis() );
                index = rand.nextInt( (int)uriPool.size() );
            }

            return uriPool.removeAt( index );
        }
    }

    throw NoSuchElementException(
        __FILE__, __LINE__, "URI Pool is currently empty." );
}

////////////////////////////////////////////////////////////////////////////////
bool URIPool::addURI(const URI& uri) {

    synchronized(&uriPool) {
        if (!uriPool.contains(uri)) {
            uriPool.add(uri);
            return true;
        }
    }

    return false;
}

////////////////////////////////////////////////////////////////////////////////
bool URIPool::addURIs(const LinkedList<URI>& uris) {

    bool result = false;

    synchronized(&uriPool) {
        std::auto_ptr<Iterator<URI> > iter(uris.iterator());

        while (iter->hasNext()) {
            URI uri = iter->next();

            if (!uriPool.contains(uri)) {
                uriPool.add(uri);
                result = true;
            }
        }
    }

    return result;
}

////////////////////////////////////////////////////////////////////////////////
bool URIPool::removeURI(const URI& uri) {

    synchronized(&uriPool) {
        if (uriPool.contains(uri)) {
            uriPool.remove(uri);
            return true;
        }
    }

    return false;
}

////////////////////////////////////////////////////////////////////////////////
bool URIPool::contains(const decaf::net::URI& uri) const {
    bool result = false;
    synchronized(&uriPool) {
        result = uriPool.contains(uri);
    }

    return result;
}

////////////////////////////////////////////////////////////////////////////////
bool URIPool::isPriority(const decaf::net::URI& uri) const {
    synchronized(&uriPool) {
        if (!uriPool.isEmpty()) {
            return uriPool.getFirst().equals(uri);
        }
    }
    return false;
}

////////////////////////////////////////////////////////////////////////////////
void URIPool::clear() {
    synchronized(&uriPool) {
        this->uriPool.clear();
    }
}
<|endoftext|>


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

    OscProcessor.cpp
    Created: 22 Jan 2015 10:56:59pm
    Author:  Daniel Lindenfelser

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

#include "OscProcessor.h"

OscProcessor::OscProcessor()
    : oscServer(this)
{
}

OscProcessor::~OscProcessor()
{
    managedOscParameters.clear();
}

void OscProcessor::handleOscMessage(osc::ReceivedPacket packet)
{
    parseOscPacket(packet);
}

void OscProcessor::changeListenerCallback(ChangeBroadcaster* source)
{
    OscParameter* parameter = static_cast<OscParameter*>(source);
    if (parameter) {
        char buffer[1024];
        osc::OutboundPacketStream packet(buffer, 1024);
        parameter->appendOscMessageToStream(packet);
        oscServer.sendMessage(packet);
    }
}

void OscProcessor::addOscParameter(OscParameter* parameter, bool internal)
{
    if (parameter) {
        managedOscParameters.addIfNotAlreadyThere(parameter);
        if (!internal) {
            //parameter->addChangeListener(this);
        }
    }
}

void OscProcessor::removeOscParameter(OscParameter* p)
{
    managedOscParameters.removeObject(p);
}

void OscProcessor::removeOscParameter(String regex)
{
    Array<OscParameter*> toRemove;
    for (int index = 0; index < managedOscParameters.size(); index++) {
        if (managedOscParameters[index]->addressMatch(regex)) {
            toRemove.add(managedOscParameters[index]);
        }
    }
    for (int index = 0; index < toRemove.size(); index++) {
        managedOscParameters.removeObject(toRemove[index]);
    }
}

Array<OscParameter*> OscProcessor::getAllOscParameter(String regex)
{
    Array<OscParameter*> parameters;
    for (int index = 0; index < managedOscParameters.size(); index++) {
        if (managedOscParameters[index]->addressMatch(regex)) {
            parameters.add(managedOscParameters[index]);
        }
    }
    return parameters;
}

OscParameter* OscProcessor::getOscParameter(String address)
{
    for (int index = 0; index < managedOscParameters.size(); index++) {
        if (managedOscParameters[index]->getAddress() == address)
        {
            return managedOscParameters[index];
        }
    }
    return nullptr;
}

Array<OscParameter*> OscProcessor::getAllOscParameter()
{
    Array<OscParameter*> parameters;
    for (int index = 0; index < managedOscParameters.size(); index++) {
        parameters.add(managedOscParameters[index]);
    }
    return parameters;
}

void OscProcessor::dumpOscParameters() {
    for (int index = 0; index < managedOscParameters.size(); index++) {
        char buffer[1024];
        osc::OutboundPacketStream packet(buffer, 1024);
        managedOscParameters[index]->appendOscMessageToStream(packet);
        oscServer.sendMessage(packet);
    }
}

var OscProcessor::getOscParameterValue(String address)
{
    for (int index = 0; index < managedOscParameters.size(); index++) {
        if (managedOscParameters[index]->getAddress() == address) {
            return var(managedOscParameters[index]->getValue());
        }
    }
    return var::null;
}

void OscProcessor::setOscParameterValue(String address, var value)
{
    for (int index = 0; index < managedOscParameters.size(); index++) {
        if (managedOscParameters[index]->getAddress() == address) {
            managedOscParameters[index]->setValue(value);
            return;
        }
    }
}

void OscProcessor::addOscParameterListener(OscParameterListener* listener, OscParameter* parameter)
{
    parameter->addOscParameterListener(listener);
}

void OscProcessor::addOscParameterListener(OscParameterListener* listener, String regex)
{
    auto parameters = getAllOscParameter(regex);
    for (int index = 0; index < parameters.size(); index++) {
        parameters[index]->addOscParameterListener(listener);
    }
}

void OscProcessor::removeOscParameterListener(OscParameterListener* listener)
{
    auto parameters = getAllOscParameter();
    for (int index = 0; index < parameters.size(); index++) {
        parameters[index]->removeOscParameterListener(listener);
    }
}

void OscProcessor::parseOscPacket(osc::ReceivedPacket packet)
{
    if (packet.Size()) {
        if (packet.IsBundle()) {
            osc::ReceivedBundle bundle(packet);
            parseOscBundle(bundle);
        }
        else {
            osc::ReceivedMessage message(packet);
            parseOscMessage(message);
        }
    }
}

void OscProcessor::parseOscMessage(osc::ReceivedMessage message)
{
    String address(message.AddressPattern());
    OscParameter* parameter = getOscParameter(address);
    if (parameter) {
        osc::ReceivedMessage::const_iterator arg = message.ArgumentsBegin();
        while (arg != message.ArgumentsEnd())
        {
            if (arg->IsFloat()) {
                parameter->setValue(var(arg->AsFloat()));
            }
            else if (arg->IsBool()) {
                parameter->setValue(var(arg->IsBool()));
            }
            else if (arg->IsInt32()) {
                parameter->setValue(var(arg->AsInt32()));
            }
            else if (arg->IsChar()) {
                parameter->setValue(var(String(arg->AsChar())));
            }
            else if (arg->IsDouble()) {
                parameter->setValue(var(arg->AsDouble()));
            }
            else if (arg->IsString()) {
                parameter->setValue(var(String(arg->AsString())));
            }
            else if (arg->IsSymbol()) {
                parameter->setValue(var(String(arg->IsSymbol())));
            }
            arg++;
        }
    }
}

void OscProcessor::parseOscBundle(osc::ReceivedBundle bundle)
{
    osc::ReceivedBundleElementIterator initiator = bundle.ElementsBegin();
    for (int i = 0; i < bundle.ElementCount(); i++) {
        initiator++;
        if (initiator->IsBundle()) {
            osc::ReceivedBundle bundle(*initiator);
            parseOscBundle(bundle);
        }
        else {
            osc::ReceivedMessage message(*initiator);
            parseOscMessage(message);
        }
    }
}

OscServer* OscProcessor::getOscServer()
{
    return &oscServer;
}
<commit_msg>enable changelistener<commit_after>/*
  ==============================================================================

    OscProcessor.cpp
    Created: 22 Jan 2015 10:56:59pm
    Author:  Daniel Lindenfelser

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

#include "OscProcessor.h"

OscProcessor::OscProcessor()
    : oscServer(this)
{
}

OscProcessor::~OscProcessor()
{
    managedOscParameters.clear();
}

void OscProcessor::handleOscMessage(osc::ReceivedPacket packet)
{
    parseOscPacket(packet);
}

void OscProcessor::changeListenerCallback(ChangeBroadcaster* source)
{
    OscParameter* parameter = static_cast<OscParameter*>(source);
    if (parameter) {
        char buffer[1024];
        osc::OutboundPacketStream packet(buffer, 1024);
        parameter->appendOscMessageToStream(packet);

        oscServer.sendMessage(packet);
    }
}

void OscProcessor::addOscParameter(OscParameter* parameter, bool internal)
{
    if (parameter) {
        managedOscParameters.addIfNotAlreadyThere(parameter);
        if (!internal) {
            parameter->addChangeListener(this);
        }
    }
}

void OscProcessor::removeOscParameter(OscParameter* p)
{
    managedOscParameters.removeObject(p);
}

void OscProcessor::removeOscParameter(String regex)
{
    Array<OscParameter*> toRemove;
    for (int index = 0; index < managedOscParameters.size(); index++) {
        if (managedOscParameters[index]->addressMatch(regex)) {
            toRemove.add(managedOscParameters[index]);
        }
    }
    for (int index = 0; index < toRemove.size(); index++) {
        managedOscParameters.removeObject(toRemove[index]);
    }
}

Array<OscParameter*> OscProcessor::getAllOscParameter(String regex)
{
    Array<OscParameter*> parameters;
    for (int index = 0; index < managedOscParameters.size(); index++) {
        if (managedOscParameters[index]->addressMatch(regex)) {
            parameters.add(managedOscParameters[index]);
        }
    }
    return parameters;
}

OscParameter* OscProcessor::getOscParameter(String address)
{
    for (int index = 0; index < managedOscParameters.size(); index++) {
        if (managedOscParameters[index]->getAddress() == address)
        {
            return managedOscParameters[index];
        }
    }
    return nullptr;
}

Array<OscParameter*> OscProcessor::getAllOscParameter()
{
    Array<OscParameter*> parameters;
    for (int index = 0; index < managedOscParameters.size(); index++) {
        parameters.add(managedOscParameters[index]);
    }
    return parameters;
}

void OscProcessor::dumpOscParameters() {
    for (int index = 0; index < managedOscParameters.size(); index++) {
        char buffer[1024];
        osc::OutboundPacketStream packet(buffer, 1024);
        managedOscParameters[index]->appendOscMessageToStream(packet);
        oscServer.sendMessage(packet);
    }
}

var OscProcessor::getOscParameterValue(String address)
{
    for (int index = 0; index < managedOscParameters.size(); index++) {
        if (managedOscParameters[index]->getAddress() == address) {
            return var(managedOscParameters[index]->getValue());
        }
    }
    return var::null;
}

void OscProcessor::setOscParameterValue(String address, var value)
{
    for (int index = 0; index < managedOscParameters.size(); index++) {
        if (managedOscParameters[index]->getAddress() == address) {
            managedOscParameters[index]->setValue(value);
            return;
        }
    }
}

void OscProcessor::addOscParameterListener(OscParameterListener* listener, OscParameter* parameter)
{
    parameter->addOscParameterListener(listener);
}

void OscProcessor::addOscParameterListener(OscParameterListener* listener, String regex)
{
    auto parameters = getAllOscParameter(regex);
    for (int index = 0; index < parameters.size(); index++) {
        parameters[index]->addOscParameterListener(listener);
    }
}

void OscProcessor::removeOscParameterListener(OscParameterListener* listener)
{
    auto parameters = getAllOscParameter();
    for (int index = 0; index < parameters.size(); index++) {
        parameters[index]->removeOscParameterListener(listener);
    }
}

void OscProcessor::parseOscPacket(osc::ReceivedPacket packet)
{
    if (packet.Size()) {
        if (packet.IsBundle()) {
            osc::ReceivedBundle bundle(packet);
            parseOscBundle(bundle);
        }
        else {
            osc::ReceivedMessage message(packet);
            parseOscMessage(message);
        }
    }
}

void OscProcessor::parseOscMessage(osc::ReceivedMessage message)
{
    String address(message.AddressPattern());
    OscParameter* parameter = getOscParameter(address);
    if (parameter) {
        osc::ReceivedMessage::const_iterator arg = message.ArgumentsBegin();
        while (arg != message.ArgumentsEnd())
        {
            if (arg->IsFloat()) {
                parameter->setValue(var(arg->AsFloat()));
            }
            else if (arg->IsBool()) {
                parameter->setValue(var(arg->IsBool()));
            }
            else if (arg->IsInt32()) {
                parameter->setValue(var(arg->AsInt32()));
            }
            else if (arg->IsChar()) {
                parameter->setValue(var(String(arg->AsChar())));
            }
            else if (arg->IsDouble()) {
                parameter->setValue(var(arg->AsDouble()));
            }
            else if (arg->IsString()) {
                parameter->setValue(var(String(arg->AsString())));
            }
            else if (arg->IsSymbol()) {
                parameter->setValue(var(String(arg->IsSymbol())));
            }
            arg++;
        }
    }
}

void OscProcessor::parseOscBundle(osc::ReceivedBundle bundle)
{
    osc::ReceivedBundleElementIterator initiator = bundle.ElementsBegin();
    for (int i = 0; i < bundle.ElementCount(); i++) {
        initiator++;
        if (initiator->IsBundle()) {
            osc::ReceivedBundle bundle(*initiator);
            parseOscBundle(bundle);
        }
        else {
            osc::ReceivedMessage message(*initiator);
            parseOscMessage(message);
        }
    }
}

OscServer* OscProcessor::getOscServer()
{
    return &oscServer;
}
<|endoftext|>


<commit_before>#include <stdio.h>
#include <inttypes.h>
#include <lcm/lcm.h>
#include <iostream>
#include <limits>

#include "joints2frames.hpp"
#include <ConciseArgs>

using namespace std;
using namespace boost;
using namespace boost::assign;

#define DO_TIMING_PROFILE FALSE

/////////////////////////////////////

joints2frames::joints2frames(boost::shared_ptr<lcm::LCM> &lcm_, bool show_labels_, bool show_triads_,
  bool standalone_head_, bool ground_height_, bool bdi_motion_estimate_):
          lcm_(lcm_), show_labels_(show_labels_), show_triads_(show_triads_),
          standalone_head_(standalone_head_), ground_height_(ground_height_),
          bdi_motion_estimate_(bdi_motion_estimate_){
  model_ = boost::shared_ptr<ModelClient>(new ModelClient(lcm_->getUnderlyingLCM(), 0));
  KDL::Tree tree;
  if (!kdl_parser::treeFromString( model_->getURDFString() ,tree)){
    cerr << "ERROR: Failed to extract kdl tree from xml robot description" << endl;
    exit(-1);
  }
  fksolver_ = shared_ptr<KDL::TreeFkSolverPosFull_recursive>(new KDL::TreeFkSolverPosFull_recursive(tree));

  // Vis Config:
  pc_vis_ = new pointcloud_vis( lcm_->getUnderlyingLCM());
  // obj: id name type reset
  pc_vis_->obj_cfg_list.push_back( obj_cfg(6001,"Frames",5,1) );
  lcm_->subscribe("EST_ROBOT_STATE",&joints2frames::robot_state_handler,this);

  pc_vis_->obj_cfg_list.push_back( obj_cfg(6003,"BDI Feet",5,1) );
  lcm_->subscribe("ATLAS_FOOT_POS_EST",&joints2frames::foot_pos_est_handler,this);

  last_ground_publish_utime_ =0;
}

Eigen::Isometry3d KDLToEigen(KDL::Frame tf){
  Eigen::Isometry3d tf_out;
  tf_out.setIdentity();
  tf_out.translation()  << tf.p[0], tf.p[1], tf.p[2];
  Eigen::Quaterniond q;
  tf.M.GetQuaternion( q.x() , q.y(), q.z(), q.w());
  tf_out.rotate(q);
  return tf_out;
}

void joints2frames::publishPose(Eigen::Isometry3d pose, int64_t utime, std::string channel){
  bot_core::pose_t pose_msg;
  pose_msg.utime =   utime;
  pose_msg.pos[0] = pose.translation().x();
  pose_msg.pos[1] = pose.translation().y();
  pose_msg.pos[2] = pose.translation().z();
  Eigen::Quaterniond r_x(pose.rotation());
  pose_msg.orientation[0] =  r_x.w();
  pose_msg.orientation[1] =  r_x.x();
  pose_msg.orientation[2] =  r_x.y();
  pose_msg.orientation[3] =  r_x.z();
  lcm_->publish( channel, &pose_msg);
}

void joints2frames::publishRigidTransform(Eigen::Isometry3d pose, int64_t utime, std::string channel){
  bot_core::rigid_transform_t tf;
  tf.utime = utime;
  tf.trans[0] = pose.translation().x();
  tf.trans[1] = pose.translation().y();
  tf.trans[2] = pose.translation().z();
  Eigen::Quaterniond quat(pose.rotation());
  tf.quat[0] = quat.w();
  tf.quat[1] = quat.x();
  tf.quat[2] = quat.y();
  tf.quat[3] = quat.z();
  lcm_->publish(channel, &tf);
}

void joints2frames::robot_state_handler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  drc::robot_state_t* msg){

  // 0. Extract World Pose of body:
  Eigen::Isometry3d world_to_body;
  world_to_body.setIdentity();
  world_to_body.translation()  << msg->pose.translation.x, msg->pose.translation.y, msg->pose.translation.z;
  Eigen::Quaterniond quat = Eigen::Quaterniond(msg->pose.rotation.w, msg->pose.rotation.x,
                                               msg->pose.rotation.y, msg->pose.rotation.z);
  world_to_body.rotate(quat);
  publishPose(world_to_body, msg->utime, "POSE_BODY" );

  // 1. Solve for Forward Kinematics:
  // call a routine that calculates the transforms the joint_state_t* msg.
  map<string, double> jointpos_in;
  map<string, KDL::Frame > cartpos_out;
  for (uint i=0; i< (uint) msg->num_joints; i++) //cast to uint to suppress compiler warning
    jointpos_in.insert(make_pair(msg->joint_name[i], msg->joint_position[i]));

  // Calculate forward position kinematics
  bool kinematics_status;
  bool flatten_tree=true; // determines absolute transforms to robot origin, otherwise relative transforms between joints.
  kinematics_status = fksolver_->JntToCart(jointpos_in,cartpos_out,flatten_tree);
  if(kinematics_status>=0){
    // cout << "Success!" <<endl;
  }else{
    cerr << "Error: could not calculate forward kinematics!" <<endl;
    return;
  }

  // 2a. Determine the required BOT_FRAMES transforms:
  Eigen::Isometry3d body_to_head, body_to_hokuyo_link;
  bool body_to_head_found =false;
  bool body_to_hokuyo_link_found = false;

  for( map<string, KDL::Frame >::iterator ii=cartpos_out.begin(); ii!=cartpos_out.end(); ++ii){
    std::string joint = (*ii).first;
    if (   (*ii).first.compare( "head" ) == 0 ){
      body_to_head = KDLToEigen( (*ii).second );
      body_to_head_found=true;
    }else if(  (*ii).first.compare( "hokuyo_link" ) == 0 ){
      body_to_hokuyo_link = KDLToEigen( (*ii).second );
      body_to_hokuyo_link_found=true;
    }else if(  (*ii).first.compare( "right_palm_left_camera_optical_frame" ) == 0 ){
      publishRigidTransform( KDLToEigen( (*ii).second ) , msg->utime, "BODY_TO_CAMERARHAND_LEFT" );
    }else if(  (*ii).first.compare( "left_palm_left_camera_optical_frame" ) == 0 ){
      publishRigidTransform( KDLToEigen( (*ii).second ) , msg->utime, "BODY_TO_CAMERALHAND_LEFT" );
    }
  }

  // 2b. Republish the required BOT_FRAMES transforms:
  if (body_to_head_found){
    /*
     * DONT PUBLISH THIS FOR SIMULATOR - CURRENTLY PUBLISHED BY LEGODO PROCESS
     */

    if (bdi_motion_estimate_){
      publishRigidTransform(body_to_head.inverse(), msg->utime, "HEAD_TO_BODY");

      Eigen::Isometry3d world_to_head = world_to_body * body_to_head ;
      publishPose(world_to_head, msg->utime, "POSE_HEAD" );
    }

    if (body_to_hokuyo_link_found){
      Eigen::Isometry3d head_to_hokuyo_link = body_to_head.inverse() * body_to_hokuyo_link ;
      publishRigidTransform(head_to_hokuyo_link, msg->utime, "HEAD_TO_HOKUYO_LINK");
    }
  }

  if (standalone_head_){
    // If publishing from the head alone, then the head is also the body link:
      publishRigidTransform(body_to_hokuyo_link, msg->utime, "HEAD_TO_HOKUYO_LINK" );
  }

  if (ground_height_){
    // Publish a pose at the lower of the feet - assumed to be on the ground
    // TODO: This doesnt need to be published at 1000Hz
    Eigen::Isometry3d body_to_l_foot = KDLToEigen(cartpos_out.find("l_foot")->second);
    Eigen::Isometry3d body_to_r_foot = KDLToEigen(cartpos_out.find("r_foot")->second);

    Eigen::Isometry3d foot_to_sole;
    foot_to_sole.setIdentity();
    foot_to_sole.translation()  << 0.0,0.,-0.0811; //distance between foot link and sole of foot

    Eigen::Isometry3d world_to_l_sole = world_to_body * body_to_l_foot * foot_to_sole;
    Eigen::Isometry3d world_to_r_sole = world_to_body * body_to_r_foot * foot_to_sole;

    // Publish lower of the soles at the ground occasionally
    if (msg->utime - last_ground_publish_utime_  > 5E5){ // every 0.5sec
      last_ground_publish_utime_ =msg->utime;
      if ( world_to_l_sole.translation().z() < world_to_r_sole.translation().z() ){
        publishPose(world_to_l_sole, msg->utime,"POSE_GROUND");
      }else{
        publishPose(world_to_r_sole, msg->utime,"POSE_GROUND");
      }
    }
  }

  Eigen::Isometry3d body_to_utorso = KDLToEigen(cartpos_out.find("utorso")->second);
  publishRigidTransform(body_to_utorso, msg->utime, "BODY_TO_UTORSO");

  // 4. Loop through joints and extract world positions:
  if (show_triads_){
    int counter =msg->utime;
    std::vector<Isometry3dTime> body_to_jointTs, world_to_jointsT;
    std::vector< int64_t > body_to_joint_utimes;
    std::vector< std::string > joint_names;
    for( map<string, KDL::Frame >::iterator ii=cartpos_out.begin(); ii!=cartpos_out.end(); ++ii){
      std::string joint = (*ii).first;
      //cout << joint  << ": \n";
      joint_names.push_back( joint  );
      body_to_joint_utimes.push_back( counter);

      Eigen::Isometry3d body_to_joint = KDLToEigen( (*ii).second );
      Isometry3dTime body_to_jointT(counter, body_to_joint);
      body_to_jointTs.push_back(body_to_jointT);
      // convert to world positions
      Isometry3dTime world_to_jointT(counter, world_to_body*body_to_joint);
      world_to_jointsT.push_back(world_to_jointT);
      counter++;
    }

    pc_vis_->pose_collection_to_lcm_from_list(6001, world_to_jointsT); // all joints in world frame
    if (show_labels_)
      pc_vis_->text_collection_to_lcm(6002, 6001, "Frames [Labels]", joint_names, body_to_joint_utimes );

  }
}

void joints2frames::foot_pos_est_handler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  drc::atlas_foot_pos_est_t* msg){
  std::cout << "got em\n";

  Eigen::Isometry3d left_pos;
  left_pos.setIdentity();
  left_pos.translation()  << msg->left_position[0], msg->left_position[1], msg->left_position[2];

  Eigen::Isometry3d right_pos;
  right_pos.setIdentity();
  right_pos.translation()  << msg->right_position[0], msg->right_position[1], msg->right_position[2];

  std::vector<Isometry3dTime> feet_posT;
  feet_posT.push_back( Isometry3dTime(msg->utime , left_pos  )  );
  feet_posT.push_back( Isometry3dTime(msg->utime+1 , right_pos  )  );
  pc_vis_->pose_collection_to_lcm_from_list(6003, feet_posT);
}

int
main(int argc, char ** argv){
  string role = "robot";
  bool labels = false;
  bool triads = false;
  bool standalone_head = false;
  bool ground_height = false;
  bool bdi_motion_estimate = false;
  ConciseArgs opt(argc, (char**)argv);
  opt.add(role, "r", "role","Role - robot or base");
  opt.add(triads, "t", "triads","Frame Triads - show no not");
  opt.add(labels, "l", "labels","Frame Labels - show no not");
  opt.add(ground_height, "g", "ground", "Publish the grounded foot pose");
  opt.add(standalone_head, "s", "standalone_head","Standalone Sensor Head");
  opt.add(bdi_motion_estimate, "b", "bdi","Use POSE_BDI to make frames [Temporary!]");
  opt.parse();
  if (labels){ // require triads if labels is to be published
    triads=true;
  }

  std::cout << "triads: " << triads << "\n";
  std::cout << "labels: " << labels << "\n";
  std::cout << "role: " << role << "\n";

  string lcm_url="";
  std::string role_upper;
  for(short i = 0; i < role.size(); ++i)
     role_upper+= (std::toupper(role[i]));
  if((role.compare("robot") == 0) || (role.compare("base") == 0) ){
    for(short i = 0; i < role_upper.size(); ++i)
       role_upper[i] = (std::toupper(role_upper[i]));
    string env_variable_name = string("LCM_URL_DRC_" + role_upper);
    char* env_variable;
    env_variable = getenv (env_variable_name.c_str());
    if (env_variable!=NULL){
      //printf ("The env_variable is: %s\n",env_variable);
      lcm_url = string(env_variable);
    }else{
      std::cout << env_variable_name << " environment variable has not been set ["<< lcm_url <<"]\n";
      exit(-1);
    }
  }else{
    std::cout << "Role not understood, choose: robot or base\n";
    return 1;
  }

  boost::shared_ptr<lcm::LCM> lcm(new lcm::LCM(lcm_url) );
  if(!lcm->good())
    return 1;

  joints2frames app(lcm,labels,triads, standalone_head, ground_height, bdi_motion_estimate);
  while(0 == lcm->handle());
  return 0;
}
<commit_msg>adding BDI foot position rendering<commit_after>#include <stdio.h>
#include <inttypes.h>
#include <lcm/lcm.h>
#include <iostream>
#include <limits>

#include "joints2frames.hpp"
#include <ConciseArgs>

using namespace std;
using namespace boost;
using namespace boost::assign;

#define DO_TIMING_PROFILE FALSE

/////////////////////////////////////

joints2frames::joints2frames(boost::shared_ptr<lcm::LCM> &lcm_, bool show_labels_, bool show_triads_,
  bool standalone_head_, bool ground_height_, bool bdi_motion_estimate_):
          lcm_(lcm_), show_labels_(show_labels_), show_triads_(show_triads_),
          standalone_head_(standalone_head_), ground_height_(ground_height_),
          bdi_motion_estimate_(bdi_motion_estimate_){
  model_ = boost::shared_ptr<ModelClient>(new ModelClient(lcm_->getUnderlyingLCM(), 0));
  KDL::Tree tree;
  if (!kdl_parser::treeFromString( model_->getURDFString() ,tree)){
    cerr << "ERROR: Failed to extract kdl tree from xml robot description" << endl;
    exit(-1);
  }
  fksolver_ = shared_ptr<KDL::TreeFkSolverPosFull_recursive>(new KDL::TreeFkSolverPosFull_recursive(tree));

  // Vis Config:
  pc_vis_ = new pointcloud_vis( lcm_->getUnderlyingLCM());
  // obj: id name type reset
  pc_vis_->obj_cfg_list.push_back( obj_cfg(6001,"Frames",5,1) );
  lcm_->subscribe("EST_ROBOT_STATE",&joints2frames::robot_state_handler,this);

  pc_vis_->obj_cfg_list.push_back( obj_cfg(6003,"BDI Feet",5,1) );
  lcm_->subscribe("ATLAS_FOOT_POS_EST",&joints2frames::foot_pos_est_handler,this);

  last_ground_publish_utime_ =0;
}

Eigen::Isometry3d KDLToEigen(KDL::Frame tf){
  Eigen::Isometry3d tf_out;
  tf_out.setIdentity();
  tf_out.translation()  << tf.p[0], tf.p[1], tf.p[2];
  Eigen::Quaterniond q;
  tf.M.GetQuaternion( q.x() , q.y(), q.z(), q.w());
  tf_out.rotate(q);
  return tf_out;
}

void joints2frames::publishPose(Eigen::Isometry3d pose, int64_t utime, std::string channel){
  bot_core::pose_t pose_msg;
  pose_msg.utime =   utime;
  pose_msg.pos[0] = pose.translation().x();
  pose_msg.pos[1] = pose.translation().y();
  pose_msg.pos[2] = pose.translation().z();
  Eigen::Quaterniond r_x(pose.rotation());
  pose_msg.orientation[0] =  r_x.w();
  pose_msg.orientation[1] =  r_x.x();
  pose_msg.orientation[2] =  r_x.y();
  pose_msg.orientation[3] =  r_x.z();
  lcm_->publish( channel, &pose_msg);
}

void joints2frames::publishRigidTransform(Eigen::Isometry3d pose, int64_t utime, std::string channel){
  bot_core::rigid_transform_t tf;
  tf.utime = utime;
  tf.trans[0] = pose.translation().x();
  tf.trans[1] = pose.translation().y();
  tf.trans[2] = pose.translation().z();
  Eigen::Quaterniond quat(pose.rotation());
  tf.quat[0] = quat.w();
  tf.quat[1] = quat.x();
  tf.quat[2] = quat.y();
  tf.quat[3] = quat.z();
  lcm_->publish(channel, &tf);
}

void joints2frames::robot_state_handler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  drc::robot_state_t* msg){

  // 0. Extract World Pose of body:
  Eigen::Isometry3d world_to_body;
  world_to_body.setIdentity();
  world_to_body.translation()  << msg->pose.translation.x, msg->pose.translation.y, msg->pose.translation.z;
  Eigen::Quaterniond quat = Eigen::Quaterniond(msg->pose.rotation.w, msg->pose.rotation.x,
                                               msg->pose.rotation.y, msg->pose.rotation.z);
  world_to_body.rotate(quat);
  publishPose(world_to_body, msg->utime, "POSE_BODY" );

  // 1. Solve for Forward Kinematics:
  // call a routine that calculates the transforms the joint_state_t* msg.
  map<string, double> jointpos_in;
  map<string, KDL::Frame > cartpos_out;
  for (uint i=0; i< (uint) msg->num_joints; i++) //cast to uint to suppress compiler warning
    jointpos_in.insert(make_pair(msg->joint_name[i], msg->joint_position[i]));

  // Calculate forward position kinematics
  bool kinematics_status;
  bool flatten_tree=true; // determines absolute transforms to robot origin, otherwise relative transforms between joints.
  kinematics_status = fksolver_->JntToCart(jointpos_in,cartpos_out,flatten_tree);
  if(kinematics_status>=0){
    // cout << "Success!" <<endl;
  }else{
    cerr << "Error: could not calculate forward kinematics!" <<endl;
    return;
  }

  // 2a. Determine the required BOT_FRAMES transforms:
  Eigen::Isometry3d body_to_head, body_to_hokuyo_link;
  bool body_to_head_found =false;
  bool body_to_hokuyo_link_found = false;

  for( map<string, KDL::Frame >::iterator ii=cartpos_out.begin(); ii!=cartpos_out.end(); ++ii){
    std::string joint = (*ii).first;
    if (   (*ii).first.compare( "head" ) == 0 ){
      body_to_head = KDLToEigen( (*ii).second );
      body_to_head_found=true;
    }else if(  (*ii).first.compare( "hokuyo_link" ) == 0 ){
      body_to_hokuyo_link = KDLToEigen( (*ii).second );
      body_to_hokuyo_link_found=true;
    }else if(  (*ii).first.compare( "right_palm_left_camera_optical_frame" ) == 0 ){
      publishRigidTransform( KDLToEigen( (*ii).second ) , msg->utime, "BODY_TO_CAMERARHAND_LEFT" );
    }else if(  (*ii).first.compare( "left_palm_left_camera_optical_frame" ) == 0 ){
      publishRigidTransform( KDLToEigen( (*ii).second ) , msg->utime, "BODY_TO_CAMERALHAND_LEFT" );
    }
  }

  // 2b. Republish the required BOT_FRAMES transforms:
  if (body_to_head_found){
    /*
     * DONT PUBLISH THIS FOR SIMULATOR - CURRENTLY PUBLISHED BY LEGODO PROCESS
     */

    if (bdi_motion_estimate_){
      publishRigidTransform(body_to_head.inverse(), msg->utime, "HEAD_TO_BODY");

      Eigen::Isometry3d world_to_head = world_to_body * body_to_head ;
      publishPose(world_to_head, msg->utime, "POSE_HEAD" );
    }

    if (body_to_hokuyo_link_found){
      Eigen::Isometry3d head_to_hokuyo_link = body_to_head.inverse() * body_to_hokuyo_link ;
      publishRigidTransform(head_to_hokuyo_link, msg->utime, "HEAD_TO_HOKUYO_LINK");
    }
  }

  if (standalone_head_){
    // If publishing from the head alone, then the head is also the body link:
      publishRigidTransform(body_to_hokuyo_link, msg->utime, "HEAD_TO_HOKUYO_LINK" );
  }

  if (ground_height_){
    // Publish a pose at the lower of the feet - assumed to be on the ground
    // TODO: This doesnt need to be published at 1000Hz
    Eigen::Isometry3d body_to_l_foot = KDLToEigen(cartpos_out.find("l_foot")->second);
    Eigen::Isometry3d body_to_r_foot = KDLToEigen(cartpos_out.find("r_foot")->second);

    Eigen::Isometry3d foot_to_sole;
    foot_to_sole.setIdentity();
    foot_to_sole.translation()  << 0.0,0.,-0.0811; //distance between foot link and sole of foot

    Eigen::Isometry3d world_to_l_sole = world_to_body * body_to_l_foot * foot_to_sole;
    Eigen::Isometry3d world_to_r_sole = world_to_body * body_to_r_foot * foot_to_sole;

    // Publish lower of the soles at the ground occasionally
    if (msg->utime - last_ground_publish_utime_  > 5E5){ // every 0.5sec
      last_ground_publish_utime_ =msg->utime;
      if ( world_to_l_sole.translation().z() < world_to_r_sole.translation().z() ){
        publishPose(world_to_l_sole, msg->utime,"POSE_GROUND");
      }else{
        publishPose(world_to_r_sole, msg->utime,"POSE_GROUND");
      }
    }
  }

  Eigen::Isometry3d body_to_utorso = KDLToEigen(cartpos_out.find("utorso")->second);
  publishRigidTransform(body_to_utorso, msg->utime, "BODY_TO_UTORSO");

  // 4. Loop through joints and extract world positions:
  if (show_triads_){
    int counter =msg->utime;
    std::vector<Isometry3dTime> body_to_jointTs, world_to_jointsT;
    std::vector< int64_t > body_to_joint_utimes;
    std::vector< std::string > joint_names;
    for( map<string, KDL::Frame >::iterator ii=cartpos_out.begin(); ii!=cartpos_out.end(); ++ii){
      std::string joint = (*ii).first;
      //cout << joint  << ": \n";
      joint_names.push_back( joint  );
      body_to_joint_utimes.push_back( counter);

      Eigen::Isometry3d body_to_joint = KDLToEigen( (*ii).second );
      Isometry3dTime body_to_jointT(counter, body_to_joint);
      body_to_jointTs.push_back(body_to_jointT);
      // convert to world positions
      Isometry3dTime world_to_jointT(counter, world_to_body*body_to_joint);
      world_to_jointsT.push_back(world_to_jointT);
      counter++;
    }

    pc_vis_->pose_collection_to_lcm_from_list(6001, world_to_jointsT); // all joints in world frame
    if (show_labels_)
      pc_vis_->text_collection_to_lcm(6002, 6001, "Frames [Labels]", joint_names, body_to_joint_utimes );

  }
}

// Visualize the foot positions from BDI:
// This can be turnned off if necessary - its not important
void joints2frames::foot_pos_est_handler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  drc::atlas_foot_pos_est_t* msg){
  Eigen::Isometry3d left_pos;
  left_pos.setIdentity();
  left_pos.translation()  << msg->left_position[0], msg->left_position[1], msg->left_position[2];

  Eigen::Isometry3d right_pos;
  right_pos.setIdentity();
  right_pos.translation()  << msg->right_position[0], msg->right_position[1], msg->right_position[2];

  std::vector<Isometry3dTime> feet_posT;
  feet_posT.push_back( Isometry3dTime(msg->utime , left_pos  )  );
  feet_posT.push_back( Isometry3dTime(msg->utime+1 , right_pos  )  );
  pc_vis_->pose_collection_to_lcm_from_list(6003, feet_posT);
}

int
main(int argc, char ** argv){
  string role = "robot";
  bool labels = false;
  bool triads = false;
  bool standalone_head = false;
  bool ground_height = false;
  bool bdi_motion_estimate = false;
  ConciseArgs opt(argc, (char**)argv);
  opt.add(role, "r", "role","Role - robot or base");
  opt.add(triads, "t", "triads","Frame Triads - show no not");
  opt.add(labels, "l", "labels","Frame Labels - show no not");
  opt.add(ground_height, "g", "ground", "Publish the grounded foot pose");
  opt.add(standalone_head, "s", "standalone_head","Standalone Sensor Head");
  opt.add(bdi_motion_estimate, "b", "bdi","Use POSE_BDI to make frames [Temporary!]");
  opt.parse();
  if (labels){ // require triads if labels is to be published
    triads=true;
  }

  std::cout << "triads: " << triads << "\n";
  std::cout << "labels: " << labels << "\n";
  std::cout << "role: " << role << "\n";

  string lcm_url="";
  std::string role_upper;
  for(short i = 0; i < role.size(); ++i)
     role_upper+= (std::toupper(role[i]));
  if((role.compare("robot") == 0) || (role.compare("base") == 0) ){
    for(short i = 0; i < role_upper.size(); ++i)
       role_upper[i] = (std::toupper(role_upper[i]));
    string env_variable_name = string("LCM_URL_DRC_" + role_upper);
    char* env_variable;
    env_variable = getenv (env_variable_name.c_str());
    if (env_variable!=NULL){
      //printf ("The env_variable is: %s\n",env_variable);
      lcm_url = string(env_variable);
    }else{
      std::cout << env_variable_name << " environment variable has not been set ["<< lcm_url <<"]\n";
      exit(-1);
    }
  }else{
    std::cout << "Role not understood, choose: robot or base\n";
    return 1;
  }

  boost::shared_ptr<lcm::LCM> lcm(new lcm::LCM(lcm_url) );
  if(!lcm->good())
    return 1;

  joints2frames app(lcm,labels,triads, standalone_head, ground_height, bdi_motion_estimate);
  while(0 == lcm->handle());
  return 0;
}
<|endoftext|>


<commit_before>// Copyright 2019 DeepMind Technologies Ltd. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "open_spiel/algorithms/is_mcts.h"

#include <random>

#include "open_spiel/abseil-cpp/absl/random/distributions.h"
#include "open_spiel/algorithms/mcts.h"
#include "open_spiel/spiel.h"
#include "open_spiel/spiel_bots.h"
#include "open_spiel/spiel_utils.h"

namespace open_spiel {
namespace {

constexpr const int kSeed = 93879211;

void PlayGame(const Game& game, algorithms::ISMCTSBot* bot, std::mt19937* rng) {
  std::unique_ptr<State> state = game.NewInitialState();
  while (!state->IsTerminal()) {
    std::cout << "State:" << std::endl;
    std::cout << state->ToString() << std::endl;

    Action chosen_action = kInvalidAction;
    if (state->IsChanceNode()) {
      chosen_action =
          SampleAction(state->ChanceOutcomes(), absl::Uniform(*rng, 0.0, 1.0))
              .first;
    } else {
      chosen_action = bot->Step(*state);
    }

    std::cout << "Chosen action: " << state->ActionToString(chosen_action)
              << std::endl;
    state->ApplyAction(chosen_action);
  }

  std::cout << "Terminal state:" << std::endl;
  std::cout << state->ToString() << std::endl;
  std::cout << "Returns: " << absl::StrJoin(state->Returns(), " ") << std::endl;
}

void ISMCTSTest_PlayGame(const std::string& game_name) {
  std::shared_ptr<const Game> game = LoadGame(game_name);
  auto evaluator =
      std::make_shared<algorithms::RandomRolloutEvaluator>(1, kSeed);

  for (algorithms::ISMCTSFinalPolicyType type:
       {algorithms::ISMCTSFinalPolicyType::kNormalizedVisitCount,
        algorithms::ISMCTSFinalPolicyType::kMaxVisitCount,
        algorithms::ISMCTSFinalPolicyType::kMaxValue}) {
    auto bot1 = std::make_unique<algorithms::ISMCTSBot>(
        kSeed, evaluator, 5.0, 1000, algorithms::kUnlimitedNumWorldSamples,
        type, false, false);

    std::mt19937 rng(kSeed);

    std::cout << "Testing " << game_name << ", bot 1" << std::endl;
    PlayGame(*game, bot1.get(), &rng);

    auto bot2 = std::make_unique<algorithms::ISMCTSBot>(
        kSeed, evaluator, 5.0, 1000, 10, type, false, false);
    std::cout << "Testing " << game_name << ", bot 2" << std::endl;
    PlayGame(*game, bot2.get(), &rng);
  }
}

void ISMCTS_BasicPlayGameTest_Kuhn() {
  ISMCTSTest_PlayGame("kuhn_poker");
  ISMCTSTest_PlayGame("kuhn_poker(players=3)");
}

void ISMCTS_BasicPlayGameTest_Leduc() {
  ISMCTSTest_PlayGame("leduc_poker");
  ISMCTSTest_PlayGame("leduc_poker(players=3)");
}

void ISMCTS_LeducObservationTest() {
  std::mt19937 rng(kSeed);
  std::shared_ptr<const Game> game = LoadGame("leduc_poker");
  auto evaluator =
      std::make_shared<algorithms::RandomRolloutEvaluator>(1, kSeed);
  auto bot = std::make_unique<algorithms::ISMCTSBot>(
      kSeed, evaluator, 10.0, 1000, algorithms::kUnlimitedNumWorldSamples,
      algorithms::ISMCTSFinalPolicyType::kNormalizedVisitCount, true, true);
  PlayGame(*game, bot.get(), &rng);
}

}  // namespace
}  // namespace open_spiel

int main(int argc, char** argv) {
  open_spiel::ISMCTS_BasicPlayGameTest_Kuhn();
  open_spiel::ISMCTS_BasicPlayGameTest_Leduc();
  open_spiel::ISMCTS_LeducObservationTest();
}<commit_msg>Fix is_mcts whitespace formatting<commit_after>// Copyright 2019 DeepMind Technologies Ltd. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "open_spiel/algorithms/is_mcts.h"

#include <random>

#include "open_spiel/abseil-cpp/absl/random/distributions.h"
#include "open_spiel/algorithms/mcts.h"
#include "open_spiel/spiel.h"
#include "open_spiel/spiel_bots.h"
#include "open_spiel/spiel_utils.h"

namespace open_spiel {
namespace {

constexpr const int kSeed = 93879211;

void PlayGame(const Game& game, algorithms::ISMCTSBot* bot, std::mt19937* rng) {
  std::unique_ptr<State> state = game.NewInitialState();
  while (!state->IsTerminal()) {
    std::cout << "State:" << std::endl;
    std::cout << state->ToString() << std::endl;

    Action chosen_action = kInvalidAction;
    if (state->IsChanceNode()) {
      chosen_action =
          SampleAction(state->ChanceOutcomes(), absl::Uniform(*rng, 0.0, 1.0))
              .first;
    } else {
      chosen_action = bot->Step(*state);
    }

    std::cout << "Chosen action: " << state->ActionToString(chosen_action)
              << std::endl;
    state->ApplyAction(chosen_action);
  }

  std::cout << "Terminal state:" << std::endl;
  std::cout << state->ToString() << std::endl;
  std::cout << "Returns: " << absl::StrJoin(state->Returns(), " ") << std::endl;
}

void ISMCTSTest_PlayGame(const std::string& game_name) {
  std::shared_ptr<const Game> game = LoadGame(game_name);
  auto evaluator =
      std::make_shared<algorithms::RandomRolloutEvaluator>(1, kSeed);

  for (algorithms::ISMCTSFinalPolicyType type :
       {algorithms::ISMCTSFinalPolicyType::kNormalizedVisitCount,
        algorithms::ISMCTSFinalPolicyType::kMaxVisitCount,
        algorithms::ISMCTSFinalPolicyType::kMaxValue}) {
    auto bot1 = std::make_unique<algorithms::ISMCTSBot>(
        kSeed, evaluator, 5.0, 1000, algorithms::kUnlimitedNumWorldSamples,
        type, false, false);

    std::mt19937 rng(kSeed);

    std::cout << "Testing " << game_name << ", bot 1" << std::endl;
    PlayGame(*game, bot1.get(), &rng);

    auto bot2 = std::make_unique<algorithms::ISMCTSBot>(
        kSeed, evaluator, 5.0, 1000, 10, type, false, false);
    std::cout << "Testing " << game_name << ", bot 2" << std::endl;
    PlayGame(*game, bot2.get(), &rng);
  }
}

void ISMCTS_BasicPlayGameTest_Kuhn() {
  ISMCTSTest_PlayGame("kuhn_poker");
  ISMCTSTest_PlayGame("kuhn_poker(players=3)");
}

void ISMCTS_BasicPlayGameTest_Leduc() {
  ISMCTSTest_PlayGame("leduc_poker");
  ISMCTSTest_PlayGame("leduc_poker(players=3)");
}

void ISMCTS_LeducObservationTest() {
  std::mt19937 rng(kSeed);
  std::shared_ptr<const Game> game = LoadGame("leduc_poker");
  auto evaluator =
      std::make_shared<algorithms::RandomRolloutEvaluator>(1, kSeed);
  auto bot = std::make_unique<algorithms::ISMCTSBot>(
      kSeed, evaluator, 10.0, 1000, algorithms::kUnlimitedNumWorldSamples,
      algorithms::ISMCTSFinalPolicyType::kNormalizedVisitCount, true, true);
  PlayGame(*game, bot.get(), &rng);
}

}  // namespace
}  // namespace open_spiel

int main(int argc, char** argv) {
  open_spiel::ISMCTS_BasicPlayGameTest_Kuhn();
  open_spiel::ISMCTS_BasicPlayGameTest_Leduc();
  open_spiel::ISMCTS_LeducObservationTest();
}
<|endoftext|>


<commit_before>//
//  main.cpp
//  HashTables
//
//  Created by Sunil on 4/28/17.
//  Copyright © 2017 Sunil. All rights reserved.
//

#include <string>
#include <chrono>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <iostream>

#include "HashTable.hpp"
#include "PlayerInfo.hpp"

using namespace std;

void die(string message) {
    cout << message << endl;
    exit(0);
}

void PopulateHashTable(string filename, HashTable* map) {
    ifstream file(filename);

    // non-existant or corrupted file
    if(file.fail()) {
        die("Could not open file");
    }

    string line;

    string ignoreHeader;
    std::getline(file, ignoreHeader);

    // cout << ignoreHeader << endl;
    // std::setiosflags(ios::fixed);

    while(std::getline(file, line)) {
        auto playerInfo = PlayerInfo::ConstructFrom(line);

        // add playerInfo to the hash table.
        auto key = PlayerInfo::MakeKey(playerInfo);
        map->put(key, playerInfo);
    }
}

int getTableSize(string size) {
    char multiplier = ' ';
    float number;

    istringstream ss(size);
    ss >> number >> multiplier;

    switch (multiplier) {
        case 'k':
        case 'K':
            // Kilo
            number *= 1000;
            break;

        case ' ':
            break;

        default:
            cout << "Only K/k multiplier is supported!" << endl;
            break;
    }

    return number;
}

int main(int argc, const char * argv[]) {

    string dmenu =  "1. Query hash table\n"
                    "2. Quit program\n";

    string help = "usage: $executable [input file] [hashtable size]\n"
                  "example: $exe PlayerData.txt 5072";

    if (argc < 3) {
        cout << "Missing arguments to the program!" << endl;
        die(help);
    }

    int choice = 0;
    bool done = false;

    string filename {argv[1]};
    int hashSize = getTableSize(argv[2]);

    std::chrono::time_point<std::chrono::system_clock> start, end;

    // create collision counter variables
    CollisionCounter linearProbeStats("(linear probe)");
    CollisionCounter chainingStats("(chaining)");

    HashTable* map = new HashTable(hashSize, new ChainingResolver(&chainingStats));
    HashTable* map2 = new HashTable(hashSize, new LinearProbingResolver(&linearProbeStats));

    PopulateHashTable(filename, map);
    PopulateHashTable(filename, map2);

    cout << "Hash table size: " << hashSize << endl;
    cout << "Collisions using open addressing: " << linearProbeStats.addCollisions << endl;
    cout << "Collisions using chaining: " << chainingStats.addCollisions << endl;

    linearProbeStats.resetCounters();
    chainingStats.resetCounters();

    do {
        cout << dmenu;
        cin >> choice;

        // flush the newlines and other characters
        cin.clear();
        cin.ignore();

        switch (choice) {
            case 1:
            {
                string firstName, lastName;

                cout << "Enter first name: " << endl;
                std::getline(cin, firstName);
                cout << "Enter last name: " << endl;
                std::getline(cin, lastName);

                auto key = PlayerInfo::MakeKey(firstName, lastName);

                // look up with chaining
                {
                    auto found = map->get(key);

                    if (found) found->show();
                    else cout << "Record not found!" << endl;

                    cout << "Search operations using chaining: " << chainingStats.lookupCollisions << endl;
                    chainingStats.resetCounters();
                }

                // look up with linear probing
                {
                    auto found = map2->get(key);

                    if (found) found->show();
                    else cout << "Record not found!" << endl;

                    cout << "Search operations using open addressing: " << linearProbeStats.lookupCollisions << endl;
                    linearProbeStats.resetCounters();
                }

                break;
            }

            case 2:
            {
                done = true;
                break;
            }

            default:
            {
                // ignore unrecognized input.
                // and let program continue.
            }
        }

    } while(!done);

    cout << "Goodbye!" << endl;
    delete map;
    delete map2;
    return 0;
}
<commit_msg>format output<commit_after>//
//  main.cpp
//  HashTables
//
//  Created by Sunil on 4/28/17.
//  Copyright © 2017 Sunil. All rights reserved.
//

#include <string>
#include <chrono>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <iostream>

#include "HashTable.hpp"
#include "PlayerInfo.hpp"

using namespace std;

void die(string message) {
    cout << message << endl;
    exit(0);
}

void PopulateHashTable(string filename, HashTable* map) {
    ifstream file(filename);

    // non-existant or corrupted file
    if(file.fail()) {
        die("Could not open file");
    }

    string line;

    string ignoreHeader;
    std::getline(file, ignoreHeader);

    // cout << ignoreHeader << endl;
    // std::setiosflags(ios::fixed);

    while(std::getline(file, line)) {
        auto playerInfo = PlayerInfo::ConstructFrom(line);

        // add playerInfo to the hash table.
        auto key = PlayerInfo::MakeKey(playerInfo);
        map->put(key, playerInfo);
    }
}

int getTableSize(string size) {
    char multiplier = ' ';
    float number;

    istringstream ss(size);
    ss >> number >> multiplier;

    switch (multiplier) {
        case 'k':
        case 'K':
            // Kilo
            number *= 1000;
            break;

        case ' ':
            break;

        default:
            cout << "Only K/k multiplier is supported!" << endl;
            break;
    }

    return number;
}

int main(int argc, const char * argv[]) {

    string dmenu =  "1. Query hash table\n"
                    "2. Quit program\n";

    string help = "usage: $executable [input file] [hashtable size]\n"
                  "example: $exe PlayerData.txt 5072";

    if (argc < 3) {
        cout << "Missing arguments to the program!" << endl;
        die(help);
    }

    int choice = 0;
    bool done = false;

    string filename {argv[1]};
    int hashSize = getTableSize(argv[2]);

    std::chrono::time_point<std::chrono::system_clock> start, end;

    // create collision counter variables
    CollisionCounter linearProbeStats("(linear probe)");
    CollisionCounter chainingStats("(chaining)");

    HashTable* map = new HashTable(hashSize, new ChainingResolver(&chainingStats));
    HashTable* map2 = new HashTable(hashSize, new LinearProbingResolver(&linearProbeStats));

    PopulateHashTable(filename, map);
    PopulateHashTable(filename, map2);

    cout << "Hash table size: " << hashSize << endl;
    cout << "Collisions using open addressing: " << linearProbeStats.addCollisions << endl;
    cout << "Collisions using chaining: " << chainingStats.addCollisions << endl;

    linearProbeStats.resetCounters();
    chainingStats.resetCounters();

    do {
        cout << dmenu;
        cin >> choice;

        // flush the newlines and other characters
        cin.clear();
        cin.ignore();

        switch (choice) {
            case 1:
            {
                string firstName, lastName;

                cout << "Enter first name: ";
                std::getline(cin, firstName);
                cout << "Enter last name: ";
                std::getline(cin, lastName);

                auto key = PlayerInfo::MakeKey(firstName, lastName);

                // look up with chaining
                {
                    auto found = map->get(key);

                    cout << endl << "Search operations using chaining: " << chainingStats.lookupCollisions << endl;
                    if (found) found->show();
                    else cout << "Record not found!" << endl;

                    chainingStats.resetCounters();
                }

                // look up with linear probing
                {
                    auto found = map2->get(key);

                    cout << "Search operations using open addressing: " << linearProbeStats.lookupCollisions << endl;
                    if (found) found->show();
                    else cout << "Record not found!" << endl;

                    linearProbeStats.resetCounters();
                }

                break;
            }

            case 2:
            {
                done = true;
                break;
            }

            default:
            {
                // ignore unrecognized input.
                // and let program continue.
            }
        }

    } while(!done);

    cout << "Goodbye!" << endl;
    delete map;
    delete map2;
    return 0;
}
<|endoftext|>


<commit_before>/*
 * opencog/atomspace/SimpleTruthValue.cc
 *
 * Copyright (C) 2002-2007 Novamente LLC
 * All Rights Reserved
 *
 * Written by Welter Silva <welter@vettalabs.com>
 *            Guilherme Lamacie

 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License v3 as
 * published by the Free Software Foundation and including the exceptions
 * at http://opencog.org/wiki/Licenses
 *
 * This program 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 General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with this program; if not, write to:
 * Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 *
 */

#include <math.h>
#include <typeinfo>

#include <opencog/util/platform.h>
#include <opencog/util/exceptions.h>

#include "SimpleTruthValue.h"

//#define DPRINTF printf
#define DPRINTF(...)

using namespace opencog;

#define KKK 800.0f
#define CVAL  0.2f

SimpleTruthValue::SimpleTruthValue(strength_t m, count_t c)
{
    mean = m;
    count = c;
}

SimpleTruthValue::SimpleTruthValue(const TruthValue& source)
{
    mean = source.getMean();
    count = source.getCount();
}
SimpleTruthValue::SimpleTruthValue(SimpleTruthValue const& source)
{
    mean = source.mean;
    count = source.count;
}

strength_t SimpleTruthValue::getMean() const
{
    return mean;
}

count_t SimpleTruthValue::getCount() const
{
    return count;
}

confidence_t SimpleTruthValue::getConfidence() const
{
    return countToConfidence(count);
}

// This is the merge formula appropriate for PLN.
TruthValuePtr SimpleTruthValue::merge(TruthValuePtr other,TVMergeStyle ms/*=DEFAULT*/) const
{
	switch(ms){
	case DEFAULT:
	{
		if (other->getType() != SIMPLE_TRUTH_VALUE)
		        throw RuntimeException(TRACE_INFO,
		           "Don't know how to merge %s into a SimpleTruthValue using the default style",
		           typeid(*other).name());
		auto count2 = other->getCount();
		auto count_new = count+ count2  - std::min(count,count2)*CVAL;
		auto mean_new = (mean*count + other->getMean()*count2)/(count+count2);
		return std::make_shared<SimpleTruthValue>(mean_new,count_new);
	}
	default:
			throw RuntimeException(TRACE_INFO,
			           "Unknown or not yet implemented merge strategy");
             }
}

std::string SimpleTruthValue::toString() const
{
    char buf[1024];
    sprintf(buf, "(stv %f %f)",
            static_cast<float>(getMean()),
            static_cast<float>(getConfidence()));
    return buf;
}

bool SimpleTruthValue::operator==(const TruthValue& rhs) const
{
    const SimpleTruthValue *stv = dynamic_cast<const SimpleTruthValue *>(&rhs);
    if (NULL == stv) return false;

#define FLOAT_ACCEPTABLE_MEAN_ERROR 0.000001
    if (FLOAT_ACCEPTABLE_MEAN_ERROR < fabs(mean - stv->mean)) return false;

// Converting from confidence to count and back again using single-precision
// float is a real accuracy killer.  In particular, 2/802 = 0.002494 but
// converting back gives 800*0.002494/(1.0-0.002494) = 2.000188 and so
// comparison tests can only be accurate to about 0.000188 or
// thereabouts.
#define FLOAT_ACCEPTABLE_COUNT_ERROR 0.0002

    if (FLOAT_ACCEPTABLE_COUNT_ERROR < fabs(1.0 - (stv->count/count))) return false;
    return true;
}

TruthValueType SimpleTruthValue::getType() const
{
    return SIMPLE_TRUTH_VALUE;
}

count_t SimpleTruthValue::confidenceToCount(confidence_t cf)
{
    // There are not quite 16 digits in double precision
    // not quite 7 in single-precision float
    cf = std::min(cf, 0.9999998f);
    return static_cast<count_t>(KKK * cf / (1.0f - cf));
}

confidence_t SimpleTruthValue::countToConfidence(count_t cn)
{
    return static_cast<confidence_t>(cn / (cn + KKK));
}
<commit_msg>added reference<commit_after>/*
 * opencog/atomspace/SimpleTruthValue.cc
 *
 * Copyright (C) 2002-2007 Novamente LLC
 * All Rights Reserved
 *
 * Written by Welter Silva <welter@vettalabs.com>
 *            Guilherme Lamacie

 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License v3 as
 * published by the Free Software Foundation and including the exceptions
 * at http://opencog.org/wiki/Licenses
 *
 * This program 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 General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with this program; if not, write to:
 * Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 *
 */

#include <math.h>
#include <typeinfo>

#include <opencog/util/platform.h>
#include <opencog/util/exceptions.h>

#include "SimpleTruthValue.h"

//#define DPRINTF printf
#define DPRINTF(...)

using namespace opencog;

#define KKK 800.0f
#define CVAL  0.2f

SimpleTruthValue::SimpleTruthValue(strength_t m, count_t c)
{
    mean = m;
    count = c;
}

SimpleTruthValue::SimpleTruthValue(const TruthValue& source)
{
    mean = source.getMean();
    count = source.getCount();
}
SimpleTruthValue::SimpleTruthValue(SimpleTruthValue const& source)
{
    mean = source.mean;
    count = source.count;
}

strength_t SimpleTruthValue::getMean() const
{
    return mean;
}

count_t SimpleTruthValue::getCount() const
{
    return count;
}

confidence_t SimpleTruthValue::getConfidence() const
{
    return countToConfidence(count);
}

// This is the merge formula appropriate for PLN.
TruthValuePtr SimpleTruthValue::merge(TruthValuePtr other,TVMergeStyle ms/*=DEFAULT*/) const
{
	switch(ms){
	case DEFAULT:
	{
		//Based on section 5.10.2(A heuristic revision rule for STV) of the PLN book
		if (other->getType() != SIMPLE_TRUTH_VALUE)
		        throw RuntimeException(TRACE_INFO,
		           "Don't know how to merge %s into a SimpleTruthValue using the default style",
		           typeid(*other).name());
		auto count2 = other->getCount();
		auto count_new = count+ count2  - std::min(count,count2)*CVAL;
		auto mean_new = (mean*count + other->getMean()*count2)/(count+count2);
		return std::make_shared<SimpleTruthValue>(mean_new,count_new);
	}
	default:
			throw RuntimeException(TRACE_INFO,
			           "Unknown or not yet implemented merge strategy");
             }
}

std::string SimpleTruthValue::toString() const
{
    char buf[1024];
    sprintf(buf, "(stv %f %f)",
            static_cast<float>(getMean()),
            static_cast<float>(getConfidence()));
    return buf;
}

bool SimpleTruthValue::operator==(const TruthValue& rhs) const
{
    const SimpleTruthValue *stv = dynamic_cast<const SimpleTruthValue *>(&rhs);
    if (NULL == stv) return false;

#define FLOAT_ACCEPTABLE_MEAN_ERROR 0.000001
    if (FLOAT_ACCEPTABLE_MEAN_ERROR < fabs(mean - stv->mean)) return false;

// Converting from confidence to count and back again using single-precision
// float is a real accuracy killer.  In particular, 2/802 = 0.002494 but
// converting back gives 800*0.002494/(1.0-0.002494) = 2.000188 and so
// comparison tests can only be accurate to about 0.000188 or
// thereabouts.
#define FLOAT_ACCEPTABLE_COUNT_ERROR 0.0002

    if (FLOAT_ACCEPTABLE_COUNT_ERROR < fabs(1.0 - (stv->count/count))) return false;
    return true;
}

TruthValueType SimpleTruthValue::getType() const
{
    return SIMPLE_TRUTH_VALUE;
}

count_t SimpleTruthValue::confidenceToCount(confidence_t cf)
{
    // There are not quite 16 digits in double precision
    // not quite 7 in single-precision float
    cf = std::min(cf, 0.9999998f);
    return static_cast<count_t>(KKK * cf / (1.0f - cf));
}

confidence_t SimpleTruthValue::countToConfidence(count_t cn)
{
    return static_cast<confidence_t>(cn / (cn + KKK));
}
<|endoftext|>