<commit_before>#define DEBUG 1
/**
 * File    : A.cpp
 * Author  : Kazune Takahashi
 * Created : 2/15/2020, 6:48:03 PM
 * Powered by Visual Studio Code
 */
#include <algorithm>
#include <bitset>
#include <cassert>
#include <cctype>
#include <chrono>
#include <cmath>
#include <complex>
#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <functional>
#include <iomanip>
#include <iostream>
#include <map>
#include <queue>
#include <random>
#include <set>
#include <stack>
#include <string>
#include <tuple>
#include <unordered_map>
#include <unordered_set>
#include <vector>
// ----- boost -----
#include <boost/rational.hpp>
#include <boost/multiprecision/cpp_int.hpp>
// ----- using directives and manipulations -----
using namespace std;
using boost::rational;
using boost::multiprecision::cpp_int;
using ll = long long;
template <typename T>
using max_heap = priority_queue<T>;
template <typename T>
using min_heap = priority_queue<T, vector<T>, greater<T>>;
// ----- constexpr for Mint and Combination -----
constexpr ll MOD{1000000007LL};
// constexpr ll MOD{998244353LL}; // be careful
constexpr ll MAX_SIZE{3000010LL};
// constexpr ll MAX_SIZE{30000010LL}; // if 10^7 is needed
// ----- ch_max and ch_min -----
template <typename T>
void ch_max(T &left, T right)
{
  if (left < right)
  {
    left = right;
  }
}
template <typename T>
void ch_min(T &left, T right)
{
  if (left > right)
  {
    left = right;
  }
}
// ----- Mint -----
template <ll MOD = MOD>
class Mint
{
public:
  ll x;
  Mint() : x{0LL} {}
  Mint(ll x) : x{(x % MOD + MOD) % MOD} {}
  Mint operator-() const { return x ? MOD - x : 0; }
  Mint &operator+=(const Mint &a)
  {
    if ((x += a.x) >= MOD)
    {
      x -= MOD;
    }
    return *this;
  }
  Mint &operator-=(const Mint &a) { return *this += -a; }
  Mint &operator++() { return *this += 1; }
  Mint &operator++(int)
  {
    Mint tmp{*this};
    ++*this;
    return tmp;
  }
  Mint &operator--() { return *this -= 1; }
  Mint &operator--(int)
  {
    Mint tmp{*this};
    --*this;
    return tmp;
  }
  Mint &operator*=(const Mint &a)
  {
    (x *= a.x) %= MOD;
    return *this;
  }
  Mint &operator/=(const Mint &a)
  {
    Mint b{a};
    return *this *= b.power(MOD - 2);
  }
  Mint operator+(const Mint &a) const { return Mint(*this) += a; }
  Mint operator-(const Mint &a) const { return Mint(*this) -= a; }
  Mint operator*(const Mint &a) const { return Mint(*this) *= a; }
  Mint operator/(const Mint &a) const { return Mint(*this) /= a; }
  bool operator<(const Mint &a) const { return x < a.x; }
  bool operator<=(const Mint &a) const { return x <= a.x; }
  bool operator>(const Mint &a) const { return x > a.x; }
  bool operator>=(const Mint &a) const { return x >= a.x; }
  bool operator==(const Mint &a) const { return x == a.x; }
  bool operator!=(const Mint &a) const { return !(*this == a); }
  const Mint power(ll N)
  {
    if (N == 0)
    {
      return 1;
    }
    else if (N % 2 == 1)
    {
      return *this * power(N - 1);
    }
    else
    {
      Mint half = power(N / 2);
      return half * half;
    }
  }
};
template <ll MOD>
Mint<MOD> operator+(ll lhs, const Mint<MOD> &rhs)
{
  return rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator-(ll lhs, const Mint<MOD> &rhs)
{
  return -rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator*(ll lhs, const Mint<MOD> &rhs)
{
  return rhs * lhs;
}
template <ll MOD>
Mint<MOD> operator/(ll lhs, const Mint<MOD> &rhs)
{
  return Mint<MOD>{lhs} / rhs;
}
template <ll MOD>
istream &operator>>(istream &stream, Mint<MOD> &a)
{
  return stream >> a.x;
}
template <ll MOD>
ostream &operator<<(ostream &stream, const Mint<MOD> &a)
{
  return stream << a.x;
}
// ----- Combination -----
template <ll MOD = MOD, ll MAX_SIZE = MAX_SIZE>
class Combination
{
public:
  vector<Mint<MOD>> inv, fact, factinv;
  Combination() : inv(MAX_SIZE), fact(MAX_SIZE), factinv(MAX_SIZE)
  {
    inv[1] = 1;
    for (auto i = 2LL; i < MAX_SIZE; i++)
    {
      inv[i] = (-inv[MOD % i]) * (MOD / i);
    }
    fact[0] = factinv[0] = 1;
    for (auto i = 1LL; i < MAX_SIZE; i++)
    {
      fact[i] = Mint<MOD>(i) * fact[i - 1];
      factinv[i] = inv[i] * factinv[i - 1];
    }
  }
  Mint<MOD> operator()(int n, int k)
  {
    if (n >= 0 && k >= 0 && n - k >= 0)
    {
      return fact[n] * factinv[k] * factinv[n - k];
    }
    return 0;
  }
  Mint<MOD> catalan(int x, int y)
  {
    return (*this)(x + y, y) - (*this)(x + y, y - 1);
  }
};
// ----- for C++14 -----
using mint = Mint<MOD>;
using combination = Combination<MOD, MAX_SIZE>;
template <typename T>
T gcd(T x, T y) { return y ? gcd(y, x % y) : x; }
template <typename T>
T lcm(T x, T y) { return x / gcd(x, y) * y; }
// ----- for C++17 -----
template <typename T>
int popcount(T x) // C++20
{
  int ans{0};
  while (x != 0)
  {
    ans += x & 1;
    x >>= 1;
  }
  return ans;
}
// ----- frequently used constexpr -----
// constexpr double epsilon{1e-10};
// constexpr ll infty{1000000000000000LL}; // or
// constexpr int infty{1'000'000'010};
// constexpr int dx[4] = {1, 0, -1, 0};
// constexpr int dy[4] = {0, 1, 0, -1};
// ----- Yes() and No() -----
void Yes()
{
  cout << "Yes" << endl;
  exit(0);
}
void No()
{
  cout << "No" << endl;
  exit(0);
}
// ----- TLE, WA, etc. -----
void TLE() { sleep(100); }
void MLE()
{
  for (auto i = 0; i < 100000; ++i)
  {
    new int[10000000];
  }
}
void WA() { exit(0); }
void RE() { assert(false); }

// ----- main() -----

int N, W, K, V;
vector<int> c, v;

int main()
{
  cin >> N >> W >> K >> V;
  for (auto i = 0; i < N; ++i)
  {
    int x, y;
    cin >> x >> y;
    c.push_back(x);
    v.push_back(y);
  }
  RE();
}
<commit_msg>submit A.cpp to 'A - ゲーマーXとモノス大会' (rcl-contest-2020-qual) [C++14 (GCC 5.4.1)]<commit_after>#define DEBUG 1
/**
 * File    : A.cpp
 * Author  : Kazune Takahashi
 * Created : 2/15/2020, 6:48:03 PM
 * Powered by Visual Studio Code
 */
#include <algorithm>
#include <bitset>
#include <cassert>
#include <cctype>
#include <chrono>
#include <cmath>
#include <complex>
#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <functional>
#include <iomanip>
#include <iostream>
#include <map>
#include <queue>
#include <random>
#include <set>
#include <stack>
#include <string>
#include <tuple>
#include <unordered_map>
#include <unordered_set>
#include <vector>
// ----- boost -----
#include <boost/rational.hpp>
#include <boost/multiprecision/cpp_int.hpp>
// ----- using directives and manipulations -----
using namespace std;
using boost::rational;
using boost::multiprecision::cpp_int;
using ll = long long;
template <typename T>
using max_heap = priority_queue<T>;
template <typename T>
using min_heap = priority_queue<T, vector<T>, greater<T>>;
// ----- constexpr for Mint and Combination -----
constexpr ll MOD{1000000007LL};
// constexpr ll MOD{998244353LL}; // be careful
constexpr ll MAX_SIZE{3000010LL};
// constexpr ll MAX_SIZE{30000010LL}; // if 10^7 is needed
// ----- ch_max and ch_min -----
template <typename T>
void ch_max(T &left, T right)
{
  if (left < right)
  {
    left = right;
  }
}
template <typename T>
void ch_min(T &left, T right)
{
  if (left > right)
  {
    left = right;
  }
}
// ----- Mint -----
template <ll MOD = MOD>
class Mint
{
public:
  ll x;
  Mint() : x{0LL} {}
  Mint(ll x) : x{(x % MOD + MOD) % MOD} {}
  Mint operator-() const { return x ? MOD - x : 0; }
  Mint &operator+=(const Mint &a)
  {
    if ((x += a.x) >= MOD)
    {
      x -= MOD;
    }
    return *this;
  }
  Mint &operator-=(const Mint &a) { return *this += -a; }
  Mint &operator++() { return *this += 1; }
  Mint &operator++(int)
  {
    Mint tmp{*this};
    ++*this;
    return tmp;
  }
  Mint &operator--() { return *this -= 1; }
  Mint &operator--(int)
  {
    Mint tmp{*this};
    --*this;
    return tmp;
  }
  Mint &operator*=(const Mint &a)
  {
    (x *= a.x) %= MOD;
    return *this;
  }
  Mint &operator/=(const Mint &a)
  {
    Mint b{a};
    return *this *= b.power(MOD - 2);
  }
  Mint operator+(const Mint &a) const { return Mint(*this) += a; }
  Mint operator-(const Mint &a) const { return Mint(*this) -= a; }
  Mint operator*(const Mint &a) const { return Mint(*this) *= a; }
  Mint operator/(const Mint &a) const { return Mint(*this) /= a; }
  bool operator<(const Mint &a) const { return x < a.x; }
  bool operator<=(const Mint &a) const { return x <= a.x; }
  bool operator>(const Mint &a) const { return x > a.x; }
  bool operator>=(const Mint &a) const { return x >= a.x; }
  bool operator==(const Mint &a) const { return x == a.x; }
  bool operator!=(const Mint &a) const { return !(*this == a); }
  const Mint power(ll N)
  {
    if (N == 0)
    {
      return 1;
    }
    else if (N % 2 == 1)
    {
      return *this * power(N - 1);
    }
    else
    {
      Mint half = power(N / 2);
      return half * half;
    }
  }
};
template <ll MOD>
Mint<MOD> operator+(ll lhs, const Mint<MOD> &rhs)
{
  return rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator-(ll lhs, const Mint<MOD> &rhs)
{
  return -rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator*(ll lhs, const Mint<MOD> &rhs)
{
  return rhs * lhs;
}
template <ll MOD>
Mint<MOD> operator/(ll lhs, const Mint<MOD> &rhs)
{
  return Mint<MOD>{lhs} / rhs;
}
template <ll MOD>
istream &operator>>(istream &stream, Mint<MOD> &a)
{
  return stream >> a.x;
}
template <ll MOD>
ostream &operator<<(ostream &stream, const Mint<MOD> &a)
{
  return stream << a.x;
}
// ----- Combination -----
template <ll MOD = MOD, ll MAX_SIZE = MAX_SIZE>
class Combination
{
public:
  vector<Mint<MOD>> inv, fact, factinv;
  Combination() : inv(MAX_SIZE), fact(MAX_SIZE), factinv(MAX_SIZE)
  {
    inv[1] = 1;
    for (auto i = 2LL; i < MAX_SIZE; i++)
    {
      inv[i] = (-inv[MOD % i]) * (MOD / i);
    }
    fact[0] = factinv[0] = 1;
    for (auto i = 1LL; i < MAX_SIZE; i++)
    {
      fact[i] = Mint<MOD>(i) * fact[i - 1];
      factinv[i] = inv[i] * factinv[i - 1];
    }
  }
  Mint<MOD> operator()(int n, int k)
  {
    if (n >= 0 && k >= 0 && n - k >= 0)
    {
      return fact[n] * factinv[k] * factinv[n - k];
    }
    return 0;
  }
  Mint<MOD> catalan(int x, int y)
  {
    return (*this)(x + y, y) - (*this)(x + y, y - 1);
  }
};
// ----- for C++14 -----
using mint = Mint<MOD>;
using combination = Combination<MOD, MAX_SIZE>;
template <typename T>
T gcd(T x, T y) { return y ? gcd(y, x % y) : x; }
template <typename T>
T lcm(T x, T y) { return x / gcd(x, y) * y; }
// ----- for C++17 -----
template <typename T>
int popcount(T x) // C++20
{
  int ans{0};
  while (x != 0)
  {
    ans += x & 1;
    x >>= 1;
  }
  return ans;
}
// ----- frequently used constexpr -----
// constexpr double epsilon{1e-10};
// constexpr ll infty{1000000000000000LL}; // or
// constexpr int infty{1'000'000'010};
// constexpr int dx[4] = {1, 0, -1, 0};
// constexpr int dy[4] = {0, 1, 0, -1};
// ----- Yes() and No() -----
void Yes()
{
  cout << "Yes" << endl;
  exit(0);
}
void No()
{
  cout << "No" << endl;
  exit(0);
}
// ----- TLE, WA, etc. -----
void TLE() { sleep(100); }
void MLE()
{
  for (auto i = 0; i < 100000; ++i)
  {
    new int[10000000];
  }
}
void WA() { exit(0); }
void RE() { assert(false); }

// ----- main() -----

int N, W, K, V;
vector<int> c, v;
vector<int> ans;

void AC()
{
  for (auto i = 0; i < N; ++i)
  {
    cout << ans[i] + 1 << endl;
  }
}

int main()
{
  cin >> N >> W >> K >> V;
  ans.resize(N);
  for (auto i = 0; i < N; ++i)
  {
    int x, y;
    cin >> x >> y;
    c.push_back(x);
    v.push_back(y);
  }
  if (v[0] == 1)
  {
    RE();
  }
  else
  {
    TLE();
  }
}
<|endoftext|>


<commit_before>#include <src/elevator.h>
#include <src/restartwrapper.h>
#include <src/test.h>
#include <wibble/raii.h>
#include <thread>

namespace elevator {

Elevator::Elevator( int id, HeartBeat &heartbeat, ConcurrentQueue< Command > *inCommands ) :
    _id( id ), _terminate( false ), _floorsToServe( 0 ), _inCommands( inCommands ),
    _heartbeat( heartbeat ), _direction( Direction::None ), _lastDirection( Direction::None ),
    _lastFloor( _driver.minFloor() )
{
    for ( int i = 0; i < 4; ++i ) {
        _floorButtons.push_back( Button( ButtonType::TargetFloor, i + 1 ) );
        if ( i < 3 )
            _floorButtons.push_back( Button( ButtonType::CallUp, i + 1 ) );
        if ( i != 0 )
            _floorButtons.push_back( Button( ButtonType::CallDown, i + 1 ) );
    }
}
Elevator::~Elevator() {
    if ( _thread.joinable() ) {
        _terminate = true;
        _thread.join();
    }
}

void Elevator::terminate() {
    assert( _thread.joinable(), "control loop not running" );
    _terminate = true;
    _thread.join();
}

void Elevator::run() {
    _thread = std::thread( restartWrapper( &Elevator::_loop ), this );
}

void Elevator::addTargetFloor( int floor ) {
    assert_leq( _driver.minFloor(), floor, "floor out of bounds" );
    assert_leq( floor, _driver.maxFloor(), "floor out of bounds" );

    int findex = floor - _driver.minFloor();
    _floorsToServe |= (1ul << findex);
}

void Elevator::removeTargetFloor( int floor ) {
    assert_leq( _driver.minFloor(), floor, "floor out of bounds" );
    assert_leq( floor, _driver.maxFloor(), "floor out of bounds" );

    int findex = floor - _driver.minFloor();
    _floorsToServe &= ~(1ul << findex);
}

void Elevator::assertConsistency() {
    assert( _driver.alive(), "elevator hardware error" );
    assert_lt( _driver.minFloor(), _driver.maxFloor(), "invalid floor bounds" );
    assert( _direction == Direction::Up || _direction == Direction::Down
            || _direction == Direction::None, "invalid direction" );
    assert( _lastDirection == Direction::Up || _direction == Direction::Down
            || _direction == Direction::None, "invalid direction" );
    assert_leq( _floorsToServe, (1ul << (_driver.maxFloor() - _driver.minFloor() + 1)) - 1,
            "invalid floor set to serve" );
    assert_leq( _driver.minFloor(), _lastFloor, "last floor out of bounds" );
}

bool Elevator::_setHas( FloorSet set, int floor ) const {
    assert_leq( _driver.minFloor(), floor, "floor out of bounds" );
    assert_leq( floor, _driver.maxFloor(), "floor out of bounds" );

    int findex = floor - _driver.minFloor();
    return set & (1ul << findex);
}

bool Elevator::_setSet( FloorSet &set, int floor, bool value ) const {
    assert_leq( _driver.minFloor(), floor, "floor out of bounds" );
    assert_leq( floor, _driver.maxFloor(), "floor out of bounds" );

    int findex = floor - _driver.minFloor();
    bool old = set & (1ul << findex);
    if ( old != value )
        set ^= 1ul << findex;
    return old;
}

Button buttonByDirection( Direction dir, int floor ) {
    assert_neq( dir, Direction::None, "Need direction" );

    return dir == Direction::Down
        ? Button( ButtonType::CallDown, floor )
        : Button( ButtonType::CallUp, floor );
}

int Elevator::_updateAndGetFloor() {
    int f = _driver.getFloor();
    if ( f != INT_MIN )
        _lastFloor = f;
    return f;
}

void Elevator::_stopElevator() {
    if ( _direction != Direction::None )
        _lastDirection = _direction;
    _driver.stopElevator();
}

void Elevator::_startElevator() {
    _startElevator( _lastDirection );
}

void Elevator::_startElevator( Direction direction ) {
    _direction = direction;
    // we may not have current information in _lastFloor, on the other hand
    // sensor never lies but may not know
    _updateAndGetFloor(); // update information if sensor senses floor
    if ( _lastFloor == _driver.minFloor() )
        _direction = Direction::Up;
    else if ( _lastFloor == _driver.maxFloor() )
        _direction = Direction::Down;
    // if we have no clue select direction by requests
    if ( _direction == Direction::None )
        _direction = _optimalDirection();
    _driver.setMotorSpeed( _direction, _speed );
}

Direction Elevator::_optimalDirection() const {
    const int floors = _driver.maxFloor() - _driver.minFloor() + 1;
    int lowerThan{ 0 }, higherThan { 0 };
    for ( int i = 0; i < floors; ++i ) {
        if ( (_floorsToServe & (1 << i)) == 0 )
            continue;
        if ( i > _lastFloor )
            ++higherThan;
        if ( i < _lastFloor )
            ++lowerThan;
    }
    return higherThan >= lowerThan ? Direction::Up : Direction::Down;
}

bool Elevator::_floorsInDirection( Direction direction ) const {
    const FloorSet lowerMask = (1ul << (_lastFloor - _driver.minFloor())) - 1;
    if ( direction == Direction::Down )
        return _floorsToServe & lowerMask;
    if ( direction == Direction::Up )
        return _floorsToServe & (~lowerMask << 1); // shift to unmask current floor
    return false;
}

void Elevator::_loop() {
    // no matter whether exit is caused by terminate flag or exception
    // we want to stop elevator (ok, it works only for exceptions caught somewhere
    // above, but nothing better exists and we are catching assertions and
    // restarting)
    wibble::raii::defer( [&]() { _stopElevator(); } );

    // for some buttons, we need to keep track about changes
    FloorSet inFloorButtons{ 0 }, inFloorButtonsLast{ 0 };
    bool stopLast{ false }, stopNow{ false };

    enum class State { Normal, WaitingForInButton, Stopped };
    State state = State::Normal;

    while ( !_terminate.load( std::memory_order::memory_order_relaxed ) ) {
        // initialize cycle
        inFloorButtonsLast = inFloorButtons;
        inFloorButtons = 0;
        // whole loop body is locked to prevent concurrent run with dirrectCommand
        SpinLock guard( _lock );

        assertConsistency();

        // handle buttons and lamps
        for ( auto b : _floorButtons ) {
            if ( _driver.getButtonSignal( b ) ) {
                if ( !_driver.getButtonLamp( b ) ) {
                    _driver.setButtonLamp( b, true );
                    if ( b.type() == ButtonType::TargetFloor )
                        // we need to serve this one on this elevator
                        addTargetFloor( b.floor() );
                }
                // furthermore we need to keep track of pressed inside buttons
                // regardless if they were already lit, this is for the purpose
                // of detection re-pressing inside button to get elevator moving
                // after entering
                if ( b.type() == ButtonType::TargetFloor )
                    _setSet( inFloorButtons, b.floor(), true );
            }
        }
        if ( (stopNow = _driver.getStop()) && stopNow != stopLast ) {
            _driver.setStopLamp( stopNow );

            /* stop button will (surprise) stop the elevator right where it is
             * movement is resumed once button is presses again */
            if ( _driver.getStopLamp() ) {
                _stopElevator();
                state = State::Stopped;
            } else {
                state = State::Normal;
                _startElevator();
            }
        }
        if ( _driver.getObstruction() ) {
            _driver.stopElevator();
            while ( _driver.getObstruction() ) {
                /* obstruction causes infinite loop which it turn causes
                 * heartbeat timeout, which will of course repeat while
                 * obstruction is pressed */
            }
        }

        int currentFloor = _updateAndGetFloor();
        // safety precautions
        if ( currentFloor == _driver.maxFloor() && _direction == Direction::Up )
            _stopElevator();
        if ( currentFloor == _driver.minFloor() && _direction == Direction::Down )
            _stopElevator();

        if ( currentFloor != INT_MIN )
            _driver.setFloorIndicator( currentFloor );

        // note: we are here working wich _floorsInDirection shared atomic variable
        // it can change between subsequent loads, but olny this thread
        // ever removes any floor from it, so there is nothing dangerous in this

        if ( state == State::Normal ) {
            if ( _direction != Direction::None ) {

                if ( currentFloor != INT_MIN && _setHas( _floorsToServe, currentFloor ) ) {
                    _stopElevator();
                    // turn off button lights
                    _driver.setButtonLamp( Button( ButtonType::TargetFloor, currentFloor ), false );
                    _driver.setButtonLamp( buttonByDirection( _direction, currentFloor ), false );
                    // open doors
                    _driver.setDoorOpenLamp( true );
                    state = State::WaitingForInButton;
                    // this floor is served (??)
                    removeTargetFloor( currentFloor );
                }
            } else if ( _floorsToServe != 0 ) {
                if ( _floorsInDirection( _lastDirection ) )
                    _startElevator( _lastDirection );
                else
                    _startElevator( Direction::None ); // decides which direction is better itself
            }

        } else if ( state == State::WaitingForInButton ) {
            // calculate buttons which were pressed since last check
            // xor means buttons which changed state, and filters only those
            // pressed now
            FloorSet newlyPressedIn = (inFloorButtonsLast ^ inFloorButtons) & inFloorButtons;
            if ( newlyPressedIn != 0 ) {
                _driver.setDoorOpenLamp( false );
                state = State::Normal;
            }
        } else if ( state == State::Stopped ) {
            // nothing to do
        } else {
            assert_unreachable();
        }

        // it is important to do heartbeat at the end so that we don't end up
        // beating even in case we are repeatedlay auto-restarted due to assertion
        // we don't need to care about beating too often, it is cheap and safe
        _heartbeat.beat();
    }
}

}
<commit_msg>elevator: Small refactoring.<commit_after>#include <src/elevator.h>
#include <src/restartwrapper.h>
#include <src/test.h>
#include <wibble/raii.h>
#include <thread>

namespace elevator {

Elevator::Elevator( int id, HeartBeat &heartbeat, ConcurrentQueue< Command > *inCommands ) :
    _id( id ), _terminate( false ), _floorsToServe( 0 ), _inCommands( inCommands ),
    _heartbeat( heartbeat ), _direction( Direction::None ), _lastDirection( Direction::None ),
    _lastFloor( _driver.minFloor() )
{
    _lock.clear();
    for ( int i = 0; i < 4; ++i ) {
        _floorButtons.push_back( Button( ButtonType::TargetFloor, i + 1 ) );
        if ( i < 3 )
            _floorButtons.push_back( Button( ButtonType::CallUp, i + 1 ) );
        if ( i != 0 )
            _floorButtons.push_back( Button( ButtonType::CallDown, i + 1 ) );
    }
}
Elevator::~Elevator() {
    if ( _thread.joinable() ) {
        _terminate = true;
        _thread.join();
    }
}

void Elevator::terminate() {
    assert( _thread.joinable(), "control loop not running" );
    _terminate = true;
    _thread.join();
}

void Elevator::run() {
    _thread = std::thread( restartWrapper( &Elevator::_loop ), this );
}

void Elevator::addTargetFloor( int floor ) {
    assert_leq( _driver.minFloor(), floor, "floor out of bounds" );
    assert_leq( floor, _driver.maxFloor(), "floor out of bounds" );

    int findex = floor - _driver.minFloor();
    _floorsToServe |= (1ul << findex);
}

void Elevator::removeTargetFloor( int floor ) {
    assert_leq( _driver.minFloor(), floor, "floor out of bounds" );
    assert_leq( floor, _driver.maxFloor(), "floor out of bounds" );

    int findex = floor - _driver.minFloor();
    _floorsToServe &= ~(1ul << findex);
}

void Elevator::assertConsistency() {
    assert( _driver.alive(), "elevator hardware error" );
    assert_lt( _driver.minFloor(), _driver.maxFloor(), "invalid floor bounds" );
    assert( _direction == Direction::Up || _direction == Direction::Down
            || _direction == Direction::None, "invalid direction" );
    assert( _lastDirection == Direction::Up || _direction == Direction::Down
            || _direction == Direction::None, "invalid direction" );
    assert_leq( _floorsToServe, (1ul << (_driver.maxFloor() - _driver.minFloor() + 1)) - 1,
            "invalid floor set to serve" );
    assert_leq( _driver.minFloor(), _lastFloor, "last floor out of bounds" );
}

bool Elevator::_setHas( FloorSet set, int floor ) const {
    assert_leq( _driver.minFloor(), floor, "floor out of bounds" );
    assert_leq( floor, _driver.maxFloor(), "floor out of bounds" );

    int findex = floor - _driver.minFloor();
    return set & (1ul << findex);
}

bool Elevator::_setSet( FloorSet &set, int floor, bool value ) const {
    assert_leq( _driver.minFloor(), floor, "floor out of bounds" );
    assert_leq( floor, _driver.maxFloor(), "floor out of bounds" );

    int findex = floor - _driver.minFloor();
    bool old = set & (1ul << findex);
    if ( old != value )
        set ^= 1ul << findex;
    return old;
}

Button buttonByDirection( Direction dir, int floor ) {
    assert_neq( dir, Direction::None, "Need direction" );

    return dir == Direction::Down
        ? Button( ButtonType::CallDown, floor )
        : Button( ButtonType::CallUp, floor );
}

int Elevator::_updateAndGetFloor() {
    int f = _driver.getFloor();
    if ( f != INT_MIN )
        _lastFloor = f;
    return f;
}

void Elevator::_stopElevator() {
    if ( _direction != Direction::None )
        _lastDirection = _direction;
    _driver.stopElevator();
}

void Elevator::_startElevator() {
    _startElevator( _lastDirection );
}

void Elevator::_startElevator( Direction direction ) {
    _direction = direction;
    // we may not have current information in _lastFloor, on the other hand
    // sensor never lies but may not know
    _updateAndGetFloor(); // update information if sensor senses floor
    if ( _lastFloor == _driver.minFloor() )
        _direction = Direction::Up;
    else if ( _lastFloor == _driver.maxFloor() )
        _direction = Direction::Down;
    // if we have no clue select direction by requests
    if ( _direction == Direction::None )
        _direction = _optimalDirection();
    _driver.setMotorSpeed( _direction, _speed );
}

Direction Elevator::_optimalDirection() const {
    const int floors = _driver.maxFloor() - _driver.minFloor() + 1;
    int lowerThan{ 0 }, higherThan { 0 };
    for ( int i = 0; i < floors; ++i ) {
        if ( (_floorsToServe & (1 << i)) == 0 )
            continue;
        if ( i > _lastFloor )
            ++higherThan;
        if ( i < _lastFloor )
            ++lowerThan;
    }
    return higherThan >= lowerThan ? Direction::Up : Direction::Down;
}

bool Elevator::_floorsInDirection( Direction direction ) const {
    const FloorSet lowerMask = (1ul << (_lastFloor - _driver.minFloor())) - 1;
    if ( direction == Direction::Down )
        return _floorsToServe & lowerMask;
    if ( direction == Direction::Up )
        return _floorsToServe & (~lowerMask << 1); // shift to unmask current floor
    return false;
}

void Elevator::_loop() {
    // no matter whether exit is caused by terminate flag or exception
    // we want to stop elevator (ok, it works only for exceptions caught somewhere
    // above, but nothing better exists and we are catching assertions and
    // restarting)
    wibble::raii::defer( [&]() { _stopElevator(); } );

    // for some buttons, we need to keep track about changes
    FloorSet inFloorButtons{ 0 }, inFloorButtonsLast{ 0 };
    bool stopLast{ false }, stopNow{ false };

    enum class State { Normal, WaitingForInButton, Stopped };
    State state = State::Normal;

    while ( !_terminate.load( std::memory_order::memory_order_relaxed ) ) {
        // initialize cycle
        inFloorButtonsLast = inFloorButtons;
        inFloorButtons = 0;
        stopLast = stopNow;

        // whole loop body is locked to prevent concurrent run with dirrectCommand
        SpinLock guard( _lock );

        assertConsistency();

        // handle buttons and lamps
        for ( auto b : _floorButtons ) {
            if ( _driver.getButtonSignal( b ) ) {
                if ( !_driver.getButtonLamp( b ) ) { // new press
                    _driver.setButtonLamp( b, true );
                    if ( b.type() == ButtonType::TargetFloor )
                        // we need to serve this one on this elevator
                        addTargetFloor( b.floor() );
                }
                // furthermore we need to keep track of pressed inside buttons
                // regardless if they were already lit, this is for the purpose
                // of detection re-pressing inside button to get elevator moving
                // after entering
                if ( b.type() == ButtonType::TargetFloor )
                    _setSet( inFloorButtons, b.floor(), true );
            }
        }
        if ( (stopNow = _driver.getStop()) && stopNow != stopLast ) {
            _driver.setStopLamp( stopNow );

            /* stop button will (surprise) stop the elevator right where it is
             * movement is resumed once button is presses again */
            if ( stopNow ) {
                _stopElevator();
                state = State::Stopped;
            } else {
                state = State::Normal;
                _startElevator();
            }
        }
        if ( _driver.getObstruction() ) {
            _driver.stopElevator();
            while ( _driver.getObstruction() ) {
                /* obstruction causes infinite loop which it turn causes
                 * heartbeat timeout, which will of course repeat while
                 * obstruction is pressed */
            }
        }

        int currentFloor = _updateAndGetFloor();
        // safety precautions
        if ( currentFloor == _driver.maxFloor() && _direction == Direction::Up )
            _stopElevator();
        if ( currentFloor == _driver.minFloor() && _direction == Direction::Down )
            _stopElevator();

        if ( currentFloor != INT_MIN )
            _driver.setFloorIndicator( currentFloor );

        // note: we are here working wich _floorsToServe shared atomic variable
        // it can change between subsequent loads, but only this thread
        // ever removes any floor from it, so there is nothing dangerous in this

        // now do what is needed depending on state
        if ( state == State::Normal ) {
            if ( _direction != Direction::None ) {

                // check if we arrived at any floor which was scheduled for us
                if ( currentFloor != INT_MIN && _setHas( _floorsToServe, currentFloor ) ) {
                    _stopElevator();
                    // turn off button lights
                    _driver.setButtonLamp( Button( ButtonType::TargetFloor, currentFloor ), false );
                    _driver.setButtonLamp( buttonByDirection( _direction, currentFloor ), false );
                    // open doors
                    _driver.setDoorOpenLamp( true );
                    state = State::WaitingForInButton;
                    // this floor is served (??)
                    removeTargetFloor( currentFloor );
                }
            } else if ( _floorsToServe != 0 ) {
                // we are not moving but we can
                if ( _floorsInDirection( _lastDirection ) )
                    _startElevator( _lastDirection );
                else
                    _startElevator( Direction::None ); // decides which direction is better itself
            }

        } else if ( state == State::WaitingForInButton ) {
            // calculate buttons which were pressed since last check
            // xor means buttons which changed state, and filters only those
            // pressed now
            FloorSet newlyPressedIn = (inFloorButtonsLast ^ inFloorButtons) & inFloorButtons;
            if ( newlyPressedIn != 0 ) {
                _driver.setDoorOpenLamp( false );
                state = State::Normal;
            }
        } else if ( state == State::Stopped ) {
            // nothing to do
        } else {
            assert_unreachable();
        }

        // it is important to do heartbeat at the end so that we don't end up
        // beating even in case we are repeatedlay auto-restarted due to assertion
        // we don't need to care about beating too often, it is cheap and safe
        _heartbeat.beat();
    }
}

}
<|endoftext|>


<commit_before>#include "CoreAudio.h"
#include "Functiondiscoverykeys_devpkey.h"
#include "../../Logger.h"

HRESULT CoreAudio::Init() {
    HRESULT hr;

    hr = m_devEnumerator.CoCreateInstance(__uuidof(MMDeviceEnumerator));
    if (SUCCEEDED(hr)) {
        hr = m_devEnumerator->RegisterEndpointNotificationCallback(this);

        if (SUCCEEDED(hr)) {
            hr = AttachDefaultDevice();
        }
    }

    return hr;
}

void CoreAudio::Dispose() {
    DetachCurrentDevice();
    m_devEnumerator->UnregisterEndpointNotificationCallback(this);
}

HRESULT CoreAudio::AttachDefaultDevice() {
    m_critSect.Enter();

    HRESULT hr;
    hr = m_devEnumerator->GetDefaultAudioEndpoint(eRender,
        eMultimedia, &m_device);

    if (SUCCEEDED(hr)) {
        hr = m_device->Activate(__uuidof(m_volumeControl),
            CLSCTX_INPROC_SERVER, NULL, (void**)&m_volumeControl);

        CLOG(L"Attached to audio device: [%s]", DeviceName().c_str());

        if (SUCCEEDED(hr)) {
            hr = m_volumeControl->RegisterControlChangeNotify(this);
            m_registeredNotifications = SUCCEEDED(hr);
        }
    } else {
        CLOG(L"Failed to find default audio device!");
    }

    m_critSect.Leave();
    return hr;
}

void CoreAudio::DetachCurrentDevice() {
    m_critSect.Enter();

    if (m_volumeControl != NULL) {

        if (m_registeredNotifications) {
            m_volumeControl->UnregisterControlChangeNotify(this);
            m_registeredNotifications = false;
        }

        m_volumeControl.Release();
    }

    if (m_device != NULL) {
        m_device.Release();
    }

    m_critSect.Leave();
}

HRESULT CoreAudio::OnNotify(PAUDIO_VOLUME_NOTIFICATION_DATA pNotify) {
    PostMessage(m_notifyHwnd, MSG_VOLCHNG, 0, 0);
    return S_OK;
}

HRESULT CoreAudio::OnDefaultDeviceChanged(
    EDataFlow flow, ERole role, LPCWSTR pwstrDefaultDeviceId) {
    if (flow == eRender) {
        PostMessage(m_notifyHwnd, MSG_DEVCHNG, 0, 0);
    }

    return S_OK;
}

void CoreAudio::ReattachDefaultDevice() {
    DetachCurrentDevice();
    AttachDefaultDevice();
}

std::wstring CoreAudio::DeviceName() {
    HRESULT hr;
    LPWSTR devId;

    m_device->GetId(&devId);

    IPropertyStore *props = NULL;
    hr = m_device->OpenPropertyStore(STGM_READ, &props);
    PROPVARIANT pvName;
    PropVariantInit(&pvName);
    props->GetValue(PKEY_Device_FriendlyName, &pvName);

    std::wstring str(pvName.pwszVal);
    //wprintf(L"Attaching to audio device: [%s]\n", pvName.pwszVal);
    //wprintf(L"(%s)\n", devId);
    CoTaskMemFree(devId);
    PropVariantClear(&pvName);
    props->Release();

    return str;
}

float CoreAudio::Volume() {
    float vol = 0.0f;
    m_volumeControl->GetMasterVolumeLevelScalar(&vol);
    return vol;
}

void CoreAudio::Volume(float vol) {
    if (vol > 1.0f) {
        vol = 1.0f;
    }

    if (vol < 0.0f) {
        vol = 0.0f;
    }

    m_volumeControl->SetMasterVolumeLevelScalar(vol, NULL);
}

bool CoreAudio::Muted() {
    BOOL muted;
    m_volumeControl->GetMute(&muted);

    return (muted == TRUE) ? true : false;
}

void CoreAudio::Muted(bool muted) {
    m_volumeControl->SetMute(muted, NULL);
}

ULONG CoreAudio::AddRef() {
    return InterlockedIncrement(&m_refCount);
}

ULONG CoreAudio::Release() {
    long lRef = InterlockedDecrement(&m_refCount);
    if (lRef == 0) {
        delete this;
    }
    return lRef;
}

HRESULT CoreAudio::QueryInterface(REFIID iid, void** ppUnk) {
    if ((iid == __uuidof(IUnknown)) ||
        (iid == __uuidof(IMMNotificationClient))) {
        *ppUnk = static_cast<IMMNotificationClient*>(this);
    } else if (iid == __uuidof(IAudioEndpointVolumeCallback)) {
        *ppUnk = static_cast<IAudioEndpointVolumeCallback*>(this);
    } else {
        *ppUnk = NULL;
        return E_NOINTERFACE;
    }

    AddRef();
    return S_OK;
}<commit_msg>Remove test comments<commit_after>#include "CoreAudio.h"
#include "Functiondiscoverykeys_devpkey.h"
#include "../../Logger.h"

HRESULT CoreAudio::Init() {
    HRESULT hr;

    hr = m_devEnumerator.CoCreateInstance(__uuidof(MMDeviceEnumerator));
    if (SUCCEEDED(hr)) {
        hr = m_devEnumerator->RegisterEndpointNotificationCallback(this);

        if (SUCCEEDED(hr)) {
            hr = AttachDefaultDevice();
        }
    }

    return hr;
}

void CoreAudio::Dispose() {
    DetachCurrentDevice();
    m_devEnumerator->UnregisterEndpointNotificationCallback(this);
}

HRESULT CoreAudio::AttachDefaultDevice() {
    m_critSect.Enter();

    HRESULT hr;
    hr = m_devEnumerator->GetDefaultAudioEndpoint(eRender,
        eMultimedia, &m_device);

    if (SUCCEEDED(hr)) {
        hr = m_device->Activate(__uuidof(m_volumeControl),
            CLSCTX_INPROC_SERVER, NULL, (void**)&m_volumeControl);

        CLOG(L"Attached to audio device: [%s]", DeviceName().c_str());

        if (SUCCEEDED(hr)) {
            hr = m_volumeControl->RegisterControlChangeNotify(this);
            m_registeredNotifications = SUCCEEDED(hr);
        }
    } else {
        CLOG(L"Failed to find default audio device!");
    }

    m_critSect.Leave();
    return hr;
}

void CoreAudio::DetachCurrentDevice() {
    m_critSect.Enter();

    if (m_volumeControl != NULL) {

        if (m_registeredNotifications) {
            m_volumeControl->UnregisterControlChangeNotify(this);
            m_registeredNotifications = false;
        }

        m_volumeControl.Release();
    }

    if (m_device != NULL) {
        m_device.Release();
    }

    m_critSect.Leave();
}

HRESULT CoreAudio::OnNotify(PAUDIO_VOLUME_NOTIFICATION_DATA pNotify) {
    PostMessage(m_notifyHwnd, MSG_VOLCHNG, 0, 0);
    return S_OK;
}

HRESULT CoreAudio::OnDefaultDeviceChanged(
    EDataFlow flow, ERole role, LPCWSTR pwstrDefaultDeviceId) {
    if (flow == eRender) {
        PostMessage(m_notifyHwnd, MSG_DEVCHNG, 0, 0);
    }

    return S_OK;
}

void CoreAudio::ReattachDefaultDevice() {
    DetachCurrentDevice();
    AttachDefaultDevice();
}

std::wstring CoreAudio::DeviceName() {
    HRESULT hr;
    LPWSTR devId;

    m_device->GetId(&devId);

    IPropertyStore *props = NULL;
    hr = m_device->OpenPropertyStore(STGM_READ, &props);
    PROPVARIANT pvName;
    PropVariantInit(&pvName);
    props->GetValue(PKEY_Device_FriendlyName, &pvName);

    std::wstring str(pvName.pwszVal);
    CoTaskMemFree(devId);
    PropVariantClear(&pvName);
    props->Release();

    return str;
}

float CoreAudio::Volume() {
    float vol = 0.0f;
    m_volumeControl->GetMasterVolumeLevelScalar(&vol);
    return vol;
}

void CoreAudio::Volume(float vol) {
    if (vol > 1.0f) {
        vol = 1.0f;
    }

    if (vol < 0.0f) {
        vol = 0.0f;
    }

    m_volumeControl->SetMasterVolumeLevelScalar(vol, NULL);
}

bool CoreAudio::Muted() {
    BOOL muted;
    m_volumeControl->GetMute(&muted);

    return (muted == TRUE) ? true : false;
}

void CoreAudio::Muted(bool muted) {
    m_volumeControl->SetMute(muted, NULL);
}

ULONG CoreAudio::AddRef() {
    return InterlockedIncrement(&m_refCount);
}

ULONG CoreAudio::Release() {
    long lRef = InterlockedDecrement(&m_refCount);
    if (lRef == 0) {
        delete this;
    }
    return lRef;
}

HRESULT CoreAudio::QueryInterface(REFIID iid, void** ppUnk) {
    if ((iid == __uuidof(IUnknown)) ||
        (iid == __uuidof(IMMNotificationClient))) {
        *ppUnk = static_cast<IMMNotificationClient*>(this);
    } else if (iid == __uuidof(IAudioEndpointVolumeCallback)) {
        *ppUnk = static_cast<IAudioEndpointVolumeCallback*>(this);
    } else {
        *ppUnk = NULL;
        return E_NOINTERFACE;
    }

    AddRef();
    return S_OK;
}<|endoftext|>


<commit_before>/*
    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

    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 "ch.h"
#include "hal.h"

#include "analog.h"
#include "encoder.h"
#include "encoderfoaw.h"
#include "simulation.pb.h"
#include "messageutil.h"
#include "filter/movingaverage.h"

#include "blink.h"
#include "saconfig.h"
#include "utility.h"

#include "parameters.h"

#include <array>
#include <type_traits>

#include "bicycle/kinematic.h" // simplified bicycle model
#include "bicycle/whipple.h" // whipple bicycle model
#include "kalman.h" // kalman filter observer
#include "simbicycle.h"
#include "transmitter.h"

namespace {
#if defined(USE_BICYCLE_KINEMATIC_MODEL)
    using model_t = model::BicycleKinematic;
    using observer_t = std::nullptr_t;
#else // defined(USE_BICYCLE_KINEMATIC_MODEL)
    using model_t = model::BicycleWhipple;
    using observer_t = observer::Kalman<model_t>;
#endif // defined(USE_BICYCLE_KINEMATIC_MODEL)
    using bicycle_t = sim::Bicycle<model_t, observer_t>;

    // sensors
    Analog analog;
    Encoder encoder_steer(sa::RLS_ROLIN_ENC, sa::RLS_ROLIN_ENC_INDEX_CFG);
    EncoderFoaw<float, 32> encoder_rear_wheel(sa::RLS_GTS35_ENC,
                                              sa::RLS_GTS35_ENC_CFG,
                                              MS2ST(1), 3.0f);
    filter::MovingAverage<float, 5> velocity_filter;

    constexpr float fixed_velocity = 5.0f;

    // pose calculation loop
    constexpr systime_t pose_loop_period = US2ST(8333); // update pose at 120 Hz

    // dynamics loop
    constexpr systime_t dynamics_loop_period = MS2ST(1); // 1 ms -> 1 kHz

    // Suspends the invoking thread until the system time arrives to the
    // specified value.
    // The system time is assumed to be between prev and time else the call is
    // assumed to have been called outside the allowed time interval, in this
    // case the thread yields the time slot and no sleep is performed.
    systime_t chThdSleepUntilWindowedOrYield(systime_t prev, systime_t next) {
        systime_t time;

        chSysLock();
        time = chVTGetSystemTimeX();
        if (chVTIsTimeWithinX(time, prev, next)) {
            chThdSleepS(next - time);
        } else {
            chSchDoYieldS();
        }
        chSysUnlock();

        return next;
    }

    dacsample_t set_handlebar_velocity(float velocity) {
        // limit velocity to a maximum magnitude of 100 deg/s
        // input is in units of rad/s
        const float saturated_velocity = util::clamp(velocity, -sa::MAX_KOLLMORGEN_VELOCITY, sa::MAX_KOLLMORGEN_VELOCITY);
        const dacsample_t aout = saturated_velocity/sa::MAX_KOLLMORGEN_VELOCITY*sa::DAC_HALF_RANGE + sa::DAC_HALF_RANGE;
        dacPutChannelX(sa::KOLLM_DAC, 0, aout); // TODO: don't hardcode zero but find the DAC channel constant
        return aout;
    }

    constexpr float adc_to_nm(adcsample_t value, adcsample_t adc_zero, float magnitude) {
        // Convert torque from ADC samples to Nm.
        // ADC samples are 12 bits.
        // It's not clear when scaling should be applied as data was never saved after the scale
        // factors were determined.
        const int16_t shifted_value = static_cast<int16_t>(value) - static_cast<int16_t>(adc_zero);
        return static_cast<float>(shifted_value)*magnitude/static_cast<float>(sa::ADC_HALF_RANGE);
    }

    template <typename T>
    struct observer_initializer{
        template <typename S = T>
        typename std::enable_if<std::is_same<typename S::observer_t, observer::Kalman<model_t>>::value, void>::type
            initialize(S& bicycle) {
            typename S::observer_t& observer = bicycle.observer();
            observer.set_Q(parameters::defaultvalue::kalman::Q(observer.dt()));
            // Reduce steer measurement noise covariance
            observer.set_R(parameters::defaultvalue::kalman::R/1000);

            // prime the Kalman gain matrix
            bicycle.prime_observer();

            // We start with steer angle equal to the measurement and all other state elements at zero.
            model_t::state_t x0 = model_t::state_t::Zero();
            model_t::set_state_element(x0, model_t::state_index_t::steer_angle,
                    util::encoder_count<float>(encoder_steer));
            observer.set_x(x0);
        }
        template <typename S = T>
        typename std::enable_if<!std::is_same<typename S::observer_t, observer::Kalman<model_t>>::value, void>::type
            initialize(S& bicycle) {
            // no-op
            (void)bicycle;
        }

        template <typename S = T>
        typename std::enable_if<std::is_same<typename S::observer_t, observer::Kalman<model_t>>::value, void>::type
            set_message(S& bicycle, SimulationMessage* msg) {
            message::set_kalman_gain(&msg->kalman, bicycle.observer());
            msg->has_kalman = true;
        }
        template <typename S = T>
        typename std::enable_if<!std::is_same<typename S::observer_t, observer::Kalman<model_t>>::value, void>::type
            set_message(S& bicycle, SimulationMessage* msg) {
            // no-op
            (void)bicycle;
            (void)msg;
        }
    };

    struct pose_thread_arg {
        bicycle_t& bicycle;
        message::Transmitter& transmitter;
    };
    THD_WORKING_AREA(wa_pose_thread, 2048);
    THD_FUNCTION(pose_thread, arg) {
        pose_thread_arg* a = static_cast<pose_thread_arg*>(arg);

        chRegSetThreadName("pose");
        systime_t deadline = chVTGetSystemTime();
        while (true) {
            a->bicycle.update_kinematics();
            BicyclePoseMessage* msg = a->transmitter.alloc_pose_message();
            if (msg != nullptr) {
                *msg = a->bicycle.pose();
                if (a->transmitter.transmit_async(msg) != MSG_OK) {
                    // Discard pose message if transmission fails. As the
                    // receiver requires the most recent pose, we should not
                    // wait to queue a pose message.
                    a->transmitter.free_message(msg);
                }
            }

            deadline = chThdSleepUntilWindowedOrYield(deadline, deadline + pose_loop_period);
        }
    }
} // namespace

int main(void) {
    // System initializations.
    // - HAL initialization, this also initializes the configured device drivers
    //   and performs the board-specific initializations.
    // - Kernel initialization, the main() function becomes a thread and the
    //   RTOS is active.
    halInit();
    chSysInit();

    // Create LED blink thread
    chBlinkThreadCreateStatic();

    // Start sensors.
    // Encoder:
    //   Initialize encoder driver 5 on pins PA0, PA1 (EXT2-4, EXT2-8).
    //   Pins for encoder driver 3 are already set in board.h.
    palSetLineMode(LINE_TIM5_CH1, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUPDR_FLOATING);
    palSetLineMode(LINE_TIM5_CH2, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUPDR_FLOATING);
    encoder_steer.start();
    encoder_rear_wheel.start();
    analog.start(10000); // trigger ADC conversion at 10 kHz

    //Set torque measurement enable line low.
    //The output of the Kistler torque sensor is not valid until after a falling edge
    //on the measurement line and it is held low. The 'LINE_TORQUE_MEAS_EN' line is
    //reversed due to NPN switch Q1.
    palClearLine(LINE_TORQUE_MEAS_EN);
    chThdSleepMilliseconds(1);
    palSetLine(LINE_TORQUE_MEAS_EN);

    // Start DAC1 driver and set output pin as analog as suggested in Reference Manual.
    // The default line configuration is OUTPUT_OPENDRAIN_PULLUP for SPI1_ENC1_NSS
    // and must be changed to use as analog output.
    palSetLineMode(LINE_KOLLM_ACTL_TORQUE, PAL_MODE_INPUT_ANALOG);
    dacStart(sa::KOLLM_DAC, sa::KOLLM_DAC_CFG);
    set_handlebar_velocity(0.0f);

    // Initialize bicycle. The initial velocity is important as we use it to prime
    // the Kalman gain matrix.
    bicycle_t bicycle(fixed_velocity, static_cast<model::real_t>(dynamics_loop_period)/CH_CFG_ST_FREQUENCY);

    // Initialize HandlebarDynamic object to estimate torque due to handlebar inertia.
    // TODO: naming here is poor
#if !defined(FLIMNAP_ZERO_INPUT)
    haptic::HandlebarDynamic handlebar_model(bicycle.model(), sa::UPPER_ASSEMBLY_INERTIA_PHYSICAL);
#endif  // !defined(FLIMNAP_ZERO_INPUT)

    observer_initializer<observer_t> oi;
    oi.initialize(bicycle);

    // Initialize time measurements
    time_measurement_t computation_time_measurement;
    time_measurement_t transmission_time_measurement;
    chTMObjectInit(&computation_time_measurement);
    chTMObjectInit(&transmission_time_measurement);

    // Initialize USB data transmission
    message::Transmitter transmitter;
    {   // Transmit initial message containing gitsha1, model, and observer data.
        SimulationMessage* msg = transmitter.alloc_simulation_message();
        *msg = SimulationMessage_init_zero;
        msg->timestamp = chVTGetSystemTime();
        message::set_simulation_full_model_observer(msg, bicycle);
        transmitter.transmit(msg); // This blocks until USB data starts getting read
    }
    transmitter.start(NORMALPRIO + 1); // start transmission thread

    // Start running pose calculation thread
    pose_thread_arg a{bicycle, transmitter};
    chThdCreateStatic(wa_pose_thread, sizeof(wa_pose_thread),
            NORMALPRIO - 1, pose_thread, static_cast<void*>(&a));

    // Normal main() thread activity. This is the dynamics simulation loop.
    systime_t deadline = chVTGetSystemTime();
    while (true) {
        systime_t starttime = chVTGetSystemTime();
        chTMStartMeasurementX(&computation_time_measurement);
        constexpr float roll_torque = 0.0f;

        // get sensor measurements
        const float kistler_torque = adc_to_nm(analog.get_adc12(),
                sa::KISTLER_ADC_ZERO_OFFSET, sa::MAX_KISTLER_TORQUE);
        const float motor_torque = adc_to_nm(analog.get_adc13(),
                sa::KOLLMORGEN_ADC_ZERO_OFFSET, sa::MAX_KOLLMORGEN_TORQUE);
        const float steer_angle = util::encoder_count<float>(encoder_steer);
        const float rear_wheel_angle = std::fmod(-util::encoder_count<float>(encoder_rear_wheel),
                                                 constants::two_pi);
        const float v = velocity_filter.output(
                -sa::REAR_WHEEL_RADIUS*(util::encoder_rate(encoder_rear_wheel)));
        (void)motor_torque; // not currently used

        // yaw angle, just use previous state value
        const float yaw_angle = util::wrap(bicycle.pose().yaw);

        // calculate rider applied torque
#if defined(FLIMNAP_ZERO_INPUT)
        const float steer_torque = 0.0f;
        (void)kistler_torque;
#else // defined(FLIMNAP_ZERO_INPUT)
        const float inertia_torque = -handlebar_model.torque(model_t::get_state_part(bicycle.full_state()));
        const float steer_torque = kistler_torque - inertia_torque;
#endif // defined(FLIMNAP_ZERO_INPUT)

        // simulate bicycle
        bicycle.set_v(fixed_velocity);
        bicycle.update_dynamics(roll_torque, steer_torque, yaw_angle, steer_angle, rear_wheel_angle);

        // generate handlebar torque output
        const float desired_velocity = model_t::get_full_state_element(bicycle.full_state(),
                model_t::full_state_index_t::steer_rate);
        const dacsample_t handlebar_velocity_dac = set_handlebar_velocity(desired_velocity);
        chTMStopMeasurementX(&computation_time_measurement);

        {   // prepare message for transmission
            SimulationMessage* msg = transmitter.alloc_simulation_message();
            if (msg != nullptr) {
                *msg = SimulationMessage_init_zero;
                msg->timestamp = starttime;
                msg->model.v = bicycle.v();
                msg->model.has_v = true;
                msg->has_model = true;
                message::set_bicycle_input(&msg->input,
                        (model_t::input_t() << roll_torque, steer_torque).finished());
                msg->has_input = true;
                message::set_simulation_state(msg, bicycle);
                message::set_simulation_auxiliary_state(msg, bicycle);
                oi.set_message(bicycle, msg);
                message::set_simulation_actuators(msg, handlebar_velocity_dac);
                message::set_simulation_sensors(msg,
                        analog.get_adc12(), analog.get_adc13(),
                        encoder_steer.count(), encoder_rear_wheel.count());
                message::set_simulation_timing(msg,
                        computation_time_measurement.last, transmission_time_measurement.last);
                if (transmitter.transmit_async(msg) != MSG_OK) {
                    // Discard simulation message if it cannot be processed quickly enough.
                    transmitter.free_message(msg);
                }
            }
        }
        deadline = chThdSleepUntilWindowedOrYield(deadline, deadline + dynamics_loop_period);
    }
}
<commit_msg>Remove bicycle fixed velocity<commit_after>/*
    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

    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 "ch.h"
#include "hal.h"

#include "analog.h"
#include "encoder.h"
#include "encoderfoaw.h"
#include "simulation.pb.h"
#include "messageutil.h"
#include "filter/movingaverage.h"

#include "blink.h"
#include "saconfig.h"
#include "utility.h"

#include "parameters.h"

#include <array>
#include <type_traits>

#include "haptic.h"
#include "simbicycle.h"
#include "transmitter.h"

namespace {
#if defined(USE_BICYCLE_KINEMATIC_MODEL)
#include "bicycle/kinematic.h" // simplified bicycle model
    using model_t = model::BicycleKinematic;
    using observer_t = std::nullptr_t;
#else // defined(USE_BICYCLE_KINEMATIC_MODEL)
#include "bicycle/whipple.h" // whipple bicycle model
#include "kalman.h" // kalman filter observer
    using model_t = model::BicycleWhipple;
    using observer_t = observer::Kalman<model_t>;
#endif // defined(USE_BICYCLE_KINEMATIC_MODEL)
    using bicycle_t = sim::Bicycle<model_t, observer_t>;

    // sensors
    Analog analog;
    Encoder encoder_steer(sa::RLS_ROLIN_ENC, sa::RLS_ROLIN_ENC_INDEX_CFG);
    EncoderFoaw<float, 32> encoder_rear_wheel(sa::RLS_GTS35_ENC,
                                              sa::RLS_GTS35_ENC_CFG,
                                              MS2ST(1), 3.0f);
    filter::MovingAverage<float, 5> velocity_filter;

    // pose calculation loop
    constexpr systime_t pose_loop_period = US2ST(8333); // update pose at 120 Hz

    // dynamics loop
    constexpr systime_t dynamics_loop_period = MS2ST(1); // 1 ms -> 1 kHz

    // Suspends the invoking thread until the system time arrives to the
    // specified value.
    // The system time is assumed to be between prev and time else the call is
    // assumed to have been called outside the allowed time interval, in this
    // case the thread yields the time slot and no sleep is performed.
    systime_t chThdSleepUntilWindowedOrYield(systime_t prev, systime_t next) {
        systime_t time;

        chSysLock();
        time = chVTGetSystemTimeX();
        if (chVTIsTimeWithinX(time, prev, next)) {
            chThdSleepS(next - time);
        } else {
            chSchDoYieldS();
        }
        chSysUnlock();

        return next;
    }

    dacsample_t set_handlebar_velocity(float velocity) {
        // limit velocity to a maximum magnitude of 100 deg/s
        // input is in units of rad/s
        const float saturated_velocity = util::clamp(velocity, -sa::MAX_KOLLMORGEN_VELOCITY, sa::MAX_KOLLMORGEN_VELOCITY);
        const dacsample_t aout = saturated_velocity/sa::MAX_KOLLMORGEN_VELOCITY*sa::DAC_HALF_RANGE + sa::DAC_HALF_RANGE;
        dacPutChannelX(sa::KOLLM_DAC, 0, aout); // TODO: don't hardcode zero but find the DAC channel constant
        return aout;
    }

    constexpr float adc_to_nm(adcsample_t value, adcsample_t adc_zero, float magnitude) {
        // Convert torque from ADC samples to Nm.
        // ADC samples are 12 bits.
        // It's not clear when scaling should be applied as data was never saved after the scale
        // factors were determined.
        const int16_t shifted_value = static_cast<int16_t>(value) - static_cast<int16_t>(adc_zero);
        return static_cast<float>(shifted_value)*magnitude/static_cast<float>(sa::ADC_HALF_RANGE);
    }

    template <typename T>
    struct observer_initializer{
        template <typename S = T>
        typename std::enable_if<std::is_same<typename S::observer_t, observer::Kalman<model_t>>::value, void>::type
            initialize(S& bicycle) {
            typename S::observer_t& observer = bicycle.observer();
            observer.set_Q(parameters::defaultvalue::kalman::Q(observer.dt()));
            // Reduce steer measurement noise covariance
            observer.set_R(parameters::defaultvalue::kalman::R/1000);

            // prime the Kalman gain matrix
            bicycle.prime_observer();

            // We start with steer angle equal to the measurement and all other state elements at zero.
            model_t::state_t x0 = model_t::state_t::Zero();
            model_t::set_state_element(x0, model_t::state_index_t::steer_angle,
                    util::encoder_count<float>(encoder_steer));
            observer.set_x(x0);
        }
        template <typename S = T>
        typename std::enable_if<!std::is_same<typename S::observer_t, observer::Kalman<model_t>>::value, void>::type
            initialize(S& bicycle) {
            // no-op
            (void)bicycle;
        }

        template <typename S = T>
        typename std::enable_if<std::is_same<typename S::observer_t, observer::Kalman<model_t>>::value, void>::type
            set_message(S& bicycle, SimulationMessage* msg) {
            message::set_kalman_gain(&msg->kalman, bicycle.observer());
            msg->has_kalman = true;
        }
        template <typename S = T>
        typename std::enable_if<!std::is_same<typename S::observer_t, observer::Kalman<model_t>>::value, void>::type
            set_message(S& bicycle, SimulationMessage* msg) {
            // no-op
            (void)bicycle;
            (void)msg;
        }
    };

    struct pose_thread_arg {
        bicycle_t& bicycle;
        message::Transmitter& transmitter;
    };
    THD_WORKING_AREA(wa_pose_thread, 2048);
    THD_FUNCTION(pose_thread, arg) {
        pose_thread_arg* a = static_cast<pose_thread_arg*>(arg);

        chRegSetThreadName("pose");
        systime_t deadline = chVTGetSystemTime();
        while (true) {
            a->bicycle.update_kinematics();
            BicyclePoseMessage* msg = a->transmitter.alloc_pose_message();
            if (msg != nullptr) {
                *msg = a->bicycle.pose();
                if (a->transmitter.transmit_async(msg) != MSG_OK) {
                    // Discard pose message if transmission fails. As the
                    // receiver requires the most recent pose, we should not
                    // wait to queue a pose message.
                    a->transmitter.free_message(msg);
                }
            }

            deadline = chThdSleepUntilWindowedOrYield(deadline, deadline + pose_loop_period);
        }
    }
} // namespace

int main(void) {
    // System initializations.
    // - HAL initialization, this also initializes the configured device drivers
    //   and performs the board-specific initializations.
    // - Kernel initialization, the main() function becomes a thread and the
    //   RTOS is active.
    halInit();
    chSysInit();

    // Create LED blink thread
    chBlinkThreadCreateStatic();

    // Start sensors.
    // Encoder:
    //   Initialize encoder driver 5 on pins PA0, PA1 (EXT2-4, EXT2-8).
    //   Pins for encoder driver 3 are already set in board.h.
    palSetLineMode(LINE_TIM5_CH1, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUPDR_FLOATING);
    palSetLineMode(LINE_TIM5_CH2, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUPDR_FLOATING);
    encoder_steer.start();
    encoder_rear_wheel.start();
    analog.start(10000); // trigger ADC conversion at 10 kHz

    //Set torque measurement enable line low.
    //The output of the Kistler torque sensor is not valid until after a falling edge
    //on the measurement line and it is held low. The 'LINE_TORQUE_MEAS_EN' line is
    //reversed due to NPN switch Q1.
    palClearLine(LINE_TORQUE_MEAS_EN);
    chThdSleepMilliseconds(1);
    palSetLine(LINE_TORQUE_MEAS_EN);

    // Start DAC1 driver and set output pin as analog as suggested in Reference Manual.
    // The default line configuration is OUTPUT_OPENDRAIN_PULLUP for SPI1_ENC1_NSS
    // and must be changed to use as analog output.
    palSetLineMode(LINE_KOLLM_ACTL_TORQUE, PAL_MODE_INPUT_ANALOG);
    dacStart(sa::KOLLM_DAC, sa::KOLLM_DAC_CFG);
    set_handlebar_velocity(0.0f);

    // Initialize bicycle. The initial velocity is important as we use it to prime
    // the Kalman gain matrix.
    bicycle_t bicycle(0.0, static_cast<model::real_t>(dynamics_loop_period)/CH_CFG_ST_FREQUENCY);

    // Initialize HandlebarDynamic object to estimate torque due to handlebar inertia.
    // TODO: naming here is poor
#if !defined(FLIMNAP_ZERO_INPUT)
    haptic::HandlebarDynamic handlebar_model(bicycle.model(), sa::UPPER_ASSEMBLY_INERTIA_PHYSICAL);
#endif  // !defined(FLIMNAP_ZERO_INPUT)

    observer_initializer<observer_t> oi;
    oi.initialize(bicycle);

    // Initialize time measurements
    time_measurement_t computation_time_measurement;
    time_measurement_t transmission_time_measurement;
    chTMObjectInit(&computation_time_measurement);
    chTMObjectInit(&transmission_time_measurement);

    // Initialize USB data transmission
    message::Transmitter transmitter;
    {   // Transmit initial message containing gitsha1, model, and observer data.
        SimulationMessage* msg = transmitter.alloc_simulation_message();
        *msg = SimulationMessage_init_zero;
        msg->timestamp = chVTGetSystemTime();
        message::set_simulation_full_model_observer(msg, bicycle);
        transmitter.transmit(msg); // This blocks until USB data starts getting read
    }
    transmitter.start(NORMALPRIO + 1); // start transmission thread

    // Start running pose calculation thread
    pose_thread_arg a{bicycle, transmitter};
    chThdCreateStatic(wa_pose_thread, sizeof(wa_pose_thread),
            NORMALPRIO - 1, pose_thread, static_cast<void*>(&a));

    // Normal main() thread activity. This is the dynamics simulation loop.
    systime_t deadline = chVTGetSystemTime();
    while (true) {
        systime_t starttime = chVTGetSystemTime();
        chTMStartMeasurementX(&computation_time_measurement);
        constexpr float roll_torque = 0.0f;

        // get sensor measurements
        const float kistler_torque = adc_to_nm(analog.get_adc12(),
                sa::KISTLER_ADC_ZERO_OFFSET, sa::MAX_KISTLER_TORQUE);
        const float motor_torque = adc_to_nm(analog.get_adc13(),
                sa::KOLLMORGEN_ADC_ZERO_OFFSET, sa::MAX_KOLLMORGEN_TORQUE);
        const float steer_angle = util::encoder_count<float>(encoder_steer);
        const float rear_wheel_angle = std::fmod(-util::encoder_count<float>(encoder_rear_wheel),
                                                 constants::two_pi);
        const float v = velocity_filter.output(
                -sa::REAR_WHEEL_RADIUS*(util::encoder_rate(encoder_rear_wheel)));
        (void)motor_torque; // not currently used

        // yaw angle, just use previous state value
        const float yaw_angle = util::wrap(bicycle.pose().yaw);

        // calculate rider applied torque
#if defined(FLIMNAP_ZERO_INPUT)
        const float steer_torque = 0.0f;
        (void)kistler_torque;
#else // defined(FLIMNAP_ZERO_INPUT)
        const float inertia_torque = -handlebar_model.torque(model_t::get_state_part(bicycle.full_state()));
        const float steer_torque = kistler_torque - inertia_torque;
#endif // defined(FLIMNAP_ZERO_INPUT)

        // simulate bicycle
        bicycle.set_v(v);
        bicycle.update_dynamics(roll_torque, steer_torque, yaw_angle, steer_angle, rear_wheel_angle);

        // generate handlebar torque output
        const float desired_velocity = model_t::get_full_state_element(bicycle.full_state(),
                model_t::full_state_index_t::steer_rate);
        const dacsample_t handlebar_velocity_dac = set_handlebar_velocity(desired_velocity);
        chTMStopMeasurementX(&computation_time_measurement);

        {   // prepare message for transmission
            SimulationMessage* msg = transmitter.alloc_simulation_message();
            if (msg != nullptr) {
                *msg = SimulationMessage_init_zero;
                msg->timestamp = starttime;
                msg->model.v = bicycle.v();
                msg->model.has_v = true;
                msg->has_model = true;
                message::set_bicycle_input(&msg->input,
                        (model_t::input_t() << roll_torque, steer_torque).finished());
                msg->has_input = true;
                message::set_simulation_state(msg, bicycle);
                message::set_simulation_auxiliary_state(msg, bicycle);
                oi.set_message(bicycle, msg);
                message::set_simulation_actuators(msg, handlebar_velocity_dac);
                message::set_simulation_sensors(msg,
                        analog.get_adc12(), analog.get_adc13(),
                        encoder_steer.count(), encoder_rear_wheel.count());
                message::set_simulation_timing(msg,
                        computation_time_measurement.last, transmission_time_measurement.last);
                if (transmitter.transmit_async(msg) != MSG_OK) {
                    // Discard simulation message if it cannot be processed quickly enough.
                    transmitter.free_message(msg);
                }
            }
        }
        deadline = chThdSleepUntilWindowedOrYield(deadline, deadline + dynamics_loop_period);
    }
}
<|endoftext|>


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

#include "hphp/runtime/ext/extension.h"
#include "hphp/runtime/vm/native-data.h"

#include "functions.h"

#include "../../../bson.h"
#include "../../../utils.h"

extern "C" {
#include "../../../libbson/src/bson/bson.h"
}

namespace HPHP {

String HHVM_FUNCTION(MongoDBBsonFromPHP, const Variant &data)
{
	bson_t *bson;
	String s;
	unsigned char *data_s;

	VariantToBsonConverter converter(data, HIPPO_BSON_NO_FLAGS);
	bson = bson_new();
	converter.convert(bson);

	s = String(bson->len, ReserveString);
	data_s = (unsigned char*) s.bufferSlice().data();
	memcpy(data_s, bson_get_data(bson), bson->len);
	s.setSize(bson->len);

	return s;
}

Variant HHVM_FUNCTION(MongoDBBsonFromJson, const String &data)
{
	bson_t bson = BSON_INITIALIZER;
	bson_error_t error;

	if (bson_init_from_json(&bson, (const char *)data.c_str(), data.length(), &error)) {
		String s;
		unsigned char *data_s;

		s = String(bson.len, ReserveString);
		data_s = (unsigned char*) s.bufferSlice().data();
		memcpy(data_s, bson_get_data(&bson), bson.len);
		s.setSize(bson.len);

		return s;
	} else {
		throw MongoDriver::Utils::throwUnexpectedValueException(error.message ? error.message : "Error parsing JSON");
		return Variant();
	}
}

Variant HHVM_FUNCTION(MongoDBBsonToPHP, const String &data, const Variant &typemap)
{
	Variant v;
	hippo_bson_conversion_options_t options = HIPPO_TYPEMAP_INITIALIZER;

	parseTypeMap(&options, typemap.toArray());

	BsonToVariantConverter convertor((const unsigned char*) data.c_str(), data.length(), options);
	if (convertor.convert(&v)) {
		return v;
	} else {
		return Variant();
	}
}

Variant HHVM_FUNCTION(MongoDBBsonToJson, const String &data)
{
	const bson_t  *b;
	bson_reader_t *reader;
	String         s;
	bool           eof = false;

	reader = bson_reader_new_from_data((const unsigned char *)data.c_str(), data.length());
	b = bson_reader_read(reader, NULL);

	if (b) {
		char   *str;
		size_t  str_len;
		unsigned char *data_s;

		str = bson_as_json(b, &str_len);

		s = String(str_len, ReserveString);
		data_s = (unsigned char*) s.bufferSlice().data();
		memcpy(data_s, str, str_len);
		s.setSize(str_len);

		bson_free(str);
	} else {
		throw MongoDriver::Utils::throwUnexpectedValueException("Could not read document from BSON reader");
		return Variant();
	}

	if (bson_reader_read(reader, &eof) || !eof) {
		throw MongoDriver::Utils::throwUnexpectedValueException("Reading document did not exhaust input buffer");
	}

	bson_reader_destroy(reader);

	return Variant(s);
}

}
<commit_msg>HHVM-183: toJSON() should throw if bson_as_json() fails<commit_after>/**
 *  Copyright 2014-2015 MongoDB, Inc.
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *  http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 */

#include "hphp/runtime/ext/extension.h"
#include "hphp/runtime/vm/native-data.h"

#include "functions.h"

#include "../../../bson.h"
#include "../../../utils.h"

extern "C" {
#include "../../../libbson/src/bson/bson.h"
}

namespace HPHP {

String HHVM_FUNCTION(MongoDBBsonFromPHP, const Variant &data)
{
	bson_t *bson;
	String s;
	unsigned char *data_s;

	VariantToBsonConverter converter(data, HIPPO_BSON_NO_FLAGS);
	bson = bson_new();
	converter.convert(bson);

	s = String(bson->len, ReserveString);
	data_s = (unsigned char*) s.bufferSlice().data();
	memcpy(data_s, bson_get_data(bson), bson->len);
	s.setSize(bson->len);

	return s;
}

Variant HHVM_FUNCTION(MongoDBBsonFromJson, const String &data)
{
	bson_t bson = BSON_INITIALIZER;
	bson_error_t error;

	if (bson_init_from_json(&bson, (const char *)data.c_str(), data.length(), &error)) {
		String s;
		unsigned char *data_s;

		s = String(bson.len, ReserveString);
		data_s = (unsigned char*) s.bufferSlice().data();
		memcpy(data_s, bson_get_data(&bson), bson.len);
		s.setSize(bson.len);

		return s;
	} else {
		throw MongoDriver::Utils::throwUnexpectedValueException(error.message ? error.message : "Error parsing JSON");
		return Variant();
	}
}

Variant HHVM_FUNCTION(MongoDBBsonToPHP, const String &data, const Variant &typemap)
{
	Variant v;
	hippo_bson_conversion_options_t options = HIPPO_TYPEMAP_INITIALIZER;

	parseTypeMap(&options, typemap.toArray());

	BsonToVariantConverter convertor((const unsigned char*) data.c_str(), data.length(), options);
	if (convertor.convert(&v)) {
		return v;
	} else {
		return Variant();
	}
}

Variant HHVM_FUNCTION(MongoDBBsonToJson, const String &data)
{
	const bson_t  *b;
	bson_reader_t *reader;
	String         s;
	bool           eof = false;

	reader = bson_reader_new_from_data((const unsigned char *)data.c_str(), data.length());
	b = bson_reader_read(reader, NULL);

	if (b) {
		char   *str;
		size_t  str_len;
		unsigned char *data_s;

		str = bson_as_json(b, &str_len);

		if (!str) {
			bson_reader_destroy(reader);
			throw MongoDriver::Utils::throwUnexpectedValueException("Could not convert BSON document to a JSON string");
		}

		s = String(str_len, ReserveString);
		data_s = (unsigned char*) s.bufferSlice().data();
		memcpy(data_s, str, str_len);
		s.setSize(str_len);

		bson_free(str);
	} else {
		throw MongoDriver::Utils::throwUnexpectedValueException("Could not read document from BSON reader");
		return Variant();
	}

	if (bson_reader_read(reader, &eof) || !eof) {
		throw MongoDriver::Utils::throwUnexpectedValueException("Reading document did not exhaust input buffer");
	}

	bson_reader_destroy(reader);

	return Variant(s);
}

}
<|endoftext|>


<commit_before>
//
//		HSP compiler class rev.3
//			onion software/onitama 2002/2
//
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "../hsp3/hsp3config.h"
#include "../hsp3/hsp3debug.h"
#include "../hsp3/hsp3struct.h"

#include "supio.h"
#include "hsc3.h"

#include "membuf.h"
#include "strnote.h"
#include "label.h"
#include "token.h"
#include "localinfo.h"

extern char *hsp_prestr[];
extern char *hsp_prepp[];

//-------------------------------------------------------------
//		Routines
//-------------------------------------------------------------

char *CHsc3::GetError( void )
{
	return errbuf->GetBuffer();
}

int CHsc3::GetErrorSize( void )
{
	return errbuf->GetBufferSize();
}

void CHsc3::ResetError( void )
{
	//		G[bZ[W
	//
	if ( errbuf == NULL ) { delete errbuf; errbuf=NULL; }
	errbuf = new CMemBuf( 0x1000 );
	hed_option = 0;
	hed_runtime[0] = 0;
}

//-------------------------------------------------------------
//		Interfaces
//-------------------------------------------------------------

CHsc3::CHsc3( void )
{
	errbuf = new CMemBuf( 0x1000 );
	lb_info = NULL;
	addkw = NULL;
	common_path[0] = 0;
}

CHsc3::~CHsc3( void )
{
	if ( addkw == NULL ) { delete addkw; addkw=NULL; }
	if ( errbuf == NULL ) { delete errbuf; errbuf=NULL; }
}

void CHsc3::AddSystemMacros( CToken *tk, int option )
{
	process_option = option;
	if (( option & HSC3_OPT_NOHSPDEF )==0 ) {
		CLocalInfo linfo;
		tk->RegistExtMacro( "__hspver__", vercode );
		tk->RegistExtMacro( "__hsp30__","" );
		tk->RegistExtMacro( "__date__",linfo.CurrentDate() );
		tk->RegistExtMacro( "__time__",linfo.CurrentTime() );
		if ( option & HSC3_OPT_DEBUGMODE ) tk->RegistExtMacro( "_debug", "" );
	}
}

int CHsc3::PreProcessAht( char *fname, void *ahtoption, int mode )
{
	//		Preprocess execute (AHT)
	//		(IPreProcessEndĂԂ)
	//
	int res;
	char mm[512];
	CToken tk;

	lb_info = NULL;
	ahtbuf = NULL;
	tk.SetErrorBuf( errbuf );
	tk.SetCommonPath( common_path );
	tk.SetAHT( (AHTMODEL *)ahtoption );
	outbuf = new CMemBuf;

	if ( mode ) {
		ahtbuf = new CMemBuf;
		tk.SetAHTBuffer( ahtbuf );
	}

	sprintf( mm,"#AHT processor ver%s / onion software 1997-2007(c)", hspver );
	tk.Mes( mm );
	res = tk.ExpandFile( outbuf, fname, fname );
	if ( res < 0 ) return -1;
	return 0;
}

/*
	rev 54
	mingw : warning : packbuf ͖Ŏgpꂤ
	ȂAꉞΏB
*/

int CHsc3::PreProcess( char *fname, char *outname, int option, char *rname, void *ahtoption )
{
	//		Preprocess execute
	//		(IPreProcessEndĂԂ)
	//			option : bit0=ver2.55 mode(ON)
	//			         bit1=debug mode(ON)
	//			         bit2=make packfile(ON)
	//					 bit3=read AHT file(on)
	//					 bit4=write AHT file(on)
	//
	int res;
	char mm[512];
	CToken tk;
	CMemBuf *packbuf = NULL;

	lb_info = NULL;
	outbuf = new CMemBuf;
	ahtbuf = NULL;

	tk.SetErrorBuf( errbuf );
	tk.SetCommonPath( common_path );
	tk.LabelRegist2( (char *)hsp_prestr );
	AddSystemMacros( &tk, option );

	if ( option & HSC3_OPT_MAKEPACK ) {
		packbuf = new CMemBuf( 0x1000 );
		tk.SetPackfileOut( packbuf );
	}
	if ( option & (HSC3_OPT_READAHT|HSC3_OPT_MAKEAHT) ) {
		tk.SetAHT( (AHTMODEL *)ahtoption );
	}

	sprintf( mm,"#%s ver%s / onion software 1997-2008(c)", HSC3TITLE, hspver );
	tk.Mes( mm );
	tk.SetAdditionMode( 1 );

	res = tk.ExpandFile( outbuf, "hspdef.as", "hspdef.as" );
	tk.SetAdditionMode( 0 );
	if ( res<-1 ) return -1;
	res = tk.ExpandFile( outbuf, fname, rname );
	if ( res<0 ) return -1;

	cmpopt = tk.GetCmpOption();
	if ( cmpopt & CMPMODE_PPOUT	 ) {
		res = outbuf->SaveFile( outname );
		if ( res<0 ) {
			tk.Mes( "#Can't write output file." );
			return -2;
		}
	}
	outbuf->Put( (int)0 );

#if 0
	//		\[X̃xǉ(~)
	if ( addkw == NULL ) { delete addkw; addkw=NULL; }
	addkw = new CMemBuf( 0x1000 );
	tk.LabelDump( addkw, DUMPMODE_DLLCMD );
#endif

	//sprintf( mm,"#Macro buffer %x.", tk.GetLabelBufferSize() );
	//tk.Mes( mm );

	if ( option & HSC3_OPT_MAKEPACK ) {
		tk.AddPackfile( "start.ax", 1 );
		res = packbuf->SaveFile( "packfile" );
		delete packbuf;
		if ( res<0 ) {
			tk.Mes( "#Can't write packfile." );
			return -3;
		}
		tk.Mes( "#packfile generated." );
	}

	hed_option = tk.GetHeaderOption();
	strcpy( hed_runtime, tk.GetHeaderRuntimeName() );
	lb_info = tk.GetLabelInfo();

	return 0;
}

void CHsc3::PreProcessEnd( void )
{
	if ( lb_info != NULL ) {
		delete lb_info;
		lb_info = NULL;
	}
	if ( outbuf != NULL ) {
		delete outbuf;
		outbuf = NULL;
	}
	if ( ahtbuf != NULL ) {
		delete ahtbuf;
		ahtbuf = NULL;
	}
}

int CHsc3::Compile( char *fname, char *outname, int mode )
{
	//		Compile
	//
	//int res;
	//res = tcomp_main( fname, outname, errbuf, mode, "" );

	int res;
	char mm[512];
	CToken tk;
	if ( lb_info != NULL ) tk.SetLabelInfo( lb_info );		// vvZbT̃x

	tk.SetErrorBuf( errbuf );
	tk.SetCommonPath( common_path );
	tk.LabelRegist( (char *)hsp_prestr, 1 );
	tk.SetHeaderOption( hed_option, hed_runtime );
	tk.SetCmpOption( cmpopt );

	sprintf( mm,"#%s ver%s / onion software 1997-2008(c)", HSC3TITLE2, hspver );
	tk.Mes( mm );

	if ( outbuf != NULL ) {
		res = tk.GenerateCode( outbuf, outname, mode );
	} else {
		res = tk.GenerateCode( fname, outname, mode );
	}

	return res;
}

void CHsc3::SetCommonPath( char *path )
{
	if ( path==NULL ) { common_path[0]=0; return; }
	strcpy( common_path, path );
}

int CHsc3::GetCmdList( int option )
{
//	int res;
	CToken tk;
//	CMemBuf outbuf;

	tk.SetErrorBuf( errbuf );
	tk.SetCommonPath( common_path );
	tk.LabelRegist3( (char *)hsp_prestr );			// WL[[h
	tk.LabelRegist3( (char *)hsp_prepp );			// vvZbTL[[h
	AddSystemMacros( &tk, option );

//	res = tk.ExpandFile( &outbuf, "hspdef.as", "hspdef.as" );
//	if ( res<-1 ) return -1;
	tk.LabelDump( errbuf, DUMPMODE_ALL );

	//errbuf->PutStr("-----\r\n");
	//if ( addkw != NULL ) errbuf->PutStr( addkw->GetBuffer() );

	return 0;
}

int CHsc3::OpenPackfile( void )
{
	pfbuf = new CMemBuf( 0x1000 );
	if ( pfbuf->PutFile( "packfile" ) < 0 ) { delete pfbuf; return -1; }
	return 0;
}

void CHsc3::GetPackfileOption( char *out, char *keyword, char *defval )
{
	int max,i;
	char tmp[512];
	char *s;
	char a1;
	CStrNote note;
	note.Select( pfbuf->GetBuffer() );
	max = note.GetMaxLine();
	strcpy( out, defval );
	for( i=0;i<max;i++ ) {
		note.GetLine( tmp, i );
		if (( tmp[0]==';' )&&( tmp[1]=='!' )) {
			s = tmp+2;while(1) {
				a1 = *s;if (( a1==0 )||( a1=='=' )) break;
				s++;
			}
			if ( a1 != 0 ) {
				s[0]=0;
				if ( tstrcmp( tmp+2, keyword )) { strcpy( out, s+1 ); }
			}
		}
	}
}

int CHsc3::GetPackfileOptionInt( char *keyword, int defval )
{
	char tmp[512];
	char deftmp[32];
	sprintf( deftmp,"%d",defval );
	GetPackfileOption( tmp, keyword, deftmp );
	if (( tmp[0]>='0' )&&( tmp[0]<='9' )) return atoi( tmp );
	return defval;
}

void CHsc3::ClosePackfile( void )
{
	delete pfbuf;
}

int CHsc3::GetRuntimeFromHeader( char *fname, char *res )
{
	FILE *fp;
	HSPHED hsphed;
	int hedsize;
	int exsize;
	int ires;
	int i;
	char *data;

	fp=fopen( fname, "rb" );
	if ( fp == NULL ) return -1;
	hedsize = sizeof(hsphed);
	fread( &hsphed, 1, hedsize, fp );
	exsize = hsphed.pt_cs - hedsize;

	if ( exsize == 0 ) {
		fclose(fp);
		return 0;
	}

	data = (char *)malloc( exsize );
	fread( data, 1, exsize, fp );
	fclose(fp);
	ires = 0;
	if ( hsphed.bootoption & HSPHED_BOOTOPT_RUNTIME ) {
		i = hsphed.runtime - hedsize;
		strcpy( res, data + i );
		ires = 1;
	}
	free( data );
	return ires;
}

int CHsc3::SaveOutbuf( char *fname )
{
	int res;
	res = outbuf->SaveFile( fname );
	if ( res<0 ) {
		return -1;
	}
	return 0;
}

int CHsc3::SaveAHTOutbuf( char *fname )
{
	int res;
	res = ahtbuf->SaveFile( fname );
	if ( res<0 ) {
		return -1;
	}
	return 0;
}

<commit_msg>delete時のNULLチェックの条件が逆だったのを修正。 （これによって今まで hsc_clrmes にメモリリークが発生していました）<commit_after>
//
//		HSP compiler class rev.3
//			onion software/onitama 2002/2
//
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "../hsp3/hsp3config.h"
#include "../hsp3/hsp3debug.h"
#include "../hsp3/hsp3struct.h"

#include "supio.h"
#include "hsc3.h"

#include "membuf.h"
#include "strnote.h"
#include "label.h"
#include "token.h"
#include "localinfo.h"

extern char *hsp_prestr[];
extern char *hsp_prepp[];

//-------------------------------------------------------------
//		Routines
//-------------------------------------------------------------

char *CHsc3::GetError( void )
{
	return errbuf->GetBuffer();
}

int CHsc3::GetErrorSize( void )
{
	return errbuf->GetBufferSize();
}

void CHsc3::ResetError( void )
{
	//		G[bZ[W
	//
	if ( errbuf != NULL ) { delete errbuf; errbuf=NULL; }
	errbuf = new CMemBuf( 0x1000 );
	hed_option = 0;
	hed_runtime[0] = 0;
}

//-------------------------------------------------------------
//		Interfaces
//-------------------------------------------------------------

CHsc3::CHsc3( void )
{
	errbuf = new CMemBuf( 0x1000 );
	lb_info = NULL;
	addkw = NULL;
	common_path[0] = 0;
}

CHsc3::~CHsc3( void )
{
	if ( addkw != NULL ) { delete addkw; addkw=NULL; }
	if ( errbuf != NULL ) { delete errbuf; errbuf=NULL; }
}

void CHsc3::AddSystemMacros( CToken *tk, int option )
{
	process_option = option;
	if (( option & HSC3_OPT_NOHSPDEF )==0 ) {
		CLocalInfo linfo;
		tk->RegistExtMacro( "__hspver__", vercode );
		tk->RegistExtMacro( "__hsp30__","" );
		tk->RegistExtMacro( "__date__",linfo.CurrentDate() );
		tk->RegistExtMacro( "__time__",linfo.CurrentTime() );
		if ( option & HSC3_OPT_DEBUGMODE ) tk->RegistExtMacro( "_debug", "" );
	}
}

int CHsc3::PreProcessAht( char *fname, void *ahtoption, int mode )
{
	//		Preprocess execute (AHT)
	//		(IPreProcessEndĂԂ)
	//
	int res;
	char mm[512];
	CToken tk;

	lb_info = NULL;
	ahtbuf = NULL;
	tk.SetErrorBuf( errbuf );
	tk.SetCommonPath( common_path );
	tk.SetAHT( (AHTMODEL *)ahtoption );
	outbuf = new CMemBuf;

	if ( mode ) {
		ahtbuf = new CMemBuf;
		tk.SetAHTBuffer( ahtbuf );
	}

	sprintf( mm,"#AHT processor ver%s / onion software 1997-2007(c)", hspver );
	tk.Mes( mm );
	res = tk.ExpandFile( outbuf, fname, fname );
	if ( res < 0 ) return -1;
	return 0;
}

/*
	rev 54
	mingw : warning : packbuf ͖Ŏgpꂤ
	ȂAꉞΏB
*/

int CHsc3::PreProcess( char *fname, char *outname, int option, char *rname, void *ahtoption )
{
	//		Preprocess execute
	//		(IPreProcessEndĂԂ)
	//			option : bit0=ver2.55 mode(ON)
	//			         bit1=debug mode(ON)
	//			         bit2=make packfile(ON)
	//					 bit3=read AHT file(on)
	//					 bit4=write AHT file(on)
	//
	int res;
	char mm[512];
	CToken tk;
	CMemBuf *packbuf = NULL;

	lb_info = NULL;
	outbuf = new CMemBuf;
	ahtbuf = NULL;

	tk.SetErrorBuf( errbuf );
	tk.SetCommonPath( common_path );
	tk.LabelRegist2( (char *)hsp_prestr );
	AddSystemMacros( &tk, option );

	if ( option & HSC3_OPT_MAKEPACK ) {
		packbuf = new CMemBuf( 0x1000 );
		tk.SetPackfileOut( packbuf );
	}
	if ( option & (HSC3_OPT_READAHT|HSC3_OPT_MAKEAHT) ) {
		tk.SetAHT( (AHTMODEL *)ahtoption );
	}

	sprintf( mm,"#%s ver%s / onion software 1997-2008(c)", HSC3TITLE, hspver );
	tk.Mes( mm );
	tk.SetAdditionMode( 1 );

	res = tk.ExpandFile( outbuf, "hspdef.as", "hspdef.as" );
	tk.SetAdditionMode( 0 );
	if ( res<-1 ) return -1;
	res = tk.ExpandFile( outbuf, fname, rname );
	if ( res<0 ) return -1;

	cmpopt = tk.GetCmpOption();
	if ( cmpopt & CMPMODE_PPOUT	 ) {
		res = outbuf->SaveFile( outname );
		if ( res<0 ) {
			tk.Mes( "#Can't write output file." );
			return -2;
		}
	}
	outbuf->Put( (int)0 );

#if 0
	//		\[X̃xǉ(~)
	if ( addkw != NULL ) { delete addkw; addkw=NULL; }
	addkw = new CMemBuf( 0x1000 );
	tk.LabelDump( addkw, DUMPMODE_DLLCMD );
#endif

	//sprintf( mm,"#Macro buffer %x.", tk.GetLabelBufferSize() );
	//tk.Mes( mm );

	if ( option & HSC3_OPT_MAKEPACK ) {
		tk.AddPackfile( "start.ax", 1 );
		res = packbuf->SaveFile( "packfile" );
		delete packbuf;
		if ( res<0 ) {
			tk.Mes( "#Can't write packfile." );
			return -3;
		}
		tk.Mes( "#packfile generated." );
	}

	hed_option = tk.GetHeaderOption();
	strcpy( hed_runtime, tk.GetHeaderRuntimeName() );
	lb_info = tk.GetLabelInfo();

	return 0;
}

void CHsc3::PreProcessEnd( void )
{
	if ( lb_info != NULL ) {
		delete lb_info;
		lb_info = NULL;
	}
	if ( outbuf != NULL ) {
		delete outbuf;
		outbuf = NULL;
	}
	if ( ahtbuf != NULL ) {
		delete ahtbuf;
		ahtbuf = NULL;
	}
}

int CHsc3::Compile( char *fname, char *outname, int mode )
{
	//		Compile
	//
	//int res;
	//res = tcomp_main( fname, outname, errbuf, mode, "" );

	int res;
	char mm[512];
	CToken tk;
	if ( lb_info != NULL ) tk.SetLabelInfo( lb_info );		// vvZbT̃x

	tk.SetErrorBuf( errbuf );
	tk.SetCommonPath( common_path );
	tk.LabelRegist( (char *)hsp_prestr, 1 );
	tk.SetHeaderOption( hed_option, hed_runtime );
	tk.SetCmpOption( cmpopt );

	sprintf( mm,"#%s ver%s / onion software 1997-2008(c)", HSC3TITLE2, hspver );
	tk.Mes( mm );

	if ( outbuf != NULL ) {
		res = tk.GenerateCode( outbuf, outname, mode );
	} else {
		res = tk.GenerateCode( fname, outname, mode );
	}

	return res;
}

void CHsc3::SetCommonPath( char *path )
{
	if ( path==NULL ) { common_path[0]=0; return; }
	strcpy( common_path, path );
}

int CHsc3::GetCmdList( int option )
{
//	int res;
	CToken tk;
//	CMemBuf outbuf;

	tk.SetErrorBuf( errbuf );
	tk.SetCommonPath( common_path );
	tk.LabelRegist3( (char *)hsp_prestr );			// WL[[h
	tk.LabelRegist3( (char *)hsp_prepp );			// vvZbTL[[h
	AddSystemMacros( &tk, option );

//	res = tk.ExpandFile( &outbuf, "hspdef.as", "hspdef.as" );
//	if ( res<-1 ) return -1;
	tk.LabelDump( errbuf, DUMPMODE_ALL );

	//errbuf->PutStr("-----\r\n");
	//if ( addkw != NULL ) errbuf->PutStr( addkw->GetBuffer() );

	return 0;
}

int CHsc3::OpenPackfile( void )
{
	pfbuf = new CMemBuf( 0x1000 );
	if ( pfbuf->PutFile( "packfile" ) < 0 ) { delete pfbuf; return -1; }
	return 0;
}

void CHsc3::GetPackfileOption( char *out, char *keyword, char *defval )
{
	int max,i;
	char tmp[512];
	char *s;
	char a1;
	CStrNote note;
	note.Select( pfbuf->GetBuffer() );
	max = note.GetMaxLine();
	strcpy( out, defval );
	for( i=0;i<max;i++ ) {
		note.GetLine( tmp, i );
		if (( tmp[0]==';' )&&( tmp[1]=='!' )) {
			s = tmp+2;while(1) {
				a1 = *s;if (( a1==0 )||( a1=='=' )) break;
				s++;
			}
			if ( a1 != 0 ) {
				s[0]=0;
				if ( tstrcmp( tmp+2, keyword )) { strcpy( out, s+1 ); }
			}
		}
	}
}

int CHsc3::GetPackfileOptionInt( char *keyword, int defval )
{
	char tmp[512];
	char deftmp[32];
	sprintf( deftmp,"%d",defval );
	GetPackfileOption( tmp, keyword, deftmp );
	if (( tmp[0]>='0' )&&( tmp[0]<='9' )) return atoi( tmp );
	return defval;
}

void CHsc3::ClosePackfile( void )
{
	delete pfbuf;
}

int CHsc3::GetRuntimeFromHeader( char *fname, char *res )
{
	FILE *fp;
	HSPHED hsphed;
	int hedsize;
	int exsize;
	int ires;
	int i;
	char *data;

	fp=fopen( fname, "rb" );
	if ( fp == NULL ) return -1;
	hedsize = sizeof(hsphed);
	fread( &hsphed, 1, hedsize, fp );
	exsize = hsphed.pt_cs - hedsize;

	if ( exsize == 0 ) {
		fclose(fp);
		return 0;
	}

	data = (char *)malloc( exsize );
	fread( data, 1, exsize, fp );
	fclose(fp);
	ires = 0;
	if ( hsphed.bootoption & HSPHED_BOOTOPT_RUNTIME ) {
		i = hsphed.runtime - hedsize;
		strcpy( res, data + i );
		ires = 1;
	}
	free( data );
	return ires;
}

int CHsc3::SaveOutbuf( char *fname )
{
	int res;
	res = outbuf->SaveFile( fname );
	if ( res<0 ) {
		return -1;
	}
	return 0;
}

int CHsc3::SaveAHTOutbuf( char *fname )
{
	int res;
	res = ahtbuf->SaveFile( fname );
	if ( res<0 ) {
		return -1;
	}
	return 0;
}

<|endoftext|>


<commit_before>////////////////////////////////////////////////////////////////////////////////
// taskwarrior - a command line task list manager.
//
// Copyright 2006 - 2010, Paul Beckingham.
// All rights reserved.
//
// 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.,
//     51 Franklin Street, Fifth Floor,
//     Boston, MA
//     02110-1301
//     USA
//
////////////////////////////////////////////////////////////////////////////////
#include <Context.h>
#include <Task.h>
#include <Subst.h>
#include <test.h>

Context context;

////////////////////////////////////////////////////////////////////////////////
int main (int argc, char** argv)
{
  UnitTest t (19);

  Task task;
  task.set ("description", "one two three four");

  context.config.set ("search.case.sensitive", "yes");

  Subst s;
  t.ok (s.valid ("/a/b/x"),         "valid /a/b/x");
  t.ok (s.valid ("/a/b/"),          "valid /a/b/");
  t.ok (s.valid ("/two/TWO/"),      "valid /two/TWO/");
  t.ok (s.valid ("/e /E /g"),       "valid /e /E /g");
  t.ok (s.valid ("/from/to/g"),     "valid /from/to/g");
  t.ok (s.valid ("/long string//"), "valid /long string//");
  t.ok (s.valid ("//fail/"),        "valid //fail/");

  bool good = true;
  try { s.parse ("/a/b/x"); } catch (...) { good = false; }
  t.notok (good, "failed /a/b/x");

  good = true;
  try { s.parse ("//to/"); } catch (...) { good = false; }
  t.notok (good, "failed //to/");

  good = true;
  try { s.parse ("/two/TWO/"); } catch (...) { good = false; }
  t.ok (good, "parsed /two/TWO/");
  if (good)
  {
    std::string description = task.get ("description");
    std::vector <Att> annotations;
    task.getAnnotations (annotations);

    s.apply (description, annotations);
    t.is (description, "one TWO three four", "single word subst");
  }
  else
  {
    t.fail ("failed to parse '/two/TWO/'");
  }

  good = true;
  try { s.parse ("/e /E /g"); } catch (...) { good = false; }
  t.ok (good, "parsed /e /E /g");
  if (good)
  {
    std::string description = task.get ("description");
    std::vector <Att> annotations;
    task.getAnnotations (annotations);

    s.apply (description, annotations);
    t.is (description, "onE two threE four", "multiple word subst");
  }
  else
  {
    t.fail ("failed to parse '/e /E /g'");
  }

  // Now repeat the last two tests with a case-insensitive setting.
  context.config.set ("search.case.sensitive", "no");
  good = true;
  try { s.parse ("/tWo/TWO/"); } catch (...) { good = false; }
  t.ok (good, "parsed /tWo/TWO/ (rc.search.case.sensitive=no)");
  if (good)
  {
    std::string description = task.get ("description");
    std::vector <Att> annotations;
    task.getAnnotations (annotations);

    s.apply (description, annotations);
    t.is (description, "one TWO three four", "single word subst (rc.search.case.sensitive=no)");
  }
  else
  {
    t.fail ("failed to parse '/tWo/TWO/' (rc.search.case.sensitive=no)");
  }

  good = true;
  try { s.parse ("/E /E /g"); } catch (...) { good = false; }
  t.ok (good, "parsed /E /E /g (rc.search.case.sensitive=no)");
  if (good)
  {
    std::string description = task.get ("description");
    std::vector <Att> annotations;
    task.getAnnotations (annotations);

    s.apply (description, annotations);
    t.is (description, "onE two threE four", "multiple word subst (rc.search.case.sensitive=no)");
  }
  else
  {
    t.fail ("failed to parse '/E /E /g' (rc.search.case.sensitive=no)");
  }

  context.config.set ("search.case.sensitive", "yes");
  good = true;
  try { s.parse ("/from/to/g"); } catch (...) { good = false; }
  t.ok (good, "parsed /from/to/g");
  if (good)
  {
    std::string description = task.get ("description");
    std::vector <Att> annotations;
    task.getAnnotations (annotations);

    s.apply (description, annotations);
    t.is (description, "one two three four", "multiple word subst mismatch");
  }
  else
  {
    t.fail ("failed to parse '/from/to/g'");
  }

  return 0;
}

////////////////////////////////////////////////////////////////////////////////
<commit_msg>Bug<commit_after>////////////////////////////////////////////////////////////////////////////////
// taskwarrior - a command line task list manager.
//
// Copyright 2006 - 2010, Paul Beckingham.
// All rights reserved.
//
// 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.,
//     51 Franklin Street, Fifth Floor,
//     Boston, MA
//     02110-1301
//     USA
//
////////////////////////////////////////////////////////////////////////////////
#include <Context.h>
#include <Task.h>
#include <Subst.h>
#include <test.h>

Context context;

////////////////////////////////////////////////////////////////////////////////
int main (int argc, char** argv)
{
  UnitTest t (18);

  Task task;
  task.set ("description", "one two three four");

  context.config.set ("search.case.sensitive", "yes");

  Subst s;
  t.ok (s.valid ("/a/b/"),          "valid /a/b/");
  t.ok (s.valid ("/two/TWO/"),      "valid /two/TWO/");
  t.ok (s.valid ("/e /E /g"),       "valid /e /E /g");
  t.ok (s.valid ("/from/to/g"),     "valid /from/to/g");
  t.ok (s.valid ("/long string//"), "valid /long string//");
  t.ok (s.valid ("//fail/"),        "valid //fail/");

  bool good = true;
  try { s.parse ("/a/b/x"); } catch (...) { good = false; }
  t.notok (good, "failed /a/b/x");

  good = true;
  try { s.parse ("//to/"); } catch (...) { good = false; }
  t.notok (good, "failed //to/");

  good = true;
  try { s.parse ("/two/TWO/"); } catch (...) { good = false; }
  t.ok (good, "parsed /two/TWO/");
  if (good)
  {
    std::string description = task.get ("description");
    std::vector <Att> annotations;
    task.getAnnotations (annotations);

    s.apply (description, annotations);
    t.is (description, "one TWO three four", "single word subst");
  }
  else
  {
    t.fail ("failed to parse '/two/TWO/'");
  }

  good = true;
  try { s.parse ("/e /E /g"); } catch (...) { good = false; }
  t.ok (good, "parsed /e /E /g");
  if (good)
  {
    std::string description = task.get ("description");
    std::vector <Att> annotations;
    task.getAnnotations (annotations);

    s.apply (description, annotations);
    t.is (description, "onE two threE four", "multiple word subst");
  }
  else
  {
    t.fail ("failed to parse '/e /E /g'");
  }

  // Now repeat the last two tests with a case-insensitive setting.
  context.config.set ("search.case.sensitive", "no");
  good = true;
  try { s.parse ("/tWo/TWO/"); } catch (...) { good = false; }
  t.ok (good, "parsed /tWo/TWO/ (rc.search.case.sensitive=no)");
  if (good)
  {
    std::string description = task.get ("description");
    std::vector <Att> annotations;
    task.getAnnotations (annotations);

    s.apply (description, annotations);
    t.is (description, "one TWO three four", "single word subst (rc.search.case.sensitive=no)");
  }
  else
  {
    t.fail ("failed to parse '/tWo/TWO/' (rc.search.case.sensitive=no)");
  }

  good = true;
  try { s.parse ("/E /E /g"); } catch (...) { good = false; }
  t.ok (good, "parsed /E /E /g (rc.search.case.sensitive=no)");
  if (good)
  {
    std::string description = task.get ("description");
    std::vector <Att> annotations;
    task.getAnnotations (annotations);

    s.apply (description, annotations);
    t.is (description, "onE two threE four", "multiple word subst (rc.search.case.sensitive=no)");
  }
  else
  {
    t.fail ("failed to parse '/E /E /g' (rc.search.case.sensitive=no)");
  }

  context.config.set ("search.case.sensitive", "yes");
  good = true;
  try { s.parse ("/from/to/g"); } catch (...) { good = false; }
  t.ok (good, "parsed /from/to/g");
  if (good)
  {
    std::string description = task.get ("description");
    std::vector <Att> annotations;
    task.getAnnotations (annotations);

    s.apply (description, annotations);
    t.is (description, "one two three four", "multiple word subst mismatch");
  }
  else
  {
    t.fail ("failed to parse '/from/to/g'");
  }

  return 0;
}

////////////////////////////////////////////////////////////////////////////////
<|endoftext|>