<commit_before>/******************************************************************************
 *                    _   _____   __________                                  *
 *                   | | / / _ | / __/_  __/     Visibility                   *
 *                   | |/ / __ |_\ \  / /          Across                     *
 *                   |___/_/ |_/___/ /_/       Space and Time                 *
 *                                                                            *
 * This file is part of VAST. It is subject to the license terms in the       *
 * LICENSE file found in the top-level directory of this distribution and at  *
 * http://vast.io/license. No part of VAST, including this file, may be       *
 * copied, modified, propagated, or distributed except according to the terms *
 * contained in the LICENSE file.                                             *
 ******************************************************************************/

#include <fstream>
#include <string>

#include "vast/detail/mmapbuf.hpp"
#include "vast/detail/system.hpp"

#define SUITE streambuf
#include "test.hpp"
#include "fixtures/filesystem.hpp"

using namespace std::string_literals;
using namespace vast;

FIXTURE_SCOPE(fixture_tests, fixtures::filesystem)

TEST(memory-mapped streambuffer) {
  MESSAGE("create a dummy file");
  auto filename = directory / "dummy.txt";
  std::ofstream ofs{filename.str()};
  auto data = "foobarbazqux"s;
  ofs << data;
  ofs.close();
  MESSAGE("performing streambuffer tests");
  detail::mmapbuf sb{filename.str()};
  CHECK_EQUAL(sb.size(), data.size());
  CHECK_EQUAL(sb.in_avail(), static_cast<std::streamsize>(sb.size()));
  std::string buf;
  buf.resize(3);
  auto n = sb.sgetn(&buf[0], 3);
  CHECK_EQUAL(n, 3);
  CHECK_EQUAL(buf, "foo");
  CHECK_EQUAL(sb.in_avail(), 9);
  buf.resize(data.size());
  n = sb.sgetn(&buf[3], 100);
  CHECK_EQUAL(n, 9);
  CHECK_EQUAL(buf, data);
  CHECK_EQUAL(sb.in_avail(), 0);
  // Seek back to beginning.
  sb.pubseekpos(0, std::ios::in);
  CHECK_EQUAL(sb.in_avail(), static_cast<std::streamsize>(sb.size()));
  data = "corge ";
  n = sb.sputn(data.data(), data.size());
  CHECK_EQUAL(n, static_cast<std::streamsize>(data.size()));
  CHECK_EQUAL(std::string(sb.data(), sb.size()), "corge bazqux");
  CHECK(sb.resize(data.size() - 1));
  // Figure out current position.
  size_t cur = sb.pubseekoff(0, std::ios::cur, std::ios::out);
  CHECK_EQUAL(cur, sb.size()); // we're at the end!
}

namespace {

void aligned_resize_test_impl(const vast::path& filename, size_t size) {
  detail::mmapbuf sb{filename.str(), size};
  REQUIRE(sb.data() != nullptr);
  CHECK_EQUAL(sb.size(), size);
  sb.sputn("Here be content", 15);
  // Aligned resizing.
  REQUIRE(sb.resize(size * 2));
  CHECK_EQUAL(sb.size(), size * 2);
  CHECK_EQUAL(std::string(sb.data() + 3, 9), "e be cont");
  // Seek in the middle and perform a random write.
  sb.pubseekpos(size, std::ios::out);
  sb.sputc('x');
  // Unaligned resizing.
  REQUIRE(sb.resize(size / 2));
  CHECK_EQUAL(sb.size(), size / 2);
  REQUIRE(sb.resize(sb.size() * 8));
  CHECK_EQUAL(sb.size(), size * 4);
  CHECK_EQUAL(std::string(sb.data() + 3, 9), "e be cont");
  sb.pubseekpos(size * 3, std::ios::out);
  sb.sputc('x');
}

}

TEST(memory-mapped streambuffer aligned resize) {
  auto filename = directory / "aligned";
  aligned_resize_test_impl(filename, detail::page_size());
}

TEST(memory-mapped streambuffer aligned resize large) {
  auto filename = directory / "aligned_large";
  auto page_size = detail::page_size();
  auto hundred_mb = size_t{100 * (1 << 20)};
  auto size = hundred_mb - (hundred_mb % page_size);
  aligned_resize_test_impl(filename, size);
}

FIXTURE_SCOPE_END()
<commit_msg>Add missing end-of-namespace comment<commit_after>/******************************************************************************
 *                    _   _____   __________                                  *
 *                   | | / / _ | / __/_  __/     Visibility                   *
 *                   | |/ / __ |_\ \  / /          Across                     *
 *                   |___/_/ |_/___/ /_/       Space and Time                 *
 *                                                                            *
 * This file is part of VAST. It is subject to the license terms in the       *
 * LICENSE file found in the top-level directory of this distribution and at  *
 * http://vast.io/license. No part of VAST, including this file, may be       *
 * copied, modified, propagated, or distributed except according to the terms *
 * contained in the LICENSE file.                                             *
 ******************************************************************************/

#include <fstream>
#include <string>

#include "vast/detail/mmapbuf.hpp"
#include "vast/detail/system.hpp"

#define SUITE streambuf
#include "test.hpp"
#include "fixtures/filesystem.hpp"

using namespace std::string_literals;
using namespace vast;

FIXTURE_SCOPE(fixture_tests, fixtures::filesystem)

TEST(memory-mapped streambuffer) {
  MESSAGE("create a dummy file");
  auto filename = directory / "dummy.txt";
  std::ofstream ofs{filename.str()};
  auto data = "foobarbazqux"s;
  ofs << data;
  ofs.close();
  MESSAGE("performing streambuffer tests");
  detail::mmapbuf sb{filename.str()};
  CHECK_EQUAL(sb.size(), data.size());
  CHECK_EQUAL(sb.in_avail(), static_cast<std::streamsize>(sb.size()));
  std::string buf;
  buf.resize(3);
  auto n = sb.sgetn(&buf[0], 3);
  CHECK_EQUAL(n, 3);
  CHECK_EQUAL(buf, "foo");
  CHECK_EQUAL(sb.in_avail(), 9);
  buf.resize(data.size());
  n = sb.sgetn(&buf[3], 100);
  CHECK_EQUAL(n, 9);
  CHECK_EQUAL(buf, data);
  CHECK_EQUAL(sb.in_avail(), 0);
  // Seek back to beginning.
  sb.pubseekpos(0, std::ios::in);
  CHECK_EQUAL(sb.in_avail(), static_cast<std::streamsize>(sb.size()));
  data = "corge ";
  n = sb.sputn(data.data(), data.size());
  CHECK_EQUAL(n, static_cast<std::streamsize>(data.size()));
  CHECK_EQUAL(std::string(sb.data(), sb.size()), "corge bazqux");
  CHECK(sb.resize(data.size() - 1));
  // Figure out current position.
  size_t cur = sb.pubseekoff(0, std::ios::cur, std::ios::out);
  CHECK_EQUAL(cur, sb.size()); // we're at the end!
}

namespace {

void aligned_resize_test_impl(const vast::path& filename, size_t size) {
  detail::mmapbuf sb{filename.str(), size};
  REQUIRE(sb.data() != nullptr);
  CHECK_EQUAL(sb.size(), size);
  sb.sputn("Here be content", 15);
  // Aligned resizing.
  REQUIRE(sb.resize(size * 2));
  CHECK_EQUAL(sb.size(), size * 2);
  CHECK_EQUAL(std::string(sb.data() + 3, 9), "e be cont");
  // Seek in the middle and perform a random write.
  sb.pubseekpos(size, std::ios::out);
  sb.sputc('x');
  // Unaligned resizing.
  REQUIRE(sb.resize(size / 2));
  CHECK_EQUAL(sb.size(), size / 2);
  REQUIRE(sb.resize(sb.size() * 8));
  CHECK_EQUAL(sb.size(), size * 4);
  CHECK_EQUAL(std::string(sb.data() + 3, 9), "e be cont");
  sb.pubseekpos(size * 3, std::ios::out);
  sb.sputc('x');
}

} // namespace <anonymous>

TEST(memory-mapped streambuffer aligned resize) {
  auto filename = directory / "aligned";
  aligned_resize_test_impl(filename, detail::page_size());
}

TEST(memory-mapped streambuffer aligned resize large) {
  auto filename = directory / "aligned_large";
  auto page_size = detail::page_size();
  auto hundred_mb = size_t{100 * (1 << 20)};
  auto size = hundred_mb - (hundred_mb % page_size);
  aligned_resize_test_impl(filename, size);
}

FIXTURE_SCOPE_END()
<|endoftext|>


<commit_before>#include "neo.h"
#include "protocol.h"
#include "serial.h"

#include <chrono>
#include <thread>

int32_t neo_get_version(void) { return NEO_VERSION; }
bool neo_is_abi_compatible(void) { return neo_get_version() >> 16u == NEO_VERSION_MAJOR; }

typedef struct neo_error {
  const char* what; // always literal, do not deallocate
} neo_error;

typedef struct neo_device {
  neo::serial::device_s serial; // serial port communication
  bool is_scanning;
} neo_device;

#define NEO_MAX_SAMPLES 4096

typedef struct neo_scan {
  int32_t angle[NEO_MAX_SAMPLES];           // in millidegrees
  int32_t distance[NEO_MAX_SAMPLES];        // in cm
  int32_t signal_strength[NEO_MAX_SAMPLES]; // range 0:255
  int32_t count;
} neo_scan;

// Constructor hidden from users
static neo_error_s neo_error_construct(const char* what) {
  NEO_ASSERT(what);

  auto out = new neo_error{what};
  return out;
}

const char* neo_error_message(neo_error_s error) {
  NEO_ASSERT(error);

  return error->what;
}

void neo_error_destruct(neo_error_s error) {
  NEO_ASSERT(error);

  delete error;
}

neo_device_s neo_device_construct_simple(const char* port, neo_error_s* error) {
  NEO_ASSERT(error);

  return neo_device_construct(port, 115200, error);
}

neo_device_s neo_device_construct(const char* port, int32_t bitrate, neo_error_s* error) {
  NEO_ASSERT(port);
  NEO_ASSERT(bitrate > 0);
  NEO_ASSERT(error);

  neo::serial::error_s serialerror = nullptr;
  neo::serial::device_s serial = neo::serial::device_construct(port, bitrate, &serialerror);

  if (serialerror) {
    *error = neo_error_construct(neo::serial::error_message(serialerror));
    neo::serial::error_destruct(serialerror);
    return nullptr;
  }

  auto out = new neo_device{serial, /*is_scanning=*/true};

  neo_error_s stoperror = nullptr;
  neo_device_stop_scanning(out, &stoperror);

  if (stoperror) {
    *error = stoperror;
    neo_device_destruct(out);
    return nullptr;
  }

  return out;
}

void neo_device_destruct(neo_device_s device) {
  NEO_ASSERT(device);

  neo_error_s ignore = nullptr;
  neo_device_stop_scanning(device, &ignore);
  (void)ignore; // nothing we can do here

  neo::serial::device_destruct(device->serial);

  delete device;
}

void neo_device_start_scanning(neo_device_s device, neo_error_s* error) {
  NEO_ASSERT(device);
  NEO_ASSERT(error);
  NEO_ASSERT(!device->is_scanning);

  if (device->is_scanning)
    return;

  neo::protocol::error_s protocolerror = nullptr;
  neo::protocol::write_command(device->serial, neo::protocol::DATA_ACQUISITION_START, &protocolerror);

  if (protocolerror) {
    *error = neo_error_construct("unable to send start scanning command");
    neo::protocol::error_destruct(protocolerror);
    return;
  }

  neo::protocol::response_header_s response;
  neo::protocol::read_response_header(device->serial, neo::protocol::DATA_ACQUISITION_START, &response, &protocolerror);

  if (protocolerror) {
    *error = neo_error_construct("unable to receive start scanning command response");
    neo::protocol::error_destruct(protocolerror);
    return;
  }

  device->is_scanning = true;
}

void neo_device_stop_scanning(neo_device_s device, neo_error_s* error) {
  NEO_ASSERT(device);
  NEO_ASSERT(error);

  if (!device->is_scanning)
    return;

  neo::protocol::error_s protocolerror = nullptr;
  neo::protocol::write_command(device->serial, neo::protocol::DATA_ACQUISITION_STOP, &protocolerror);

  if (protocolerror) {
    *error = neo_error_construct("unable to send stop scanning command");
    neo::protocol::error_destruct(protocolerror);
    return;
  }

  // Wait until device stopped sending
  std::this_thread::sleep_for(std::chrono::milliseconds(20));

  neo::serial::error_s serialerror = nullptr;
  neo::serial::device_flush(device->serial, &serialerror);

  if (serialerror) {
    *error = neo_error_construct("unable to flush serial device for stopping scanning command");
    neo::serial::error_destruct(serialerror);
    return;
  }

  neo::protocol::write_command(device->serial, neo::protocol::DATA_ACQUISITION_STOP, &protocolerror);

  if (protocolerror) {
    *error = neo_error_construct("unable to send stop scanning command");
    neo::protocol::error_destruct(protocolerror);
    return;
  }

  neo::protocol::response_header_s response;
  neo::protocol::read_response_header(device->serial, neo::protocol::DATA_ACQUISITION_STOP, &response, &protocolerror);

  if (protocolerror) {
    *error = neo_error_construct("unable to receive stop scanning command response");
    neo::protocol::error_destruct(protocolerror);
    return;
  }

  device->is_scanning = false;
}

neo_scan_s neo_device_get_scan(neo_device_s device, neo_error_s* error) {
  NEO_ASSERT(device);
  NEO_ASSERT(error);
  NEO_ASSERT(device->is_scanning);

  neo::protocol::error_s protocolerror = nullptr;

  neo::protocol::response_scan_packet_s responses[NEO_MAX_SAMPLES];

  int32_t received = 0;

  while (received < NEO_MAX_SAMPLES) {
    neo::protocol::read_response_scan(device->serial, &responses[received], &protocolerror);

    if (protocolerror) {
      *error = neo_error_construct("unable to receive neo scan response");
      neo::protocol::error_destruct(protocolerror);
      return nullptr;
    }

    const bool is_sync = responses[received].sync_error & neo::protocol::response_scan_packet_sync::sync;
    const bool has_error = (responses[received].sync_error >> 1) != 0; // shift out sync bit, others are errors

    if (!has_error) {
      received++;
    }

    if (is_sync) {
      break;
    }
  }

  auto out = new neo_scan;

  out->count = received;

  for (int32_t it = 0; it < received; ++it) {
    // Convert angle from compact serial format to float (in degrees).
    // In addition convert from degrees to milli-degrees.
    out->angle[it] = static_cast<int32_t>(neo::protocol::u16_to_f32(responses[it].angle) * 1000.f);
    out->distance[it] = responses[it].distance;
    out->signal_strength[it] = responses[it].signal_strength;
  }

  return out;
}

int32_t neo_scan_get_number_of_samples(neo_scan_s scan) {
  NEO_ASSERT(scan);
  NEO_ASSERT(scan->count >= 0);

  return scan->count;
}

int32_t neo_scan_get_angle(neo_scan_s scan, int32_t sample) {
  NEO_ASSERT(scan);
  NEO_ASSERT(sample >= 0 && sample < scan->count && "sample index out of bounds");

  return scan->angle[sample];
}

int32_t neo_scan_get_distance(neo_scan_s scan, int32_t sample) {
  NEO_ASSERT(scan);
  NEO_ASSERT(sample >= 0 && sample < scan->count && "sample index out of bounds");

  return scan->distance[sample];
}

int32_t neo_scan_get_signal_strength(neo_scan_s scan, int32_t sample) {
  NEO_ASSERT(scan);
  NEO_ASSERT(sample >= 0 && sample < scan->count && "sample index out of bounds");

  return scan->signal_strength[sample];
}

void neo_scan_destruct(neo_scan_s scan) {
  NEO_ASSERT(scan);

  delete scan;
}

int32_t neo_device_get_motor_speed(neo_device_s device, neo_error_s* error) {
  NEO_ASSERT(device);
  NEO_ASSERT(error);
  NEO_ASSERT(!device->is_scanning);

  neo::protocol::error_s protocolerror = nullptr;

  neo::protocol::write_command(device->serial, neo::protocol::MOTOR_INFORMATION, &protocolerror);

  if (protocolerror) {
    *error = neo_error_construct("unable to send motor speed command");
    neo::protocol::error_destruct(protocolerror);
    return 0;
  }

  neo::protocol::response_info_motor_s response;
  neo::protocol::read_response_info_motor(device->serial, neo::protocol::MOTOR_INFORMATION, &response, &protocolerror);

  if (protocolerror) {
    *error = neo_error_construct("unable to receive motor speed command response");
    neo::protocol::error_destruct(protocolerror);
    return 0;
  }

  int32_t speed = neo::protocol::ascii_bytes_to_integral(response.motor_speed);
  NEO_ASSERT(speed >= 0);

  return speed;
}

void neo_device_set_motor_speed(neo_device_s device, int32_t hz, neo_error_s* error) {
  NEO_ASSERT(device);
  NEO_ASSERT(hz >= 0 && hz <= 10);
  NEO_ASSERT(error);
  NEO_ASSERT(!device->is_scanning);

  uint8_t args[2] = {0};
  neo::protocol::integral_to_ascii_bytes(hz, args);

  neo::protocol::error_s protocolerror = nullptr;

  neo::protocol::write_command_with_arguments(device->serial, neo::protocol::MOTOR_SPEED_ADJUST, args, &protocolerror);

  if (protocolerror) {
    *error = neo_error_construct("unable to send motor speed command");
    neo::protocol::error_destruct(protocolerror);
    return;
  }

  neo::protocol::response_param_s response;
  neo::protocol::read_response_param(device->serial, neo::protocol::MOTOR_SPEED_ADJUST, &response, &protocolerror);

  if (protocolerror) {
    *error = neo_error_construct("unable to receive motor speed command response");
    neo::protocol::error_destruct(protocolerror);
    return;
  }
}

int32_t neo_device_get_sample_rate(neo_device_s device, neo_error_s* error) {
  NEO_ASSERT(device);
  NEO_ASSERT(error);
  NEO_ASSERT(!device->is_scanning);

  neo::protocol::error_s protocolerror = nullptr;

  neo::protocol::write_command(device->serial, neo::protocol::SAMPLE_RATE_INFORMATION, &protocolerror);

  if (protocolerror) {
    *error = neo_error_construct("unable to send sample rate command");
    neo::protocol::error_destruct(protocolerror);
    return 0;
  }

  neo::protocol::response_info_sample_rate_s response;
  neo::protocol::read_response_info_sample_rate(device->serial, neo::protocol::SAMPLE_RATE_INFORMATION, &response,
                                                  &protocolerror);

  if (protocolerror) {
    *error = neo_error_construct("unable to receive sample rate command response");
    neo::protocol::error_destruct(protocolerror);
    return 0;
  }

  // 01: 500-600Hz, 02: 750-800Hz, 03: 1000-1050Hz
  int32_t code = neo::protocol::ascii_bytes_to_integral(response.sample_rate);
  int32_t rate = 0;

  switch (code) {
  case 1:
    rate = 500;
    break;
  case 2:
    rate = 750;
    break;
  case 3:
    rate = 1000;
    break;
  default:
    NEO_ASSERT(false && "sample rate code unknown");
  }

  return rate;
}

void neo_device_set_sample_rate(neo_device_s device, int32_t hz, neo_error_s* error) {
  NEO_ASSERT(device);
  NEO_ASSERT(hz == 500 || hz == 750 || hz == 1000);
  NEO_ASSERT(error);
  NEO_ASSERT(!device->is_scanning);

  // 01: 500-600Hz, 02: 750-800Hz, 03: 1000-1050Hz
  int32_t code = 1;

  switch (hz) {
  case 500:
    code = 1;
    break;
  case 750:
    code = 2;
    break;
  case 1000:
    code = 3;
    break;
  default:
    NEO_ASSERT(false && "sample rate unknown");
  }

  uint8_t args[2] = {0};
  neo::protocol::integral_to_ascii_bytes(code, args);

  neo::protocol::error_s protocolerror = nullptr;

  neo::protocol::write_command_with_arguments(device->serial, neo::protocol::SAMPLE_RATE_ADJUST, args, &protocolerror);

  if (protocolerror) {
    *error = neo_error_construct("unable to send sample rate command");
    neo::protocol::error_destruct(protocolerror);
    return;
  }

  neo::protocol::response_param_s response;
  neo::protocol::read_response_param(device->serial, neo::protocol::SAMPLE_RATE_ADJUST, &response, &protocolerror);

  if (protocolerror) {
    *error = neo_error_construct("unable to receive sample rate command response");
    neo::protocol::error_destruct(protocolerror);
    return;
  }
}

void neo_device_reset(neo_device_s device, neo_error_s* error) {
  NEO_ASSERT(device);
  NEO_ASSERT(error);
  NEO_ASSERT(!device->is_scanning);

  neo::protocol::error_s protocolerror = nullptr;

  neo::protocol::write_command(device->serial, neo::protocol::RESET_DEVICE, &protocolerror);

  if (protocolerror) {
    *error = neo_error_construct("unable to send device reset command");
    neo::protocol::error_destruct(protocolerror);
    return;
  }
}
<commit_msg>fix(the angle of the last one is large than the first one). 1. I' the data is not sort in ascending order, it's confusion; 2. II & III scan data the last data is small than the first(in angle), and this change is to fix it.<commit_after>#include "neo.h"
#include "protocol.h"
#include "serial.h"

#include <chrono>
#include <thread>

int32_t neo_get_version(void) { return NEO_VERSION; }
bool neo_is_abi_compatible(void) { return neo_get_version() >> 16u == NEO_VERSION_MAJOR; }

typedef struct neo_error {
  const char* what; // always literal, do not deallocate
} neo_error;

typedef struct neo_device {
  neo::serial::device_s serial; // serial port communication
  bool is_scanning;
} neo_device;

#define NEO_MAX_SAMPLES 4096

typedef struct neo_scan {
  int32_t angle[NEO_MAX_SAMPLES];           // in millidegrees
  int32_t distance[NEO_MAX_SAMPLES];        // in cm
  int32_t signal_strength[NEO_MAX_SAMPLES]; // range 0:255
  int32_t count;
} neo_scan;

// Constructor hidden from users
static neo_error_s neo_error_construct(const char* what) {
  NEO_ASSERT(what);

  auto out = new neo_error{what};
  return out;
}

const char* neo_error_message(neo_error_s error) {
  NEO_ASSERT(error);

  return error->what;
}

void neo_error_destruct(neo_error_s error) {
  NEO_ASSERT(error);

  delete error;
}

neo_device_s neo_device_construct_simple(const char* port, neo_error_s* error) {
  NEO_ASSERT(error);

  return neo_device_construct(port, 115200, error);
}

neo_device_s neo_device_construct(const char* port, int32_t bitrate, neo_error_s* error) {
  NEO_ASSERT(port);
  NEO_ASSERT(bitrate > 0);
  NEO_ASSERT(error);

  neo::serial::error_s serialerror = nullptr;
  neo::serial::device_s serial = neo::serial::device_construct(port, bitrate, &serialerror);

  if (serialerror) {
    *error = neo_error_construct(neo::serial::error_message(serialerror));
    neo::serial::error_destruct(serialerror);
    return nullptr;
  }

  auto out = new neo_device{serial, /*is_scanning=*/true};

  neo_error_s stoperror = nullptr;
  neo_device_stop_scanning(out, &stoperror);

  if (stoperror) {
    *error = stoperror;
    neo_device_destruct(out);
    return nullptr;
  }

  return out;
}

void neo_device_destruct(neo_device_s device) {
  NEO_ASSERT(device);

  neo_error_s ignore = nullptr;
  neo_device_stop_scanning(device, &ignore);
  (void)ignore; // nothing we can do here

  neo::serial::device_destruct(device->serial);

  delete device;
}

void neo_device_start_scanning(neo_device_s device, neo_error_s* error) {
  NEO_ASSERT(device);
  NEO_ASSERT(error);
  NEO_ASSERT(!device->is_scanning);

  if (device->is_scanning)
    return;

  neo::protocol::error_s protocolerror = nullptr;
  neo::protocol::write_command(device->serial, neo::protocol::DATA_ACQUISITION_START, &protocolerror);

  if (protocolerror) {
    *error = neo_error_construct("unable to send start scanning command");
    neo::protocol::error_destruct(protocolerror);
    return;
  }

  neo::protocol::response_header_s response;
  neo::protocol::read_response_header(device->serial, neo::protocol::DATA_ACQUISITION_START, &response, &protocolerror);

  if (protocolerror) {
    *error = neo_error_construct("unable to receive start scanning command response");
    neo::protocol::error_destruct(protocolerror);
    return;
  }

  device->is_scanning = true;
}

void neo_device_stop_scanning(neo_device_s device, neo_error_s* error) {
  NEO_ASSERT(device);
  NEO_ASSERT(error);

  if (!device->is_scanning)
    return;

  neo::protocol::error_s protocolerror = nullptr;
  neo::protocol::write_command(device->serial, neo::protocol::DATA_ACQUISITION_STOP, &protocolerror);

  if (protocolerror) {
    *error = neo_error_construct("unable to send stop scanning command");
    neo::protocol::error_destruct(protocolerror);
    return;
  }

  // Wait until device stopped sending
  std::this_thread::sleep_for(std::chrono::milliseconds(20));

  neo::serial::error_s serialerror = nullptr;
  neo::serial::device_flush(device->serial, &serialerror);

  if (serialerror) {
    *error = neo_error_construct("unable to flush serial device for stopping scanning command");
    neo::serial::error_destruct(serialerror);
    return;
  }

  neo::protocol::write_command(device->serial, neo::protocol::DATA_ACQUISITION_STOP, &protocolerror);

  if (protocolerror) {
    *error = neo_error_construct("unable to send stop scanning command");
    neo::protocol::error_destruct(protocolerror);
    return;
  }

  neo::protocol::response_header_s response;
  neo::protocol::read_response_header(device->serial, neo::protocol::DATA_ACQUISITION_STOP, &response, &protocolerror);

  if (protocolerror) {
    *error = neo_error_construct("unable to receive stop scanning command response");
    neo::protocol::error_destruct(protocolerror);
    return;
  }

  device->is_scanning = false;
}

neo_scan_s neo_device_get_scan(neo_device_s device, neo_error_s* error) {
  NEO_ASSERT(device);
  NEO_ASSERT(error);
  NEO_ASSERT(device->is_scanning);

  neo::protocol::error_s protocolerror = nullptr;

  neo::protocol::response_scan_packet_s responses[NEO_MAX_SAMPLES];

  int32_t received = 1;

  while (received < NEO_MAX_SAMPLES) {
    neo::protocol::read_response_scan(device->serial, &responses[received], &protocolerror);

    if (protocolerror) {
      *error = neo_error_construct("unable to receive neo scan response");
      neo::protocol::error_destruct(protocolerror);
      return nullptr;
    }

    const bool is_sync = responses[received].sync_error & neo::protocol::response_scan_packet_sync::sync;
    const bool has_error = (responses[received].sync_error >> 1) != 0; // shift out sync bit, others are errors

    if (!has_error) {
      received++;
    }

    if (is_sync) {
      responses[0] = responses[received];
      break;
    }
  }

  auto out = new neo_scan;

  out->count = received - 1;

  for (int32_t it = 0; it < received; ++it) {
    // Convert angle from compact serial format to float (in degrees).
    // In addition convert from degrees to milli-degrees.
    out->angle[it] = static_cast<int32_t>(neo::protocol::u16_to_f32(responses[it].angle) * 1000.f);
    out->distance[it] = responses[it].distance;
    out->signal_strength[it] = responses[it].signal_strength;
  }

  return out;
}

int32_t neo_scan_get_number_of_samples(neo_scan_s scan) {
  NEO_ASSERT(scan);
  NEO_ASSERT(scan->count >= 0);

  return scan->count;
}

int32_t neo_scan_get_angle(neo_scan_s scan, int32_t sample) {
  NEO_ASSERT(scan);
  NEO_ASSERT(sample >= 0 && sample < scan->count && "sample index out of bounds");

  return scan->angle[sample];
}

int32_t neo_scan_get_distance(neo_scan_s scan, int32_t sample) {
  NEO_ASSERT(scan);
  NEO_ASSERT(sample >= 0 && sample < scan->count && "sample index out of bounds");

  return scan->distance[sample];
}

int32_t neo_scan_get_signal_strength(neo_scan_s scan, int32_t sample) {
  NEO_ASSERT(scan);
  NEO_ASSERT(sample >= 0 && sample < scan->count && "sample index out of bounds");

  return scan->signal_strength[sample];
}

void neo_scan_destruct(neo_scan_s scan) {
  NEO_ASSERT(scan);

  delete scan;
}

int32_t neo_device_get_motor_speed(neo_device_s device, neo_error_s* error) {
  NEO_ASSERT(device);
  NEO_ASSERT(error);
  NEO_ASSERT(!device->is_scanning);

  neo::protocol::error_s protocolerror = nullptr;

  neo::protocol::write_command(device->serial, neo::protocol::MOTOR_INFORMATION, &protocolerror);

  if (protocolerror) {
    *error = neo_error_construct("unable to send motor speed command");
    neo::protocol::error_destruct(protocolerror);
    return 0;
  }

  neo::protocol::response_info_motor_s response;
  neo::protocol::read_response_info_motor(device->serial, neo::protocol::MOTOR_INFORMATION, &response, &protocolerror);

  if (protocolerror) {
    *error = neo_error_construct("unable to receive motor speed command response");
    neo::protocol::error_destruct(protocolerror);
    return 0;
  }

  int32_t speed = neo::protocol::ascii_bytes_to_integral(response.motor_speed);
  NEO_ASSERT(speed >= 0);

  return speed;
}

void neo_device_set_motor_speed(neo_device_s device, int32_t hz, neo_error_s* error) {
  NEO_ASSERT(device);
  NEO_ASSERT(hz >= 0 && hz <= 10);
  NEO_ASSERT(error);
  NEO_ASSERT(!device->is_scanning);

  uint8_t args[2] = {0};
  neo::protocol::integral_to_ascii_bytes(hz, args);

  neo::protocol::error_s protocolerror = nullptr;

  neo::protocol::write_command_with_arguments(device->serial, neo::protocol::MOTOR_SPEED_ADJUST, args, &protocolerror);

  if (protocolerror) {
    *error = neo_error_construct("unable to send motor speed command");
    neo::protocol::error_destruct(protocolerror);
    return;
  }

  neo::protocol::response_param_s response;
  neo::protocol::read_response_param(device->serial, neo::protocol::MOTOR_SPEED_ADJUST, &response, &protocolerror);

  if (protocolerror) {
    *error = neo_error_construct("unable to receive motor speed command response");
    neo::protocol::error_destruct(protocolerror);
    return;
  }
}

int32_t neo_device_get_sample_rate(neo_device_s device, neo_error_s* error) {
  NEO_ASSERT(device);
  NEO_ASSERT(error);
  NEO_ASSERT(!device->is_scanning);

  neo::protocol::error_s protocolerror = nullptr;

  neo::protocol::write_command(device->serial, neo::protocol::SAMPLE_RATE_INFORMATION, &protocolerror);

  if (protocolerror) {
    *error = neo_error_construct("unable to send sample rate command");
    neo::protocol::error_destruct(protocolerror);
    return 0;
  }

  neo::protocol::response_info_sample_rate_s response;
  neo::protocol::read_response_info_sample_rate(device->serial, neo::protocol::SAMPLE_RATE_INFORMATION, &response,
                                                  &protocolerror);

  if (protocolerror) {
    *error = neo_error_construct("unable to receive sample rate command response");
    neo::protocol::error_destruct(protocolerror);
    return 0;
  }

  // 01: 500-600Hz, 02: 750-800Hz, 03: 1000-1050Hz
  int32_t code = neo::protocol::ascii_bytes_to_integral(response.sample_rate);
  int32_t rate = 0;

  switch (code) {
  case 1:
    rate = 500;
    break;
  case 2:
    rate = 750;
    break;
  case 3:
    rate = 1000;
    break;
  default:
    NEO_ASSERT(false && "sample rate code unknown");
  }

  return rate;
}

void neo_device_set_sample_rate(neo_device_s device, int32_t hz, neo_error_s* error) {
  NEO_ASSERT(device);
  NEO_ASSERT(hz == 500 || hz == 750 || hz == 1000);
  NEO_ASSERT(error);
  NEO_ASSERT(!device->is_scanning);

  // 01: 500-600Hz, 02: 750-800Hz, 03: 1000-1050Hz
  int32_t code = 1;

  switch (hz) {
  case 500:
    code = 1;
    break;
  case 750:
    code = 2;
    break;
  case 1000:
    code = 3;
    break;
  default:
    NEO_ASSERT(false && "sample rate unknown");
  }

  uint8_t args[2] = {0};
  neo::protocol::integral_to_ascii_bytes(code, args);

  neo::protocol::error_s protocolerror = nullptr;

  neo::protocol::write_command_with_arguments(device->serial, neo::protocol::SAMPLE_RATE_ADJUST, args, &protocolerror);

  if (protocolerror) {
    *error = neo_error_construct("unable to send sample rate command");
    neo::protocol::error_destruct(protocolerror);
    return;
  }

  neo::protocol::response_param_s response;
  neo::protocol::read_response_param(device->serial, neo::protocol::SAMPLE_RATE_ADJUST, &response, &protocolerror);

  if (protocolerror) {
    *error = neo_error_construct("unable to receive sample rate command response");
    neo::protocol::error_destruct(protocolerror);
    return;
  }
}

void neo_device_reset(neo_device_s device, neo_error_s* error) {
  NEO_ASSERT(device);
  NEO_ASSERT(error);
  NEO_ASSERT(!device->is_scanning);

  neo::protocol::error_s protocolerror = nullptr;

  neo::protocol::write_command(device->serial, neo::protocol::RESET_DEVICE, &protocolerror);

  if (protocolerror) {
    *error = neo_error_construct("unable to send device reset command");
    neo::protocol::error_destruct(protocolerror);
    return;
  }
}
<|endoftext|>


<commit_before>/* Copyright (C) 2013, David Eklov
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <iostream>
#include <cstring>

#include "gdbserver.hh"

using namespace std;
using namespace gdb;

class FakeARMv7Context : public Context {
public:
    FakeARMv7Context()
    {
#define ADDR_START  0x8394
#define ADDR_TARGET 0x83a0
#define ADDR_BRANCH 0x83b0
        static char mem_[] = {
            0x04, 0xb0, 0x2d, 0xe5,  /* 8394:  push  {fp}            */
            0x00, 0xb0, 0x8d, 0xe2,  /* 8398:  add   fp, sp, #0      */
            0x14, 0xd0, 0x4d, 0xe2,  /* 839c:  sub   sp, sp, #20     */
            0x08, 0x20, 0x1b, 0xe5,  /* 83a0:  ldr   r2, [fp, #-8]   */
            0x0c, 0x30, 0x1b, 0xe5,  /* 83a4:  ldr   r3, [fp, #-12]  */
            0x03, 0x30, 0x82, 0xe0,  /* 83a8:  add   r3, r2, r3      */
            0x10, 0x30, 0x0b, 0x50,  /* 83ac:  str   r3, [fp, #-16]  */
            0xfa, 0xff, 0xff, 0xea,  /* 83b0:  b     83a0            */
        };
        mem = mem_;

        regs[ARMv7_REG_PC] = ADDR_START;
    }

    void rd_reg(int reg_no)
    {
        put_reg(regs[reg_no]);
    }

    void rd_mem(uint64_t addr)
    {
        if (ADDR_START <= addr && addr < ADDR_BRANCH) {
            addr -= ADDR_START;
            put_mem(mem[addr]);
        } else
            put_mem(0);
    }

    int num_regs(void)
    {
        return ARMv7_NUM_REGS;
    }

public:
    uint32_t regs[ARMv7_NUM_REGS];
    char *mem;
};

int
main(int argc, char **argv)
{
    FakeARMv7Context *ctx = new FakeARMv7Context();
    context_ptr ctx_ptr = context_ptr(ctx);

    Server server(ctx_ptr);

    try {
        uint64_t pc = ADDR_START;

        do {
            ctx->mem[ARMv7_REG_PC] = pc;
            server.update(pc);
            pc += 4;
            if (pc == ADDR_BRANCH)
                pc = ADDR_TARGET;
        } while (1);

    } catch (gdb::exception &e) {
        cerr << e.what() << endl;
        exit(EXIT_FAILURE);
    }

    return 0;
}
<commit_msg>Fixed errors in the memory emulation of the test program<commit_after>/* Copyright (C) 2013, David Eklov
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <iostream>
#include <cstring>

#include "gdbserver.hh"

using namespace std;
using namespace gdb;

class FakeARMv7Context : public Context {
public:
    FakeARMv7Context()
    {
#define ADDR_START  (0x8394)
#define ADDR_TARGET (0x83a0)
#define ADDR_BRANCH (0x83b0 + 4)
        static char mem_[] = {
            0x04, 0xb0, 0x2d, 0xe5,  /* 8394:  push  {fp}            */
            0x00, 0xb0, 0x8d, 0xe2,  /* 8398:  add   fp, sp, #0      */
            0x14, 0xd0, 0x4d, 0xe2,  /* 839c:  sub   sp, sp, #20     */
            0x08, 0x20, 0x1b, 0xe5,  /* 83a0:  ldr   r2, [fp, #-8]   */
            0x0c, 0x30, 0x1b, 0xe5,  /* 83a4:  ldr   r3, [fp, #-12]  */
            0x03, 0x30, 0x82, 0xe0,  /* 83a8:  add   r3, r2, r3      */
            0x10, 0x30, 0x0b, 0xe5,  /* 83ac:  str   r3, [fp, #-16]  */
            0xfa, 0xff, 0xff, 0xea,  /* 83b0:  b     83a0            */
        };
        mem = mem_;

        regs[ARMv7_REG_PC] = ADDR_START;
    }

    void rd_reg(int reg_no)
    {
        put_reg(regs[reg_no]);
    }

    void rd_mem(uint64_t addr)
    {
        if (ADDR_START <= addr && addr <= ADDR_BRANCH) {
            addr -= ADDR_START;
            put_mem(mem[addr]);
        } else
            put_mem(0);
    }

    int num_regs(void)
    {
        return ARMv7_NUM_REGS;
    }

public:
    uint32_t regs[ARMv7_NUM_REGS];
    char *mem;
};

int
main(int argc, char **argv)
{
    FakeARMv7Context *ctx = new FakeARMv7Context();
    context_ptr ctx_ptr = context_ptr(ctx);

    Server server(ctx_ptr);

    try {
        uint64_t pc = ADDR_START;

        do {
            ctx->regs[ARMv7_REG_PC] = pc;
            server.update(pc);
            pc += 4;
            if (pc == ADDR_BRANCH)
                pc = ADDR_TARGET;
        } while (1);

    } catch (gdb::exception &e) {
        cerr << e.what() << endl;
        exit(EXIT_FAILURE);
    }

    return 0;
}
<|endoftext|>


<commit_before>/*=========================================================================
*
* Copyright Marius Staring, Stefan Klein, David Doria. 2011.
*
* 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.txt
*
* 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.
*
*=========================================================================*/
/** \file
 \brief Reshape an image.

 \verbinclude reshape.help
 */
#include "itkCommandLineArgumentParser.h"
#include "CommandLineArgumentHelper.h"

#include "itkImageFileReader.h"
#include "itkReshapeImageToImageFilter.h"
#include "itkImageFileWriter.h"
#include <itksys/SystemTools.hxx>
//-------------------------------------------------------------------------------------

/** run: A macro to call a function. */
#define run(function,type,dim) \
if ( ComponentTypeIn == #type && Dimension == dim ) \
{ \
  typedef itk::Image< type, dim > ImageType; \
  function< ImageType >( inputFilename, outputFilename, outputSize ); \
  supported = true; \
}

//-------------------------------------------------------------------------------------

/* Declare PerformPCA. */
template< class ImageType >
void Reshape(
  const std::string & inputFileName,
  const std::string & outputFileName,
  const std::vector<unsigned long> & outputSize );

/** Declare other functions. */
std::string GetHelpString( void );

//-------------------------------------------------------------------------------------

int main( int argc, char **argv )
{
  /** Create a command line argument parser. */
  itk::CommandLineArgumentParser::Pointer parser = itk::CommandLineArgumentParser::New();
  parser->SetCommandLineArguments( argc, argv );
  parser->SetProgramHelpText( GetHelpString() );

  parser->MarkArgumentAsRequired( "-in", "The input filename." );
  parser->MarkArgumentAsRequired( "-s", "Output size." );

  itk::CommandLineArgumentParser::ReturnValue validateArguments = parser->CheckForRequiredArguments();

  if(validateArguments == itk::CommandLineArgumentParser::FAILED)
  {
    return EXIT_FAILURE;
  }
  else if(validateArguments == itk::CommandLineArgumentParser::HELPREQUESTED)
  {
    return EXIT_SUCCESS;
  }

  /** Get arguments. */
  std::string inputFilename = "";
  parser->GetCommandLineArgument( "-in", inputFilename );

  std::string base = itksys::SystemTools::GetFilenameWithoutLastExtension(
    inputFilename );
  std::string ext  = itksys::SystemTools::GetFilenameLastExtension(
    inputFilename );
  std::string outputFilename = base + "_reshaped" + ext;
  parser->GetCommandLineArgument( "-out", outputFilename );

  std::vector<unsigned long> outputSize;
  parser->GetCommandLineArgument( "-s", outputSize );

  /** Determine image properties. */
  std::string ComponentTypeIn = "short";
  std::string PixelType; //we don't use this
  unsigned int Dimension = 3;
  unsigned int NumberOfComponents = 1;
  std::vector<unsigned int> inputSize( Dimension, 0 );
  int retgip = GetImageProperties(
    inputFilename,
    PixelType,
    ComponentTypeIn,
    Dimension,
    NumberOfComponents,
    inputSize );
  if ( retgip != 0 )
  {
    return 1;
  }

  /** Check for vector images. */
  if ( NumberOfComponents > 1 )
  {
    std::cerr << "ERROR: The NumberOfComponents is larger than 1!" << std::endl;
    std::cerr << "  Vector images are not supported." << std::endl;
    return 1;
  }

  /** Get rid of the possible "_" in ComponentType. */
  ReplaceUnderscoreWithSpace( ComponentTypeIn );

  /** Check dimensions. */
  if ( inputSize.size() != outputSize.size() )
  {
    std::cerr << "ERROR: input and output dimension should be the same.\n";
    std::cerr << "  Please, specify only " << Dimension
      << "numbers with \"-s\"." << std::endl;
    return 1;
  }

  /** Run the program. */
  bool supported = false;
  try
  {
    run( Reshape, unsigned char, 2 );
    run( Reshape, char, 2 );
    run( Reshape, unsigned short, 2 );
    run( Reshape, short, 2 );
    run( Reshape, unsigned int, 2 );
    run( Reshape, int, 2 );
    run( Reshape, unsigned long, 2 );
    run( Reshape, long, 2 );
    run( Reshape, float, 2 );
    run( Reshape, double, 2 );

    /*run( Reshape, unsigned char, 3 );
    run( Reshape, char, 3 );
    run( Reshape, unsigned short, 3 );
    run( Reshape, short, 3 );
    run( Reshape, unsigned int, 3 );
    run( Reshape, int, 3 );
    run( Reshape, unsigned long, 3 );
    run( Reshape, long, 3 );
    run( Reshape, float, 3 );
    run( Reshape, double, 3 );*/
  }
  catch ( itk::ExceptionObject & e )
  {
    std::cerr << "Caught ITK exception: " << e << std::endl;
    return 1;
  }
  if ( !supported )
  {
    std::cerr << "ERROR: this combination of pixeltype and dimension is not supported!" << std::endl;
    std::cerr
      << "pixel (component) type = " << ComponentTypeIn
      << " ; dimension = " << Dimension
      << std::endl;
    return 1;
  }

  /** End program. */
  return 0;

} // end main()

/*
 * ******************* Reshape *******************
 */

template< class ImageType >
void Reshape(
  const std::string & inputFilename,
  const std::string & outputFilename,
  const std::vector<unsigned long> & outputSize )
{
  /** Typedefs. */
  typedef itk::ImageFileReader< ImageType >           ReaderType;
  typedef itk::ReshapeImageToImageFilter< ImageType > ReshapeFilterType;
  typedef itk::ImageFileWriter< ImageType >           WriterType;
  typedef typename ReshapeFilterType::SizeType        SizeType;

  /** Translate vector to SizeType. */
  SizeType size;
  for ( unsigned int i = 0; i < outputSize.size(); ++i )
  {
    size[ i ] = outputSize[ i ];
  }

  /** Reader. */
  typename ReaderType::Pointer reader = ReaderType::New();
  reader->SetFileName( inputFilename.c_str() );

  /** Reshaper. */
  typename ReshapeFilterType::Pointer reshaper = ReshapeFilterType::New();
  reshaper->SetInput( reader->GetOutput() );
  reshaper->SetOutputSize( size );
  reshaper->Update();

  /** Writer. */
  typename WriterType::Pointer writer = WriterType::New();
  writer->SetFileName( outputFilename.c_str() );
  writer->SetInput( reshaper->GetOutput() );
  writer->Update();

} // end Reshape()

/**
 * ******************* GetHelpString *******************
 */

std::string GetHelpString( void )
{
  std::stringstream ss;
  ss << "Usage:" << std::endl
  << "pxpca" << std::endl
  << "  -in      inputFilename" << std::endl
  << "  [-out]   outputFileName, default inputFileName_reshaped" << std::endl
  << "  -s       size of the output image" << std::endl
  << "Supported: 2D, 3D, (unsigned) char, (unsigned) short, (unsigned) int, (unsigned) long, float, double.";

  return ss.str();

} // end GetHelpString()

<commit_msg>ENH: converted RescaleIntensityImageFilter to template<commit_after>/*=========================================================================
*
* Copyright Marius Staring, Stefan Klein, David Doria. 2011.
*
* 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.txt
*
* 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.
*
*=========================================================================*/
/** \file
 \brief Reshape an image.

 \verbinclude reshape.help
 */
#include "itkCommandLineArgumentParser.h"
#include "CommandLineArgumentHelper.h"

#include "itkImageFileReader.h"
#include "itkReshapeImageToImageFilter.h"
#include "itkImageFileWriter.h"
#include <itksys/SystemTools.hxx>
//-------------------------------------------------------------------------------------

class ReshapeBase : public itktools::ITKToolsBase
{
public:
  ReshapeBase(){};
  ~ReshapeBase(){};

  /** Input parameters */
  std::string m_InputFileName;
  std::string m_OutputFileName;
  std::vector<unsigned long> m_OutputSize;

}; // end ReshapeBase

template< class TComponentType, unsigned int VDimension >
class Reshape : public ReshapeBase
{
public:
  typedef Reshape Self;

  Reshape(){};
  ~Reshape(){};

  static Self * New( itktools::EnumComponentType componentType, unsigned int dim )
  {
    if ( itktools::IsType<TComponentType>( componentType ) && VDimension == dim )
    {
      return new Self;
    }
    return 0;
  }

  void Run(void)
  {
    /** Typedefs. */
    typedef itk::Image<TComponentType, VDimension>      ImageType;
    typedef itk::ImageFileReader< ImageType >           ReaderType;
    typedef itk::ReshapeImageToImageFilter< ImageType > ReshapeFilterType;
    typedef itk::ImageFileWriter< ImageType >           WriterType;
    typedef typename ReshapeFilterType::SizeType        SizeType;

    /** Translate vector to SizeType. */
    SizeType size;
    for ( unsigned int i = 0; i < m_OutputSize.size(); ++i )
    {
      size[ i ] = m_OutputSize[ i ];
    }

    /** Reader. */
    typename ReaderType::Pointer reader = ReaderType::New();
    reader->SetFileName( m_InputFileName.c_str() );

    /** Reshaper. */
    typename ReshapeFilterType::Pointer reshaper = ReshapeFilterType::New();
    reshaper->SetInput( reader->GetOutput() );
    reshaper->SetOutputSize( size );
    reshaper->Update();

    /** Writer. */
    typename WriterType::Pointer writer = WriterType::New();
    writer->SetFileName( m_OutputFileName.c_str() );
    writer->SetInput( reshaper->GetOutput() );
    writer->Update();
  }

}; // end Reshape

//-------------------------------------------------------------------------------------

/** Declare other functions. */
std::string GetHelpString( void );

//-------------------------------------------------------------------------------------

int main( int argc, char **argv )
{
  /** Create a command line argument parser. */
  itk::CommandLineArgumentParser::Pointer parser = itk::CommandLineArgumentParser::New();
  parser->SetCommandLineArguments( argc, argv );
  parser->SetProgramHelpText( GetHelpString() );

  parser->MarkArgumentAsRequired( "-in", "The input filename." );
  parser->MarkArgumentAsRequired( "-s", "Output size." );

  itk::CommandLineArgumentParser::ReturnValue validateArguments = parser->CheckForRequiredArguments();

  if(validateArguments == itk::CommandLineArgumentParser::FAILED)
  {
    return EXIT_FAILURE;
  }
  else if(validateArguments == itk::CommandLineArgumentParser::HELPREQUESTED)
  {
    return EXIT_SUCCESS;
  }

  /** Get arguments. */
  std::string inputFilename = "";
  parser->GetCommandLineArgument( "-in", inputFilename );

  std::string base = itksys::SystemTools::GetFilenameWithoutLastExtension(
    inputFilename );
  std::string ext  = itksys::SystemTools::GetFilenameLastExtension(
    inputFilename );
  std::string outputFilename = base + "_reshaped" + ext;
  parser->GetCommandLineArgument( "-out", outputFilename );

  std::vector<unsigned long> outputSize;
  parser->GetCommandLineArgument( "-s", outputSize );

  /** Determine image properties. */
  std::string ComponentTypeIn = "short";
  std::string PixelType; //we don't use this
  unsigned int Dimension = 3;
  unsigned int NumberOfComponents = 1;
  std::vector<unsigned int> inputSize( Dimension, 0 );
  int retgip = GetImageProperties(
    inputFilename,
    PixelType,
    ComponentTypeIn,
    Dimension,
    NumberOfComponents,
    inputSize );
  if ( retgip != 0 )
  {
    return 1;
  }

  /** Check for vector images. */
  if ( NumberOfComponents > 1 )
  {
    std::cerr << "ERROR: The NumberOfComponents is larger than 1!" << std::endl;
    std::cerr << "  Vector images are not supported." << std::endl;
    return 1;
  }

  /** Get rid of the possible "_" in ComponentType. */
  ReplaceUnderscoreWithSpace( ComponentTypeIn );

  /** Check dimensions. */
  if ( inputSize.size() != outputSize.size() )
  {
    std::cerr << "ERROR: input and output dimension should be the same.\n";
    std::cerr << "  Please, specify only " << Dimension
      << "numbers with \"-s\"." << std::endl;
    return 1;
  }

  /** Class that does the work */
  ReshapeBase * reshape = 0;

  /** Short alias */
  unsigned int imageDimension = Dimension;

  /** \todo some progs allow user to override the pixel type,
   * so we need a method to convert string to EnumComponentType */
  itktools::EnumComponentType componentType = itktools::GetImageComponentType(inputFilename);

  std::cout << "Detected component type: " <<
    componentType << std::endl;

  try
  {
    if (!reshape) reshape = Reshape< unsigned char, 2 >::New( componentType, imageDimension );
    if (!reshape) reshape = Reshape< char, 2 >::New( componentType, imageDimension );
    if (!reshape) reshape = Reshape< unsigned short, 2 >::New( componentType, imageDimension );
    if (!reshape) reshape = Reshape< short, 2 >::New( componentType, imageDimension );
    if (!reshape) reshape = Reshape< unsigned int, 2 >::New( componentType, imageDimension );
    if (!reshape) reshape = Reshape< int, 2 >::New( componentType, imageDimension );
    if (!reshape) reshape = Reshape< unsigned long, 2 >::New( componentType, imageDimension );
    if (!reshape) reshape = Reshape< long, 2 >::New( componentType, imageDimension );
    if (!reshape) reshape = Reshape< float, 2 >::New( componentType, imageDimension );
    if (!reshape) reshape = Reshape< double, 2 >::New( componentType, imageDimension );
#ifdef ITKTOOLS_3D_SUPPORT
    if (!reshape) reshape = Reshape< unsigned char, 3 >::New( componentType, imageDimension );
    if (!reshape) reshape = Reshape< char, 3 >::New( componentType, imageDimension );
    if (!reshape) reshape = Reshape< unsigned short, 3 >::New( componentType, imageDimension );
    if (!reshape) reshape = Reshape< short, 3 >::New( componentType, imageDimension );
    if (!reshape) reshape = Reshape< unsigned int, 3 >::New( componentType, imageDimension );
    if (!reshape) reshape = Reshape< int, 3 >::New( componentType, imageDimension );
    if (!reshape) reshape = Reshape< unsigned long, 3 >::New( componentType, imageDimension );
    if (!reshape) reshape = Reshape< long, 3 >::New( componentType, imageDimension );
    if (!reshape) reshape = Reshape< float, 3 >::New( componentType, imageDimension );
    if (!reshape) reshape = Reshape< double, 3 >::New( componentType, imageDimension );
#endif
    if (!reshape)
    {
      std::cerr << "ERROR: this combination of pixeltype and dimension is not supported!" << std::endl;
      std::cerr
        << "pixel (component) type = " << componentType
        << " ; dimension = " << Dimension
        << std::endl;
      return 1;
    }

    reshape->m_InputFileName = inputFilename;
    reshape->m_OutputFileName = outputFilename;
    reshape->m_OutputSize = outputSize;

    reshape->Run();

    delete reshape;
  }
  catch( itk::ExceptionObject &e )
  {
    std::cerr << "Caught ITK exception: " << e << std::endl;
    delete reshape;
    return 1;
  }

  /** End program. */
  return 0;

} // end main()

/**
 * ******************* GetHelpString *******************
 */

std::string GetHelpString( void )
{
  std::stringstream ss;
  ss << "Usage:" << std::endl
  << "pxpca" << std::endl
  << "  -in      inputFilename" << std::endl
  << "  [-out]   outputFileName, default inputFileName_reshaped" << std::endl
  << "  -s       size of the output image" << std::endl
  << "Supported: 2D, 3D, (unsigned) char, (unsigned) short, (unsigned) int, (unsigned) long, float, double.";

  return ss.str();

} // end GetHelpString()

<|endoftext|>


<commit_before>/*
 * Adplug - Replayer for many OPL2/OPL3 audio file formats.
 * Copyright (C) 1999 - 2004 Simon Peter, <dn.tlp@gmx.net>, et al.
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 *
 * hsc.cpp - HSC Player by Simon Peter <dn.tlp@gmx.net>
 */

#include <string.h>

#include "hsc.h"
#include "debug.h"

/*** public methods **************************************/

CPlayer *ChscPlayer::factory(Copl *newopl)
{
  return new ChscPlayer(newopl);
}

bool ChscPlayer::load(const std::string &filename, const CFileProvider &fp)
{
  binistream	*f = fp.open(filename);
  int		i;

  // file validation section
  if(!f || !fp.extension(filename, ".hsc") || fp.filesize(f) > 59187) {
    AdPlug_LogWrite("ChscPlayer::load(\"%s\"): Not a HSC file!\n", filename.c_str());
    fp.close(f);
    return false;
  }

  // load section
  for(i=0;i<128*12;i++)		// load instruments
    *((unsigned char *)instr + i) = f->readInt(1);
  for (i=0;i<128;i++) {			// correct instruments
    instr[i][2] ^= (instr[i][2] & 0x40) << 1;
    instr[i][3] ^= (instr[i][3] & 0x40) << 1;
    instr[i][11] >>= 4;			// slide
  }
  for(i=0;i<51;i++) song[i] = f->readInt(1);	// load tracklist
  for(i=0;i<50*64*9;i++)			// load patterns
    *((char *)patterns + i) = f->readInt(1);

  fp.close(f);
  rewind(0);					// rewind module
  return true;
}

bool ChscPlayer::update()
{
  // general vars
  unsigned char		chan,pattnr,note,effect,eff_op,inst,vol,Okt,db;
  unsigned short	Fnr;
  unsigned long		pattoff;

  del--;                      // player speed handling
  if(del)
    return !songend;		// nothing done

  if(fadein)					// fade-in handling
    fadein--;

  pattnr = song[songpos];
  // 0xff indicates song end, but this prevents a crash for some songs that
  // use other weird values, like 0xbf
  if(pattnr >= 0xb2) {			// arrangement handling
    songend = 1;				// set end-flag
    songpos = 0;
    pattnr = song[songpos];
  } else
    if ((pattnr & 128) && (pattnr <= 0xb1)) { // goto pattern "nr"
      songpos = song[songpos] & 127;
      pattpos = 0;
      pattnr = song[songpos];
      songend = 1;
    }

  pattoff = pattpos*9;
  for (chan=0;chan<9;chan++) {			// handle all channels
    note = patterns[pattnr][pattoff].note;
    effect = patterns[pattnr][pattoff].effect;
    pattoff++;

    if(note & 128) {                    // set instrument
      setinstr(chan,effect);
      continue;
    }
    eff_op = effect & 0x0f;
    inst = channel[chan].inst;
    if(note)
      channel[chan].slide = 0;

    switch (effect & 0xf0) {			// effect handling
    case 0:								// global effect
      /* The following fx are unimplemented on purpose:
       * 02 - Slide Mainvolume up
       * 03 - Slide Mainvolume down (here: fade in)
       * 04 - Set Mainvolume to 0
       *
       * This is because i've never seen any HSC modules using the fx this way.
       * All modules use the fx the way, i've implemented it.
       */
      switch(eff_op) {
      case 1: pattbreak++; break;	// jump to next pattern
      case 3: fadein = 31; break;	// fade in (divided by 2)
      case 5: mode6 = 1; break;	// 6 voice mode on
      case 6: mode6 = 0; break;	// 6 voice mode off
      }
      break;
    case 0x20:
    case 0x10:		                    // manual slides
      if (effect & 0x10) {
	channel[chan].freq += eff_op;
	channel[chan].slide += eff_op;
      } else {
	channel[chan].freq -= eff_op;
	channel[chan].slide -= eff_op;
      }
      if(!note)
	setfreq(chan,channel[chan].freq);
      break;
    case 0x50:							// set percussion instrument (unimplemented)
      break;
    case 0x60:							// set feedback
      opl->write(0xc0 + chan, (instr[channel[chan].inst][8] & 1) + (eff_op << 1));
      break;
    case 0xa0:		                    // set carrier volume
      vol = eff_op << 2;
      opl->write(0x43 + op_table[chan], vol | (instr[channel[chan].inst][2] & ~63));
      break;
    case 0xb0:		                    // set modulator volume
      vol = eff_op << 2;
      if (instr[inst][8] & 1)
	opl->write(0x40 + op_table[chan], vol | (instr[channel[chan].inst][3] & ~63));
      else
	opl->write(0x40 + op_table[chan],vol | (instr[inst][3] & ~63));
      break;
    case 0xc0:		                    // set instrument volume
      db = eff_op << 2;
      opl->write(0x43 + op_table[chan], db | (instr[channel[chan].inst][2] & ~63));
      if (instr[inst][8] & 1)
	opl->write(0x40 + op_table[chan], db | (instr[channel[chan].inst][3] & ~63));
      break;
    case 0xd0: pattbreak++; songpos = eff_op; songend = 1; break;	// position jump
    case 0xf0:							// set speed
      speed = eff_op;
      del = ++speed;
      break;
    }

    if(fadein)						// fade-in volume setting
      setvolume(chan,fadein*2,fadein*2);

    if(!note)						// note handling
      continue;
    note--;

    if ((note == 0x7f-1) || ((note/12) & ~7)) {    // pause (7fh)
      adl_freq[chan] &= ~32;
      opl->write(0xb0 + chan,adl_freq[chan]);
      continue;
    }

    // play the note
    if(mtkmode)		// imitate MPU-401 Trakker bug
      note--;
    Okt = ((note/12) & 7) << 2;
    Fnr = note_table[(note % 12)] + instr[inst][11] + channel[chan].slide;
    channel[chan].freq = Fnr;
    if(!mode6 || chan < 6)
      adl_freq[chan] = Okt | 32;
    else
      adl_freq[chan] = Okt;		// never set key for drums
    opl->write(0xb0 + chan, 0);
    setfreq(chan,Fnr);
    if(mode6) {
      switch(chan) {		// play drums
      case 6: opl->write(0xbd,bd & ~16); bd |= 48; break;	// bass drum
      case 7: opl->write(0xbd,bd & ~1); bd |= 33; break;	// hihat
      case 8: opl->write(0xbd,bd & ~2); bd |= 34; break;	// cymbal
      }
      opl->write(0xbd,bd);
    }
  }

  del = speed;		// player speed-timing
  if(pattbreak) {		// do post-effect handling
    pattpos=0;			// pattern break!
    pattbreak=0;
    songpos++;
    songpos %= 50;
    if(!songpos)
      songend = 1;
  } else {
    pattpos++;
    pattpos &= 63;		// advance in pattern data
    if (!pattpos) {
      songpos++;
      songpos %= 50;
      if(!songpos)
	songend = 1;
    }
  }
  return !songend;		// still playing
}

void ChscPlayer::rewind(int subsong)
{
  int i;								// counter

  // rewind HSC player
  pattpos = 0; songpos = 0; pattbreak = 0; speed = 2;
  del = 1; songend = 0; mode6 = 0; bd = 0; fadein = 0;

  opl->init();						// reset OPL chip
  opl->write(1,32); opl->write(8,128); opl->write(0xbd,0);

  for(i=0;i<9;i++)
    setinstr((char) i,(char) i);	// init channels
}

unsigned int ChscPlayer::getpatterns()
{
  unsigned char	poscnt,pattcnt=0;

  // count patterns
  for(poscnt=0;poscnt<51 && song[poscnt] != 0xff;poscnt++)
    if(song[poscnt] > pattcnt)
      pattcnt = song[poscnt];

  return (pattcnt+1);
}

unsigned int ChscPlayer::getorders()
{
  unsigned char poscnt;

  // count positions
  for(poscnt=0;poscnt<51;poscnt++)
    if(song[poscnt] == 0xff)
      break;

  return poscnt;
}

unsigned int ChscPlayer::getinstruments()
{
  unsigned char	instcnt,instnum=0,i;
  bool		isinst;

  // count instruments
  for(instcnt=0;instcnt<128;instcnt++) {
    isinst = false;
    for(i=0;i<12;i++)
      if(instr[instcnt][i])
	isinst = true;
    if(isinst)
      instnum++;
  }

  return instnum;
}

/*** private methods *************************************/

void ChscPlayer::setfreq(unsigned char chan, unsigned short freq)
{
  adl_freq[chan] = (adl_freq[chan] & ~3) | (freq >> 8);

  opl->write(0xa0 + chan, freq & 0xff);
  opl->write(0xb0 + chan, adl_freq[chan]);
}

void ChscPlayer::setvolume(unsigned char chan, int volc, int volm)
{
  unsigned char	*ins = instr[channel[chan].inst];
  char		op = op_table[chan];

  opl->write(0x43 + op,volc | (ins[2] & ~63));
  if (ins[8] & 1)							// carrier
    opl->write(0x40 + op,volm | (ins[3] & ~63));
  else
    opl->write(0x40 + op, ins[3]);		// modulator
}

void ChscPlayer::setinstr(unsigned char chan, unsigned char insnr)
{
  unsigned char	*ins = instr[insnr];
  char		op = op_table[chan];

  channel[chan].inst = insnr;		// set internal instrument
  opl->write(0xb0 + chan,0);			// stop old note

  // set instrument
  opl->write(0xc0 + chan, ins[8]);
  opl->write(0x23 + op, ins[0]);        // carrier
  opl->write(0x20 + op, ins[1]);        // modulator
  opl->write(0x63 + op, ins[4]);        // bits 0..3 = decay; 4..7 = attack
  opl->write(0x60 + op, ins[5]);
  opl->write(0x83 + op, ins[6]);        // 0..3 = release; 4..7 = sustain
  opl->write(0x80 + op, ins[7]);
  opl->write(0xe3 + op, ins[9]);        // bits 0..1 = Wellenform
  opl->write(0xe0 + op, ins[10]);
  setvolume(chan, ins[2] & 63, ins[3] & 63);
}
<commit_msg>hsc: Handle out of range patterns more gracefully<commit_after>/*
 * Adplug - Replayer for many OPL2/OPL3 audio file formats.
 * Copyright (C) 1999 - 2004 Simon Peter, <dn.tlp@gmx.net>, et al.
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 *
 * hsc.cpp - HSC Player by Simon Peter <dn.tlp@gmx.net>
 */

#include <string.h>

#include "hsc.h"
#include "debug.h"

/*** public methods **************************************/

CPlayer *ChscPlayer::factory(Copl *newopl)
{
  return new ChscPlayer(newopl);
}

bool ChscPlayer::load(const std::string &filename, const CFileProvider &fp)
{
  binistream	*f = fp.open(filename);
  int		i;

  // file validation section
  if(!f || !fp.extension(filename, ".hsc") || fp.filesize(f) > 59187) {
    AdPlug_LogWrite("ChscPlayer::load(\"%s\"): Not a HSC file!\n", filename.c_str());
    fp.close(f);
    return false;
  }

  int total_patterns_in_hsc = (fp.filesize(f) - 1587) / 1152;

  // load section
  for(i=0;i<128*12;i++)		// load instruments
    *((unsigned char *)instr + i) = f->readInt(1);
  for (i=0;i<128;i++) {			// correct instruments
    instr[i][2] ^= (instr[i][2] & 0x40) << 1;
    instr[i][3] ^= (instr[i][3] & 0x40) << 1;
    instr[i][11] >>= 4;			// slide
  }
  for(i=0;i<51;i++) {	// load tracklist
    song[i] = f->readInt(1);
    // if out of range, song ends here
    if (
      ((song[i] & 0x7F) > 0x31)
      || ((song[i] & 0x7F) >= total_patterns_in_hsc)
    ) song[i] = 0xFF;
  }
  for(i=0;i<50*64*9;i++)			// load patterns
    *((char *)patterns + i) = f->readInt(1);

  fp.close(f);
  rewind(0);					// rewind module
  return true;
}

bool ChscPlayer::update()
{
  // general vars
  unsigned char		chan,pattnr,note,effect,eff_op,inst,vol,Okt,db;
  unsigned short	Fnr;
  unsigned long		pattoff;

  del--;                      // player speed handling
  if(del)
    return !songend;		// nothing done

  if(fadein)					// fade-in handling
    fadein--;

  pattnr = song[songpos];
  // 0xff indicates song end, but this prevents a crash for some songs that
  // use other weird values, like 0xbf
  if(pattnr >= 0xb2) {			// arrangement handling
    songend = 1;				// set end-flag
    songpos = 0;
    pattnr = song[songpos];
  } else
    if ((pattnr & 128) && (pattnr <= 0xb1)) { // goto pattern "nr"
      songpos = song[songpos] & 127;
      pattpos = 0;
      pattnr = song[songpos];
      songend = 1;
    }

  pattoff = pattpos*9;
  for (chan=0;chan<9;chan++) {			// handle all channels
    note = patterns[pattnr][pattoff].note;
    effect = patterns[pattnr][pattoff].effect;
    pattoff++;

    if(note & 128) {                    // set instrument
      setinstr(chan,effect);
      continue;
    }
    eff_op = effect & 0x0f;
    inst = channel[chan].inst;
    if(note)
      channel[chan].slide = 0;

    switch (effect & 0xf0) {			// effect handling
    case 0:								// global effect
      /* The following fx are unimplemented on purpose:
       * 02 - Slide Mainvolume up
       * 03 - Slide Mainvolume down (here: fade in)
       * 04 - Set Mainvolume to 0
       *
       * This is because i've never seen any HSC modules using the fx this way.
       * All modules use the fx the way, i've implemented it.
       */
      switch(eff_op) {
      case 1: pattbreak++; break;	// jump to next pattern
      case 3: fadein = 31; break;	// fade in (divided by 2)
      case 5: mode6 = 1; break;	// 6 voice mode on
      case 6: mode6 = 0; break;	// 6 voice mode off
      }
      break;
    case 0x20:
    case 0x10:		                    // manual slides
      if (effect & 0x10) {
	channel[chan].freq += eff_op;
	channel[chan].slide += eff_op;
      } else {
	channel[chan].freq -= eff_op;
	channel[chan].slide -= eff_op;
      }
      if(!note)
	setfreq(chan,channel[chan].freq);
      break;
    case 0x50:							// set percussion instrument (unimplemented)
      break;
    case 0x60:							// set feedback
      opl->write(0xc0 + chan, (instr[channel[chan].inst][8] & 1) + (eff_op << 1));
      break;
    case 0xa0:		                    // set carrier volume
      vol = eff_op << 2;
      opl->write(0x43 + op_table[chan], vol | (instr[channel[chan].inst][2] & ~63));
      break;
    case 0xb0:		                    // set modulator volume
      vol = eff_op << 2;
      if (instr[inst][8] & 1)
	opl->write(0x40 + op_table[chan], vol | (instr[channel[chan].inst][3] & ~63));
      else
	opl->write(0x40 + op_table[chan],vol | (instr[inst][3] & ~63));
      break;
    case 0xc0:		                    // set instrument volume
      db = eff_op << 2;
      opl->write(0x43 + op_table[chan], db | (instr[channel[chan].inst][2] & ~63));
      if (instr[inst][8] & 1)
	opl->write(0x40 + op_table[chan], db | (instr[channel[chan].inst][3] & ~63));
      break;
    case 0xd0: pattbreak++; songpos = eff_op; songend = 1; break;	// position jump
    case 0xf0:							// set speed
      speed = eff_op;
      del = ++speed;
      break;
    }

    if(fadein)						// fade-in volume setting
      setvolume(chan,fadein*2,fadein*2);

    if(!note)						// note handling
      continue;
    note--;

    if ((note == 0x7f-1) || ((note/12) & ~7)) {    // pause (7fh)
      adl_freq[chan] &= ~32;
      opl->write(0xb0 + chan,adl_freq[chan]);
      continue;
    }

    // play the note
    if(mtkmode)		// imitate MPU-401 Trakker bug
      note--;
    Okt = ((note/12) & 7) << 2;
    Fnr = note_table[(note % 12)] + instr[inst][11] + channel[chan].slide;
    channel[chan].freq = Fnr;
    if(!mode6 || chan < 6)
      adl_freq[chan] = Okt | 32;
    else
      adl_freq[chan] = Okt;		// never set key for drums
    opl->write(0xb0 + chan, 0);
    setfreq(chan,Fnr);
    if(mode6) {
      switch(chan) {		// play drums
      case 6: opl->write(0xbd,bd & ~16); bd |= 48; break;	// bass drum
      case 7: opl->write(0xbd,bd & ~1); bd |= 33; break;	// hihat
      case 8: opl->write(0xbd,bd & ~2); bd |= 34; break;	// cymbal
      }
      opl->write(0xbd,bd);
    }
  }

  del = speed;		// player speed-timing
  if(pattbreak) {		// do post-effect handling
    pattpos=0;			// pattern break!
    pattbreak=0;
    songpos++;
    songpos %= 50;
    if(!songpos)
      songend = 1;
  } else {
    pattpos++;
    pattpos &= 63;		// advance in pattern data
    if (!pattpos) {
      songpos++;
      songpos %= 50;
      if(!songpos)
	songend = 1;
    }
  }
  return !songend;		// still playing
}

void ChscPlayer::rewind(int subsong)
{
  int i;								// counter

  // rewind HSC player
  pattpos = 0; songpos = 0; pattbreak = 0; speed = 2;
  del = 1; songend = 0; mode6 = 0; bd = 0; fadein = 0;

  opl->init();						// reset OPL chip
  opl->write(1,32); opl->write(8,128); opl->write(0xbd,0);

  for(i=0;i<9;i++)
    setinstr((char) i,(char) i);	// init channels
}

unsigned int ChscPlayer::getpatterns()
{
  unsigned char	poscnt,pattcnt=0;

  // count patterns
  for(poscnt=0;poscnt<51 && song[poscnt] != 0xff;poscnt++)
    if(song[poscnt] > pattcnt)
      pattcnt = song[poscnt];

  return (pattcnt+1);
}

unsigned int ChscPlayer::getorders()
{
  unsigned char poscnt;

  // count positions
  for(poscnt=0;poscnt<51;poscnt++)
    if(song[poscnt] == 0xff)
      break;

  return poscnt;
}

unsigned int ChscPlayer::getinstruments()
{
  unsigned char	instcnt,instnum=0,i;
  bool		isinst;

  // count instruments
  for(instcnt=0;instcnt<128;instcnt++) {
    isinst = false;
    for(i=0;i<12;i++)
      if(instr[instcnt][i])
	isinst = true;
    if(isinst)
      instnum++;
  }

  return instnum;
}

/*** private methods *************************************/

void ChscPlayer::setfreq(unsigned char chan, unsigned short freq)
{
  adl_freq[chan] = (adl_freq[chan] & ~3) | (freq >> 8);

  opl->write(0xa0 + chan, freq & 0xff);
  opl->write(0xb0 + chan, adl_freq[chan]);
}

void ChscPlayer::setvolume(unsigned char chan, int volc, int volm)
{
  unsigned char	*ins = instr[channel[chan].inst];
  char		op = op_table[chan];

  opl->write(0x43 + op,volc | (ins[2] & ~63));
  if (ins[8] & 1)							// carrier
    opl->write(0x40 + op,volm | (ins[3] & ~63));
  else
    opl->write(0x40 + op, ins[3]);		// modulator
}

void ChscPlayer::setinstr(unsigned char chan, unsigned char insnr)
{
  unsigned char	*ins = instr[insnr];
  char		op = op_table[chan];

  channel[chan].inst = insnr;		// set internal instrument
  opl->write(0xb0 + chan,0);			// stop old note

  // set instrument
  opl->write(0xc0 + chan, ins[8]);
  opl->write(0x23 + op, ins[0]);        // carrier
  opl->write(0x20 + op, ins[1]);        // modulator
  opl->write(0x63 + op, ins[4]);        // bits 0..3 = decay; 4..7 = attack
  opl->write(0x60 + op, ins[5]);
  opl->write(0x83 + op, ins[6]);        // 0..3 = release; 4..7 = sustain
  opl->write(0x80 + op, ins[7]);
  opl->write(0xe3 + op, ins[9]);        // bits 0..1 = Wellenform
  opl->write(0xe0 + op, ins[10]);
  setvolume(chan, ins[2] & 63, ins[3] & 63);
}
<|endoftext|>


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

  Program:   Visualization Toolkit
  Module:    vtkInterpolatedVelocityField.cxx
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

  Copyright (c) 1993-2002 Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
#include "vtkInterpolatedVelocityField.h"

#include "vtkDataArray.h"
#include "vtkDataSet.h"
#include "vtkGenericCell.h"
#include "vtkObjectFactory.h"
#include "vtkPointData.h"

#include <vtkstd/vector>

vtkCxxRevisionMacro(vtkInterpolatedVelocityField, "1.29");
vtkStandardNewMacro(vtkInterpolatedVelocityField);

typedef vtkstd::vector< vtkDataSet* > DataSetsTypeBase;
class vtkInterpolatedVelocityFieldDataSetsType: public DataSetsTypeBase {};

vtkInterpolatedVelocityField::vtkInterpolatedVelocityField()
{
  this->NumFuncs = 3; // u, v, w
  this->NumIndepVars = 4; // x, y, z, t
  this->Weights = 0;
  this->WeightsSize = 0;
  this->GenCell = vtkGenericCell::New();
  this->LastCellId = -1;
  this->CacheHit = 0;
  this->CacheMiss = 0;
  this->Caching = 1; // Caching on by default

  this->Cell = vtkGenericCell::New();
  this->VectorsSelection = 0;

  this->DataSets = new vtkInterpolatedVelocityFieldDataSetsType;
  this->LastDataSet = 0;
}

vtkInterpolatedVelocityField::~vtkInterpolatedVelocityField()
{
  this->NumFuncs = 0;
  this->NumIndepVars = 0;
  this->GenCell->Delete();
  delete[] this->Weights;
  this->Weights = 0;

  this->Cell->Delete();
  this->SetVectorsSelection(0);

  delete this->DataSets;
}

static int tmp_count=0;
// Evaluate u,v,w at x,y,z,t
int vtkInterpolatedVelocityField::FunctionValues(float* x, float* f)
{
  vtkDataSet* ds=0;
  if(!this->LastDataSet && !this->DataSets->empty())
    {
    ds = (*this->DataSets)[0];
    this->LastDataSet = ds;
    }
  else
    {
    ds = this->LastDataSet;
    }
  int retVal = this->FunctionValues(ds, x, f);
  if (!retVal)
    {
    tmp_count = 0;
    for(DataSetsTypeBase::iterator i = this->DataSets->begin();
        i != this->DataSets->end(); ++i)
      {
      ds = *i;
      if(ds && ds != this->LastDataSet)
        {
        this->ClearLastCellId();
        retVal = this->FunctionValues(ds, x, f);
        if (retVal)
          {
          this->LastDataSet = ds;
          return retVal;
          }
        }
      }
    this->ClearLastCellId();
    return 0;
    }
  tmp_count++;
  return retVal;
}

const float vtkInterpolatedVelocityField::TOLERANCE_SCALE = 1.0E-10;

// Evaluate u,v,w at x,y,z,t
int vtkInterpolatedVelocityField::FunctionValues(vtkDataSet* dataset,
                                                 float* x,
                                                 float* f)
{
  int i, j, subId , numPts, id;
  vtkDataArray* vectors;
  float vec[3];
  float dist2;
  int ret;

  for(i=0; i<3; i++)
    {
    f[i] = 0;
    }

  // See if a dataset has been specified and if there are input vectors
  if (!dataset ||
      !(vectors = dataset->GetPointData()->GetVectors(this->VectorsSelection)))
    {
    vtkErrorMacro(<<"Can't evaluate dataset!");
    return 0;
    }

  float tol2 =
    dataset->GetLength() * vtkInterpolatedVelocityField::TOLERANCE_SCALE;

  int found = 0;

  if (this->Caching)
    {
    // See if the point is in the cached cell
    if (this->LastCellId == -1 ||
        !(ret=this->GenCell->EvaluatePosition(x, 0, subId,
                                              this->LastPCoords, dist2,
                                              this->Weights))
        || ret == -1)
      {
      // if not, find and get it
      if (this->LastCellId != - 1 )
        {
        this->CacheMiss++;

        dataset->GetCell(this->LastCellId, this->Cell);

        this->LastCellId =
          dataset->FindCell(x, this->Cell, this->GenCell, -1, tol2,
                            subId, this->LastPCoords, this->Weights);
        if (this->LastCellId != - 1)
          {
          dataset->GetCell(this->LastCellId, this->GenCell);
          found = 1;
          }
        }
      }
    else
      {
      this->CacheHit++;
      found = 1;
      }
    }

  if (!found)
    {
    // if the cell is not found, do a global search (ignore initial
    // cell if there is one)
    this->LastCellId =
      dataset->FindCell(x, 0, this->GenCell, -1, tol2,
                              subId, this->LastPCoords, this->Weights);
    if (this->LastCellId != - 1)
      {
      dataset->GetCell(this->LastCellId, this->GenCell);
      }
    else
      {
      return 0;
      }
    }

  // if the cell is valid
  if (this->LastCellId >= 0)
    {
    numPts = this->GenCell->GetNumberOfPoints();
    // interpolate the vectors
    for (j=0; j < numPts; j++)
      {
      id = this->GenCell->PointIds->GetId(j);
      vectors->GetTuple(id, vec);
      for (i=0; i < 3; i++)
        {
        f[i] +=  vec[i] * this->Weights[j];
        }
      }
    }
  // if not, return false
  else
    {
    return 0;
    }

  return 1;
}

void vtkInterpolatedVelocityField::AddDataSet(vtkDataSet* dataset)
{
  if (!dataset)
    {
    return;
    }

  this->DataSets->push_back(dataset);

  int size = dataset->GetMaxCellSize();
  if (size > this->WeightsSize)
    {
    this->WeightsSize = size;
    delete[] this->Weights;
    this->Weights = new float[size];
    }
}

int vtkInterpolatedVelocityField::GetLastWeights(float* w)
{
  int j, numPts;

  // If last cell is valid, fill w with the interpolation weights
  // and return true
  if (this->LastCellId >= 0)
    {
    numPts = this->GenCell->GetNumberOfPoints();
    for (j=0; j < numPts; j++)
      {
      w[j] = this->Weights[j];
      }
    return 1;
    }
  // otherwise, return false
  else
    {
    return 0;
    }
}

int vtkInterpolatedVelocityField::GetLastLocalCoordinates(float pcoords[3])
{
  int j;

  // If last cell is valid, fill p with the local coordinates
  // and return true
  if (this->LastCellId >= 0)
    {
    for (j=0; j < 3; j++)
      {
      pcoords[j] = this->LastPCoords[j];
      }
    return 1;
    }
  // otherwise, return false
  else
    {
    return 0;
    }
}

void vtkInterpolatedVelocityField::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os, indent);
  if ( this->VectorsSelection )
    {
    os << indent << "VectorsSelection: " << this->VectorsSelection << endl;
    }
  else
    {
    os << indent << "VectorsSelection: (none)" << endl;
    }
  if ( this->GenCell )
    {
    os << indent << "Last cell: " << this->GenCell << endl;
    }
  else
    {
    os << indent << "Last cell: (none)" << endl;
    }
  os << indent << "Weights: " << this->Weights << endl;
  os << indent << "Last cell Id: " << this->LastCellId << endl;
  os << indent << "Cache hit: " << this->CacheHit << endl;
  os << indent << "Cache miss: " << this->CacheMiss << endl;
  os << indent << "Caching: ";
  if ( this->Caching )
    {
    os << "on." << endl;
    }
  else
    {
    os << "off." << endl;
    }

  os << indent << "VectorsSelection: "
     << (this->VectorsSelection?this->VectorsSelection:"(none)") << endl;
  os << indent << "LastDataSet : "
     << this->LastDataSet << endl;

}

<commit_msg>The tolerance was too low. In certain 2D cases, streamline integration was terminating too early<commit_after>/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkInterpolatedVelocityField.cxx
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

  Copyright (c) 1993-2002 Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
#include "vtkInterpolatedVelocityField.h"

#include "vtkDataArray.h"
#include "vtkDataSet.h"
#include "vtkGenericCell.h"
#include "vtkObjectFactory.h"
#include "vtkPointData.h"

#include <vtkstd/vector>

vtkCxxRevisionMacro(vtkInterpolatedVelocityField, "1.30");
vtkStandardNewMacro(vtkInterpolatedVelocityField);

typedef vtkstd::vector< vtkDataSet* > DataSetsTypeBase;
class vtkInterpolatedVelocityFieldDataSetsType: public DataSetsTypeBase {};

vtkInterpolatedVelocityField::vtkInterpolatedVelocityField()
{
  this->NumFuncs = 3; // u, v, w
  this->NumIndepVars = 4; // x, y, z, t
  this->Weights = 0;
  this->WeightsSize = 0;
  this->GenCell = vtkGenericCell::New();
  this->LastCellId = -1;
  this->CacheHit = 0;
  this->CacheMiss = 0;
  this->Caching = 1; // Caching on by default

  this->Cell = vtkGenericCell::New();
  this->VectorsSelection = 0;

  this->DataSets = new vtkInterpolatedVelocityFieldDataSetsType;
  this->LastDataSet = 0;
}

vtkInterpolatedVelocityField::~vtkInterpolatedVelocityField()
{
  this->NumFuncs = 0;
  this->NumIndepVars = 0;
  this->GenCell->Delete();
  delete[] this->Weights;
  this->Weights = 0;

  this->Cell->Delete();
  this->SetVectorsSelection(0);

  delete this->DataSets;
}

static int tmp_count=0;
// Evaluate u,v,w at x,y,z,t
int vtkInterpolatedVelocityField::FunctionValues(float* x, float* f)
{
  vtkDataSet* ds=0;
  if(!this->LastDataSet && !this->DataSets->empty())
    {
    ds = (*this->DataSets)[0];
    this->LastDataSet = ds;
    }
  else
    {
    ds = this->LastDataSet;
    }
  int retVal = this->FunctionValues(ds, x, f);
  if (!retVal)
    {
    tmp_count = 0;
    for(DataSetsTypeBase::iterator i = this->DataSets->begin();
        i != this->DataSets->end(); ++i)
      {
      ds = *i;
      if(ds && ds != this->LastDataSet)
        {
        this->ClearLastCellId();
        retVal = this->FunctionValues(ds, x, f);
        if (retVal)
          {
          this->LastDataSet = ds;
          return retVal;
          }
        }
      }
    this->ClearLastCellId();
    return 0;
    }
  tmp_count++;
  return retVal;
}

const float vtkInterpolatedVelocityField::TOLERANCE_SCALE = 1.0E-8;

// Evaluate u,v,w at x,y,z,t
int vtkInterpolatedVelocityField::FunctionValues(vtkDataSet* dataset,
                                                 float* x,
                                                 float* f)
{
  int i, j, subId , numPts, id;
  vtkDataArray* vectors;
  float vec[3];
  float dist2;
  int ret;

  for(i=0; i<3; i++)
    {
    f[i] = 0;
    }

  // See if a dataset has been specified and if there are input vectors
  if (!dataset ||
      !(vectors = dataset->GetPointData()->GetVectors(this->VectorsSelection)))
    {
    vtkErrorMacro(<<"Can't evaluate dataset!");
    return 0;
    }

  float tol2 =
    dataset->GetLength() * vtkInterpolatedVelocityField::TOLERANCE_SCALE;

  int found = 0;

  if (this->Caching)
    {
    // See if the point is in the cached cell
    if (this->LastCellId == -1 ||
        !(ret=this->GenCell->EvaluatePosition(x, 0, subId,
                                              this->LastPCoords, dist2,
                                              this->Weights))
        || ret == -1)
      {
      // if not, find and get it
      if (this->LastCellId != - 1 )
        {
        this->CacheMiss++;

        dataset->GetCell(this->LastCellId, this->Cell);

        this->LastCellId =
          dataset->FindCell(x, this->Cell, this->GenCell, -1, tol2,
                            subId, this->LastPCoords, this->Weights);
        if (this->LastCellId != - 1)
          {
          dataset->GetCell(this->LastCellId, this->GenCell);
          found = 1;
          }
        }
      }
    else
      {
      this->CacheHit++;
      found = 1;
      }
    }

  if (!found)
    {
    // if the cell is not found, do a global search (ignore initial
    // cell if there is one)
    this->LastCellId =
      dataset->FindCell(x, 0, this->GenCell, -1, tol2,
                              subId, this->LastPCoords, this->Weights);
    if (this->LastCellId != - 1)
      {
      dataset->GetCell(this->LastCellId, this->GenCell);
      }
    else
      {
      return 0;
      }
    }

  // if the cell is valid
  if (this->LastCellId >= 0)
    {
    numPts = this->GenCell->GetNumberOfPoints();
    // interpolate the vectors
    for (j=0; j < numPts; j++)
      {
      id = this->GenCell->PointIds->GetId(j);
      vectors->GetTuple(id, vec);
      for (i=0; i < 3; i++)
        {
        f[i] +=  vec[i] * this->Weights[j];
        }
      }
    }
  // if not, return false
  else
    {
    return 0;
    }

  return 1;
}

void vtkInterpolatedVelocityField::AddDataSet(vtkDataSet* dataset)
{
  if (!dataset)
    {
    return;
    }

  this->DataSets->push_back(dataset);

  int size = dataset->GetMaxCellSize();
  if (size > this->WeightsSize)
    {
    this->WeightsSize = size;
    delete[] this->Weights;
    this->Weights = new float[size];
    }
}

int vtkInterpolatedVelocityField::GetLastWeights(float* w)
{
  int j, numPts;

  // If last cell is valid, fill w with the interpolation weights
  // and return true
  if (this->LastCellId >= 0)
    {
    numPts = this->GenCell->GetNumberOfPoints();
    for (j=0; j < numPts; j++)
      {
      w[j] = this->Weights[j];
      }
    return 1;
    }
  // otherwise, return false
  else
    {
    return 0;
    }
}

int vtkInterpolatedVelocityField::GetLastLocalCoordinates(float pcoords[3])
{
  int j;

  // If last cell is valid, fill p with the local coordinates
  // and return true
  if (this->LastCellId >= 0)
    {
    for (j=0; j < 3; j++)
      {
      pcoords[j] = this->LastPCoords[j];
      }
    return 1;
    }
  // otherwise, return false
  else
    {
    return 0;
    }
}

void vtkInterpolatedVelocityField::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os, indent);
  if ( this->VectorsSelection )
    {
    os << indent << "VectorsSelection: " << this->VectorsSelection << endl;
    }
  else
    {
    os << indent << "VectorsSelection: (none)" << endl;
    }
  if ( this->GenCell )
    {
    os << indent << "Last cell: " << this->GenCell << endl;
    }
  else
    {
    os << indent << "Last cell: (none)" << endl;
    }
  os << indent << "Weights: " << this->Weights << endl;
  os << indent << "Last cell Id: " << this->LastCellId << endl;
  os << indent << "Cache hit: " << this->CacheHit << endl;
  os << indent << "Cache miss: " << this->CacheMiss << endl;
  os << indent << "Caching: ";
  if ( this->Caching )
    {
    os << "on." << endl;
    }
  else
    {
    os << "off." << endl;
    }

  os << indent << "VectorsSelection: "
     << (this->VectorsSelection?this->VectorsSelection:"(none)") << endl;
  os << indent << "LastDataSet : "
     << this->LastDataSet << endl;

}

<|endoftext|>