<commit_before>/*
 * Copyright (C) 2003-2006 Funambol
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * 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 General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#include "base/fscapi.h"
#include "base/messages.h"
#include "base/util/utils.h"
#include "base/util/WString.h"
#include "base/util/StringBuffer.h"

const size_t WString::npos = 0xFFFFFFFF;
static size_t growup = 5;

#if 0
/// FIXME: Debug code
#include <stdio.h>
void abort(const char *msg)
{
    FILE *f=fopen("\fatal.txt", "w");
    fputs(msg, f);
    fclose(f);
    exit(1);
}
size_t charlen = sizeof(wchar_t);
    if(charlen != 2) {
        abort("Panic: wide char size in not 2");
    }

#endif

WString::WString(const wchar_t* str, size_t len) {
    size = 0;
    s = 0;

    // if the given string is null, leave this null,
    // otherwise set it, even empty.
    if (str) {
        size_t slen = wcslen(str);
        size_t newlen = (len > slen) ? slen : len ;

        if(newlen) {
            getmem(newlen);
            wcsncpy(s, str, newlen);
            s[newlen]=0;
        }
        else {  // empty string
            getmem(1);
            s[0] = 0;
        }
    }
}

WString::WString(const WString &sb) {
    size = 0;
    s = NULL;
    append(sb.c_str());
}

WString::~WString() {
    freemem();
}

WString& WString::append(const wchar_t* sNew) {
    if (sNew == NULL) {
        return *this;
    }

    unsigned long len = wcslen(sNew);

    if (len == 0) {
        return *this;
    }
    if (s) {
        getmem(wcslen(s) + len);
        wcscat(s, sNew);
    }
    else {
        getmem(len);
        wcscpy(s, sNew);
    }

    return *this;
}

WString& WString::append(unsigned long i, BOOL sign) {
    wchar_t v[12];

    if (sign) {
        // wcsprintf(v, "%ld", i);
        wsprintf(v, TEXT("%ld"), i);
    } else {
        // wcsprintf(v, "%lu", i);
        wsprintf(v, TEXT("%lu"), i);
    }

    append(v);

    return *this;
}

WString& WString::append(WString& str) {
    return append(str.getChars());
}

WString& WString::append(WString* str) {
    if (str)
        return append(str->getChars());
    else
        return *this;
}

WString& WString::set(const wchar_t* sNew) {
    if (sNew) {
        size_t len = wcslen(sNew);
        if ( len ) {
            getmem( len );
            wcscpy(s, sNew);
        }
        else if (s) {
            s[0]=0; // just make the string empty
        } else {
             getmem( 2 );
             wcscpy(s, TEXT(""));
        }
    }
    else {
        freemem();  // release the string and set it to null
    }

    return *this;
}

const wchar_t* WString::getChars() const { return s; }

unsigned long WString::length() const {
    return (s) ? wcslen(s) : 0;
}

WString& WString::reset() {
    freemem();
    return *this;
}

size_t WString::find(const wchar_t *str, size_t pos) const
{
    if (pos >= length())
        return npos;
    wchar_t *p = wcsstr(s+pos, str);
    if(!p)
        return npos;
    return (p-s);
}

/***** FIXME
size_t WString::ifind(const wchar_t *str, size_t pos) const
{
    if (pos >= length())
        return npos;
    wchar_t *ls = wcstolower(s+pos);
    wchar_t *lstr = wcstolower(str);

    wchar_t *p = wcsstr(ls, lstr);

    size_t ret = (p) ? p-ls : npos;

    delete [] ls;
    delete [] lstr;

    return ret;
}
*/
size_t WString::replace(const wchar_t *from, const wchar_t *to, size_t pos)
{
	size_t ret = npos;

    if (!s)
        return npos;

    if(pos>=length())
        return npos;

    wchar_t *p = wcsstr(s+pos, from);
    if (p) {
        size_t flen = wcslen(from), tlen = wcslen(to);
        wchar_t *tail = 0;
        int ldiff = tlen - flen ;

        // reallocate if needed
        getmem(length() + ldiff);
        // check is there is a remainder after the replaced token
        if( p[flen] ) {
            tail = new wchar_t[length()];
            wcscpy(tail, p+flen);
        }
        // copy to in place of from
        wcscpy(p, to);
        // copy the remainder of old string, if there is one
        if( tail ) {
            wcscpy(p+tlen, tail);
            delete [] tail;
        }
		ret = p - s;
    }
    return ret;
}

// TODO: implement some smarter argorithm to avoid multiple reallocations
int WString::replaceAll(const wchar_t *from, const wchar_t *to, size_t pos) {
    int i=0;
    size_t next;
    for(next=replace(from, to, pos); next != npos; next=replace(from,to,next) ) {
        i++;
    }
    return i;
}

ArrayList& WString::split(ArrayList &tokens, const wchar_t *separator) const {
    tokens.clear();
    size_t seplen = wcslen(separator);
    wchar_t *base = s;
    wchar_t *p = wcsstr( base, separator );

    while( p )
    {
        WString token(base, p-base);
        tokens.add( token );
        base = p + seplen;
        p = wcsstr( base, separator );
    }
    WString token(base);
    tokens.add( token );

    return tokens;
}

WString& WString::join(ArrayList &tokens, const wchar_t *separator) {
    WString *line;
    size_t totlen = 0, seplen = wcslen(separator);
    // Calc total size
    for (line=(WString *)tokens.front();
		 line;
         line=(WString *)tokens.next() ) {
        totlen += line->length() + seplen;
    }
    // Reserve mem
    reserve(totlen);
    // Join strings
    for (line=(WString *)tokens.front();
		 line;
         line=(WString *)tokens.next() ) {
        this->append(line->c_str());
        this->append(separator);
    }
    return *this;
}

WString WString::substr(size_t pos, size_t len) const {
    if(pos > wcslen(s))
        return WString(TEXT(""));

	return (WString(s+pos, len));
}

void WString::reserve(size_t len) {
    getmem(len);
}

WString& WString::upperCase() {
    wchar_t* p = NULL;

    for(p = s; *p; p++) {
        *p=towupper(*p);
    }

    return *this;
}

WString& WString::lowerCase() {
    wchar_t* p = NULL;

    for(p = s; *p; p++) {
        *p=towlower(*p);
    }

    return *this;
}

/**
 * Perform case insensitive compare
 */
bool WString::icmp(const wchar_t *sc) const {
    return (_wcsicmp(s, sc)==0);
}

ArrayElement* WString::clone() {
    return new WString(s);
}

bool WString::empty() const {
    if(!s)
        return true;
    if(!*s)
        return true;
    return false;
}

bool WString::null() const { return (s==0); }

// Member Operators
WString& WString::operator= (const wchar_t* sc)
    { return set(sc); }
WString& WString::operator= (const WString& sb)
    { return set(sb); }
WString& WString::operator= (const StringBuffer& sb) {
    wchar_t* t = NULL;
    t = toWideChar(sb.c_str());
    WString& w = set(t);
    delete [] t;
    return w;
}
WString& WString::operator+= (const wchar_t* sc)
    { append(sc); return *this; }
WString& WString::operator+= (const WString& s)
    { append(s); return *this; }
bool  WString::operator== (const wchar_t* sc) const
    { return wcscmp(s, sc) == 0; }
bool  WString::operator== (const WString& sb) const
    { return wcscmp(s, sb.c_str()) == 0; }
bool  WString::operator!= (const wchar_t* sc) const
    { return !(*this == sc); }
bool  WString::operator!= (const WString& s) const
    { return !(*this == s); }

// Function operators
WString operator+(const WString& x, const wchar_t *y)
{
  WString result(x);
  result.append(y);
  return result;
}

//size_t WString_memcount = 0;

// Private allocator
void WString::getmem(size_t len)
{
    //fprintf(stderr, "GETMEM len=%ld s=%lx size=%ld\n", len, s, size);
    if(len > size) {
        // Remember the old length (0 for the null string)
        size_t oldlen = length();
        // Realloc the string (like malloc when s is null)
        s = (wchar_t *)realloc(s, (len+1) * sizeof(wchar_t) );
        //WString_memcount += (len-size);
        size = len;
        // Make sure s is terminated at the old position
        // (malloc may fill the buffer with rubbish)
        s[oldlen] = 0;
        //fprintf(stderr, "\t\ts=%lx size=%ld alloc=%ld\n", s, size, alloclen);
    }
}

// Private deallocator
void WString::freemem()
{
    if(s) {
        free(s); s = 0;
        //fprintf(stderr, "FREEMEM s=%lx size=%ld\n", s, size);
    }
    //WString_memcount -= size;
    size = 0;
}

<commit_msg>- fix in replaceAll(), to avoid continue auto replace. Ex. for replacing & with &<commit_after>/*
 * Copyright (C) 2003-2006 Funambol
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * 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 General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#include "base/fscapi.h"
#include "base/messages.h"
#include "base/util/utils.h"
#include "base/util/WString.h"
#include "base/util/StringBuffer.h"

const size_t WString::npos = 0xFFFFFFFF;
static size_t growup = 5;

#if 0
/// FIXME: Debug code
#include <stdio.h>
void abort(const char *msg)
{
    FILE *f=fopen("\fatal.txt", "w");
    fputs(msg, f);
    fclose(f);
    exit(1);
}
size_t charlen = sizeof(wchar_t);
    if(charlen != 2) {
        abort("Panic: wide char size in not 2");
    }

#endif

WString::WString(const wchar_t* str, size_t len) {
    size = 0;
    s = 0;

    // if the given string is null, leave this null,
    // otherwise set it, even empty.
    if (str) {
        size_t slen = wcslen(str);
        size_t newlen = (len > slen) ? slen : len ;

        if(newlen) {
            getmem(newlen);
            wcsncpy(s, str, newlen);
            s[newlen]=0;
        }
        else {  // empty string
            getmem(1);
            s[0] = 0;
        }
    }
}

WString::WString(const WString &sb) {
    size = 0;
    s = NULL;
    append(sb.c_str());
}

WString::~WString() {
    freemem();
}

WString& WString::append(const wchar_t* sNew) {
    if (sNew == NULL) {
        return *this;
    }

    unsigned long len = wcslen(sNew);

    if (len == 0) {
        return *this;
    }
    if (s) {
        getmem(wcslen(s) + len);
        wcscat(s, sNew);
    }
    else {
        getmem(len);
        wcscpy(s, sNew);
    }

    return *this;
}

WString& WString::append(unsigned long i, BOOL sign) {
    wchar_t v[12];

    if (sign) {
        // wcsprintf(v, "%ld", i);
        wsprintf(v, TEXT("%ld"), i);
    } else {
        // wcsprintf(v, "%lu", i);
        wsprintf(v, TEXT("%lu"), i);
    }

    append(v);

    return *this;
}

WString& WString::append(WString& str) {
    return append(str.getChars());
}

WString& WString::append(WString* str) {
    if (str)
        return append(str->getChars());
    else
        return *this;
}

WString& WString::set(const wchar_t* sNew) {
    if (sNew) {
        size_t len = wcslen(sNew);
        if ( len ) {
            getmem( len );
            wcscpy(s, sNew);
        }
        else if (s) {
            s[0]=0; // just make the string empty
        } else {
             getmem( 2 );
             wcscpy(s, TEXT(""));
        }
    }
    else {
        freemem();  // release the string and set it to null
    }

    return *this;
}

const wchar_t* WString::getChars() const { return s; }

unsigned long WString::length() const {
    return (s) ? wcslen(s) : 0;
}

WString& WString::reset() {
    freemem();
    return *this;
}

size_t WString::find(const wchar_t *str, size_t pos) const
{
    if (pos >= length())
        return npos;
    wchar_t *p = wcsstr(s+pos, str);
    if(!p)
        return npos;
    return (p-s);
}

/***** FIXME
size_t WString::ifind(const wchar_t *str, size_t pos) const
{
    if (pos >= length())
        return npos;
    wchar_t *ls = wcstolower(s+pos);
    wchar_t *lstr = wcstolower(str);

    wchar_t *p = wcsstr(ls, lstr);

    size_t ret = (p) ? p-ls : npos;

    delete [] ls;
    delete [] lstr;

    return ret;
}
*/
size_t WString::replace(const wchar_t *from, const wchar_t *to, size_t pos)
{
	size_t ret = npos;

    if (!s)
        return npos;

    if(pos>=length())
        return npos;

    wchar_t *p = wcsstr(s+pos, from);
    if (p) {
        size_t flen = wcslen(from), tlen = wcslen(to);
        wchar_t *tail = 0;
        int ldiff = tlen - flen ;

        // reallocate if needed
        getmem(length() + ldiff);
        // check is there is a remainder after the replaced token
        if( p[flen] ) {
            tail = new wchar_t[length()];
            wcscpy(tail, p+flen);
        }
        // copy to in place of from
        wcscpy(p, to);
        // copy the remainder of old string, if there is one
        if( tail ) {
            wcscpy(p+tlen, tail);
            delete [] tail;
        }
		ret = p - s;
    }
    return ret;
}

// TODO: implement some smarter argorithm to avoid multiple reallocations
int WString::replaceAll(const wchar_t *from, const wchar_t *to, size_t pos) {
    int i=0;
    int len = wcslen(to);
    size_t next;
    for(next=replace(from, to, pos); next != npos; next=replace(from,to,next + len) ) {
        i++;
    }
    return i;
}

ArrayList& WString::split(ArrayList &tokens, const wchar_t *separator) const {
    tokens.clear();
    size_t seplen = wcslen(separator);
    wchar_t *base = s;
    wchar_t *p = wcsstr( base, separator );

    while( p )
    {
        WString token(base, p-base);
        tokens.add( token );
        base = p + seplen;
        p = wcsstr( base, separator );
    }
    WString token(base);
    tokens.add( token );

    return tokens;
}

WString& WString::join(ArrayList &tokens, const wchar_t *separator) {
    WString *line;
    size_t totlen = 0, seplen = wcslen(separator);
    // Calc total size
    for (line=(WString *)tokens.front();
		 line;
         line=(WString *)tokens.next() ) {
        totlen += line->length() + seplen;
    }
    // Reserve mem
    reserve(totlen);
    // Join strings
    for (line=(WString *)tokens.front();
		 line;
         line=(WString *)tokens.next() ) {
        this->append(line->c_str());
        this->append(separator);
    }
    return *this;
}

WString WString::substr(size_t pos, size_t len) const {
    if(pos > wcslen(s))
        return WString(TEXT(""));

	return (WString(s+pos, len));
}

void WString::reserve(size_t len) {
    getmem(len);
}

WString& WString::upperCase() {
    wchar_t* p = NULL;

    for(p = s; *p; p++) {
        *p=towupper(*p);
    }

    return *this;
}

WString& WString::lowerCase() {
    wchar_t* p = NULL;

    for(p = s; *p; p++) {
        *p=towlower(*p);
    }

    return *this;
}

/**
 * Perform case insensitive compare
 */
bool WString::icmp(const wchar_t *sc) const {
    return (_wcsicmp(s, sc)==0);
}

ArrayElement* WString::clone() {
    return new WString(s);
}

bool WString::empty() const {
    if(!s)
        return true;
    if(!*s)
        return true;
    return false;
}

bool WString::null() const { return (s==0); }

// Member Operators
WString& WString::operator= (const wchar_t* sc)
    { return set(sc); }
WString& WString::operator= (const WString& sb)
    { return set(sb); }
WString& WString::operator= (const StringBuffer& sb) {
    wchar_t* t = NULL;
    t = toWideChar(sb.c_str());
    WString& w = set(t);
    delete [] t;
    return w;
}
WString& WString::operator+= (const wchar_t* sc)
    { append(sc); return *this; }
WString& WString::operator+= (const WString& s)
    { append(s); return *this; }
bool  WString::operator== (const wchar_t* sc) const
    { return wcscmp(s, sc) == 0; }
bool  WString::operator== (const WString& sb) const
    { return wcscmp(s, sb.c_str()) == 0; }
bool  WString::operator!= (const wchar_t* sc) const
    { return !(*this == sc); }
bool  WString::operator!= (const WString& s) const
    { return !(*this == s); }

// Function operators
WString operator+(const WString& x, const wchar_t *y)
{
  WString result(x);
  result.append(y);
  return result;
}

//size_t WString_memcount = 0;

// Private allocator
void WString::getmem(size_t len)
{
    //fprintf(stderr, "GETMEM len=%ld s=%lx size=%ld\n", len, s, size);
    if(len > size) {
        // Remember the old length (0 for the null string)
        size_t oldlen = length();
        // Realloc the string (like malloc when s is null)
        s = (wchar_t *)realloc(s, (len+1) * sizeof(wchar_t) );
        //WString_memcount += (len-size);
        size = len;
        // Make sure s is terminated at the old position
        // (malloc may fill the buffer with rubbish)
        s[oldlen] = 0;
        //fprintf(stderr, "\t\ts=%lx size=%ld alloc=%ld\n", s, size, alloclen);
    }
}

// Private deallocator
void WString::freemem()
{
    if(s) {
        free(s); s = 0;
        //fprintf(stderr, "FREEMEM s=%lx size=%ld\n", s, size);
    }
    //WString_memcount -= size;
    size = 0;
}

<|endoftext|>


<commit_before>/**
 * This file is part of the "libfnord" project
 *   Copyright (c) 2015 Paul Asmuth
 *
 * FnordMetric is free software: you can redistribute it and/or modify it under
 * the terms of the GNU General Public License v3.0. You should have received a
 * copy of the GNU General Public License along with this program. If not, see
 * <http://www.gnu.org/licenses/>.
 */
#include <fnord/stdtypes.h>
#include <fnord/exception.h>
#include <fnord/test/unittest.h>
#include "chartsql/runtime/DefaultRuntime.h"
#include "chartsql/qtree/SelectProjectAggregateNode.h"
#include "chartsql/qtree/ColumnReferenceNode.h"
#include "chartsql/qtree/CallExpressionNode.h"
#include "chartsql/qtree/LiteralExpressionNode.h"

using namespace fnord;
using namespace csql;

UNIT_TEST(RuntimeTest);

TEST_CASE(RuntimeTest, TestStaticExpression, [] () {
  DefaultRuntime runtime;

  auto expr = mkRef(
      new csql::CallExpressionNode(
          "add",
          {
            new csql::LiteralExpressionNode(SValue(SValue::IntegerType(1))),
            new csql::LiteralExpressionNode(SValue(SValue::IntegerType(2))),
          }));

  auto compiled = runtime.compiler()->compile(expr.get());

  SValue out;
  compiled->evaluateStatic(0, nullptr, &out);

  fnord::iputs("result: $0", out.toString());
  EXPECT_EQ(out.getInteger(), 3);
});

<commit_msg>run 100k times<commit_after>/**
 * This file is part of the "libfnord" project
 *   Copyright (c) 2015 Paul Asmuth
 *
 * FnordMetric is free software: you can redistribute it and/or modify it under
 * the terms of the GNU General Public License v3.0. You should have received a
 * copy of the GNU General Public License along with this program. If not, see
 * <http://www.gnu.org/licenses/>.
 */
#include <fnord/stdtypes.h>
#include <fnord/exception.h>
#include <fnord/wallclock.h>
#include <fnord/test/unittest.h>
#include "chartsql/runtime/DefaultRuntime.h"
#include "chartsql/qtree/SelectProjectAggregateNode.h"
#include "chartsql/qtree/ColumnReferenceNode.h"
#include "chartsql/qtree/CallExpressionNode.h"
#include "chartsql/qtree/LiteralExpressionNode.h"

using namespace fnord;
using namespace csql;

UNIT_TEST(RuntimeTest);

TEST_CASE(RuntimeTest, TestStaticExpression, [] () {
  DefaultRuntime runtime;

  auto expr = mkRef(
      new csql::CallExpressionNode(
          "add",
          {
            new csql::LiteralExpressionNode(SValue(SValue::IntegerType(1))),
            new csql::LiteralExpressionNode(SValue(SValue::IntegerType(2))),
          }));

  auto compiled = runtime.compiler()->compile(expr.get());

  auto t0 = WallClock::unixMicros();
  SValue out;
  for (int i = 0; i < 1000000; ++i) {
    compiled->evaluateStatic(0, nullptr, &out);
  }
  auto t1 = WallClock::unixMicros();

  EXPECT_EQ(out.getInteger(), 3);
});

<|endoftext|>


<commit_before>#include <aikido/statespace/SO3StateSpace.hpp>
#include <aikido/statespace/SO3StateSpaceSampleableConstraint.hpp>
#include <boost/math/constants/constants.hpp>
#include <dart/math/Geometry.h>
#include <iostream>

namespace aikido {
namespace statespace {

//=============================================================================
SO3StateSpace::SO3StateSpace::State::State()
  : mValue(1., 0., 0., 0.)
{
}

//=============================================================================
SO3StateSpace::SO3StateSpace::State::State(const Quaternion& _quaternion)
  : mValue(_quaternion)
{
  // TODO: Check if normalized.
}

//=============================================================================
auto SO3StateSpace::State::getQuaternion() const -> const Quaternion&
{
  return mValue;
}

//=============================================================================
void SO3StateSpace::State::setQuaternion(const Quaternion& _quaternion)
{
  // TODO: Check if normalized.
  mValue = _quaternion;
}

//=============================================================================
auto SO3StateSpace::createState() const -> ScopedState
{
  return ScopedState(this);
}

//=============================================================================
auto SO3StateSpace::getQuaternion(const State* _state) const
  -> const Quaternion&
{
  return _state->mValue;
}

//=============================================================================
void SO3StateSpace::setQuaternion(
  State* _state, const Quaternion& _quaternion) const
{
  _state->mValue = _quaternion;
}

//=============================================================================
size_t SO3StateSpace::getStateSizeInBytes() const
{
  return sizeof(State);
}

//=============================================================================
StateSpace::State* SO3StateSpace::allocateStateInBuffer(void* _buffer) const
{
  return new (_buffer) State;
}

//=============================================================================
void SO3StateSpace::freeStateInBuffer(StateSpace::State* _state) const
{
  static_cast<State*>(_state)->~State();
}

//=============================================================================
void SO3StateSpace::compose(
  const StateSpace::State* _state1, const StateSpace::State* _state2,
  StateSpace::State* _out) const
{
  auto state1 = static_cast<const State*>(_state1);
  auto state2 = static_cast<const State*>(_state2);
  auto out = static_cast<State*>(_out);

  out->mValue = state1->mValue * state2->mValue;
}

//=============================================================================
unsigned int SO3StateSpace::getDimension() const
{
    return 3;
}

//=============================================================================
double SO3StateSpace::getMaximumExtent() const
{
    return 0.5 * boost::math::constants::pi<double>(); // OMPL
}

//=============================================================================
double SO3StateSpace::getMeasure() const
{
    // half of the surface area of a unit 3-sphere
    return boost::math::constants::pi<double>() * boost::math::constants::pi<double>(); //OMPL
}

//=============================================================================
void SO3StateSpace::copyState(StateSpace::State* _destination,
                              const StateSpace::State* _source) const
{
    auto destination = static_cast<State*>(_destination);
    auto source = static_cast<const State*>(_source);

    setQuaternion(destination, getQuaternion(source));
}

//=============================================================================
double SO3StateSpace::distance(const StateSpace::State* _state1,
                               const StateSpace::State* _state2) const
{
    auto state1 = static_cast<const State*>(_state1);
    auto state2 = static_cast<const State*>(_state2);
    return getQuaternion(state1).angularDistance(getQuaternion(state2));
}

//=============================================================================
bool SO3StateSpace::equalStates(const StateSpace::State* _state1,
                                const StateSpace::State* _state2) const
{
    return distance(_state1, _state2) < std::numeric_limits<double>::epsilon();
}

//=============================================================================
void SO3StateSpace::interpolate(const StateSpace::State* _from,
                                const StateSpace::State* _to,
                                const double _t,
                                StateSpace::State* _state) const
{

    auto from = static_cast<const State*>(_from);
    auto to = static_cast<const State*>(_to);
    auto state = static_cast<State*>(_state);

    setQuaternion(state, getQuaternion(from).slerp(_t, getQuaternion(to)));
}

//=============================================================================
void SO3StateSpace::expMap(
  const Eigen::VectorXd& _tangent, StateSpace::State* _out) const
{
  auto out = static_cast<State*>(_out);

  // TODO: Skip these checks in release mode.
  if (_tangent.rows() != 3)
  {
    std::stringstream msg;
    msg << "_tangent has incorrect size: expected 3"
        << ", got " << _tangent.rows() << ".\n";
    throw std::runtime_error(msg.str());
  }

  Eigen::Vector6d tangent(Eigen::Vector6d::Zero());
  tangent.topRows(3) = _tangent;

  Eigen::Isometry3d transform = dart::math::expMap(tangent);
  out->setQuaternion(Quaternion(transform.rotation()));
}

} // namespace statespace
} // namespace aikido
<commit_msg>Fixed failing tests.<commit_after>#include <aikido/statespace/SO3StateSpace.hpp>
#include <aikido/statespace/SO3StateSpaceSampleableConstraint.hpp>
#include <boost/math/constants/constants.hpp>
#include <dart/math/Geometry.h>
#include <iostream>

namespace aikido {
namespace statespace {

//=============================================================================
SO3StateSpace::SO3StateSpace::State::State()
  : mValue(1., 0., 0., 0.)
{
}

//=============================================================================
SO3StateSpace::SO3StateSpace::State::State(const Quaternion& _quaternion)
  : mValue(_quaternion)
{
  // TODO: Check if normalized.
}

//=============================================================================
auto SO3StateSpace::State::getQuaternion() const -> const Quaternion&
{
  return mValue;
}

//=============================================================================
void SO3StateSpace::State::setQuaternion(const Quaternion& _quaternion)
{
  // TODO: Check if normalized.
  mValue = _quaternion;
}

//=============================================================================
auto SO3StateSpace::createState() const -> ScopedState
{
  return ScopedState(this);
}

//=============================================================================
auto SO3StateSpace::getQuaternion(const State* _state) const
  -> const Quaternion&
{
  return _state->mValue;
}

//=============================================================================
void SO3StateSpace::setQuaternion(
  State* _state, const Quaternion& _quaternion) const
{
  _state->mValue = _quaternion;
}

//=============================================================================
size_t SO3StateSpace::getStateSizeInBytes() const
{
  return sizeof(State);
}

//=============================================================================
StateSpace::State* SO3StateSpace::allocateStateInBuffer(void* _buffer) const
{
  return new (_buffer) State;
}

//=============================================================================
void SO3StateSpace::freeStateInBuffer(StateSpace::State* _state) const
{
  static_cast<State*>(_state)->~State();
}

//=============================================================================
void SO3StateSpace::compose(
  const StateSpace::State* _state1, const StateSpace::State* _state2,
  StateSpace::State* _out) const
{
  auto state1 = static_cast<const State*>(_state1);
  auto state2 = static_cast<const State*>(_state2);
  auto out = static_cast<State*>(_out);

  out->mValue = state1->mValue * state2->mValue;
}

//=============================================================================
unsigned int SO3StateSpace::getDimension() const
{
    return 3;
}

//=============================================================================
double SO3StateSpace::getMaximumExtent() const
{
    return 0.5 * boost::math::constants::pi<double>(); // OMPL
}

//=============================================================================
double SO3StateSpace::getMeasure() const
{
    // half of the surface area of a unit 3-sphere
    return boost::math::constants::pi<double>() * boost::math::constants::pi<double>(); //OMPL
}

//=============================================================================
void SO3StateSpace::copyState(StateSpace::State* _destination,
                              const StateSpace::State* _source) const
{
    auto destination = static_cast<State*>(_destination);
    auto source = static_cast<const State*>(_source);

    setQuaternion(destination, getQuaternion(source));
}

//=============================================================================
double SO3StateSpace::distance(const StateSpace::State* _state1,
                               const StateSpace::State* _state2) const
{
    auto state1 = static_cast<const State*>(_state1);
    auto state2 = static_cast<const State*>(_state2);
    return getQuaternion(state1).angularDistance(getQuaternion(state2));
}

//=============================================================================
bool SO3StateSpace::equalStates(const StateSpace::State* _state1,
                                const StateSpace::State* _state2) const
{
    // TODO: Make this a parameter.
    return distance(_state1, _state2) < 1e-7;
}

//=============================================================================
void SO3StateSpace::interpolate(const StateSpace::State* _from,
                                const StateSpace::State* _to,
                                const double _t,
                                StateSpace::State* _state) const
{

    auto from = static_cast<const State*>(_from);
    auto to = static_cast<const State*>(_to);
    auto state = static_cast<State*>(_state);

    setQuaternion(state, getQuaternion(from).slerp(_t, getQuaternion(to)));
}

//=============================================================================
void SO3StateSpace::expMap(
  const Eigen::VectorXd& _tangent, StateSpace::State* _out) const
{
  auto out = static_cast<State*>(_out);

  // TODO: Skip these checks in release mode.
  if (_tangent.rows() != 3)
  {
    std::stringstream msg;
    msg << "_tangent has incorrect size: expected 3"
        << ", got " << _tangent.rows() << ".\n";
    throw std::runtime_error(msg.str());
  }

  Eigen::Vector6d tangent(Eigen::Vector6d::Zero());
  tangent.topRows(3) = _tangent;

  Eigen::Isometry3d transform = dart::math::expMap(tangent);
  out->setQuaternion(Quaternion(transform.rotation()));
}

} // namespace statespace
} // namespace aikido
<|endoftext|>


<commit_before>/*
Copyright (c) 2012 Carsten Burstedde, Donna Calhoun
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

 * Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include "amr_forestclaw.H"
#include "amr_utils.H"
#include "clawpack_fort.H"
#include "fclaw2d_solvers.H"

// Put this here so that I don't have to include "ClawPatch.H"
void set_clawpatch(fclaw2d_domain_t* domain, fclaw2d_patch_t *this_patch,
                   int blockno, int patchno);

// This is essentially the same function that is in amr_regrid.cpp
static
void cb_tag4refinement_init(fclaw2d_domain_t *domain,
                            fclaw2d_patch_t *this_patch,
                            int this_block_idx,
                            int this_patch_idx,
                            void *user)
{
    const amr_options_t *gparms = get_domain_parms(domain);

    int maxlevel = gparms->maxlevel;
    int level = this_patch->level;
    int initflag = 1;

    if (level < maxlevel)
    {
        fclaw2d_regrid_functions_t* rf = get_regrid_functions(domain);
        fclaw_bool refine_patch =
            (rf->f_patch_tag4refinement)(domain,this_patch,this_block_idx,
                                         this_patch_idx,initflag);
        if (refine_patch)
        {
            fclaw2d_patch_mark_refine(domain, this_block_idx, this_patch_idx);
        }
    }
}

/* -----------------------------------------------------------------
   Initial grid
   ----------------------------------------------------------------- */
static
void cb_initialize (fclaw2d_domain_t *domain,
                    fclaw2d_patch_t *this_patch,
                    int this_block_idx,
                    int this_patch_idx,
                    void *user)
{
    set_clawpatch(domain,this_patch,this_block_idx, this_patch_idx);

    fclaw2d_solver_functions_t *sf = get_solver_functions(domain);

    // One-time setup of patch
    (sf->f_patch_setup)(domain,this_patch,this_block_idx,this_patch_idx);

    // Set initial values on patch
    (sf->f_patch_initialize)(domain,this_patch,this_block_idx,this_patch_idx);
}

static
void cb_domain_adapt_init (fclaw2d_domain_t * old_domain,
                           fclaw2d_patch_t * old_patch,
                           fclaw2d_domain_t * new_domain,
                           fclaw2d_patch_t * new_patch,
                           fclaw2d_patch_relation_t newsize,
                           int blockno,
                           int old_patchno, int new_patchno,
                           void *user)
{
    if (newsize == FCLAW2D_PATCH_SAMESIZE)
    {
        // Grid doesn't change
        set_clawpatch(new_domain,new_patch,blockno,new_patchno);

        // Setup new patch using solver specific routine
        fclaw2d_solver_functions_t *sf = get_solver_functions(old_domain);
        (sf->f_patch_setup)(new_domain,new_patch,blockno,new_patchno);

        // Need a copy function in regrid_functions
        fclaw2d_regrid_functions_t *rf = get_regrid_functions(old_domain);
        (rf->f_patch_copy2samesize)(new_domain,old_patch,new_patch,blockno,old_patchno,
                                    new_patchno);
    }
    else if (newsize == FCLAW2D_PATCH_HALFSIZE)
    {
        // Patch has been refined.
        fclaw2d_patch_t *fine_siblings = new_patch;

        // Loop over four siblings (z-ordering) and interpolate
        // data from coarser parent grid to each finer sibling grid.
        for (int igrid = 0; igrid < NumSiblings; igrid++)
        {
            fclaw2d_patch_t *fine_patch = &fine_siblings[igrid];
            int fine_patchno = new_patchno + igrid;

            // Create new ClawPatch and assign patch pointer to it.
            set_clawpatch(new_domain, fine_patch, blockno, fine_patchno);

            // Do one-time setup on new patch
            fclaw2d_solver_functions_t *sf = get_solver_functions(old_domain);
            (sf->f_patch_setup)(new_domain,fine_patch,blockno,fine_patchno);

            // This is only used here, since only in the initial grid layout do we
            // create fine grids from coarser grids.
            (sf->f_patch_initialize)(new_domain,fine_patch,blockno,fine_patchno);
        }
    }
    else if (newsize == FCLAW2D_PATCH_DOUBLESIZE)
    {
        // We don't coarsen for the initial time step
    }
    else
    {
        printf("cb_adapt_domain : newsize not recognized\n");
        exit(1);
    }
}

// Initialize a base level of grids
void amrinit (fclaw2d_domain_t **domain)
{
    const amr_options_t *gparms = get_domain_parms(*domain);
    fclaw2d_domain_data_t* ddata = get_domain_data(*domain);

    // Set problem dependent parameters for Riemann solvers, etc.
    (ddata->f_problem_setup)(*domain);

    double t = 0;

    set_domain_time(*domain,t);

    int minlevel = gparms->minlevel;
    int maxlevel = gparms->maxlevel;

    init_block_and_patch_data(*domain);  /* Allocate block and patch data */

    // Initialize base level grid - combine with 'amr_set_base_level' above?
    fclaw2d_domain_iterate_level(*domain, minlevel, cb_initialize,
                                 (void *) NULL);

    int num = (*domain)->num_blocks;
    for (int i = 0; i < num; i++)
    {
        fclaw2d_block_t *block = &(*domain)->blocks[i];
        set_block_data(block,gparms->mthbc);
    }

    set_phys_bc(*domain,minlevel,t);

    // Refine as needed.

    for (int level = minlevel; level < maxlevel; level++)
    {
        // TODO: don't use level_refined since it is not agreed upon in parallel
        // the fclaw2d_domain_adapt and _partition calls work fine in parallel

        fclaw2d_domain_iterate_level(*domain, level,
                                     cb_tag4refinement_init,
                                     (void *) NULL);

        // Rebuild domain if necessary
        fclaw2d_domain_t *new_domain = fclaw2d_domain_adapt(*domain);

        if (new_domain != NULL)
        {
            // This is just for fun; remove when it gets annoying.
            // fclaw2d_domain_list_adapted(*domain, new_domain, SC_LP_STATISTICS);

            // Allocate memory for user data types (but they don't get set)
            init_domain_data(new_domain);
            copy_domain_data(*domain,new_domain);

            // Initialize new grids.  Assume that all ghost cells are filled
            //in by qinit.
            init_block_and_patch_data(new_domain);

            // Physical BCs are needed in boundary level exchange
            // Assume only one block, since we are assuming mthbc
            int num = new_domain->num_blocks;
            for (int i = 0; i < num; i++)
            {
                fclaw2d_block_t *block = &new_domain->blocks[i];
                // This is kind of dumb for now, since block won't in general
                // have the same physical boundary conditions types.
                set_block_data(block,gparms->mthbc);
            }

            fclaw2d_domain_iterate_adapted(*domain, new_domain,
                                           cb_domain_adapt_init,
                                           (void *) NULL);

            int new_level = level+1;
            // Upon initialization, we don't do any ghost cell exchanges, because we assume
            // that the initial conditions have set all internal ghost cells.
            set_phys_bc(new_domain,new_level,t);

            // free all memory associated with old domain
            amrreset(domain);
            *domain = new_domain;

            fclaw2d_domain_t *domain_partitioned =
                fclaw2d_domain_partition (*domain);
            if (domain_partitioned != NULL)
            {
                // TODO: allocate patch and block etc. memory for domain_partitioned

                // TODO: write a function to transfer values in parallel */

                /* then the old domain is no longer necessary */
                amrreset(domain);
                *domain = domain_partitioned;

                /* internal clean up */
                fclaw2d_domain_complete(*domain);
            }
        }
        else
        {
            // exit loop;  we are done refining
            break;
        }
    }
}
<commit_msg>Changed comment in init version of domain_adapt;  no interpolation is done in this version<commit_after>/*
Copyright (c) 2012 Carsten Burstedde, Donna Calhoun
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

 * Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include "amr_forestclaw.H"
#include "amr_utils.H"
#include "clawpack_fort.H"
#include "fclaw2d_solvers.H"

// Put this here so that I don't have to include "ClawPatch.H"
void set_clawpatch(fclaw2d_domain_t* domain, fclaw2d_patch_t *this_patch,
                   int blockno, int patchno);

// This is essentially the same function that is in amr_regrid.cpp
static
void cb_tag4refinement_init(fclaw2d_domain_t *domain,
                            fclaw2d_patch_t *this_patch,
                            int this_block_idx,
                            int this_patch_idx,
                            void *user)
{
    const amr_options_t *gparms = get_domain_parms(domain);

    int maxlevel = gparms->maxlevel;
    int level = this_patch->level;
    int initflag = 1;

    if (level < maxlevel)
    {
        fclaw2d_regrid_functions_t* rf = get_regrid_functions(domain);
        fclaw_bool refine_patch =
            (rf->f_patch_tag4refinement)(domain,this_patch,this_block_idx,
                                         this_patch_idx,initflag);
        if (refine_patch)
        {
            fclaw2d_patch_mark_refine(domain, this_block_idx, this_patch_idx);
        }
    }
}

/* -----------------------------------------------------------------
   Initial grid
   ----------------------------------------------------------------- */
static
void cb_initialize (fclaw2d_domain_t *domain,
                    fclaw2d_patch_t *this_patch,
                    int this_block_idx,
                    int this_patch_idx,
                    void *user)
{
    set_clawpatch(domain,this_patch,this_block_idx, this_patch_idx);

    fclaw2d_solver_functions_t *sf = get_solver_functions(domain);

    // One-time setup of patch
    (sf->f_patch_setup)(domain,this_patch,this_block_idx,this_patch_idx);

    // Set initial values on patch
    (sf->f_patch_initialize)(domain,this_patch,this_block_idx,this_patch_idx);
}

static
void cb_domain_adapt_init (fclaw2d_domain_t * old_domain,
                           fclaw2d_patch_t * old_patch,
                           fclaw2d_domain_t * new_domain,
                           fclaw2d_patch_t * new_patch,
                           fclaw2d_patch_relation_t newsize,
                           int blockno,
                           int old_patchno, int new_patchno,
                           void *user)
{
    if (newsize == FCLAW2D_PATCH_SAMESIZE)
    {
        // Grid doesn't change
        set_clawpatch(new_domain,new_patch,blockno,new_patchno);

        // Setup new patch using solver specific routine
        fclaw2d_solver_functions_t *sf = get_solver_functions(old_domain);
        (sf->f_patch_setup)(new_domain,new_patch,blockno,new_patchno);

        // Need a copy function in regrid_functions
        fclaw2d_regrid_functions_t *rf = get_regrid_functions(old_domain);
        (rf->f_patch_copy2samesize)(new_domain,old_patch,new_patch,blockno,old_patchno,
                                    new_patchno);
    }
    else if (newsize == FCLAW2D_PATCH_HALFSIZE)
    {
        // Patch has been refined.
        fclaw2d_patch_t *fine_siblings = new_patch;

        // Loop over four siblings (z-ordering) and initialize each one
        for (int igrid = 0; igrid < NumSiblings; igrid++)
        {
            fclaw2d_patch_t *fine_patch = &fine_siblings[igrid];
            int fine_patchno = new_patchno + igrid;

            // Create new ClawPatch and assign patch pointer to it.
            set_clawpatch(new_domain, fine_patch, blockno, fine_patchno);

            // Do one-time setup on new patch
            fclaw2d_solver_functions_t *sf = get_solver_functions(old_domain);
            (sf->f_patch_setup)(new_domain,fine_patch,blockno,fine_patchno);

            // This is only used here, since only in the initial grid layout do we
            // create fine grids from coarser grids.
            (sf->f_patch_initialize)(new_domain,fine_patch,blockno,fine_patchno);
        }
    }
    else if (newsize == FCLAW2D_PATCH_DOUBLESIZE)
    {
        // We don't coarsen for the initial time step
    }
    else
    {
        printf("cb_adapt_domain : newsize not recognized\n");
        exit(1);
    }
}

// Initialize a base level of grids
void amrinit (fclaw2d_domain_t **domain)
{
    const amr_options_t *gparms = get_domain_parms(*domain);
    fclaw2d_domain_data_t* ddata = get_domain_data(*domain);

    // Set problem dependent parameters for Riemann solvers, etc.
    (ddata->f_problem_setup)(*domain);

    double t = 0;

    set_domain_time(*domain,t);

    int minlevel = gparms->minlevel;
    int maxlevel = gparms->maxlevel;

    init_block_and_patch_data(*domain);  /* Allocate block and patch data */

    // Initialize base level grid - combine with 'amr_set_base_level' above?
    fclaw2d_domain_iterate_level(*domain, minlevel, cb_initialize,
                                 (void *) NULL);

    int num = (*domain)->num_blocks;
    for (int i = 0; i < num; i++)
    {
        fclaw2d_block_t *block = &(*domain)->blocks[i];
        set_block_data(block,gparms->mthbc);
    }

    set_phys_bc(*domain,minlevel,t);

    // Refine as needed.

    for (int level = minlevel; level < maxlevel; level++)
    {
        // TODO: don't use level_refined since it is not agreed upon in parallel
        // the fclaw2d_domain_adapt and _partition calls work fine in parallel

        fclaw2d_domain_iterate_level(*domain, level,
                                     cb_tag4refinement_init,
                                     (void *) NULL);

        // Rebuild domain if necessary
        fclaw2d_domain_t *new_domain = fclaw2d_domain_adapt(*domain);

        if (new_domain != NULL)
        {
            // This is just for fun; remove when it gets annoying.
            // fclaw2d_domain_list_adapted(*domain, new_domain, SC_LP_STATISTICS);

            // Allocate memory for user data types (but they don't get set)
            init_domain_data(new_domain);
            copy_domain_data(*domain,new_domain);

            // Initialize new grids.  Assume that all ghost cells are filled
            //in by qinit.
            init_block_and_patch_data(new_domain);

            // Physical BCs are needed in boundary level exchange
            // Assume only one block, since we are assuming mthbc
            int num = new_domain->num_blocks;
            for (int i = 0; i < num; i++)
            {
                fclaw2d_block_t *block = &new_domain->blocks[i];
                // This is kind of dumb for now, since block won't in general
                // have the same physical boundary conditions types.
                set_block_data(block,gparms->mthbc);
            }

            fclaw2d_domain_iterate_adapted(*domain, new_domain,
                                           cb_domain_adapt_init,
                                           (void *) NULL);

            int new_level = level+1;
            // Upon initialization, we don't do any ghost cell exchanges, because we assume
            // that the initial conditions have set all internal ghost cells.
            set_phys_bc(new_domain,new_level,t);

            // free all memory associated with old domain
            amrreset(domain);
            *domain = new_domain;

            fclaw2d_domain_t *domain_partitioned =
                fclaw2d_domain_partition (*domain);
            if (domain_partitioned != NULL)
            {
                // TODO: allocate patch and block etc. memory for domain_partitioned

                // TODO: write a function to transfer values in parallel */

                /* then the old domain is no longer necessary */
                amrreset(domain);
                *domain = domain_partitioned;

                /* internal clean up */
                fclaw2d_domain_complete(*domain);
            }
        }
        else
        {
            // exit loop;  we are done refining
            break;
        }
    }
}
<|endoftext|>


<commit_before>// Copyright (c) 2009-2010 Satoshi Nakamoto
// Copyright (c) 2009-2014 The Bitcoin developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "pow.h"

#include "chainparams.h"
#include "core.h"
#include "main.h"
#include "timedata.h"
#include "uint256.h"
#include "util.h"

unsigned int static GetNextWorkRequired_V1(const CBlockIndex* pindexLast, const CBlockHeader *pblock)
{
    unsigned int nProofOfWorkLimit = Params().ProofOfWorkLimit().GetCompact();

    // Genesis block
    if (pindexLast == NULL)
        return nProofOfWorkLimit;

    // Only change once per interval
    if ((pindexLast->nHeight+1) % Params().Interval() != 0)
    {
        if (Params().AllowMinDifficultyBlocks())
        {
            // Special difficulty rule for testnet:
            // If the new block's timestamp is more than 2* 10 minutes
            // then allow mining of a min-difficulty block.
            if (pblock->GetBlockTime() > pindexLast->GetBlockTime() + Params().TargetSpacing()*2)
                return nProofOfWorkLimit;
            else
            {
                // Return the last non-special-min-difficulty-rules-block
                const CBlockIndex* pindex = pindexLast;
                while (pindex->pprev && pindex->nHeight % Params().Interval() != 0 && pindex->nBits == nProofOfWorkLimit)
                    pindex = pindex->pprev;
                return pindex->nBits;
            }
        }
        return pindexLast->nBits;
    }

    // Go back by what we want to be 14 days worth of blocks
    const CBlockIndex* pindexFirst = pindexLast;
    for (int i = 0; pindexFirst && i < Params().Interval()-1; i++)
        pindexFirst = pindexFirst->pprev;
    assert(pindexFirst);

    // Limit adjustment step
    int64_t nActualTimespan = pindexLast->GetBlockTime() - pindexFirst->GetBlockTime();
    LogPrintf("  nActualTimespan = %d  before bounds\n", nActualTimespan);
    if (nActualTimespan < Params().TargetTimespan()/4)
        nActualTimespan = Params().TargetTimespan()/4;
    if (nActualTimespan > Params().TargetTimespan()*4)
        nActualTimespan = Params().TargetTimespan()*4;

    // Retarget
    uint256 bnNew;
    uint256 bnOld;
    bnNew.SetCompact(pindexLast->nBits);
    bnOld = bnNew;
    bnNew *= nActualTimespan;
    bnNew /= Params().TargetTimespan();

    if (bnNew > Params().ProofOfWorkLimit())
        bnNew = Params().ProofOfWorkLimit();

    /// debug print
    LogPrintf("GetNextWorkRequired RETARGET\n");
    LogPrintf("Params().TargetTimespan() = %d    nActualTimespan = %d\n", Params().TargetTimespan(), nActualTimespan);
    LogPrintf("Before: %08x  %s\n", pindexLast->nBits, bnOld.ToString());
    LogPrintf("After:  %08x  %s\n", bnNew.GetCompact(), bnNew.ToString());

    return bnNew.GetCompact();
}

/* current difficulty formula, DarkGravity v3, written by Evan Duffield - evan@darkcoin.io */
unsigned int static DarkGravityWave3(const CBlockIndex* pindexLast, const CBlockHeader *pblock, int64_t version)
{
    const CBlockIndex *BlockLastSolved = pindexLast;
    const CBlockIndex *BlockReading = pindexLast;
    int64_t nActualTimespan = 0;
    int64_t LastBlockTime = 0;
    int64_t PastBlocksMin = 24;
    int64_t PastBlocksMax = 24;

    if (version == 1) {
        PastBlocksMin = 24;
        PastBlocksMax = 24;
    } else if (version == 2) {
        PastBlocksMin = 72;
        PastBlocksMax = 72;
    }

    int64_t CountBlocks = 0;
    uint256 PastDifficultyAverage;
    uint256 PastDifficultyAveragePrev;

    if (BlockLastSolved == NULL || BlockLastSolved->nHeight == 0 || BlockLastSolved->nHeight < PastBlocksMin) {
        return Params().ProofOfWorkLimit().GetCompact();
    }

    for (unsigned int i = 1; BlockReading && BlockReading->nHeight > 0; i++) {
        if (PastBlocksMax > 0 && i > PastBlocksMax) { break; }
            CountBlocks++;

        if (CountBlocks <= PastBlocksMin) {
            if (CountBlocks == 1)
                PastDifficultyAverage.SetCompact(BlockReading->nBits);
            else { PastDifficultyAverage = ((PastDifficultyAveragePrev * CountBlocks)+(uint256().SetCompact(BlockReading->nBits))) / (CountBlocks+1); }
                PastDifficultyAveragePrev = PastDifficultyAverage;
        }

        if (LastBlockTime > 0) {
            int64_t Diff = (LastBlockTime - BlockReading->GetBlockTime());
            nActualTimespan += Diff;
        }
        LastBlockTime = BlockReading->GetBlockTime();

        if (BlockReading->pprev == NULL) { assert(BlockReading); break; }
            BlockReading = BlockReading->pprev;
    }

    uint256 bnNew(PastDifficultyAverage);

    if (version == 2)
        --CountBlocks;

    int64_t nTargetTimespan = CountBlocks*Params().TargetSpacing();

    int64_t div = 3;
    if (version == 1)
        div = 3;
    else if (version == 2)
        div = 2;

    if (nActualTimespan < nTargetTimespan/div)
        nActualTimespan = nTargetTimespan/div;
    if (nActualTimespan > nTargetTimespan*div)
        nActualTimespan = nTargetTimespan*div;

    // Retarget
    bnNew *= nActualTimespan;
    bnNew /= nTargetTimespan;

    if (bnNew > Params().ProofOfWorkLimit()) {
        bnNew = Params().ProofOfWorkLimit();
    }

    return bnNew.GetCompact();
}

unsigned int GetNextWorkRequired(const CBlockIndex* pindexLast, const CBlockHeader *pblock)
{
    if (pindexLast->nHeight+1 >= 451000 || (Params().AllowMinDifficultyBlocks() && pindexLast->nHeight+1 >= 300000)) {
        return DarkGravityWave3(pindexLast, pblock, 2);
    } else if (pindexLast->nHeight+1 >= 3600) {
        return DarkGravityWave3(pindexLast, pblock, 1);
    } else {
        return GetNextWorkRequired_V1(pindexLast, pblock);
    }
}

bool CheckProofOfWork(uint256 hash, unsigned int nBits)
{
    bool fNegative;
    bool fOverflow;
    uint256 bnTarget;

    if (Params().SkipProofOfWorkCheck())
       return true;

    bnTarget.SetCompact(nBits, &fNegative, &fOverflow);

    // Check range
    if (fNegative || bnTarget == 0 || fOverflow || bnTarget > Params().ProofOfWorkLimit())
        return error("CheckProofOfWork() : nBits below minimum work");

    // Check proof of work matches claimed amount
    if (hash > bnTarget)
        return error("CheckProofOfWork() : hash doesn't match nBits");

    return true;
}

void UpdateTime(CBlockHeader* pblock, const CBlockIndex* pindexPrev)
{
    pblock->nTime = std::max(pindexPrev->GetMedianTimePast()+1, GetAdjustedTime());

    // Updating time can change work required on testnet:
    if (Params().AllowMinDifficultyBlocks())
        pblock->nBits = GetNextWorkRequired(pindexPrev, pblock);
}

uint256 GetProofIncrement(unsigned int nBits)
{
    uint256 bnTarget;
    bool fNegative;
    bool fOverflow;
    bnTarget.SetCompact(nBits, &fNegative, &fOverflow);
    if (fNegative || fOverflow || bnTarget == 0)
        return 0;
    // We need to compute 2**256 / (bnTarget+1), but we can't represent 2**256
    // as it's too large for a uint256. However, as 2**256 is at least as large
    // as bnTarget+1, it is equal to ((2**256 - bnTarget - 1) / (bnTarget+1)) + 1,
    // or ~bnTarget / (nTarget+1) + 1.
    return (~bnTarget / (bnTarget + 1)) + 1;
}
<commit_msg>AntiGravity retargetting<commit_after>// Copyright (c) 2009-2010 Satoshi Nakamoto
// Copyright (c) 2009-2014 The Bitcoin developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "pow.h"

#include "chainparams.h"
#include "core.h"
#include "main.h"
#include "timedata.h"
#include "uint256.h"
#include "util.h"

unsigned int static GetNextWorkRequired_V1(const CBlockIndex* pindexLast, const CBlockHeader *pblock)
{
    unsigned int nProofOfWorkLimit = Params().ProofOfWorkLimit().GetCompact();

    // Genesis block
    if (pindexLast == NULL)
        return nProofOfWorkLimit;

    // Only change once per interval
    if ((pindexLast->nHeight+1) % Params().Interval() != 0)
    {
        if (Params().AllowMinDifficultyBlocks())
        {
            // Special difficulty rule for testnet:
            // If the new block's timestamp is more than 2* 10 minutes
            // then allow mining of a min-difficulty block.
            if (pblock->GetBlockTime() > pindexLast->GetBlockTime() + Params().TargetSpacing()*2)
                return nProofOfWorkLimit;
            else
            {
                // Return the last non-special-min-difficulty-rules-block
                const CBlockIndex* pindex = pindexLast;
                while (pindex->pprev && pindex->nHeight % Params().Interval() != 0 && pindex->nBits == nProofOfWorkLimit)
                    pindex = pindex->pprev;
                return pindex->nBits;
            }
        }
        return pindexLast->nBits;
    }

    // Go back by what we want to be 14 days worth of blocks
    const CBlockIndex* pindexFirst = pindexLast;
    for (int i = 0; pindexFirst && i < Params().Interval()-1; i++)
        pindexFirst = pindexFirst->pprev;
    assert(pindexFirst);

    // Limit adjustment step
    int64_t nActualTimespan = pindexLast->GetBlockTime() - pindexFirst->GetBlockTime();
    LogPrintf("  nActualTimespan = %d  before bounds\n", nActualTimespan);
    if (nActualTimespan < Params().TargetTimespan()/4)
        nActualTimespan = Params().TargetTimespan()/4;
    if (nActualTimespan > Params().TargetTimespan()*4)
        nActualTimespan = Params().TargetTimespan()*4;

    // Retarget
    uint256 bnNew;
    uint256 bnOld;
    bnNew.SetCompact(pindexLast->nBits);
    bnOld = bnNew;
    bnNew *= nActualTimespan;
    bnNew /= Params().TargetTimespan();

    if (bnNew > Params().ProofOfWorkLimit())
        bnNew = Params().ProofOfWorkLimit();

    /// debug print
    LogPrintf("GetNextWorkRequired RETARGET\n");
    LogPrintf("Params().TargetTimespan() = %d    nActualTimespan = %d\n", Params().TargetTimespan(), nActualTimespan);
    LogPrintf("Before: %08x  %s\n", pindexLast->nBits, bnOld.ToString());
    LogPrintf("After:  %08x  %s\n", bnNew.GetCompact(), bnNew.ToString());

    return bnNew.GetCompact();
}

// AntiGravityWave by reorder, derived from code by Evan Duffield - evan@darkcoin.io
unsigned int static AntiGravityWave(const CBlockIndex* pindexLast, const CBlockHeader *pblock, int64_t version)
{
    const CBlockIndex *BlockLastSolved = pindexLast;
    const CBlockIndex *BlockReading = pindexLast;
    int64_t nActualTimespan = 0;
    int64_t LastBlockTime = 0;
    int64_t PastBlocksMin = 24;
    int64_t PastBlocksMax = 24;

    if (version == 1) {
        PastBlocksMin = 24;
        PastBlocksMax = 24;
    } else if (version == 2) {
        PastBlocksMin = 72;
        PastBlocksMax = 72;
    }

    int64_t CountBlocks = 0;
    uint256 PastDifficultyAverage;
    uint256 PastDifficultyAveragePrev;

    if (BlockLastSolved == NULL || BlockLastSolved->nHeight == 0 || BlockLastSolved->nHeight < PastBlocksMin) {
        return Params().ProofOfWorkLimit().GetCompact();
    }

    for (unsigned int i = 1; BlockReading && BlockReading->nHeight > 0; i++) {
        if (PastBlocksMax > 0 && i > PastBlocksMax) { break; }
            CountBlocks++;

        if (CountBlocks <= PastBlocksMin) {
            if (CountBlocks == 1)
                PastDifficultyAverage.SetCompact(BlockReading->nBits);
            else { PastDifficultyAverage = ((PastDifficultyAveragePrev * CountBlocks)+(uint256().SetCompact(BlockReading->nBits))) / (CountBlocks+1); }
                PastDifficultyAveragePrev = PastDifficultyAverage;
        }

        if (LastBlockTime > 0) {
            int64_t Diff = (LastBlockTime - BlockReading->GetBlockTime());
            nActualTimespan += Diff;
        }
        LastBlockTime = BlockReading->GetBlockTime();

        if (BlockReading->pprev == NULL) { assert(BlockReading); break; }
            BlockReading = BlockReading->pprev;
    }

    uint256 bnNew(PastDifficultyAverage);

    if (version == 2)
        --CountBlocks;

    int64_t nTargetTimespan = CountBlocks*Params().TargetSpacing();

    int64_t div = 3;
    if (version == 1)
        div = 3;
    else if (version == 2)
        div = 2;

    if (nActualTimespan < nTargetTimespan/div)
        nActualTimespan = nTargetTimespan/div;
    if (nActualTimespan > nTargetTimespan*div)
        nActualTimespan = nTargetTimespan*div;

    // Retarget
    bnNew *= nActualTimespan;
    bnNew /= nTargetTimespan;

    if (bnNew > Params().ProofOfWorkLimit()) {
        bnNew = Params().ProofOfWorkLimit();
    }

    return bnNew.GetCompact();
}

unsigned int GetNextWorkRequired(const CBlockIndex* pindexLast, const CBlockHeader *pblock)
{
    if (pindexLast->nHeight+1 >= 451000 || (Params().AllowMinDifficultyBlocks() && pindexLast->nHeight+1 >= 300000)) {
        return AntiGravityWave(pindexLast, pblock, 2);
    } else if (pindexLast->nHeight+1 >= 3600) {
        return AntiGravityWave(pindexLast, pblock, 1);
    } else {
        return GetNextWorkRequired_V1(pindexLast, pblock);
    }
}

bool CheckProofOfWork(uint256 hash, unsigned int nBits)
{
    bool fNegative;
    bool fOverflow;
    uint256 bnTarget;

    if (Params().SkipProofOfWorkCheck())
       return true;

    bnTarget.SetCompact(nBits, &fNegative, &fOverflow);

    // Check range
    if (fNegative || bnTarget == 0 || fOverflow || bnTarget > Params().ProofOfWorkLimit())
        return error("CheckProofOfWork() : nBits below minimum work");

    // Check proof of work matches claimed amount
    if (hash > bnTarget)
        return error("CheckProofOfWork() : hash doesn't match nBits");

    return true;
}

void UpdateTime(CBlockHeader* pblock, const CBlockIndex* pindexPrev)
{
    pblock->nTime = std::max(pindexPrev->GetMedianTimePast()+1, GetAdjustedTime());

    // Updating time can change work required on testnet:
    if (Params().AllowMinDifficultyBlocks())
        pblock->nBits = GetNextWorkRequired(pindexPrev, pblock);
}

uint256 GetProofIncrement(unsigned int nBits)
{
    uint256 bnTarget;
    bool fNegative;
    bool fOverflow;
    bnTarget.SetCompact(nBits, &fNegative, &fOverflow);
    if (fNegative || fOverflow || bnTarget == 0)
        return 0;
    // We need to compute 2**256 / (bnTarget+1), but we can't represent 2**256
    // as it's too large for a uint256. However, as 2**256 is at least as large
    // as bnTarget+1, it is equal to ((2**256 - bnTarget - 1) / (bnTarget+1)) + 1,
    // or ~bnTarget / (nTarget+1) + 1.
    return (~bnTarget / (bnTarget + 1)) + 1;
}
<|endoftext|>


<commit_before>//
// Classic DP routines
//

//
// LIS
// Returns the length of the LIS and store lengths for each element of seq
// in ls
//
typedef int LisT;
LisT I[MAXN + 1];
int lis(const LisT *seq, int n, LisT *ls)
{
	int len = 0;
	for (int i = 0; i < n; ++i) {
		int lo = 1, hi = len;
		while (lo <= hi) {
			int m = (lo + hi) / 2;
			if (I[m] < seq[i]) lo = m + 1;
			else hi = m - 1;
		}
		I[lo] = seq[i], ls[i] = lo;
		if (len < lo) len = lo;
	}
	return len;
}

//
// Coin change
//	 W:  answer
//	 C:  coin values
//	 n:  number of coins
//	 k:  maximum amount of money
//	 pre-cond: W is cleared
//
void cc(int *W, const int *C, int n, int k)
{
	W[0] = 1;
	for (int i = n - 1; i >= 0; --i)
		for (int a = 0, b = C[i]; b <= k; ++a, ++b)
			W[b] = (W[b] + W[a]) % MOD;
}

//
// DPString
// Similar to a Trie. Useful for DP problems in which a certain string (for
// example, the lexicographically lowest) is requested.
//
const int MAX_NODES = MAXLEN;
char buf[MAXLEN + 1];
struct DPString {
	struct Node {
		int p;   // parent
		char c;  // character
	};
	Node nodes[MAX_NODES];
	int n;

	void init() { nodes[0].p = -1, nodes[0].c = 0, n = 1; }
	int append(int node, char c) {
		nodes[n].p = node;
		nodes[n].c = c;
		return n++;
	}
	void print(int k) const {
		for (int len=0; k >= 0; k = nodes[k].p) buf[len++] = nodes[k].c;
		puts(buf);
	}
};
DPString dps;
<commit_msg>Added CYK algorithm<commit_after>//
// Classic DP routines
//

//
// LIS
// Returns the length of the LIS and store lengths for each element of seq
// in ls
//
typedef int LisT;
LisT I[MAXN + 1];
int lis(const LisT *seq, int n, LisT *ls)
{
	int len = 0;
	for (int i = 0; i < n; ++i) {
		int lo = 1, hi = len;
		while (lo <= hi) {
			int m = (lo + hi) / 2;
			if (I[m] < seq[i]) lo = m + 1;
			else hi = m - 1;
		}
		I[lo] = seq[i], ls[i] = lo;
		if (len < lo) len = lo;
	}
	return len;
}

//
// Coin change
//	 W:  answer
//	 C:  coin values
//	 n:  number of coins
//	 k:  maximum amount of money
//	 pre-cond: W is cleared
//
void cc(int *W, const int *C, int n, int k)
{
	W[0] = 1;
	for (int i = n - 1; i >= 0; --i)
		for (int a = 0, b = C[i]; b <= k; ++a, ++b)
			W[b] = (W[b] + W[a]) % MOD;
}

//
// DPString
// Similar to a Trie. Useful for DP problems in which a certain string (for
// example, the lexicographically lowest) is requested.
//
const int MAX_NODES = MAXLEN;
char buf[MAXLEN + 1];
struct DPString {
	struct Node {
		int p;   // parent
		char c;  // character
	};
	Node nodes[MAX_NODES];
	int n;

	void init() { nodes[0].p = -1, nodes[0].c = 0, n = 1; }
	int append(int node, char c) {
		nodes[n].p = node;
		nodes[n].c = c;
		return n++;
	}
	void print(int k) const {
		for (int len=0; k >= 0; k = nodes[k].p) buf[len++] = nodes[k].c;
		puts(buf);
	}
};
DPString dps;

//
// CYK (Cocke-Younger-Kasami) algorithm for parsing context-free grammars
//
const int MaxN = MAXC;       // max length of input string
const int MaxS = MAXCOLORS;  // max number of symbols

struct Rule { int X, A, B; };  // X -> A B

// CYK(i, j, k) is the interface to the DP table. Each cell is true if
// symbol k produces the substring Si..Sj
int CYK_N, CYK_S;
#define CYK(i,j,k)       (_cyk_dp[(i)*CYK_N*CYK_S + (j)*CYK_S + (k)])
#define CYK_AUX(i,j)     (_cyk_aux[(i)*CYK_N + (j)])
#define CYK_SET(i, j, k) (CYK_AUX(i, j) = CYK(i, j, k) = true)
bool _cyk_dp[MaxN * MaxN * MaxS];
bool _cyk_aux[MaxN * MaxN];  // for optimization purposes

// call with the length of the input string (N) and the number of symbols (S)
void cyk_init(int N, int S)
{
	Clr(_cyk_dp);
	Clr(_cyk_aux);
	CYK_N = N, CYK_S = S;
}

// pre-cond: calling cyk_init, and then calling CYK_SET(i, i, k) for each i
// if the ith token is symbol k
void cyk_run(Rule *rs, int R)
{
	for (int sz = 2; sz <= CYK_N; ++sz)
		for (int i = 0, j = i+sz-1; j < CYK_N; ++i, ++j)
			for (int k = i; k < j; ++k)
				if (CYK_AUX(i, k) && CYK_AUX(k+1, j))
					for (int p = 0; p < R; ++p) {
						Rule &r = rs[p];

						if (CYK(i, k, r.A) && CYK(k+1, j, r.B))
							CYK_SET(i, j, r.X);
					}

	// post-cond: CYK(0, N-1, k) == true iff the symbol k produces the input
}
<|endoftext|>


<commit_before>/*
 * Open Pixel Control server for Fadecandy
 *
 * Copyright (c) 2013 Micah Elizabeth Scott
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of
 * this software and associated documentation files (the "Software"), to deal in
 * the Software without restriction, including without limitation the rights to
 * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
 * the Software, and to permit persons to whom the Software is furnished to do so,
 * subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
 * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
 * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
 * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

#include "fcserver.h"
#include "usbdevice.h"
#include "fcdevice.h"
#include "enttecdmxdevice.h"
#include <ctype.h>
#include <iostream>

FCServer::FCServer(rapidjson::Document &config)
    : mListen(config["listen"]),
      mColor(config["color"]),
      mDevices(config["devices"]),
      mVerbose(config["verbose"].IsTrue()),
      mNetServer(cbOpcMessage, cbJsonMessage, this, mVerbose),
      mUSBHotplugThread(0),
      mUSB(0)
{
    /*
     * Validate the listen [host, port] list.
     */

    if (mListen.IsArray() && mListen.Size() == 2) {
        const Value &host = mListen[0u];
        const Value &port = mListen[1];
        const char *hostStr = 0;

        if (host.IsString()) {
            hostStr = host.GetString();
        } else if (!host.IsNull()) {
            mError << "Hostname in 'listen' must be null (any) or a hostname string.\n";
        }

        if (!port.IsUint()) {
            mError << "The 'listen' port must be an integer.\n";
        }
    } else {
        mError << "The required 'listen' configuration key must be a [host, port] list.\n";
    }

    /*
     * Minimal validation on 'devices'
     */

    if (!mDevices.IsArray()) {
        mError << "The required 'devices' configuration key must be an array.\n";
    }
}

bool FCServer::start(libusb_context *usb)
{
    const Value &host = mListen[0u];
    const Value &port = mListen[1];
    const char *hostStr = host.IsString() ? host.GetString() : NULL;

    return mNetServer.start(hostStr, port.GetUint()) && startUSB(usb);
}

bool FCServer::startUSB(libusb_context *usb)
{
    mUSB = usb;

    // Enumerate all attached devices, and get notified of hotplug events
    libusb_hotplug_register_callback(mUSB,
        libusb_hotplug_event(LIBUSB_HOTPLUG_EVENT_DEVICE_ARRIVED |
                             LIBUSB_HOTPLUG_EVENT_DEVICE_LEFT),
        LIBUSB_HOTPLUG_ENUMERATE,
        LIBUSB_HOTPLUG_MATCH_ANY,
        LIBUSB_HOTPLUG_MATCH_ANY,
        LIBUSB_HOTPLUG_MATCH_ANY,
        cbHotplug, this, 0);

    // On platforms without real USB hotplug, emulate it with a polling thread
    if (!libusb_has_capability(LIBUSB_CAP_HAS_HOTPLUG)) {
        mUSBHotplugThread = new tthread::thread(usbHotplugThreadFunc, this);
    }

    return true;
}

void FCServer::cbOpcMessage(OPC::Message &msg, void *context)
{
    /*
     * Broadcast the OPC message to all configured devices.
     */

    FCServer *self = static_cast<FCServer*>(context);
    self->mEventMutex.lock();

    for (std::vector<USBDevice*>::iterator i = self->mUSBDevices.begin(), e = self->mUSBDevices.end(); i != e; ++i) {
        USBDevice *dev = *i;
        dev->writeMessage(msg);
    }

    self->mEventMutex.unlock();
}

int FCServer::cbHotplug(libusb_context *ctx, libusb_device *device, libusb_hotplug_event event, void *user_data)
{
    FCServer *self = static_cast<FCServer*>(user_data);

    self->mEventMutex.lock();

    if (event & LIBUSB_HOTPLUG_EVENT_DEVICE_ARRIVED) {
        self->usbDeviceArrived(device);
    }
    if (event & LIBUSB_HOTPLUG_EVENT_DEVICE_LEFT) {
        self->usbDeviceLeft(device);
    }

    self->mEventMutex.unlock();
    return false;
}

void FCServer::usbDeviceArrived(libusb_device *device)
{
    /*
     * New USB device. Is this a device we recognize?
     */

    USBDevice *dev;

    if (FCDevice::probe(device)) {
        dev = new FCDevice(device, mVerbose);

    } else if (EnttecDMXDevice::probe(device)) {
        dev = new EnttecDMXDevice(device, mVerbose);

    } else {
        return;
    }

    int r = dev->open();
    if (r < 0) {
        if (mVerbose) {
            switch (r) {

                // Errors that may occur transiently while WinUSB is installing...
                #ifdef _WIN32
                case LIBUSB_ERROR_NOT_FOUND:
                case LIBUSB_ERROR_NOT_SUPPORTED:
                    std::clog << "Waiting for Windows to install " << dev->getName() << " driver. This may take a moment...\n";
                    break;
                #endif

                default:
                    std::clog << "Error opening " << dev->getName() << ": " << libusb_strerror(libusb_error(r)) << "\n";
                    break;
            }
        }
        delete dev;
        return;
    }

    if (!dev->probeAfterOpening()) {
        // We were mistaken, this device isn't actually one we want.
        delete dev;
        return;
    }

    for (unsigned i = 0; i < mDevices.Size(); ++i) {
        if (dev->matchConfiguration(mDevices[i])) {
            // Found a matching configuration for this device. We're keeping it!

            dev->writeColorCorrection(mColor);
            mUSBDevices.push_back(dev);

            if (mVerbose) {
                std::clog << "USB device " << dev->getName() << " attached.\n";
            }
            return;
        }
    }

    if (mVerbose) {
        std::clog << "USB device " << dev->getName() << " has no matching configuration. Not using it.\n";
    }
    delete dev;
}

void FCServer::usbDeviceLeft(libusb_device *device)
{
    /*
     * Is this a device we recognize? If so, delete it.
     */

    for (std::vector<USBDevice*>::iterator i = mUSBDevices.begin(), e = mUSBDevices.end(); i != e; ++i) {
        USBDevice *dev = *i;
        if (dev->getDevice() == device) {
            usbDeviceLeft(i);
            break;
        }
    }
}

void FCServer::usbDeviceLeft(std::vector<USBDevice*>::iterator iter)
{
    USBDevice *dev = *iter;
    if (mVerbose) {
        std::clog << "USB device " << dev->getName() << " removed.\n";
    }
    mUSBDevices.erase(iter);
    delete dev;
}

void FCServer::mainLoop()
{
    for (;;) {
        int err = libusb_handle_events_completed(mUSB, 0);
        if (err) {
            std::clog << "Error handling USB events: " << libusb_strerror(libusb_error(err)) << "\n";
            // Sometimes this happens on Windows during normal operation if we're queueing a lot of output URBs. Meh.
        }

        // Flush completed transfers
        mEventMutex.lock();
        for (std::vector<USBDevice*>::iterator i = mUSBDevices.begin(), e = mUSBDevices.end(); i != e; ++i) {
            USBDevice *dev = *i;
            dev->flush();
        }
        mEventMutex.unlock();
    }
}

bool FCServer::usbHotplugPoll()
{
    /*
     * For platforms without libusbx hotplug support,
     * see if we can fake it by polling for new devices. This can
     * happen on a different thread, and it probably should.
     * Returns true on success.
     */

    libusb_device **list;
    ssize_t listSize;

    listSize = libusb_get_device_list(mUSB, &list);
    if (listSize < 0) {
        std::clog << "Error polling for USB devices: " << libusb_strerror(libusb_error(listSize)) << "\n";
        return false;
    }

    // Take the lock after get_device_list completes
    mEventMutex.lock();

    // Look for devices that were added
    for (ssize_t listItem = 0; listItem < listSize; ++listItem) {
        bool isNew = true;

        for (std::vector<USBDevice*>::iterator i = mUSBDevices.begin(), e = mUSBDevices.end(); i != e; ++i) {
            USBDevice *dev = *i;
            if (dev->getDevice() == list[listItem]) {
                isNew = false;
            }
        }

        if (isNew) {
            usbDeviceArrived(list[listItem]);
        }
    }

    // Look for devices that were removed
    for (std::vector<USBDevice*>::iterator i = mUSBDevices.begin(), e = mUSBDevices.end(); i != e; ++i) {
        USBDevice *dev = *i;
        libusb_device *usbdev = dev->getDevice();
        bool isRemoved = true;

        for (ssize_t listItem = 0; listItem < listSize; ++listItem) {
            if (list[listItem] == usbdev) {
                isRemoved = false;
            }
        }

        if (isRemoved) {
            usbDeviceLeft(i);
        }
    }

    mEventMutex.unlock();
    libusb_free_device_list(list, true);
    return true;
}

void FCServer::usbHotplugThreadFunc(void *arg)
{
    FCServer *self = (FCServer*) arg;

    while (self->usbHotplugPoll()) {
        tthread::this_thread::sleep_for(tthread::chrono::seconds(1));
    }
}

void FCServer::cbJsonMessage(libwebsocket *wsi, rapidjson::Document &message, void *context)
{
    // Received a JSON message from a WebSockets client.
    // Replies are formed by modifying the original message.

    FCServer *self = (FCServer*) context;

    const Value &vtype = message["type"];
    if (!vtype.IsString()) {
        lwsl_notice("NOTICE: Received JSON is missing mandatory \"type\" string\n");
        return;
    }
    const char *type = vtype.GetString();

    if (!strcmp(type, "list_connected_devices")) {
        self->jsonListConectedDevices(message);
    } else {
        message.AddMember("error", "Unknown message type", message.GetAllocator());
    }

    self->mNetServer.jsonReply(wsi, message);
}

void FCServer::jsonListConectedDevices(rapidjson::Document &message)
{
    message.AddMember("devices", rapidjson::kArrayType, message.GetAllocator());
    Value &list = message["devices"];

    for (unsigned i = 0; i != mUSBDevices.size(); i++) {
        USBDevice *usbDev = mUSBDevices[i];
        list.PushBack(rapidjson::kObjectType, message.GetAllocator());
        mUSBDevices[i]->describe(list[i], message.GetAllocator());
    }
}
<commit_msg>Hold the event lock while dispatching JSON messages<commit_after>/*
 * Open Pixel Control server for Fadecandy
 *
 * Copyright (c) 2013 Micah Elizabeth Scott
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of
 * this software and associated documentation files (the "Software"), to deal in
 * the Software without restriction, including without limitation the rights to
 * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
 * the Software, and to permit persons to whom the Software is furnished to do so,
 * subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
 * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
 * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
 * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

#include "fcserver.h"
#include "usbdevice.h"
#include "fcdevice.h"
#include "enttecdmxdevice.h"
#include <ctype.h>
#include <iostream>

FCServer::FCServer(rapidjson::Document &config)
    : mListen(config["listen"]),
      mColor(config["color"]),
      mDevices(config["devices"]),
      mVerbose(config["verbose"].IsTrue()),
      mNetServer(cbOpcMessage, cbJsonMessage, this, mVerbose),
      mUSBHotplugThread(0),
      mUSB(0)
{
    /*
     * Validate the listen [host, port] list.
     */

    if (mListen.IsArray() && mListen.Size() == 2) {
        const Value &host = mListen[0u];
        const Value &port = mListen[1];
        const char *hostStr = 0;

        if (host.IsString()) {
            hostStr = host.GetString();
        } else if (!host.IsNull()) {
            mError << "Hostname in 'listen' must be null (any) or a hostname string.\n";
        }

        if (!port.IsUint()) {
            mError << "The 'listen' port must be an integer.\n";
        }
    } else {
        mError << "The required 'listen' configuration key must be a [host, port] list.\n";
    }

    /*
     * Minimal validation on 'devices'
     */

    if (!mDevices.IsArray()) {
        mError << "The required 'devices' configuration key must be an array.\n";
    }
}

bool FCServer::start(libusb_context *usb)
{
    const Value &host = mListen[0u];
    const Value &port = mListen[1];
    const char *hostStr = host.IsString() ? host.GetString() : NULL;

    return mNetServer.start(hostStr, port.GetUint()) && startUSB(usb);
}

bool FCServer::startUSB(libusb_context *usb)
{
    mUSB = usb;

    // Enumerate all attached devices, and get notified of hotplug events
    libusb_hotplug_register_callback(mUSB,
        libusb_hotplug_event(LIBUSB_HOTPLUG_EVENT_DEVICE_ARRIVED |
                             LIBUSB_HOTPLUG_EVENT_DEVICE_LEFT),
        LIBUSB_HOTPLUG_ENUMERATE,
        LIBUSB_HOTPLUG_MATCH_ANY,
        LIBUSB_HOTPLUG_MATCH_ANY,
        LIBUSB_HOTPLUG_MATCH_ANY,
        cbHotplug, this, 0);

    // On platforms without real USB hotplug, emulate it with a polling thread
    if (!libusb_has_capability(LIBUSB_CAP_HAS_HOTPLUG)) {
        mUSBHotplugThread = new tthread::thread(usbHotplugThreadFunc, this);
    }

    return true;
}

void FCServer::cbOpcMessage(OPC::Message &msg, void *context)
{
    /*
     * Broadcast the OPC message to all configured devices.
     */

    FCServer *self = static_cast<FCServer*>(context);
    self->mEventMutex.lock();

    for (std::vector<USBDevice*>::iterator i = self->mUSBDevices.begin(), e = self->mUSBDevices.end(); i != e; ++i) {
        USBDevice *dev = *i;
        dev->writeMessage(msg);
    }

    self->mEventMutex.unlock();
}

int FCServer::cbHotplug(libusb_context *ctx, libusb_device *device, libusb_hotplug_event event, void *user_data)
{
    FCServer *self = static_cast<FCServer*>(user_data);

    self->mEventMutex.lock();

    if (event & LIBUSB_HOTPLUG_EVENT_DEVICE_ARRIVED) {
        self->usbDeviceArrived(device);
    }
    if (event & LIBUSB_HOTPLUG_EVENT_DEVICE_LEFT) {
        self->usbDeviceLeft(device);
    }

    self->mEventMutex.unlock();
    return false;
}

void FCServer::usbDeviceArrived(libusb_device *device)
{
    /*
     * New USB device. Is this a device we recognize?
     */

    USBDevice *dev;

    if (FCDevice::probe(device)) {
        dev = new FCDevice(device, mVerbose);

    } else if (EnttecDMXDevice::probe(device)) {
        dev = new EnttecDMXDevice(device, mVerbose);

    } else {
        return;
    }

    int r = dev->open();
    if (r < 0) {
        if (mVerbose) {
            switch (r) {

                // Errors that may occur transiently while WinUSB is installing...
                #ifdef _WIN32
                case LIBUSB_ERROR_NOT_FOUND:
                case LIBUSB_ERROR_NOT_SUPPORTED:
                    std::clog << "Waiting for Windows to install " << dev->getName() << " driver. This may take a moment...\n";
                    break;
                #endif

                default:
                    std::clog << "Error opening " << dev->getName() << ": " << libusb_strerror(libusb_error(r)) << "\n";
                    break;
            }
        }
        delete dev;
        return;
    }

    if (!dev->probeAfterOpening()) {
        // We were mistaken, this device isn't actually one we want.
        delete dev;
        return;
    }

    for (unsigned i = 0; i < mDevices.Size(); ++i) {
        if (dev->matchConfiguration(mDevices[i])) {
            // Found a matching configuration for this device. We're keeping it!

            dev->writeColorCorrection(mColor);
            mUSBDevices.push_back(dev);

            if (mVerbose) {
                std::clog << "USB device " << dev->getName() << " attached.\n";
            }
            return;
        }
    }

    if (mVerbose) {
        std::clog << "USB device " << dev->getName() << " has no matching configuration. Not using it.\n";
    }
    delete dev;
}

void FCServer::usbDeviceLeft(libusb_device *device)
{
    /*
     * Is this a device we recognize? If so, delete it.
     */

    for (std::vector<USBDevice*>::iterator i = mUSBDevices.begin(), e = mUSBDevices.end(); i != e; ++i) {
        USBDevice *dev = *i;
        if (dev->getDevice() == device) {
            usbDeviceLeft(i);
            break;
        }
    }
}

void FCServer::usbDeviceLeft(std::vector<USBDevice*>::iterator iter)
{
    USBDevice *dev = *iter;
    if (mVerbose) {
        std::clog << "USB device " << dev->getName() << " removed.\n";
    }
    mUSBDevices.erase(iter);
    delete dev;
}

void FCServer::mainLoop()
{
    for (;;) {
        int err = libusb_handle_events_completed(mUSB, 0);
        if (err) {
            std::clog << "Error handling USB events: " << libusb_strerror(libusb_error(err)) << "\n";
            // Sometimes this happens on Windows during normal operation if we're queueing a lot of output URBs. Meh.
        }

        // Flush completed transfers
        mEventMutex.lock();
        for (std::vector<USBDevice*>::iterator i = mUSBDevices.begin(), e = mUSBDevices.end(); i != e; ++i) {
            USBDevice *dev = *i;
            dev->flush();
        }
        mEventMutex.unlock();
    }
}

bool FCServer::usbHotplugPoll()
{
    /*
     * For platforms without libusbx hotplug support,
     * see if we can fake it by polling for new devices. This can
     * happen on a different thread, and it probably should.
     * Returns true on success.
     */

    libusb_device **list;
    ssize_t listSize;

    listSize = libusb_get_device_list(mUSB, &list);
    if (listSize < 0) {
        std::clog << "Error polling for USB devices: " << libusb_strerror(libusb_error(listSize)) << "\n";
        return false;
    }

    // Take the lock after get_device_list completes
    mEventMutex.lock();

    // Look for devices that were added
    for (ssize_t listItem = 0; listItem < listSize; ++listItem) {
        bool isNew = true;

        for (std::vector<USBDevice*>::iterator i = mUSBDevices.begin(), e = mUSBDevices.end(); i != e; ++i) {
            USBDevice *dev = *i;
            if (dev->getDevice() == list[listItem]) {
                isNew = false;
            }
        }

        if (isNew) {
            usbDeviceArrived(list[listItem]);
        }
    }

    // Look for devices that were removed
    for (std::vector<USBDevice*>::iterator i = mUSBDevices.begin(), e = mUSBDevices.end(); i != e; ++i) {
        USBDevice *dev = *i;
        libusb_device *usbdev = dev->getDevice();
        bool isRemoved = true;

        for (ssize_t listItem = 0; listItem < listSize; ++listItem) {
            if (list[listItem] == usbdev) {
                isRemoved = false;
            }
        }

        if (isRemoved) {
            usbDeviceLeft(i);
        }
    }

    mEventMutex.unlock();
    libusb_free_device_list(list, true);
    return true;
}

void FCServer::usbHotplugThreadFunc(void *arg)
{
    FCServer *self = (FCServer*) arg;

    while (self->usbHotplugPoll()) {
        tthread::this_thread::sleep_for(tthread::chrono::seconds(1));
    }
}

void FCServer::cbJsonMessage(libwebsocket *wsi, rapidjson::Document &message, void *context)
{
    // Received a JSON message from a WebSockets client.
    // Replies are formed by modifying the original message.

    FCServer *self = (FCServer*) context;

    const Value &vtype = message["type"];
    if (!vtype.IsString()) {
        lwsl_notice("NOTICE: Received JSON is missing mandatory \"type\" string\n");
        return;
    }
    const char *type = vtype.GetString();

    // Hold the event lock while dispatching
    self->mEventMutex.lock();

    if (!strcmp(type, "list_connected_devices")) {
        self->jsonListConectedDevices(message);
    } else {
        message.AddMember("error", "Unknown message type", message.GetAllocator());
    }

    self->mEventMutex.unlock();
    self->mNetServer.jsonReply(wsi, message);
}

void FCServer::jsonListConectedDevices(rapidjson::Document &message)
{
    message.AddMember("devices", rapidjson::kArrayType, message.GetAllocator());
    Value &list = message["devices"];

    for (unsigned i = 0; i != mUSBDevices.size(); i++) {
        USBDevice *usbDev = mUSBDevices[i];
        list.PushBack(rapidjson::kObjectType, message.GetAllocator());
        mUSBDevices[i]->describe(list[i], message.GetAllocator());
    }
}
<|endoftext|>