<commit_before>#include "actor.h"
#include "gamelevel.h"
#include "modules.h"
#include "util.h"
#include "assets.h"

// === Submarine ===

void Submarine::initialize() {
  x = W << 8;
  y = (SCREEN_HEIGHT / 2) << 8;
  extraLives = START_LIVES;
  armor      = ARMOR_FRAMES;
}

void Submarine::move(GameLevel& context) {
  if(context.isGameover()) { return; }

  // control
  static const int SPD = 1 << 7;
  static const int R2  = 90;  // 0.5/sqrt(2) / (1/256)
  if(context.core.pressed(BTN_U) && context.core.pressed(BTN_L)) {
    x -= R2;
    y -= R2;
  }
  else if(context.core.pressed(BTN_U) && context.core.pressed(BTN_R)) {
    x += R2;
    y -= R2;
  }
  else if(context.core.pressed(BTN_D) && context.core.pressed(BTN_L)) {
    x -= R2;
    y += R2;
  }
  else if(context.core.pressed(BTN_D) && context.core.pressed(BTN_R)) {
    x += R2;
    y += R2;
  }
  else if(context.core.pressed(BTN_U)) {
    y -= SPD;
  }
  else if(context.core.pressed(BTN_L)) {
    x -= SPD;
  }
  else if(context.core.pressed(BTN_R)) {
    x += SPD;
  }
  else if(context.core.pressed(BTN_D)) {
    y += SPD;
  }

  // clamping into field
  static const fixed MARGIN = 6;
  x = Clamp(x, 0, (SCREEN_WIDTH - MARGIN) << 8);
  y = Clamp(y, (MARGIN - H) << 8, (SCREEN_HEIGHT - MARGIN) << 8);

  // launching torpedo
  if(extraLives >= 0 && (context.core.pushed(BTN_A) || context.core.pushed(BTN_B))) {
    context.launchTorpedo(fieldX() + 10, fieldY() + 1);
  }

  //firing auto shot
#ifdef LOW_FLASH_MEMORY
  if(context.frameCount() % 5 == 0) {
    context.fireAutoShot(fieldX() + 3, fieldY() - 3);
  }
#else
  if(context.lookForEnemy() && context.frameCount() % 5 == 0) {
    context.fireAutoShot(fieldX() + 3, fieldY() - 3);
  }
#endif

  // updating armor timer
  if(armor > 0) { --armor; }
}

void Submarine::draw(GameLevel& context) const {
  if(armor == 0 || (extraLives >= 0 && context.frameCount() / 3 % 2 == 0)) {
    context.core.drawBitmap(fieldX() - 1, fieldY() - 2, bitmapSubmarine, 2);
    if(context.frameCount() % ECHO_CYCLE > 30) {  // sustain: 30 frames
      context.core.drawPixel(fieldX() + 6, fieldY() - 1, 1);
    }
  }
  //context.getArduboy().drawRect(x, y, W, H, 0);
}

void Submarine::onHit(GameLevel& context) {
  if(armor <= 0) {
    if(extraLives >= 0) {
      context.spawnParticle(fieldX() - 2, fieldY() - 4, PARTICLE_EXPLOSION);
      context.core.tone(185, 250);
    }
    --extraLives;
    if(extraLives < 0) {
      context.setGameover();
      // ToDo: add gameover sfx
      inactivate();
    }
    armor = ARMOR_FRAMES;
  }
}

// === Torpedo ===

bool Torpedo::launch(const char x, const char y) {
  if(!exist()) {
    this->x = x;
    this->y = y;
    vx = 0;
    return true;
  }
  return false;
}

void Torpedo::move() {
  static const fixed ACC = 1 << 7;  // 0.5
  if(!exist()) { return; }
  vx += ACC - (vx >> 4);
  x  += vx >> 8;
  if(x >= FIELD_WIDTH) {
    inactivate();
  }
}

void Torpedo::draw(GameLevel& context) const {
  if(!exist()) { return; }
  // body
  context.core.drawBitmap(x, y - 1, bitmapTorpedo, 2);
  // shockwave
  if(vx > 4 << 8) { // well accelerated
    const byte w = bitmapShockwave0[0];
    const byte h = bitmapShockwave0[1];
    context.core.drawBitmap(x -  8, y - h/2, bitmapShockwave0, 2);
    context.core.drawBitmap(x - 16, y - h/2, bitmapShockwave1, 2);
    context.core.drawBitmap(x - 24, y - h/2, bitmapShockwave2, 2);
  }
  //context.getArduboy.drawFastHLine(x, y, W, 0);
}

// === AutoShot ===

void AutoShot::initialize(const char x, const char y) {
  this->x = x;
  this->y = y;
}

void AutoShot::move() {
  // moving
  x += 8;

  // frame out
  if(x > SCREEN_WIDTH) {
    inactivate();
  }
}

void AutoShot::draw(GameLevel& context) const {
  context.core.drawBitmap((int)x + 2, (int)y + 3, bitmapAutoShot, 2);
  //context.core.getArduboy().drawRect(x, y, W, H, 1);
}

// === BigEnemy ===

void BigEnemy::initialize(const char y) {
  this->x = FIELD_WIDTH;
  this->y = y;
  state   = 0;
  timer   = 0;
}

void BigEnemy::move(GameLevel& context) {
  // moving
  if(grazed() && x > SCREEN_WIDTH - 5) {
    x += -3; // submarine found
  }
  else if(timer % 5 == 0) { // -0.2
    --x;
  }

  // frame out
  if(x + bitmapCruEnemy0[0] < 0) {
    inactivate();
  }

  // setting sonar reaction
  context.echo.add(x, y, y+H);

  // firing bullet
  if(x < SCREEN_WIDTH - W / 2) {
    if(!onScreen()) {
      timer = 0;  // in order to sync fire cycle
      state |= ON_SCREEN_MASK;
    }
    const byte d = context.difficulty();
    if(timer == 0 || (d >= 60 && timer == 40)) {  // freq up when 60 and over
      const char bulletY     = y + bitmapCruEnemy0[1] / 2;
      const char bulletSpeed = (d >= 80) ? BULLET_TYPE2_SPD : BULLET_TYPE0_SPD;
      const char bulletType  = (d >= 80) ? 2 : 0;
      context.fireBullet(x, bulletY, context.getFutureSubmarineAngle(x, bulletY, bulletSpeed), bulletType);
    }
  }

  // updating timer
  static const byte PERIOD = 150;
  timer = (timer + 1) % PERIOD;
}

void BigEnemy::draw(GameLevel& context) const {
  if(x - 3 > SCREEN_WIDTH) { return; }
  context.core.drawBitmap(x - 3, y - 2, bitmapCruEnemy0, 2);
}

void BigEnemy::onHit(GameLevel& context) {
  // add score
  context.addScore(10);
  if(x > SCREEN_WIDTH) {  // far bonus
    context.addScore((x - SCREEN_WIDTH) * 10 / (FIELD_WIDTH - SCREEN_WIDTH));
  }
  // near
  if(x < SCREEN_WIDTH + 20) {
    context.core.tone(1047, 250);
    context.spawnParticle(x + random(-2,  8), y + random(-4, 4), PARTICLE_EXPLOSION);
    context.spawnParticle(x + random( 8, 18), y + random(-4, 4), PARTICLE_EXPLOSION);
#ifndef LOW_FLASH_MEMORY
    context.removeAllBullets();
    context.core.setQuake();
#endif
  }
  // reset
  inactivate();
}

// === SmallEnemy ===

void SmallEnemy::initialize(const char y, const byte type) {
  this->x    = FIELD_WIDTH;
  this->y    = y;
  this->type = type;
  timer      = 0;
}

void SmallEnemy::move(GameLevel& context) {
  const byte period = (getType() == 0) ? ZIG_PERIOD : TRI_PERIOD;

  switch(getType()) {
    // zigzag
    case 0: {
      // moving
      // x -= 1.5, y -= 0.5
      if(timer % 2 == 0) {
        x -= 2;
        y += (timer / (ZIG_PERIOD / 4) % 2 == 0) ? 1 : -1;
      }
      else {
        --x;
      }

      // firing bullet
      const byte d = context.difficulty();
      const byte typeCond = canFire() ||
        (d >= 70 && d < 80) || d >= 100;  // [70,80) or 100
      if(
        timer == 80 && typeCond &&    // time / type
        x < SCREEN_WIDTH - W / 2 &&   // position
        d >= 20                       // dont fire less than 20
      ) {
        const byte  bulletType = (d >= 80) ? 3 : 1;  // rapid when 80 and over
        const float subAngle   = context.getSubmarineAngle(x, y);
        context.fireBullet(x, y, subAngle, bulletType);
        // 3way
        if((d >= 50 && d < 70) || d >= 90) {
          context.fireBullet(x, y, subAngle - radians(10), 1);
          context.fireBullet(x, y, subAngle + radians(10), 1);
        }
      }
    } break;

    // triangle
    default: {
      // moving
      if(timer % 6 == 0) { --x; }
      const int half = period / 2;
      int tmp = half - timer % half;
      tmp = tmp*tmp / (half*half/4) - 1;
      if(tmp < 0) { tmp = 0; }
      x -= tmp;

      // firing bullet
      const bool timeCond = timer == 64 ||
        (context.difficulty() >= 25 && timer == 32) ||
        (context.difficulty() >= 75 && timer == 96);
      if(
        timeCond && canFire() &&                    // time / type
        x < SCREEN_WIDTH && x > SCREEN_WIDTH / 2 && // position
        context.difficulty() >= 20                  // difficulty
      ) {
        context.fireBullet(x, y, context.getSubmarineAngle(x, y), 1);
      }
    } break;
  }

  // frame out
  if(x + 12 < 0) {
    context.platoons.checkBonus(getPlatoon(), false);
    inactivate();
  }

  // setting sonar reaction
  context.echo.add(x, y, y+H);

  // updating timer
  timer = (timer + 1) % period;
}

void SmallEnemy::draw(GameLevel& context) const {
  if(x - 4 > SCREEN_WIDTH) { return; }
  switch(getType()) {
    // zigzag
    case 0: {
      const byte* bitmaps[] = {bitmapZigEnemy0, bitmapZigEnemy1};
      const byte frame = timer / (ZIG_PERIOD / 4) % 2;
      context.core.drawBitmap(x - 3, y - 3, bitmaps[frame], 2);
    } break;

    // triangle
    default: {
      const byte* bitmaps[] = {bitmapTriEnemy0, bitmapTriEnemy1};
      const byte frame = (timer % (TRI_PERIOD / 2) < 24) ? 1 : 0;
      context.core.drawBitmap(x - 3, y - 4, bitmaps[frame], 2);
      if(frame == 1) {
        context.core.drawBitmap(x + 7, y - 2, bitmapBoostEffect, 2);
      }
    } break;
  }
  //context.core.getArduboy().drawRect(x, y, W, H, 0);
}

void SmallEnemy::onHit(GameLevel& context) {
  context.addScore(1);
  if(x < SCREEN_WIDTH + 20) { // near
    context.spawnParticle(round(x) - 5, round(y) - 4, PARTICLE_EXPLOSION);
    context.core.tone(880, 62);
  }
  // platoon elimination
  if(context.platoons.checkBonus(getPlatoon(), true)) {
    if(x < SCREEN_WIDTH + 20) { // near
      context.spawnParticle(round(x) - 2, round(y) - 2, PARTICLE_TEN_POINT);
    }
    context.addScore(10);
  }
  inactivate();
}

// === Bullet ===

void Bullet::initialize(const char x, const char y, const float radian, const byte type) {
  this->x    = x << 8;
  this->y    = y << 8;
  this->type = type;

  const byte SPEEDS[] = {BULLET_TYPE0_SPD, BULLET_TYPE1_SPD, BULLET_TYPE2_SPD, BULLET_TYPE3_SPD};
  byte speed = SPEEDS[type];
  vx = round(cos(radian) * speed / (1.f / 256));
  vy = round(sin(radian) * speed / (1.f / 256));
}

void Bullet::move(GameLevel& context) {
  x += vx;
  y += vy;

  // frame out
  static const char MARGIN = 4;
  if(
    fieldX() < -MARGIN || fieldX() > SCREEN_WIDTH  + MARGIN ||
    fieldY() < -MARGIN || fieldY() > SCREEN_HEIGHT + MARGIN
  ) {
    inactivate();
  }
}

void Bullet::draw(GameLevel& context) const {
  // ToDo: async animation (if there are enough memories)
  const byte frame = context.frameCount() / 3 % 2;

  if(type % 2 == 0) {
    const byte* bitmaps[] = {bitmapMbullet0, bitmapMbullet1};
    context.core.drawBitmap(fieldX() - bitmapMbullet0[0]/2, fieldY() - bitmapMbullet0[1]/2, bitmaps[frame], 2);
  }
  else {
    const byte* bitmaps[] = {bitmapSbullet0, bitmapSbullet1};
    context.core.drawBitmap(fieldX() - bitmapSbullet0[0]/2, fieldY() - bitmapSbullet0[1]/2, bitmaps[frame], 2);
  }
  //arduboy.drawPixel(x, y, 0);
}

void Bullet::onHit(GameLevel& context) {
  context.core.drawCircle(fieldX(), fieldY(), 6, 1);
  inactivate();
}

// === Particle ===

void Particle::move(GameLevel& context) {
  --limit;
  if(limit <= 0) {
    inactivate();
  }
}

void Particle::draw(GameLevel& context) const {
  switch(type) {
    case 0: {
      if(limit > 8) {
        context.core.drawBitmap(x, y, bitmapExplosion0, 2);
      }
      else if(limit > 4) {
        context.core.drawBitmap(x, y, bitmapExplosion1, 2);
      }
      else {
        context.core.drawBitmap(x, y, bitmapExplosion2, 2);
      }
    } break;

    default: {
      context.core.drawBitmap(x, y, bitmapTen, 2);
    } break;
  }
}

<commit_msg>[fix] remove unused variable<commit_after>#include "actor.h"
#include "gamelevel.h"
#include "modules.h"
#include "util.h"
#include "assets.h"

// === Submarine ===

void Submarine::initialize() {
  x = W << 8;
  y = (SCREEN_HEIGHT / 2) << 8;
  extraLives = START_LIVES;
  armor      = ARMOR_FRAMES;
}

void Submarine::move(GameLevel& context) {
  if(context.isGameover()) { return; }

  // control
  static const int SPD = 1 << 7;
  static const int R2  = 90;  // 0.5/sqrt(2) / (1/256)
  if(context.core.pressed(BTN_U) && context.core.pressed(BTN_L)) {
    x -= R2;
    y -= R2;
  }
  else if(context.core.pressed(BTN_U) && context.core.pressed(BTN_R)) {
    x += R2;
    y -= R2;
  }
  else if(context.core.pressed(BTN_D) && context.core.pressed(BTN_L)) {
    x -= R2;
    y += R2;
  }
  else if(context.core.pressed(BTN_D) && context.core.pressed(BTN_R)) {
    x += R2;
    y += R2;
  }
  else if(context.core.pressed(BTN_U)) {
    y -= SPD;
  }
  else if(context.core.pressed(BTN_L)) {
    x -= SPD;
  }
  else if(context.core.pressed(BTN_R)) {
    x += SPD;
  }
  else if(context.core.pressed(BTN_D)) {
    y += SPD;
  }

  // clamping into field
  static const fixed MARGIN = 6;
  x = Clamp(x, 0, (SCREEN_WIDTH - MARGIN) << 8);
  y = Clamp(y, (MARGIN - H) << 8, (SCREEN_HEIGHT - MARGIN) << 8);

  // launching torpedo
  if(extraLives >= 0 && (context.core.pushed(BTN_A) || context.core.pushed(BTN_B))) {
    context.launchTorpedo(fieldX() + 10, fieldY() + 1);
  }

  //firing auto shot
#ifdef LOW_FLASH_MEMORY
  if(context.frameCount() % 5 == 0) {
    context.fireAutoShot(fieldX() + 3, fieldY() - 3);
  }
#else
  if(context.lookForEnemy() && context.frameCount() % 5 == 0) {
    context.fireAutoShot(fieldX() + 3, fieldY() - 3);
  }
#endif

  // updating armor timer
  if(armor > 0) { --armor; }
}

void Submarine::draw(GameLevel& context) const {
  if(armor == 0 || (extraLives >= 0 && context.frameCount() / 3 % 2 == 0)) {
    context.core.drawBitmap(fieldX() - 1, fieldY() - 2, bitmapSubmarine, 2);
    if(context.frameCount() % ECHO_CYCLE > 30) {  // sustain: 30 frames
      context.core.drawPixel(fieldX() + 6, fieldY() - 1, 1);
    }
  }
  //context.getArduboy().drawRect(x, y, W, H, 0);
}

void Submarine::onHit(GameLevel& context) {
  if(armor <= 0) {
    if(extraLives >= 0) {
      context.spawnParticle(fieldX() - 2, fieldY() - 4, PARTICLE_EXPLOSION);
      context.core.tone(185, 250);
    }
    --extraLives;
    if(extraLives < 0) {
      context.setGameover();
      // ToDo: add gameover sfx
      inactivate();
    }
    armor = ARMOR_FRAMES;
  }
}

// === Torpedo ===

bool Torpedo::launch(const char x, const char y) {
  if(!exist()) {
    this->x = x;
    this->y = y;
    vx = 0;
    return true;
  }
  return false;
}

void Torpedo::move() {
  static const fixed ACC = 1 << 7;  // 0.5
  if(!exist()) { return; }
  vx += ACC - (vx >> 4);
  x  += vx >> 8;
  if(x >= FIELD_WIDTH) {
    inactivate();
  }
}

void Torpedo::draw(GameLevel& context) const {
  if(!exist()) { return; }
  // body
  context.core.drawBitmap(x, y - 1, bitmapTorpedo, 2);
  // shockwave
  if(vx > 4 << 8) { // well accelerated
    const byte h = bitmapShockwave0[1];
    context.core.drawBitmap(x -  8, y - h/2, bitmapShockwave0, 2);
    context.core.drawBitmap(x - 16, y - h/2, bitmapShockwave1, 2);
    context.core.drawBitmap(x - 24, y - h/2, bitmapShockwave2, 2);
  }
  //context.getArduboy.drawFastHLine(x, y, W, 0);
}

// === AutoShot ===

void AutoShot::initialize(const char x, const char y) {
  this->x = x;
  this->y = y;
}

void AutoShot::move() {
  // moving
  x += 8;

  // frame out
  if(x > SCREEN_WIDTH) {
    inactivate();
  }
}

void AutoShot::draw(GameLevel& context) const {
  context.core.drawBitmap((int)x + 2, (int)y + 3, bitmapAutoShot, 2);
  //context.core.getArduboy().drawRect(x, y, W, H, 1);
}

// === BigEnemy ===

void BigEnemy::initialize(const char y) {
  this->x = FIELD_WIDTH;
  this->y = y;
  state   = 0;
  timer   = 0;
}

void BigEnemy::move(GameLevel& context) {
  // moving
  if(grazed() && x > SCREEN_WIDTH - 5) {
    x += -3; // submarine found
  }
  else if(timer % 5 == 0) { // -0.2
    --x;
  }

  // frame out
  if(x + bitmapCruEnemy0[0] < 0) {
    inactivate();
  }

  // setting sonar reaction
  context.echo.add(x, y, y+H);

  // firing bullet
  if(x < SCREEN_WIDTH - W / 2) {
    if(!onScreen()) {
      timer = 0;  // in order to sync fire cycle
      state |= ON_SCREEN_MASK;
    }
    const byte d = context.difficulty();
    if(timer == 0 || (d >= 60 && timer == 40)) {  // freq up when 60 and over
      const char bulletY     = y + bitmapCruEnemy0[1] / 2;
      const char bulletSpeed = (d >= 80) ? BULLET_TYPE2_SPD : BULLET_TYPE0_SPD;
      const char bulletType  = (d >= 80) ? 2 : 0;
      context.fireBullet(x, bulletY, context.getFutureSubmarineAngle(x, bulletY, bulletSpeed), bulletType);
    }
  }

  // updating timer
  static const byte PERIOD = 150;
  timer = (timer + 1) % PERIOD;
}

void BigEnemy::draw(GameLevel& context) const {
  if(x - 3 > SCREEN_WIDTH) { return; }
  context.core.drawBitmap(x - 3, y - 2, bitmapCruEnemy0, 2);
}

void BigEnemy::onHit(GameLevel& context) {
  // add score
  context.addScore(10);
  if(x > SCREEN_WIDTH) {  // far bonus
    context.addScore((x - SCREEN_WIDTH) * 10 / (FIELD_WIDTH - SCREEN_WIDTH));
  }
  // near
  if(x < SCREEN_WIDTH + 20) {
    context.core.tone(1047, 250);
    context.spawnParticle(x + random(-2,  8), y + random(-4, 4), PARTICLE_EXPLOSION);
    context.spawnParticle(x + random( 8, 18), y + random(-4, 4), PARTICLE_EXPLOSION);
#ifndef LOW_FLASH_MEMORY
    context.removeAllBullets();
    context.core.setQuake();
#endif
  }
  // reset
  inactivate();
}

// === SmallEnemy ===

void SmallEnemy::initialize(const char y, const byte type) {
  this->x    = FIELD_WIDTH;
  this->y    = y;
  this->type = type;
  timer      = 0;
}

void SmallEnemy::move(GameLevel& context) {
  const byte period = (getType() == 0) ? ZIG_PERIOD : TRI_PERIOD;

  switch(getType()) {
    // zigzag
    case 0: {
      // moving
      // x -= 1.5, y -= 0.5
      if(timer % 2 == 0) {
        x -= 2;
        y += (timer / (ZIG_PERIOD / 4) % 2 == 0) ? 1 : -1;
      }
      else {
        --x;
      }

      // firing bullet
      const byte d = context.difficulty();
      const byte typeCond = canFire() ||
        (d >= 70 && d < 80) || d >= 100;  // [70,80) or 100
      if(
        timer == 80 && typeCond &&    // time / type
        x < SCREEN_WIDTH - W / 2 &&   // position
        d >= 20                       // dont fire less than 20
      ) {
        const byte  bulletType = (d >= 80) ? 3 : 1;  // rapid when 80 and over
        const float subAngle   = context.getSubmarineAngle(x, y);
        context.fireBullet(x, y, subAngle, bulletType);
        // 3way
        if((d >= 50 && d < 70) || d >= 90) {
          context.fireBullet(x, y, subAngle - radians(10), 1);
          context.fireBullet(x, y, subAngle + radians(10), 1);
        }
      }
    } break;

    // triangle
    default: {
      // moving
      if(timer % 6 == 0) { --x; }
      const int half = period / 2;
      int tmp = half - timer % half;
      tmp = tmp*tmp / (half*half/4) - 1;
      if(tmp < 0) { tmp = 0; }
      x -= tmp;

      // firing bullet
      const bool timeCond = timer == 64 ||
        (context.difficulty() >= 25 && timer == 32) ||
        (context.difficulty() >= 75 && timer == 96);
      if(
        timeCond && canFire() &&                    // time / type
        x < SCREEN_WIDTH && x > SCREEN_WIDTH / 2 && // position
        context.difficulty() >= 20                  // difficulty
      ) {
        context.fireBullet(x, y, context.getSubmarineAngle(x, y), 1);
      }
    } break;
  }

  // frame out
  if(x + 12 < 0) {
    context.platoons.checkBonus(getPlatoon(), false);
    inactivate();
  }

  // setting sonar reaction
  context.echo.add(x, y, y+H);

  // updating timer
  timer = (timer + 1) % period;
}

void SmallEnemy::draw(GameLevel& context) const {
  if(x - 4 > SCREEN_WIDTH) { return; }
  switch(getType()) {
    // zigzag
    case 0: {
      const byte* bitmaps[] = {bitmapZigEnemy0, bitmapZigEnemy1};
      const byte frame = timer / (ZIG_PERIOD / 4) % 2;
      context.core.drawBitmap(x - 3, y - 3, bitmaps[frame], 2);
    } break;

    // triangle
    default: {
      const byte* bitmaps[] = {bitmapTriEnemy0, bitmapTriEnemy1};
      const byte frame = (timer % (TRI_PERIOD / 2) < 24) ? 1 : 0;
      context.core.drawBitmap(x - 3, y - 4, bitmaps[frame], 2);
      if(frame == 1) {
        context.core.drawBitmap(x + 7, y - 2, bitmapBoostEffect, 2);
      }
    } break;
  }
  //context.core.getArduboy().drawRect(x, y, W, H, 0);
}

void SmallEnemy::onHit(GameLevel& context) {
  context.addScore(1);
  if(x < SCREEN_WIDTH + 20) { // near
    context.spawnParticle(round(x) - 5, round(y) - 4, PARTICLE_EXPLOSION);
    context.core.tone(880, 62);
  }
  // platoon elimination
  if(context.platoons.checkBonus(getPlatoon(), true)) {
    if(x < SCREEN_WIDTH + 20) { // near
      context.spawnParticle(round(x) - 2, round(y) - 2, PARTICLE_TEN_POINT);
    }
    context.addScore(10);
  }
  inactivate();
}

// === Bullet ===

void Bullet::initialize(const char x, const char y, const float radian, const byte type) {
  this->x    = x << 8;
  this->y    = y << 8;
  this->type = type;

  const byte SPEEDS[] = {BULLET_TYPE0_SPD, BULLET_TYPE1_SPD, BULLET_TYPE2_SPD, BULLET_TYPE3_SPD};
  byte speed = SPEEDS[type];
  vx = round(cos(radian) * speed / (1.f / 256));
  vy = round(sin(radian) * speed / (1.f / 256));
}

void Bullet::move(GameLevel& context) {
  x += vx;
  y += vy;

  // frame out
  static const char MARGIN = 4;
  if(
    fieldX() < -MARGIN || fieldX() > SCREEN_WIDTH  + MARGIN ||
    fieldY() < -MARGIN || fieldY() > SCREEN_HEIGHT + MARGIN
  ) {
    inactivate();
  }
}

void Bullet::draw(GameLevel& context) const {
  // ToDo: async animation (if there are enough memories)
  const byte frame = context.frameCount() / 3 % 2;

  if(type % 2 == 0) {
    const byte* bitmaps[] = {bitmapMbullet0, bitmapMbullet1};
    context.core.drawBitmap(fieldX() - bitmapMbullet0[0]/2, fieldY() - bitmapMbullet0[1]/2, bitmaps[frame], 2);
  }
  else {
    const byte* bitmaps[] = {bitmapSbullet0, bitmapSbullet1};
    context.core.drawBitmap(fieldX() - bitmapSbullet0[0]/2, fieldY() - bitmapSbullet0[1]/2, bitmaps[frame], 2);
  }
  //arduboy.drawPixel(x, y, 0);
}

void Bullet::onHit(GameLevel& context) {
  context.core.drawCircle(fieldX(), fieldY(), 6, 1);
  inactivate();
}

// === Particle ===

void Particle::move(GameLevel& context) {
  --limit;
  if(limit <= 0) {
    inactivate();
  }
}

void Particle::draw(GameLevel& context) const {
  switch(type) {
    case 0: {
      if(limit > 8) {
        context.core.drawBitmap(x, y, bitmapExplosion0, 2);
      }
      else if(limit > 4) {
        context.core.drawBitmap(x, y, bitmapExplosion1, 2);
      }
      else {
        context.core.drawBitmap(x, y, bitmapExplosion2, 2);
      }
    } break;

    default: {
      context.core.drawBitmap(x, y, bitmapTen, 2);
    } break;
  }
}

<|endoftext|>


<commit_before>//
// Created by FyS on 20/04/17.
//

#define BOOST_TEST_MODULE test_bus

#include <boost/test/unit_test.hpp>
#include <FysBus.hh>

//BOOST_AUTO_TEST_CASE( test_bus_ini )
//{
//    fys::mq::FysBus<std::string, 100> bus("/home/FyS/ClionProjects/FySMQ/ressource/test_bus_inifile.ini");
//}
//
//BOOST_AUTO_TEST_CASE_EXPECTED_FAILURES( test_bus_ini_error1, 1 )
//BOOST_AUTO_TEST_CASE( test_bus_ini_error1 )
//{
//    fys::mq::FysBus<std::string, 100> bus("/home/FyS/ClionProjects/FySMQ/ressource/test_bus_inifile_error1.ini");
//}
//
//BOOST_AUTO_TEST_CASE_EXPECTED_FAILURES( test_bus_ini_error2, 1 )
//BOOST_AUTO_TEST_CASE( test_bus_ini_error2 )
//{
//    fys::mq::FysBus<std::string, 100> bus("/home/FyS/ClionProjects/FySMQ/ressource/test_bus_inifile_error2.ini");
//}
<commit_msg>[Gateway][MQ] : commit files for MQ<commit_after>//
// Created by FyS on 16/06/17.
//

#define BOOST_TEST_MODULE test_bus

#include <boost/test/unit_test.hpp>
#include <boost/thread.hpp>
#include "../../Server/test/FysBusTest.hh"

/**
 * ------------------------------------------------------
 *  Bus Initialisation test------------------------------
 * ------------------------------------------------------
 */

BOOST_AUTO_TEST_CASE( test_bus_ini )
{
    fys::mq::test::FysBusTest<std::string, 100> bus("/home/FyS/ClionProjects/FreeYourSoul_Server/FySMQ/resource/test_bus_inifile.ini");
    bus.validateBusConfiguration(2);
}

BOOST_AUTO_TEST_CASE_EXPECTED_FAILURES( test_bus_ini_error1, 2 )
BOOST_AUTO_TEST_CASE( test_bus_ini_error1 )
{
    fys::mq::test::FysBusTest<std::string, 100> bus("/home/FyS/ClionProjects/FreeYourSoul_Server/FySMQ/resource/test_bus_inifile_error1.ini");
    bus.validateBusConfiguration(2);
}

BOOST_AUTO_TEST_CASE_EXPECTED_FAILURES( test_bus_ini_error2, 2 )
BOOST_AUTO_TEST_CASE( test_bus_ini_error2 )
{
    fys::mq::test::FysBusTest<std::string, 100> bus("/home/FyS/ClionProjects/FreeYourSoul_Server/FySMQ/resource/test_bus_inifile_error2e.ini");
    bus.validateBusConfiguration(3);
}

/**
 *------------------------------------------------------
 * Bus exections test ----------------------------------
 *------------------------------------------------------
 */
fys::mq::LockFreeQueue<fys::mq::QueueContainer<std::string>, 5000> *lockFreeQueue;
int readValues;

void addinlockfreequeue(const fys::mq::QueueContainer<std::string> &container) {

    for (int i = 0; i < 1500; ++i) {
        lockFreeQueue->push(container);
    }
}

void readLockFreeQueue() {
    for (int i = 0; i < 5000; ++i) {
        fys::mq::QueueContainer<std::string> *container;

        container = lockFreeQueue->pop();
        usleep(100);
        if (container != NULL)
            ++readValues;
    }
}

void initTestExecution() {
    lockFreeQueue = new fys::mq::LockFreeQueue<fys::mq::QueueContainer<std::string>, 5000>();
    lockFreeQueue->setLockingWhenEmpty(false);
    readValues = 0;
}

void cleanTestExecution() {
    if (lockFreeQueue != nullptr) {
        delete(lockFreeQueue);
        lockFreeQueue = nullptr;
    }
}

BOOST_AUTO_TEST_CASE( test_queue_exec ) {
    fys::mq::QueueContainer<std::string> c1("A");
    fys::mq::QueueContainer<std::string> c2("B");
    fys::mq::QueueContainer<std::string> c3("C");

    std::cout << "begin test exec" << std::endl;
    initTestExecution();
    boost::thread workerRead(readLockFreeQueue);
    boost::thread w1(boost::bind(addinlockfreequeue, c1));
    boost::thread w2(boost::bind(addinlockfreequeue, c2));
    boost::thread w3(boost::bind(addinlockfreequeue, c3));

    w1.join();
    w2.join();
    w3.join();
    workerRead.join();
    cleanTestExecution();
    std::cout << "end test exec : readValue = " << readValues << std::endl;
    BOOST_CHECK(readValues == 4500);
}<|endoftext|>


<commit_before>#include "stdafx.h"
#include "cartrige.h"

struct rom_header
{
	u8 start_vector[4];
	u8 nintendo_logo[48];
	u8 game_title[11];
	u8 manufacturer_code[4];
	u8 cgb_flag;
	u8 new_license_code[2];
	u8 sgb_flag;
	u8 cartrige_type;
	u8 rom_size;
	u8 ram_size;
	u8 destination_code;
	u8 old_license_code;
	u8 rom_version;
	u8 checksum;
	u8 global_checksum[2];
};

bool in_range(u32 value, u32 begin, u32 end)
{
	return (value >= begin) && (value <= end);
}

u32 get_ram_size(u8 val)
{
	assert(val < 6);

	static const u32 sizes[] = { 0, 0x800, 0x2000, 0x8000, 0x20000, 0x10000 };

	return sizes[val];
}

Cartrige::~Cartrige()
{
	const rom_header* header = reinterpret_cast<rom_header*>(&rom[0x100]);

	if (battery_ram)
	{
		std::ofstream ram_file(file_name + "_ram", std::ios::trunc | std::ios::binary);
		ram_file.write(reinterpret_cast<char*>(ram.get()), ram_size);
	}

	if (in_range(header->cartrige_type, 0x0F, 0x10))
	{
		std::ofstream rtc_file(file_name + "_rtc", std::ios::trunc);

		memory_interface.reset(); //make sure that MBC3 update rtc_regs

		auto epoch = std::chrono::system_clock::now().time_since_epoch();
		auto cur_timestamp = std::chrono::duration_cast<std::chrono::seconds>(epoch);

		rtc_file << cur_timestamp.count();
		rtc_file.write(reinterpret_cast<char*>(rtc_regs), sizeof(u8) * 5);
	}
}

void Cartrige::load_ram()
{
	rom_header* header = reinterpret_cast<rom_header*>(&rom[0x100]);

	//MBC2 has always header->ram_size == 0, but it has 512 bytes actually!
	if (in_range(header->cartrige_type, 0x05, 0x06))
		ram_size = 512;

	else
		ram_size = get_ram_size(header->ram_size);

	ram = ram_size ? std::make_unique<u8[]>(ram_size) : nullptr;

	switch (header->cartrige_type)
	{
		case 0x03:
		case 0x06:
		case 0x09:
		case 0x0D:
		case 0x0F:
		case 0x10:
		case 0x13:
		case 0x1B:
		case 0x1E:
			battery_ram = true;
			break;

		default:
			battery_ram = false;
			break;
	}

	if (battery_ram)
	{
		std::ifstream ram_file(file_name + "_ram", std::ios::binary);

		if (ram_file.is_open())
			ram_file.read(reinterpret_cast<char*>(ram.get()), ram_size);
	}

	if (in_range(header->cartrige_type, 0x0F, 0x10))
	{
		std::ifstream rtc_file(file_name + "_rtc", std::ios::binary);

		if (rtc_file.is_open())
		{
			i64 saved_timestamp;
			rtc_file >> saved_timestamp;
			rtc_file.read(reinterpret_cast<char*>(rtc_regs), sizeof(u8) * 5);

			auto epoch = std::chrono::system_clock::now().time_since_epoch();
			auto cur_timestamp = std::chrono::duration_cast<std::chrono::seconds>(epoch);
			auto delta = cur_timestamp.count() - saved_timestamp;

			if (delta <= 0 || !check_bit(rtc_regs[4], 6))
				return;

			auto ns = rtc_regs[0] + delta;
			auto nm = rtc_regs[1] + ns / 60;
			auto nh = rtc_regs[2] + nm / 60;
			auto nd = (((rtc_regs[4] & 1) << 8) | rtc_regs[3]) + nh / 24;

			rtc_regs[0] = ns % 60;
			rtc_regs[1] = nm % 60;
			rtc_regs[2] = nh % 24;
			rtc_regs[3] = (nd % 512) & 0xFF;
			rtc_regs[4] = change_bit(rtc_regs[4], (nd % 512) > 255, 0);
			rtc_regs[4] = change_bit(rtc_regs[4], nd > 511, 7);
		}
	}
}

bool Cartrige::load_cartrige(const std::string& name)
{
	std::ifstream cart_file(name, std::ios::binary | std::ios::ate);

	if (!cart_file.is_open())
		return false;

	size_t size = cart_file.tellg();
	cart_file.seekg(0, std::ios_base::beg);

	rom = std::make_unique<u8[]>(size);
	cart_file.read(reinterpret_cast<char*>(rom.get()), size);

	file_name = name;
	load_ram();
	dispatch();

	return true;
}

IMemory* Cartrige::get_memory_controller() const
{
	return memory_interface.get();
}

IDmaMemory* Cartrige::get_dma_controller() const
{
	return dma_interface;
}

bool Cartrige::is_cgb_ready() const
{
	const rom_header* header = reinterpret_cast<rom_header*>(&rom[0x100]);

	return (header->cgb_flag == 0x80) || (header->cgb_flag == 0xC0);
}

void Cartrige::serialize(std::ostream& stream)
{
	//TODO: update rtc regs before serialization
	stream << battery_ram << ram_size;
	stream.write(reinterpret_cast<char*>(ram.get()), sizeof(u8) * ram_size);
	stream.write(reinterpret_cast<char*>(rtc_regs), sizeof(u8) * 5);
}

void Cartrige::deserialize(std::istream & stream)
{
	stream >> battery_ram >> ram_size;
	stream.read(reinterpret_cast<char*>(ram.get()), sizeof(u8) * ram_size);
	stream.read(reinterpret_cast<char*>(rtc_regs), sizeof(u8) * 5);
}

void Cartrige::dispatch()
{
	rom_header* header = reinterpret_cast<rom_header*>(&rom[0x100]);
	u8 type = header->cartrige_type;

	if (type == 0x00 || type == 0x08 || type == 0x09)
	{
		auto tmp = std::make_unique<NoMBC>(NoMBC(rom.get(), ram.get()));
		dma_interface = tmp.get();
		memory_interface = std::move(tmp);
	}

	else if (in_range(type, 0x01, 0x03))
	{
		auto tmp = std::make_unique<MBC1>(MBC1(rom.get(), ram.get()));
		dma_interface = tmp.get();
		memory_interface = std::move(tmp);
	}

	else if (in_range(type, 0x05, 0x06))
	{
		auto tmp = std::make_unique<MBC2>(MBC2(rom.get(), ram.get()));
		dma_interface = tmp.get();
		memory_interface = std::move(tmp);
	}

	else if (in_range(type, 0x0F, 0x13))
	{
		auto tmp = std::make_unique<MBC3>(MBC3(rom.get(), ram.get(), (type <= 0x10 ? rtc_regs : nullptr)));
		dma_interface = tmp.get();
		memory_interface = std::move(tmp);
	}

	else if (in_range(type, 0x1A, 0x1E))
	{
		auto tmp = std::make_unique<MBC5>(MBC5(rom.get(), ram.get()));
		dma_interface = tmp.get();
		memory_interface = std::move(tmp);
	}

	else
		memory_interface = nullptr;
}
<commit_msg>fixed small bug<commit_after>#include "stdafx.h"
#include "cartrige.h"

struct rom_header
{
	u8 start_vector[4];
	u8 nintendo_logo[48];
	u8 game_title[11];
	u8 manufacturer_code[4];
	u8 cgb_flag;
	u8 new_license_code[2];
	u8 sgb_flag;
	u8 cartrige_type;
	u8 rom_size;
	u8 ram_size;
	u8 destination_code;
	u8 old_license_code;
	u8 rom_version;
	u8 checksum;
	u8 global_checksum[2];
};

bool in_range(u32 value, u32 begin, u32 end)
{
	return (value >= begin) && (value <= end);
}

u32 get_ram_size(u8 val)
{
	assert(val < 6);

	static const u32 sizes[] = { 0, 0x800, 0x2000, 0x8000, 0x20000, 0x10000 };

	return sizes[val];
}

Cartrige::~Cartrige()
{
	const rom_header* header = reinterpret_cast<rom_header*>(&rom[0x100]);

	if (battery_ram)
	{
		std::ofstream ram_file(file_name + "_ram", std::ios::trunc | std::ios::binary);
		ram_file.write(reinterpret_cast<char*>(ram.get()), ram_size);
	}

	if (in_range(header->cartrige_type, 0x0F, 0x10))
	{
		std::ofstream rtc_file(file_name + "_rtc", std::ios::trunc);

		memory_interface.reset(); //make sure that MBC3 update rtc_regs

		auto epoch = std::chrono::system_clock::now().time_since_epoch();
		auto cur_timestamp = std::chrono::duration_cast<std::chrono::seconds>(epoch);

		rtc_file << cur_timestamp.count();
		rtc_file.write(reinterpret_cast<char*>(rtc_regs), sizeof(u8) * 5);
	}
}

void Cartrige::load_ram()
{
	rom_header* header = reinterpret_cast<rom_header*>(&rom[0x100]);

	//MBC2 has always header->ram_size == 0, but it has 512 bytes actually!
	if (in_range(header->cartrige_type, 0x05, 0x06))
		ram_size = 512;

	else
		ram_size = get_ram_size(header->ram_size);

	ram = ram_size ? std::make_unique<u8[]>(ram_size) : nullptr;

	switch (header->cartrige_type)
	{
		case 0x03:
		case 0x06:
		case 0x09:
		case 0x0D:
		case 0x0F:
		case 0x10:
		case 0x13:
		case 0x1B:
		case 0x1E:
			battery_ram = true;
			break;

		default:
			battery_ram = false;
			break;
	}

	if (battery_ram)
	{
		std::ifstream ram_file(file_name + "_ram", std::ios::binary);

		if (ram_file.is_open())
			ram_file.read(reinterpret_cast<char*>(ram.get()), ram_size);
	}

	if (in_range(header->cartrige_type, 0x0F, 0x10))
	{
		std::ifstream rtc_file(file_name + "_rtc", std::ios::binary);

		if (rtc_file.is_open())
		{
			i64 saved_timestamp;
			rtc_file >> saved_timestamp;
			rtc_file.read(reinterpret_cast<char*>(rtc_regs), sizeof(u8) * 5);

			auto epoch = std::chrono::system_clock::now().time_since_epoch();
			auto cur_timestamp = std::chrono::duration_cast<std::chrono::seconds>(epoch);
			auto delta = cur_timestamp.count() - saved_timestamp;

			if (delta <= 0 || check_bit(rtc_regs[4], 6))
				return;

			auto ns = rtc_regs[0] + delta;
			auto nm = rtc_regs[1] + ns / 60;
			auto nh = rtc_regs[2] + nm / 60;
			auto nd = (((rtc_regs[4] & 1) << 8) | rtc_regs[3]) + nh / 24;

			rtc_regs[0] = ns % 60;
			rtc_regs[1] = nm % 60;
			rtc_regs[2] = nh % 24;
			rtc_regs[3] = (nd % 512) & 0xFF;
			rtc_regs[4] = change_bit(rtc_regs[4], (nd % 512) > 255, 0);
			rtc_regs[4] = change_bit(rtc_regs[4], nd > 511, 7);
		}
	}
}

bool Cartrige::load_cartrige(const std::string& name)
{
	std::ifstream cart_file(name, std::ios::binary | std::ios::ate);

	if (!cart_file.is_open())
		return false;

	size_t size = cart_file.tellg();
	cart_file.seekg(0, std::ios_base::beg);

	rom = std::make_unique<u8[]>(size);
	cart_file.read(reinterpret_cast<char*>(rom.get()), size);

	file_name = name;
	load_ram();
	dispatch();

	return true;
}

IMemory* Cartrige::get_memory_controller() const
{
	return memory_interface.get();
}

IDmaMemory* Cartrige::get_dma_controller() const
{
	return dma_interface;
}

bool Cartrige::is_cgb_ready() const
{
	const rom_header* header = reinterpret_cast<rom_header*>(&rom[0x100]);

	return (header->cgb_flag == 0x80) || (header->cgb_flag == 0xC0);
}

void Cartrige::serialize(std::ostream& stream)
{
	//TODO: update rtc regs before serialization
	stream << battery_ram << ram_size;
	stream.write(reinterpret_cast<char*>(ram.get()), sizeof(u8) * ram_size);
	stream.write(reinterpret_cast<char*>(rtc_regs), sizeof(u8) * 5);
}

void Cartrige::deserialize(std::istream & stream)
{
	stream >> battery_ram >> ram_size;
	stream.read(reinterpret_cast<char*>(ram.get()), sizeof(u8) * ram_size);
	stream.read(reinterpret_cast<char*>(rtc_regs), sizeof(u8) * 5);
}

void Cartrige::dispatch()
{
	rom_header* header = reinterpret_cast<rom_header*>(&rom[0x100]);
	u8 type = header->cartrige_type;

	if (type == 0x00 || type == 0x08 || type == 0x09)
	{
		auto tmp = std::make_unique<NoMBC>(NoMBC(rom.get(), ram.get()));
		dma_interface = tmp.get();
		memory_interface = std::move(tmp);
	}

	else if (in_range(type, 0x01, 0x03))
	{
		auto tmp = std::make_unique<MBC1>(MBC1(rom.get(), ram.get()));
		dma_interface = tmp.get();
		memory_interface = std::move(tmp);
	}

	else if (in_range(type, 0x05, 0x06))
	{
		auto tmp = std::make_unique<MBC2>(MBC2(rom.get(), ram.get()));
		dma_interface = tmp.get();
		memory_interface = std::move(tmp);
	}

	else if (in_range(type, 0x0F, 0x13))
	{
		auto tmp = std::make_unique<MBC3>(MBC3(rom.get(), ram.get(), (type <= 0x10 ? rtc_regs : nullptr)));
		dma_interface = tmp.get();
		memory_interface = std::move(tmp);
	}

	else if (in_range(type, 0x1A, 0x1E))
	{
		auto tmp = std::make_unique<MBC5>(MBC5(rom.get(), ram.get()));
		dma_interface = tmp.get();
		memory_interface = std::move(tmp);
	}

	else
		memory_interface = nullptr;
}
<|endoftext|>


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

#include "process_name.hpp"

#include <assert.h>

#if defined(__linux__)
#include <features.h>  // __GLIBC__
#endif

#include <iostream>

#if defined(__GLIBC__)

#include <dlfcn.h>
#include <string.h>

// TODO: Intercept readlink("/proc/self/exe")

void
setProcessName(const char *processName)
{
    char **p__progname_full = (char **)dlsym(RTLD_DEFAULT, "__progname_full");
    if (p__progname_full == nullptr) {
        std::cerr << "error: failed to get address of __progname_full symbol" << std::endl;
        return;
    }

    char *progname_full = strdup(processName);
    assert(progname_full != nullptr);

    *p__progname_full = progname_full;

    char **p__progname = (char **)dlsym(RTLD_DEFAULT, "__progname");
    if (p__progname == nullptr) {
        std::cerr << "error: failed to get address of __progname symbol" << std::endl;
        return;
    }

    char *progname = strrchr(progname_full, '/');
    if (progname == nullptr) {
        progname = progname_full;
    } else {
        progname += 1;
    }

    *p__progname = progname;
}

#elif defined(_WIN32)

#include <windows.h>
#include "mhook.h"

#ifdef _MSC_VER
#define ReturnAddress() _ReturnAddress()
#else
#define ReturnAddress() __builtin_return_address(0)
#endif

typedef DWORD (WINAPI *PFNGETMODULEFILENAMEA)(HMODULE hModule, LPSTR lpFilename, DWORD nSize);
static PFNGETMODULEFILENAMEA pfnGetModuleFileNameA = &GetModuleFileNameA;

static inline HMODULE
GetModuleFromAddress(PVOID pAddress)
{
    HMODULE hModule = nullptr;
    BOOL bRet = GetModuleHandleEx(GET_MODULE_HANDLE_EX_FLAG_FROM_ADDRESS |
                                  GET_MODULE_HANDLE_EX_FLAG_UNCHANGED_REFCOUNT,
                                  (LPCTSTR)pAddress,
                                  &hModule);
    return bRet ? hModule : nullptr;
}

static std::string g_processName;

static DWORD WINAPI
MyGetModuleFileNameA(HMODULE hModule, LPSTR lpFilename, DWORD nSize)
{
    if (hModule == nullptr) {
#ifndef NDEBUG
        void *pCallerAddr = ReturnAddress();
        HMODULE hCallerModule = GetModuleFromAddress(pCallerAddr);
        char szCaller[MAX_PATH];
        DWORD dwRet = pfnGetModuleFileNameA(hCallerModule, szCaller, sizeof szCaller);
        assert(dwRet > 0);

        std::cerr << "GetModuleFileNameA(" << hModule << ") from " << szCaller << "\n";
#endif

        assert(!g_processName.empty());
        assert(nSize != 0);

        size_t len = g_processName.length();
        if (len < nSize) {
            memcpy(lpFilename, g_processName.data(), len);
            lpFilename[len] = 0;
            SetLastError(ERROR_SUCCESS);
            return len;
        } else {
            memcpy(lpFilename, g_processName.data(), nSize - 1);
            lpFilename[nSize - 1] = 0;
            SetLastError(ERROR_INSUFFICIENT_BUFFER);
            return nSize;
        }
    }

    return pfnGetModuleFileNameA(hModule, lpFilename, nSize);
}

void
setProcessName(const char *processName)
{
    g_processName = processName;

    static BOOL bHooked = FALSE;
    if (!bHooked) {
        bHooked = TRUE;

        LPVOID lpOrigAddress = (LPVOID)GetProcAddress(GetModuleHandleA("kernel32"), "GetModuleFileNameA");
        if (lpOrigAddress) {
            LPVOID lpHookAddress = (LPVOID)&MyGetModuleFileNameA;
            LPVOID lpRealAddress = lpOrigAddress;
            if (!Mhook_SetHook(&lpRealAddress, lpHookAddress)) {
                std::cerr << "error: failed to GetModuleFileNameA\n";
            }
            pfnGetModuleFileNameA = (PFNGETMODULEFILENAMEA)lpRealAddress;
        }
    }
}

#else

void
setProcessName(const char *processName)
{
}

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

#include "process_name.hpp"

#include <assert.h>

#if defined(__linux__)
#include <features.h>  // __GLIBC__
#endif

#include <iostream>

#include "os.hpp"
#include "os_symbols.hpp"

#if defined(__GLIBC__)

#include <dlfcn.h>
#include <string.h>
#include <unistd.h>

static std::string g_processName;

extern "C"  {

PUBLIC ssize_t readlink(const char *pathname, char *buf, size_t bufsiz)
{
    unsigned long pid = 0;
    if (strcmp(pathname, "/proc/self/exe") == 0 ||
        (sscanf(pathname, "/proc/%lu/exe", &pid) == 1 &&
         pid == getpid())) {
#ifndef _NDEBUG
        std::cerr << "readlink(" << pathname << ") from " << getModuleFromAddress(ReturnAddress()) << "\n";
#endif
        if (!g_processName.empty()) {
            size_t len = g_processName.length();
            if (len < bufsiz) {
                memcpy(buf, g_processName.data(), len);
                buf[len] = 0;
                return len;
            } else {
                memcpy(buf, g_processName.data(), bufsiz);
                return bufsiz;
            }
        } else {
            // FIXME: We need to defer retrace::setUp() until after the first trace is loaded.
            std::cerr << "warning: process name not yet set\n";
            return -1;
        }
    }

    typedef ssize_t (*p_readlink)(const char *pathname, char *buf, size_t bufsiz);
    p_readlink p = (p_readlink)dlsym(RTLD_NEXT, "readlink");
    assert(p != nullptr);
    return p(pathname, buf, bufsiz);
}

} /* extern C */

void
setProcessName(const char *processName)
{
    g_processName = processName;

    char **p__progname_full = (char **)dlsym(RTLD_DEFAULT, "__progname_full");
    if (p__progname_full == nullptr) {
        std::cerr << "error: failed to get address of __progname_full symbol" << std::endl;
        return;
    }

    char *progname_full = strdup(processName);
    assert(progname_full != nullptr);

    *p__progname_full = progname_full;

    char **p__progname = (char **)dlsym(RTLD_DEFAULT, "__progname");
    if (p__progname == nullptr) {
        std::cerr << "error: failed to get address of __progname symbol" << std::endl;
        return;
    }

    char *progname = strrchr(progname_full, '/');
    if (progname == nullptr) {
        progname = progname_full;
    } else {
        progname += 1;
    }

    *p__progname = progname;
}

#elif defined(_WIN32)

#include <windows.h>
#include "mhook.h"

#ifdef _MSC_VER
#define ReturnAddress() _ReturnAddress()
#else
#define ReturnAddress() __builtin_return_address(0)
#endif

typedef DWORD (WINAPI *PFNGETMODULEFILENAMEA)(HMODULE hModule, LPSTR lpFilename, DWORD nSize);
static PFNGETMODULEFILENAMEA pfnGetModuleFileNameA = &GetModuleFileNameA;

static inline HMODULE
GetModuleFromAddress(PVOID pAddress)
{
    HMODULE hModule = nullptr;
    BOOL bRet = GetModuleHandleEx(GET_MODULE_HANDLE_EX_FLAG_FROM_ADDRESS |
                                  GET_MODULE_HANDLE_EX_FLAG_UNCHANGED_REFCOUNT,
                                  (LPCTSTR)pAddress,
                                  &hModule);
    return bRet ? hModule : nullptr;
}

static std::string g_processName;

static DWORD WINAPI
MyGetModuleFileNameA(HMODULE hModule, LPSTR lpFilename, DWORD nSize)
{
    if (hModule == nullptr) {
#ifndef NDEBUG
        void *pCallerAddr = ReturnAddress();
        HMODULE hCallerModule = GetModuleFromAddress(pCallerAddr);
        char szCaller[MAX_PATH];
        DWORD dwRet = pfnGetModuleFileNameA(hCallerModule, szCaller, sizeof szCaller);
        assert(dwRet > 0);

        std::cerr << "GetModuleFileNameA(" << hModule << ") from " << szCaller << "\n";
#endif

        assert(!g_processName.empty());
        assert(nSize != 0);

        size_t len = g_processName.length();
        if (len < nSize) {
            memcpy(lpFilename, g_processName.data(), len);
            lpFilename[len] = 0;
            SetLastError(ERROR_SUCCESS);
            return len;
        } else {
            memcpy(lpFilename, g_processName.data(), nSize - 1);
            lpFilename[nSize - 1] = 0;
            SetLastError(ERROR_INSUFFICIENT_BUFFER);
            return nSize;
        }
    }

    return pfnGetModuleFileNameA(hModule, lpFilename, nSize);
}

void
setProcessName(const char *processName)
{
    g_processName = processName;

    static BOOL bHooked = FALSE;
    if (!bHooked) {
        bHooked = TRUE;

        LPVOID lpOrigAddress = (LPVOID)GetProcAddress(GetModuleHandleA("kernel32"), "GetModuleFileNameA");
        if (lpOrigAddress) {
            LPVOID lpHookAddress = (LPVOID)&MyGetModuleFileNameA;
            LPVOID lpRealAddress = lpOrigAddress;
            if (!Mhook_SetHook(&lpRealAddress, lpHookAddress)) {
                std::cerr << "error: failed to GetModuleFileNameA\n";
            }
            pfnGetModuleFileNameA = (PFNGETMODULEFILENAMEA)lpRealAddress;
        }
    }
}

#else

void
setProcessName(const char *processName)
{
}

#endif
<|endoftext|>


<commit_before>#include<cstdio>
#include<cstdlib>
#include<cstring>
#include<utility>
using namespace std;
#define MS(m,z) memset(m,z,sizeof(m))
typedef unsigned U; typedef pair<int,int > P; typedef long long ll;
void inp();
int main(){
#ifdef XS
	//freopen(".in","r",stdin);
#endif
	//int ttt;scanf("%d",&ttt);for(int i=1;i<=ttt;i++)
	inp();
	return 0;
}
void inp(){
}
<commit_msg>update<commit_after>#include<cstdio>
#include<cstdlib>
#include<cstring>
#include<utility>
using namespace std;
#define MS(m,z) memset(m,z,sizeof(m))
typedef unsigned U;typedef long long ll;typedef pair<int,int > P;
void inp();
int main(){
#ifdef XS
	//freopen(".in","r",stdin);
#endif
	//int ttt;scanf("%d",&ttt);for(int i=1;i<=ttt;i++)
	inp();
	return 0;
}
void inp(){
}
<|endoftext|>


<commit_before>#include <iostream>

#define SEQAN_DEBUG
#define SEQAN_TEST

#include <seqan/basic/basic_testing.h>
#include <seqan/index.h>
#include <seqan/seeds.h>

#include <demos/swift_local.h>

using namespace seqan;

// calls local swift and compares swift hits to expected swift hits
template<typename THaystack, typename TIndex>
void testLocalSwift(Finder<THaystack, Swift<SwiftLocal> > & finder,
					Pattern<TIndex, Swift<SwiftLocal> > & pattern,
					double epsilon,
					int minLength,
                    String<String<char> > & expectedPositions) {
	typedef typename Position<String<String<char> > >::Type TPosition;
	TPosition i = 0;
	while (find(finder, pattern, epsilon, minLength)) {
        // write swift hit into string and compare to expected output
        std::ostringstream pos;
		pos << "< " << positionRange(finder) << " , " << positionRange(pattern) << " >";
		SEQAN_ASSERT_EQ(pos.str().c_str(), value(expectedPositions,i));
		++i;
	}
}

void testOneLocalSwiftHit() {
	// a single pattern and a single hit
    typedef Finder<DnaString, Swift<SwiftLocal> > TFinder;
	DnaString text = "aaaaaaacgatcgatgcaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa";
	TFinder finder_swift(text);

    typedef Index<DnaString, Index_QGram<UngappedShape<4> > > TQGramIndexSimple;
	TQGramIndexSimple index_4gram("tttcgatcgatgctttttttttttttttttttttttttttttt");
    Pattern<TQGramIndexSimple, Swift<SwiftLocal> > pattern_swift(index_4gram);

    String<String<char> > expectedPositions;
    appendValue(expectedPositions, "< < 7 , 17 > , < 0 , 17 > >");

	testLocalSwift(finder_swift, pattern_swift, 0.1, 6, expectedPositions);
}

void testOneLocalSwiftHit2() {
    // a single pattern and a single hit 2
    typedef Finder<DnaString, Swift<SwiftLocal> > TFinder;
	DnaString text = "aaaaagagaccccccagagaaaaaa";
	TFinder finder_swift(text);

    typedef Index<DnaString, Index_QGram<UngappedShape<4> > > TQGramIndexSimple;
	TQGramIndexSimple index_4gram("tttttggccccccggtttttt");
    Pattern<TQGramIndexSimple, Swift<SwiftLocal> > pattern_swift(index_4gram);

    String<String<char> > expectedPositions;
    appendValue(expectedPositions, "< < 9 , 15 > , < 0 , 15 > >");

	testLocalSwift(finder_swift, pattern_swift, 0.1, 6, expectedPositions);
}

void testOneLocalSwiftHitStringSet() {
	// a single hit, pattern in a StringSet
    typedef Finder<DnaString, Swift<SwiftLocal> > TFinder;
	DnaString text = "aaaaaaacgatcgatgcaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa";
	TFinder finder_swift(text);

	typedef Index<StringSet<DnaString>, Index_QGram< UngappedShape<4> > > TQGramIndex;
	TQGramIndex index_4gram;
	appendValue(indexText(index_4gram), "tttcgatcgatgctttttttttttttttttttttttttttttt");
    Pattern<TQGramIndex, Swift<SwiftLocal> > pattern_swift(index_4gram);

    String<String<char> > expectedPositions;
    appendValue(expectedPositions, "< < 7 , 17 > , < < 0 , 0 > , < 0 , 17 > > >");

	testLocalSwift(finder_swift, pattern_swift, 0.1, 6, expectedPositions);
}

void testOneLocalSwiftHitBucketBorder() {
	// hit at bucket border
    typedef Finder<DnaString, Swift<SwiftLocal> > TFinder;
	DnaString text = "aaacgatcgatgcaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa";
	TFinder finder_swift(text);

	typedef Index<StringSet<DnaString>, Index_QGram< UngappedShape<4> > > TQGramIndex;
	TQGramIndex index_4gram;
	appendValue(indexText(index_4gram), "tttcgatcgatgctttttttttttttttttttttttttttttt");
    Pattern<TQGramIndex, Swift<SwiftLocal> > pattern_swift(index_4gram);

    String<String<char> > expectedPositions;
    appendValue(expectedPositions, "< < 3 , 13 > , < < 0 , 0 > , < 0 , 13 > > >");
    appendValue(expectedPositions, "< < 3 , 13 > , < < 0 , 2 > , < 0 , 29 > > >");

	testLocalSwift(finder_swift, pattern_swift, 0.1, 6, expectedPositions);
}

void testOneLocalSwiftHitNegDiag() {

	// hit at lower haystack position -> diagonal start position negative
    typedef Finder<DnaString, Swift<SwiftLocal> > TFinder;
	DnaString text = "aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaacgatcgatgc";
	TFinder finder_swift(text);

	typedef Index<StringSet<DnaString>, Index_QGram< UngappedShape<4> > > TQGramIndex;
	TQGramIndex index_4gram;
	appendValue(indexText(index_4gram), "ttttcgatcgatgctttttttttttttttttttttttttttttt");
    Pattern<TQGramIndex, Swift<SwiftLocal> > pattern_swift(index_4gram);

    String<String<char> > expectedPositions;
    appendValue(expectedPositions, "< < 36 , 46 > , < < 0 , 3 > , < 0 , 30 > > >");
    appendValue(expectedPositions, "< < 36 , 46 > , < < 0 , 0 > , < 0 , 14 > > >");

	testLocalSwift(finder_swift, pattern_swift, 0.1, 10, expectedPositions);
}

void testLocalSwiftTwoPatterns() {
	// two pattern sequences
    typedef Finder<DnaString, Swift<SwiftLocal> > TFinder;
    DnaString text = "aaaacgttccaaaaaaa";
	TFinder finder_swift(text);

	typedef Index<StringSet<DnaString>, Index_QGram< UngappedShape<4> > > TQGramIndex;
	TQGramIndex index_4gram;
	appendValue(indexText(index_4gram), "ttttcgttcctttttttttttttttttttttt"); // length = 32
    appendValue(indexText(index_4gram), "ttcgttcctt"); // length = 10
	Pattern<TQGramIndex, Swift<SwiftLocal> > pattern_swift(index_4gram);

    String<String<char> > expectedPositions;
    appendValue(expectedPositions, "< < 4 , 10 > , < < 0 , 0 > , < 0 , 10 > > >");
    appendValue(expectedPositions, "< < 4 , 10 > , < < 0 , 3 > , < 0 , 26 > > >");
    appendValue(expectedPositions, "< < 4 , 10 > , < < 1 , 0 > , < 1 , 10 > > >");

	testLocalSwift(finder_swift, pattern_swift, 0.1, 6, expectedPositions);
}

void testLocalSwiftLongPatterns() {

	// two longer pattern sequences
    typedef Finder<DnaString, Swift<SwiftLocal> > TFinder;
	DnaString text = "aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaacgatcagtgacaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa";
	TFinder finder_swift(text);

	typedef Index<StringSet<DnaString>, Index_QGram< UngappedShape<4> > > TQGramIndex;
	TQGramIndex index_4gram;
	appendValue(indexText(index_4gram), "ttttttttttttttcgatcagtgacttttttttttttttttttttttttttttttttatcagt"); // length = 63
    appendValue(indexText(index_4gram), "ttttttttttttttcgatcagtgacttttttttttttttttttttttttttttttttatcagt"); // length = 63
	Pattern<TQGramIndex, Swift<SwiftLocal> > pattern_swift(index_4gram);

    String<String<char> > expectedPositions;
    appendValue(expectedPositions, "< < 90 , 96 > , < < 0 , 41 > , < 0 , 63 > > >");
    appendValue(expectedPositions, "< < 88 , 99 > , < < 0 , 7 > , < 0 , 35 > > >");
    appendValue(expectedPositions, "< < 90 , 96 > , < < 1 , 41 > , < 1 , 63 > > >");
    appendValue(expectedPositions, "< < 88 , 99 > , < < 1 , 7 > , < 1 , 35 > > >");

	testLocalSwift(finder_swift, pattern_swift, 0.1, 6, expectedPositions);
}

void testShrink1() {
    DnaString seq1 = "ACCTTTGCCCCCCCCCCTAAAAAAAATTAAAA";
    DnaString seq2 = "ACGTTTACCCCCCCCCCGAAAAAAAAGAAAA";

    Align<DnaString> alignment;
    resize(rows(alignment), 2);
    assignSource(row(alignment, 0), seq1);
    assignSource(row(alignment, 1), seq2);

    Score<int> score(1, -1, -1);
    globalAlignment(alignment, score);

    longestEpsMatch(alignment, 6, 0.125);

    SEQAN_ASSERT_TRUE(row(alignment, 0) == "ACCTTTGCCCCCCCCCCTAAAAAAAA");
    SEQAN_ASSERT_TRUE(row(alignment, 1) == "ACGTTTACCCCCCCCCCGAAAAAAAA");
}

void testShrink2() {
    DnaString seq1 = "ACCTTTGCCCCCCCCCCTAAAAAAAATTAAAA";
    DnaString seq2 = "ACGTTTCCCCCCCCCCGAAAAAAAAGAAAA";

    Align<DnaString> alignment;
    resize(rows(alignment), 2);
    assignSource(row(alignment, 0), seq1);
    assignSource(row(alignment, 1), seq2);

    Score<int> score(1, -1, -1);
    globalAlignment(alignment, score);

    longestEpsMatch(alignment, 6, 0.125);

    SEQAN_ASSERT_TRUE(row(alignment, 0) == "ACCTTTGCCCCCCCCCCTAAAAAAAA");
    SEQAN_ASSERT_TRUE(row(alignment, 1) == "ACGTTT-CCCCCCCCCCGAAAAAAAA");
}

void testShrink3() {
    DnaString seq1 = "AAAATTAAAAAAAATCCCCCCCCCCGTTTCCA";
    DnaString seq2 = "AAAAGAAAAAAAAGCCCCCCCCCCTTTGCA";

    Align<DnaString> alignment;
    resize(rows(alignment), 2);
    assignSource(row(alignment, 0), seq1);
    assignSource(row(alignment, 1), seq2);

    Score<int> score(1, -1, -1);
    globalAlignment(alignment, score);

    longestEpsMatch(alignment, 6, 0.125);

    SEQAN_ASSERT_TRUE(row(alignment, 0) == "AAAAAAAATCCCCCCCCCCGTTTCCA");
    SEQAN_ASSERT_TRUE(row(alignment, 1) == "AAAAAAAAGCCCCCCCCCC-TTTGCA");
}

SEQAN_DEFINE_TEST(test_find_swift) {
    testOneLocalSwiftHit();
    testOneLocalSwiftHit2();
    testOneLocalSwiftHitStringSet();
    testOneLocalSwiftHitBucketBorder();
    testOneLocalSwiftHitNegDiag();
    testLocalSwiftTwoPatterns();
    testLocalSwiftLongPatterns();
}

SEQAN_DEFINE_TEST(test_find_longest_epsMatch) {
    testShrink1();
    testShrink2();
    testShrink3();
}

SEQAN_BEGIN_TESTSUITE(test_find_swift) {
    SEQAN_CALL_TEST(test_find_swift);
    SEQAN_CALL_TEST(test_find_longest_epsMatch);

    SEQAN_VERIFY_CHECKPOINTS("projects/library/seqan/index/find_swift.h");
    SEQAN_VERIFY_CHECKPOINTS("projects/library/demos/swift_local.h");
}
SEQAN_END_TESTSUITE
<commit_msg>more testing in new style<commit_after>#ifndef SEQAN_HEADER_TEST_FIND_SWIFT_H
#define SEQAN_HEADER_TEST_FIND_SWIFT_H

#include <seqan/basic.h>
#include <seqan/index.h>
#include <seqan/seeds.h>

#include <demos/swift_local.h>

using namespace seqan;

// calls local swift and compares swift hits to expected swift hits
template<typename THaystack, typename TIndex>
void testLocalSwift(Finder<THaystack, Swift<SwiftLocal> > & finder,
					Pattern<TIndex, Swift<SwiftLocal> > & pattern,
					double epsilon,
					int minLength,
                    String<String<char> > & expectedPositions) {
	typedef typename Position<String<String<char> > >::Type TPosition;
	TPosition i = 0;
	while (find(finder, pattern, epsilon, minLength)) {
        // write swift hit into string and compare to expected output
        std::ostringstream pos;
		pos << "< " << positionRange(finder) << " , " << positionRange(pattern) << " >";
		SEQAN_ASSERT_EQ(pos.str().c_str(), value(expectedPositions,i));
		++i;
	}
}

void testOneLocalSwiftHit() {
	// a single pattern and a single hit
    typedef Finder<DnaString, Swift<SwiftLocal> > TFinder;
	DnaString text = "aaaaaaacgatcgatgcaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa";
	TFinder finder_swift(text);

    typedef Index<DnaString, Index_QGram<UngappedShape<4> > > TQGramIndexSimple;
	TQGramIndexSimple index_4gram("tttcgatcgatgctttttttttttttttttttttttttttttt");
    Pattern<TQGramIndexSimple, Swift<SwiftLocal> > pattern_swift(index_4gram);

    String<String<char> > expectedPositions;
    appendValue(expectedPositions, "< < 7 , 17 > , < 0 , 17 > >");

	testLocalSwift(finder_swift, pattern_swift, 0.1, 6, expectedPositions);
}

void testOneLocalSwiftHit2() {
    // a single pattern and a single hit 2
    typedef Finder<DnaString, Swift<SwiftLocal> > TFinder;
	DnaString text = "aaaaagagaccccccagagaaaaaa";
	TFinder finder_swift(text);

    typedef Index<DnaString, Index_QGram<UngappedShape<4> > > TQGramIndexSimple;
	TQGramIndexSimple index_4gram("tttttggccccccggtttttt");
    Pattern<TQGramIndexSimple, Swift<SwiftLocal> > pattern_swift(index_4gram);

    String<String<char> > expectedPositions;
    appendValue(expectedPositions, "< < 9 , 15 > , < 0 , 15 > >");

	testLocalSwift(finder_swift, pattern_swift, 0.1, 6, expectedPositions);
}

void testOneLocalSwiftHitStringSet() {
	// a single hit, pattern in a StringSet
    typedef Finder<DnaString, Swift<SwiftLocal> > TFinder;
	DnaString text = "aaaaaaacgatcgatgcaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa";
	TFinder finder_swift(text);

	typedef Index<StringSet<DnaString>, Index_QGram< UngappedShape<4> > > TQGramIndex;
	TQGramIndex index_4gram;
	appendValue(indexText(index_4gram), "tttcgatcgatgctttttttttttttttttttttttttttttt");
    Pattern<TQGramIndex, Swift<SwiftLocal> > pattern_swift(index_4gram);

    String<String<char> > expectedPositions;
    appendValue(expectedPositions, "< < 7 , 17 > , < < 0 , 0 > , < 0 , 17 > > >");

	testLocalSwift(finder_swift, pattern_swift, 0.1, 6, expectedPositions);
}

void testOneLocalSwiftHitBucketBorder() {
	// hit at bucket border
    typedef Finder<DnaString, Swift<SwiftLocal> > TFinder;
	DnaString text = "aaacgatcgatgcaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa";
	TFinder finder_swift(text);

	typedef Index<StringSet<DnaString>, Index_QGram< UngappedShape<4> > > TQGramIndex;
	TQGramIndex index_4gram;
	appendValue(indexText(index_4gram), "tttcgatcgatgctttttttttttttttttttttttttttttt");
    Pattern<TQGramIndex, Swift<SwiftLocal> > pattern_swift(index_4gram);

    String<String<char> > expectedPositions;
    appendValue(expectedPositions, "< < 3 , 13 > , < < 0 , 0 > , < 0 , 13 > > >");
    appendValue(expectedPositions, "< < 3 , 13 > , < < 0 , 2 > , < 0 , 29 > > >");

	testLocalSwift(finder_swift, pattern_swift, 0.1, 6, expectedPositions);
}

void testOneLocalSwiftHitNegDiag() {

	// hit at lower haystack position -> diagonal start position negative
    typedef Finder<DnaString, Swift<SwiftLocal> > TFinder;
	DnaString text = "aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaacgatcgatgc";
	TFinder finder_swift(text);

	typedef Index<StringSet<DnaString>, Index_QGram< UngappedShape<4> > > TQGramIndex;
	TQGramIndex index_4gram;
	appendValue(indexText(index_4gram), "ttttcgatcgatgctttttttttttttttttttttttttttttt");
    Pattern<TQGramIndex, Swift<SwiftLocal> > pattern_swift(index_4gram);

    String<String<char> > expectedPositions;
    appendValue(expectedPositions, "< < 36 , 46 > , < < 0 , 3 > , < 0 , 30 > > >");
    appendValue(expectedPositions, "< < 36 , 46 > , < < 0 , 0 > , < 0 , 14 > > >");

	testLocalSwift(finder_swift, pattern_swift, 0.1, 10, expectedPositions);
}

void testLocalSwiftTwoPatterns() {
	// two pattern sequences
    typedef Finder<DnaString, Swift<SwiftLocal> > TFinder;
    DnaString text = "aaaacgttccaaaaaaa";
	TFinder finder_swift(text);

	typedef Index<StringSet<DnaString>, Index_QGram< UngappedShape<4> > > TQGramIndex;
	TQGramIndex index_4gram;
	appendValue(indexText(index_4gram), "ttttcgttcctttttttttttttttttttttt"); // length = 32
    appendValue(indexText(index_4gram), "ttcgttcctt"); // length = 10
	Pattern<TQGramIndex, Swift<SwiftLocal> > pattern_swift(index_4gram);

    String<String<char> > expectedPositions;
    appendValue(expectedPositions, "< < 4 , 10 > , < < 0 , 0 > , < 0 , 10 > > >");
    appendValue(expectedPositions, "< < 4 , 10 > , < < 0 , 3 > , < 0 , 26 > > >");
    appendValue(expectedPositions, "< < 4 , 10 > , < < 1 , 0 > , < 1 , 10 > > >");

	testLocalSwift(finder_swift, pattern_swift, 0.1, 6, expectedPositions);
}

void testLocalSwiftLongPatterns() {

	// two longer pattern sequences
    typedef Finder<DnaString, Swift<SwiftLocal> > TFinder;
	DnaString text = "aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaacgatcagtgacaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa";
	TFinder finder_swift(text);

	typedef Index<StringSet<DnaString>, Index_QGram< UngappedShape<4> > > TQGramIndex;
	TQGramIndex index_4gram;
	appendValue(indexText(index_4gram), "ttttttttttttttcgatcagtgacttttttttttttttttttttttttttttttttatcagt"); // length = 63
    appendValue(indexText(index_4gram), "ttttttttttttttcgatcagtgacttttttttttttttttttttttttttttttttatcagt"); // length = 63
	Pattern<TQGramIndex, Swift<SwiftLocal> > pattern_swift(index_4gram);

    String<String<char> > expectedPositions;
    appendValue(expectedPositions, "< < 90 , 96 > , < < 0 , 41 > , < 0 , 63 > > >");
    appendValue(expectedPositions, "< < 88 , 99 > , < < 0 , 7 > , < 0 , 35 > > >");
    appendValue(expectedPositions, "< < 90 , 96 > , < < 1 , 41 > , < 1 , 63 > > >");
    appendValue(expectedPositions, "< < 88 , 99 > , < < 1 , 7 > , < 1 , 35 > > >");

	testLocalSwift(finder_swift, pattern_swift, 0.1, 6, expectedPositions);
}

void testShrink1() {
    DnaString seq1 = "ACCTTTGCCCCCCCCCCTAAAAAAAATTAAAA";
    DnaString seq2 = "ACGTTTACCCCCCCCCCGAAAAAAAAGAAAA";

    Align<DnaString> alignment;
    resize(rows(alignment), 2);
    assignSource(row(alignment, 0), seq1);
    assignSource(row(alignment, 1), seq2);

    Score<int> score(1, -1, -1);
    globalAlignment(alignment, score);

    longestEpsMatch(alignment, 6, 0.125);

    SEQAN_ASSERT_TRUE(row(alignment, 0) == "ACCTTTGCCCCCCCCCCTAAAAAAAA");
    SEQAN_ASSERT_TRUE(row(alignment, 1) == "ACGTTTACCCCCCCCCCGAAAAAAAA");
}

void testShrink2() {
    DnaString seq1 = "ACCTTTGCCCCCCCCCCTAAAAAAAATTAAAA";
    DnaString seq2 = "ACGTTTCCCCCCCCCCGAAAAAAAAGAAAA";

    Align<DnaString> alignment;
    resize(rows(alignment), 2);
    assignSource(row(alignment, 0), seq1);
    assignSource(row(alignment, 1), seq2);

    Score<int> score(1, -1, -1);
    globalAlignment(alignment, score);

    longestEpsMatch(alignment, 6, 0.125);

    SEQAN_ASSERT_TRUE(row(alignment, 0) == "ACCTTTGCCCCCCCCCCTAAAAAAAA");
    SEQAN_ASSERT_TRUE(row(alignment, 1) == "ACGTTT-CCCCCCCCCCGAAAAAAAA");
}

void testShrink3() {
    DnaString seq1 = "AAAATTAAAAAAAATCCCCCCCCCCGTTTCCA";
    DnaString seq2 = "AAAAGAAAAAAAAGCCCCCCCCCCTTTGCA";

    Align<DnaString> alignment;
    resize(rows(alignment), 2);
    assignSource(row(alignment, 0), seq1);
    assignSource(row(alignment, 1), seq2);

    Score<int> score(1, -1, -1);
    globalAlignment(alignment, score);

    longestEpsMatch(alignment, 6, 0.125);

    SEQAN_ASSERT_TRUE(row(alignment, 0) == "AAAAAAAATCCCCCCCCCCGTTTCCA");
    SEQAN_ASSERT_TRUE(row(alignment, 1) == "AAAAAAAAGCCCCCCCCCC-TTTGCA");
}

SEQAN_DEFINE_TEST(test_find_swift) {
    testOneLocalSwiftHit();
    testOneLocalSwiftHit2();
    testOneLocalSwiftHitStringSet();
    testOneLocalSwiftHitBucketBorder();
    testOneLocalSwiftHitNegDiag();
    testLocalSwiftTwoPatterns();
    testLocalSwiftLongPatterns();
}

SEQAN_DEFINE_TEST(test_longest_epsMatch) {
    testShrink1();
    testShrink2();
    testShrink3();
}

SEQAN_BEGIN_TESTSUITE(test_find_swift) {
    SEQAN_CALL_TEST(test_find_swift);
    SEQAN_CALL_TEST(test_longest_epsMatch);

    //SEQAN_VERIFY_CHECKPOINTS("projects/library/seqan/index/find_swift.h");
    SEQAN_VERIFY_CHECKPOINTS("projects/library/demos/swift_local.h");
}
SEQAN_END_TESTSUITE

#endif
<|endoftext|>


<commit_before>/*
 * teleop_pr2
 * Copyright (c) 2009, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the <ORGANIZATION> nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

//\author: Blaise Gassend

#include <unistd.h>
#include <math.h>
#include <linux/joystick.h>
#include <fcntl.h>
#include <diagnostic_updater/diagnostic_updater.h>
#include "ros/ros.h"
#include "joy/Joy.h"

///\brief Opens, reads from and publishes joystick events
class Joystick
{
private:
  ros::NodeHandle nh_;
  bool open_;
  std::string joy_dev_;
  double deadzone_;
  double autorepeat_rate_;  // in Hz.  0 for no repeat.
  double coalesce_interval_; // Defaults to 100 Hz rate limit.
  int event_count_;
  int pub_count_;
  ros::Publisher pub_;
  double lastDiagTime_;

  diagnostic_updater::Updater diagnostic_;

  ///\brief Publishes diagnostics and status
  void diagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat)
  {
    double now = ros::Time::now().toSec();
    double interval = now - lastDiagTime_;
    if (open_)
      stat.summary(0, "OK");
    else
      stat.summary(2, "Joystick not open.");

    stat.add("topic", pub_.getTopic());
    stat.add("device", joy_dev_);
    stat.add("dead zone", deadzone_);
    stat.add("autorepeat rate (Hz)", autorepeat_rate_);
    stat.add("coalesce interval (s)", coalesce_interval_);
    stat.add("recent joystick event rate (Hz)", event_count_ / interval);
    stat.add("recent publication rate (Hz)", pub_count_ / interval);
    stat.add("subscribers", pub_.getNumSubscribers());
    event_count_ = 0;
    pub_count_ = 0;
    lastDiagTime_ = now;
  }

public:
  Joystick() : nh_(), diagnostic_(nh_)
  {}

  ///\brief Opens joystick port, reads from port and publishes while node is active
  int main(int argc, char **argv)
  {
    diagnostic_.add("Joystick Driver Status", this, &Joystick::diagnostics);

    // Parameters
    ros::NodeHandle nh_param("~");
    pub_ = nh_.advertise<joy::Joy>("joy", 1);
    nh_param.param<std::string>("dev", joy_dev_, "/dev/input/js0");
    nh_param.param<double>("deadzone", deadzone_, 0.05);
    nh_param.param<double>("autorepeat_rate", autorepeat_rate_, 0);
    nh_param.param<double>("coalesce_interval", coalesce_interval_, 0.001);

    // Checks on parameters
    if (autorepeat_rate_ > 1 / coalesce_interval_)
      ROS_WARN("joy_node: autorepeat_rate (%f Hz) > 1/coalesce_interval (%f Hz) does not make sense. Timing behavior is not well defined.", autorepeat_rate_, 1/coalesce_interval_);

    if (deadzone_ >= 1)
    {
      ROS_WARN("joy_node: deadzone greater than 1 was requested. The semantics of deadzone have changed. It is now related to the range [-1:1] instead of [-32767:32767]. For now I am dividing your deadzone by 32767, but this behavior is deprecated so you need to update your launch file.");
      deadzone_ /= 32767;
    }

    if (deadzone_ > 0.9)
    {
      ROS_WARN("joy_node: deadzone (%f) greater than 0.9, setting it to 0.9", deadzone_);
      deadzone_ = 0.9;
    }

    if (deadzone_ < 0)
    {
      ROS_WARN("joy_node: deadzone_ (%f) less than 0, setting to 0.", deadzone_);
      deadzone_ = 0;
    }

    if (autorepeat_rate_ < 0)
    {
      ROS_WARN("joy_node: autorepeat_rate (%f) less than 0, setting to 0.", autorepeat_rate_);
      autorepeat_rate_ = 0;
    }

    if (coalesce_interval_ < 0)
    {
      ROS_WARN("joy_node: coalesce_interval (%f) less than 0, setting to 0.", coalesce_interval_);
      coalesce_interval_ = 0;
    }

    // Parameter conversions
    double autorepeat_interval = 1 / autorepeat_rate_;
    double scale = -1. / (1. - deadzone_) / 32767.;
    double unscaled_deadzone = 32767. * deadzone_;

    js_event event;
    struct timeval tv;
    fd_set set;
    int joy_fd;
    event_count_ = 0;
    pub_count_ = 0;
    lastDiagTime_ = ros::Time::now().toSec();

    // Big while loop opens, publishes
    while (nh_.ok())
    {
      open_ = false;
      diagnostic_.force_update();
      bool first_fault = true;
      while (true)
      {
        ros::spinOnce();
        if (!nh_.ok())
          goto cleanup;
        joy_fd = open(joy_dev_.c_str(), O_RDONLY);
        if (joy_fd != -1)
        {
          // There seems to be a bug in the driver or something where the
          // initial events that are to define the initial state of the
          // joystick are not the values of the joystick when it was opened
          // but rather the values of the joystick when it was last closed.
          // Opening then closing and opening again is a hack to get more
          // accurate initial state data.
          close(joy_fd);
          joy_fd = open(joy_dev_.c_str(), O_RDONLY);
        }
        if (joy_fd != -1)
          break;
        if (first_fault)
        {
          ROS_ERROR("Couldn't open joystick %s. Will retry every second.", joy_dev_.c_str());
          first_fault = false;
        }
        sleep(1.0);
        diagnostic_.update();
      }

      ROS_INFO("Opened joystick: %s. deadzone_: %f.", joy_dev_.c_str(), deadzone_);
      open_ = true;
      diagnostic_.force_update();

      bool tv_set = false;
      bool publication_pending = false;
      tv.tv_sec = 1;
      tv.tv_usec = 0;
      joy::Joy joy_msg; // Here because we want to reset it on device close.
      while (nh_.ok())
      {
        ros::spinOnce();

        bool publish_now = false;
        bool publish_soon = false;
        FD_ZERO(&set);
        FD_SET(joy_fd, &set);

        //ROS_INFO("Select...");
        int select_out = select(joy_fd+1, &set, NULL, NULL, &tv);
        //ROS_INFO("Tick...");
        if (select_out == -1)
        {
          tv.tv_sec = 0;
          tv.tv_usec = 0;
          //ROS_INFO("Select returned negative. %i", ros::isShuttingDown());
          continue;
          //				break; // Joystick is probably closed. Not sure if this case is useful.
        }

        if (FD_ISSET(joy_fd, &set))
        {
          if (read(joy_fd, &event, sizeof(js_event)) == -1 && errno != EAGAIN)
            break; // Joystick is probably closed. Definitely occurs.

          //ROS_INFO("Read data...");

          event_count_++;
          switch(event.type)
          {
          case JS_EVENT_BUTTON:
          case JS_EVENT_BUTTON | JS_EVENT_INIT:
            if(event.number >= joy_msg.get_buttons_size())
            {
              int old_size = joy_msg.buttons.size();
              joy_msg.buttons.resize(event.number+1);
              for(unsigned int i=old_size;i<joy_msg.buttons.size();i++)
                joy_msg.buttons[i] = 0.0;
            }
            joy_msg.buttons[event.number] = (event.value ? 1 : 0);
            // For initial events, wait a bit before sending to try to catch
            // all the initial events.
            if (!(event.type & JS_EVENT_INIT))
              publish_now = true;
            else
              publish_soon = true;
            break;
          case JS_EVENT_AXIS:
          case JS_EVENT_AXIS | JS_EVENT_INIT:
            if(event.number >= joy_msg.get_axes_size())
            {
              int old_size = joy_msg.axes.size();
              joy_msg.axes.resize(event.number+1);
              for(unsigned int i=old_size;i<joy_msg.axes.size();i++)
                joy_msg.axes[i] = 0.0;
            }
            {
              double val = event.value;
              // Allows deadzone to be "smooth"
              if (val > unscaled_deadzone)
                val -= unscaled_deadzone;
              else if (val < -unscaled_deadzone)
                val += unscaled_deadzone;
              else
                val = 0;
              joy_msg.axes[event.number] = val * scale;
            }
            // Will wait a bit before sending to try to combine events.
            publish_soon = true;
            break;
          default:
            ROS_WARN("joy_node: Unknown event type. Please file a ticket. time=%u, value=%d, type=%Xh, number=%d", event.time, event.value, event.type, event.number);
            break;
          }
        }
        else if (tv_set) // Assume that the timer has expired.
          publish_now = true;

        if (publish_now)
        {
          // Assume that all the JS_EVENT_INIT messages have arrived already.
          // This should be the case as the kernel sends them along as soon as
          // the device opens.
          //ROS_INFO("Publish...");
          pub_.publish(joy_msg);
          publish_now = false;
          tv_set = false;
          publication_pending = false;
          publish_soon = false;
          pub_count_++;
        }

        // If an axis event occurred, start a timer to combine with other
        // events.
        if (!publication_pending && publish_soon)
        {
          tv.tv_sec = trunc(coalesce_interval_);
          tv.tv_usec = (coalesce_interval_ - tv.tv_sec) * 1e6;
          publication_pending = true;
          tv_set = true;
          //ROS_INFO("Pub pending...");
        }

        // If nothing is going on, start a timer to do autorepeat.
        if (!tv_set && autorepeat_rate_ > 0)
        {
          tv.tv_sec = trunc(autorepeat_interval);
          tv.tv_usec = (autorepeat_interval - tv.tv_sec) * 1e6;
          tv_set = true;
          //ROS_INFO("Autorepeat pending... %i %i", tv.tv_sec, tv.tv_usec);
        }

        if (!tv_set)
        {
          tv.tv_sec = 1;
          tv.tv_usec = 0;
        }

        diagnostic_.update();
      } // End of joystick open loop.

      close(joy_fd);
      ros::spinOnce();
      if (nh_.ok())
        ROS_ERROR("Connection to joystick device lost unexpectedly. Will reopen.");
    }

  cleanup:
    ROS_INFO("joy_node shut down.");

    return 0;
  }
};

int main(int argc, char **argv)
{
  ros::init(argc, argv, "joy_node");
  Joystick j;
  return j.main(argc, argv);
}
<commit_msg>joy_node now ignores axis values in JS_EVENT_INIT because they are bogus anyways.<commit_after>/*
 * teleop_pr2
 * Copyright (c) 2009, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the <ORGANIZATION> nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

//\author: Blaise Gassend

#include <unistd.h>
#include <math.h>
#include <linux/joystick.h>
#include <fcntl.h>
#include <diagnostic_updater/diagnostic_updater.h>
#include "ros/ros.h"
#include "joy/Joy.h"

///\brief Opens, reads from and publishes joystick events
class Joystick
{
private:
  ros::NodeHandle nh_;
  bool open_;
  std::string joy_dev_;
  double deadzone_;
  double autorepeat_rate_;  // in Hz.  0 for no repeat.
  double coalesce_interval_; // Defaults to 100 Hz rate limit.
  int event_count_;
  int pub_count_;
  ros::Publisher pub_;
  double lastDiagTime_;

  diagnostic_updater::Updater diagnostic_;

  ///\brief Publishes diagnostics and status
  void diagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat)
  {
    double now = ros::Time::now().toSec();
    double interval = now - lastDiagTime_;
    if (open_)
      stat.summary(0, "OK");
    else
      stat.summary(2, "Joystick not open.");

    stat.add("topic", pub_.getTopic());
    stat.add("device", joy_dev_);
    stat.add("dead zone", deadzone_);
    stat.add("autorepeat rate (Hz)", autorepeat_rate_);
    stat.add("coalesce interval (s)", coalesce_interval_);
    stat.add("recent joystick event rate (Hz)", event_count_ / interval);
    stat.add("recent publication rate (Hz)", pub_count_ / interval);
    stat.add("subscribers", pub_.getNumSubscribers());
    event_count_ = 0;
    pub_count_ = 0;
    lastDiagTime_ = now;
  }

public:
  Joystick() : nh_(), diagnostic_(nh_)
  {}

  ///\brief Opens joystick port, reads from port and publishes while node is active
  int main(int argc, char **argv)
  {
    diagnostic_.add("Joystick Driver Status", this, &Joystick::diagnostics);

    // Parameters
    ros::NodeHandle nh_param("~");
    pub_ = nh_.advertise<joy::Joy>("joy", 1);
    nh_param.param<std::string>("dev", joy_dev_, "/dev/input/js0");
    nh_param.param<double>("deadzone", deadzone_, 0.05);
    nh_param.param<double>("autorepeat_rate", autorepeat_rate_, 0);
    nh_param.param<double>("coalesce_interval", coalesce_interval_, 0.001);

    // Checks on parameters
    if (autorepeat_rate_ > 1 / coalesce_interval_)
      ROS_WARN("joy_node: autorepeat_rate (%f Hz) > 1/coalesce_interval (%f Hz) does not make sense. Timing behavior is not well defined.", autorepeat_rate_, 1/coalesce_interval_);

    if (deadzone_ >= 1)
    {
      ROS_WARN("joy_node: deadzone greater than 1 was requested. The semantics of deadzone have changed. It is now related to the range [-1:1] instead of [-32767:32767]. For now I am dividing your deadzone by 32767, but this behavior is deprecated so you need to update your launch file.");
      deadzone_ /= 32767;
    }

    if (deadzone_ > 0.9)
    {
      ROS_WARN("joy_node: deadzone (%f) greater than 0.9, setting it to 0.9", deadzone_);
      deadzone_ = 0.9;
    }

    if (deadzone_ < 0)
    {
      ROS_WARN("joy_node: deadzone_ (%f) less than 0, setting to 0.", deadzone_);
      deadzone_ = 0;
    }

    if (autorepeat_rate_ < 0)
    {
      ROS_WARN("joy_node: autorepeat_rate (%f) less than 0, setting to 0.", autorepeat_rate_);
      autorepeat_rate_ = 0;
    }

    if (coalesce_interval_ < 0)
    {
      ROS_WARN("joy_node: coalesce_interval (%f) less than 0, setting to 0.", coalesce_interval_);
      coalesce_interval_ = 0;
    }

    // Parameter conversions
    double autorepeat_interval = 1 / autorepeat_rate_;
    double scale = -1. / (1. - deadzone_) / 32767.;
    double unscaled_deadzone = 32767. * deadzone_;

    js_event event;
    struct timeval tv;
    fd_set set;
    int joy_fd;
    event_count_ = 0;
    pub_count_ = 0;
    lastDiagTime_ = ros::Time::now().toSec();

    // Big while loop opens, publishes
    while (nh_.ok())
    {
      open_ = false;
      diagnostic_.force_update();
      bool first_fault = true;
      while (true)
      {
        ros::spinOnce();
        if (!nh_.ok())
          goto cleanup;
        joy_fd = open(joy_dev_.c_str(), O_RDONLY);
        if (joy_fd != -1)
        {
          // There seems to be a bug in the driver or something where the
          // initial events that are to define the initial state of the
          // joystick are not the values of the joystick when it was opened
          // but rather the values of the joystick when it was last closed.
          // Opening then closing and opening again is a hack to get more
          // accurate initial state data.
          close(joy_fd);
          joy_fd = open(joy_dev_.c_str(), O_RDONLY);
        }
        if (joy_fd != -1)
          break;
        if (first_fault)
        {
          ROS_ERROR("Couldn't open joystick %s. Will retry every second.", joy_dev_.c_str());
          first_fault = false;
        }
        sleep(1.0);
        diagnostic_.update();
      }

      ROS_INFO("Opened joystick: %s. deadzone_: %f.", joy_dev_.c_str(), deadzone_);
      open_ = true;
      diagnostic_.force_update();

      bool tv_set = false;
      bool publication_pending = false;
      tv.tv_sec = 1;
      tv.tv_usec = 0;
      joy::Joy joy_msg; // Here because we want to reset it on device close.
      while (nh_.ok())
      {
        ros::spinOnce();

        bool publish_now = false;
        bool publish_soon = false;
        FD_ZERO(&set);
        FD_SET(joy_fd, &set);

        //ROS_INFO("Select...");
        int select_out = select(joy_fd+1, &set, NULL, NULL, &tv);
        //ROS_INFO("Tick...");
        if (select_out == -1)
        {
          tv.tv_sec = 0;
          tv.tv_usec = 0;
          //ROS_INFO("Select returned negative. %i", ros::isShuttingDown());
          continue;
          //				break; // Joystick is probably closed. Not sure if this case is useful.
        }

        if (FD_ISSET(joy_fd, &set))
        {
          if (read(joy_fd, &event, sizeof(js_event)) == -1 && errno != EAGAIN)
            break; // Joystick is probably closed. Definitely occurs.

          //ROS_INFO("Read data...");

          ROS_WARN("joy_node: Unknown event type. Please file a ticket. time=%u, value=%d, type=%Xh, number=%d", event.time, event.value, event.type, event.number);

          event_count_++;
          switch(event.type)
          {
          case JS_EVENT_BUTTON:
          case JS_EVENT_BUTTON | JS_EVENT_INIT:
            if(event.number >= joy_msg.get_buttons_size())
            {
              int old_size = joy_msg.buttons.size();
              joy_msg.buttons.resize(event.number+1);
              for(unsigned int i=old_size;i<joy_msg.buttons.size();i++)
                joy_msg.buttons[i] = 0.0;
            }
            joy_msg.buttons[event.number] = (event.value ? 1 : 0);
            // For initial events, wait a bit before sending to try to catch
            // all the initial events.
            if (!(event.type & JS_EVENT_INIT))
              publish_now = true;
            else
              publish_soon = true;
            break;
          case JS_EVENT_AXIS:
          case JS_EVENT_AXIS | JS_EVENT_INIT:
            if(event.number >= joy_msg.get_axes_size())
            {
              int old_size = joy_msg.axes.size();
              joy_msg.axes.resize(event.number+1);
              for(unsigned int i=old_size;i<joy_msg.axes.size();i++)
                joy_msg.axes[i] = 0.0;
            }
            if (!(event.type & JS_EVENT_INIT)) // Init event.value is wrong.
            {
              double val = event.value;
              // Allows deadzone to be "smooth"
              if (val > unscaled_deadzone)
                val -= unscaled_deadzone;
              else if (val < -unscaled_deadzone)
                val += unscaled_deadzone;
              else
                val = 0;
              joy_msg.axes[event.number] = val * scale;
            }
            // Will wait a bit before sending to try to combine events.
            publish_soon = true;
            break;
          default:
            ROS_WARN("joy_node: Unknown event type. Please file a ticket. time=%u, value=%d, type=%Xh, number=%d", event.time, event.value, event.type, event.number);
            break;
          }
        }
        else if (tv_set) // Assume that the timer has expired.
          publish_now = true;

        if (publish_now)
        {
          // Assume that all the JS_EVENT_INIT messages have arrived already.
          // This should be the case as the kernel sends them along as soon as
          // the device opens.
          //ROS_INFO("Publish...");
          pub_.publish(joy_msg);
          publish_now = false;
          tv_set = false;
          publication_pending = false;
          publish_soon = false;
          pub_count_++;
        }

        // If an axis event occurred, start a timer to combine with other
        // events.
        if (!publication_pending && publish_soon)
        {
          tv.tv_sec = trunc(coalesce_interval_);
          tv.tv_usec = (coalesce_interval_ - tv.tv_sec) * 1e6;
          publication_pending = true;
          tv_set = true;
          //ROS_INFO("Pub pending...");
        }

        // If nothing is going on, start a timer to do autorepeat.
        if (!tv_set && autorepeat_rate_ > 0)
        {
          tv.tv_sec = trunc(autorepeat_interval);
          tv.tv_usec = (autorepeat_interval - tv.tv_sec) * 1e6;
          tv_set = true;
          //ROS_INFO("Autorepeat pending... %i %i", tv.tv_sec, tv.tv_usec);
        }

        if (!tv_set)
        {
          tv.tv_sec = 1;
          tv.tv_usec = 0;
        }

        diagnostic_.update();
      } // End of joystick open loop.

      close(joy_fd);
      ros::spinOnce();
      if (nh_.ok())
        ROS_ERROR("Connection to joystick device lost unexpectedly. Will reopen.");
    }

  cleanup:
    ROS_INFO("joy_node shut down.");

    return 0;
  }
};

int main(int argc, char **argv)
{
  ros::init(argc, argv, "joy_node");
  Joystick j;
  return j.main(argc, argv);
}
<|endoftext|>