<commit_before>// Copyright 2009 Minor Gordon.
// This source comes from the XtreemFS project. It is licensed under the GPLv2 (see COPYING for terms and conditions).

#include "org/xtreemfs/client/file.h"
#include "org/xtreemfs/client/mrc_proxy.h"
#include "org/xtreemfs/client/osd_proxy.h"
#include "org/xtreemfs/client/volume.h"
#include "multi_response_target.h"
using namespace org::xtreemfs::client;

#include <errno.h>

#ifdef _WIN32
#include <windows.h>
#pragma warning( push )
#pragma warning( disable: 4100 )
#else
#include <errno.h>
#endif
#include "org/xtreemfs/interfaces/osd_interface.h"
#ifdef _WIN32
#pragma warning( pop )
#endif

File::File( YIELD::auto_Object<Volume> parent_volume, YIELD::auto_Object<MRCProxy> mrc_proxy, const YIELD::Path& path, const org::xtreemfs::interfaces::FileCredentials& file_credentials )
: parent_volume( parent_volume ), mrc_proxy( mrc_proxy ), path( path ), file_credentials( file_credentials )
{ }

File::~File()
{
  flush();
}

bool File::datasync()
{
  flush();
  return true;
}

bool File::close()
{
  flush();
  return true;
}

bool File::flush()
{
  try
  {
    if ( !latest_osd_write_response.get_new_file_size().empty() )
    {
      mrc_proxy->xtreemfs_update_file_size( file_credentials.get_xcap(), latest_osd_write_response );
      latest_osd_write_response.set_new_file_size( org::xtreemfs::interfaces::NewFileSize() );
    }
    return true;
  }
  catch ( ProxyExceptionResponse& proxy_exception_response )
  {
    YIELD::Exception::set_errno( proxy_exception_response.get_platform_error_code() );
  }
  catch ( std::exception& )
  {
    YIELD::Exception::set_errno( EIO );
  }

  return false;
}

YIELD::auto_Stat File::getattr()
{
  return parent_volume->getattr( path );
}

uint64_t File::get_size()
{
  if ( !latest_osd_write_response.get_new_file_size().empty() )
    return latest_osd_write_response.get_new_file_size()[0].get_size_in_bytes();
  else
    return YIELD::File::get_size();
}

bool File::getxattr( const std::string& name, std::string& out_value )
{
  return parent_volume->getxattr( path, name, out_value );
}

bool File::listxattr( std::vector<std::string>& out_names )
{
  return parent_volume->listxattr( path, out_names );
}

ssize_t File::read( void* rbuf, size_t size, uint64_t offset )
{
  try
  {
    YIELD::auto_Log log = parent_volume->get_log();
    char *rbuf_start = static_cast<char*>( rbuf ), *rbuf_p = static_cast<char*>( rbuf );
#define RBUF_REMAINING ( size - static_cast<size_t>( rbuf_p - rbuf_start ) )
    uint32_t stripe_size = file_credentials.get_xlocs().get_replicas()[0].get_striping_policy().get_stripe_size() * 1024;

    for ( ;; )
    {
      uint64_t object_number = offset / stripe_size;
      uint32_t object_offset = offset % stripe_size;
      uint64_t object_size = RBUF_REMAINING;
      if ( object_offset + object_size > stripe_size )
        object_size = stripe_size - object_offset;

      log->getStream( YIELD::Log::LOG_INFO ) <<
        "org::xtreemfs::client::File: reading " << object_size <<
        " bytes from offset " << object_offset <<
        " in object number " << object_number <<
        " of file " << file_credentials.get_xcap().get_file_id() <<
        ", file offset = " << offset <<
        ", remaining buffer size = " << RBUF_REMAINING <<
        ".";

      org::xtreemfs::interfaces::ObjectData object_data;
      parent_volume->get_osd_proxy_mux()->read( file_credentials, file_credentials.get_xcap().get_file_id(), object_number, 0, object_offset, static_cast<uint32_t>( object_size ), object_data );

      log->getStream( YIELD::Log::LOG_INFO ) <<
        "org::xtreemfs::client::File: read " << object_data.get_data()->size() <<
        " bytes from file " << file_credentials.get_xcap().get_file_id() <<
        " with " << object_data.get_zero_padding() << " bytes of zero padding.";

      YIELD::auto_Buffer data = object_data.get_data();
      if ( !data->empty() )
      {
        if ( data->size() <= RBUF_REMAINING )
        {
          memcpy_s( rbuf_p, RBUF_REMAINING, static_cast<void*>( *data ), data->size() );
          rbuf_p += data->size();
          offset += data->size();
        }
        else
        {
          log->getStream( YIELD::Log::LOG_ERR ) << "org::xtreemfs::client::File: received data (size=" << data->size() << ") larger than available buffer space (" << RBUF_REMAINING << ")";
          YIELD::ExceptionResponse::set_errno( EIO );
          return -1;
        }
      }

      uint32_t zero_padding = object_data.get_zero_padding();
      if ( zero_padding > 0 )
      {
        if ( zero_padding <= RBUF_REMAINING )
        {
          memset( rbuf_p, 0, zero_padding );
          rbuf_p += zero_padding;
          offset += zero_padding;
        }
        else
        {
          log->getStream( YIELD::Log::LOG_ERR ) << "org::xtreemfs::client::File: received zero_padding (" << zero_padding << ") larger than available buffer space (" << ( size - static_cast<size_t>( rbuf_p - rbuf_start ) );
          YIELD::ExceptionResponse::set_errno( EIO );
          return -1;
        }
      }

      if ( data->size() < object_size || RBUF_REMAINING == 0 )
        break;
    }

#ifdef _DEBUG
    if ( static_cast<size_t>( rbuf_p - rbuf_start ) > size ) YIELD::DebugBreak();
#endif
    return static_cast<ssize_t>( rbuf_p - rbuf_start );
  }
  catch ( ProxyExceptionResponse& proxy_exception_response )
  {
    YIELD::Exception::set_errno( proxy_exception_response.get_platform_error_code() );
  }
  catch ( std::exception& )
  {
    YIELD::Exception::set_errno( EIO );
  }

  return -1;
}

bool File::removexattr( const std::string& name )
{
  return parent_volume->removexattr( path, name );
}

bool File::setxattr( const std::string& name, const std::string& value, int flags )
{
  return parent_volume->setxattr( path, name, value, flags );
}

bool File::sync()
{
  flush();
  return true;
}

bool File::truncate( uint64_t new_size )
{
  try
  {
    org::xtreemfs::interfaces::XCap truncate_xcap;
    mrc_proxy->ftruncate( file_credentials.get_xcap(), truncate_xcap );
    file_credentials.set_xcap( truncate_xcap );
    org::xtreemfs::interfaces::OSDWriteResponse osd_write_response;
    parent_volume->get_osd_proxy_mux()->truncate( file_credentials, file_credentials.get_xcap().get_file_id(), new_size, osd_write_response );
    if ( osd_write_response > latest_osd_write_response )
      latest_osd_write_response = osd_write_response;
    if ( ( parent_volume->get_flags() & Volume::VOLUME_FLAG_CACHE_METADATA ) != Volume::VOLUME_FLAG_CACHE_METADATA )
      flush();
    return true;
  }
  catch ( ProxyExceptionResponse& proxy_exception_response )
  {
    YIELD::Exception::set_errno( proxy_exception_response.get_platform_error_code() );
  }
  catch ( std::exception& )
  {
    YIELD::Exception::set_errno( EIO );
  }

  return false;
}

ssize_t File::write( const void* buffer, size_t buffer_len, uint64_t offset )
{
  try
  {
    YIELD::auto_Object< YIELD::OneSignalEventQueue<> > write_response_queue( new YIELD::OneSignalEventQueue<> );

    const char* wbuf_p = static_cast<const char*>( buffer );
    uint64_t file_offset = offset, file_offset_max = offset + buffer_len;
    uint32_t stripe_size = file_credentials.get_xlocs().get_replicas()[0].get_striping_policy().get_stripe_size() * 1024;
    size_t expected_write_response_count = 0;

    while ( file_offset < file_offset_max )
    {
      uint64_t object_number = file_offset / stripe_size;
      uint32_t object_offset = file_offset % stripe_size;
      uint64_t object_size = file_offset_max - file_offset;
      if ( object_offset + object_size > stripe_size )
        object_size = stripe_size - object_offset;
      org::xtreemfs::interfaces::ObjectData object_data( 0, false, 0, new YIELD::StringLiteralBuffer( wbuf_p, static_cast<uint32_t>( object_size ) ) );
      org::xtreemfs::interfaces::OSDInterface::writeRequest* write_request = new org::xtreemfs::interfaces::OSDInterface::writeRequest( file_credentials, file_credentials.get_xcap().get_file_id(), object_number, 0, object_offset, 0, object_data );

      write_request->set_response_target( write_response_queue->incRef() );
      parent_volume->get_osd_proxy_mux()->send( *write_request );
      expected_write_response_count++;

      wbuf_p += object_size;
      file_offset += object_size;
    }

    for ( size_t write_response_i = 0; write_response_i < expected_write_response_count; write_response_i++ )
    {
      org::xtreemfs::interfaces::OSDInterface::writeResponse& write_response = write_response_queue->dequeue_typed<org::xtreemfs::interfaces::OSDInterface::writeResponse>();
      if ( write_response.get_osd_write_response() > latest_osd_write_response )
        latest_osd_write_response = write_response.get_osd_write_response();
      YIELD::Object::decRef( write_response );
    }

    if ( ( parent_volume->get_flags() & Volume::VOLUME_FLAG_CACHE_METADATA ) != Volume::VOLUME_FLAG_CACHE_METADATA )
      flush();

    return static_cast<ssize_t>( file_offset - offset );
  }
  catch ( ProxyExceptionResponse& proxy_exception_response )
  {
    YIELD::Exception::set_errno( proxy_exception_response.get_platform_error_code() );
  }
  catch ( std::exception& )
  {
    YIELD::Exception::set_errno( EIO );
  }

  return -1;
}

ssize_t File::writev( const struct iovec* buffers, uint32_t buffers_count, uint64_t offset )
{
  if ( buffers_count == 1 )
    return write( buffers[0].iov_base, buffers[0].iov_len, offset );
  else
  {
#ifdef _WIN32
    ::SetLastError( ERROR_NOT_SUPPORTED );
#else
    errno = ENOTSUP;
#endif
    return -1;
  }
}
<commit_msg>client: print more info on read errors<commit_after>// Copyright 2009 Minor Gordon.
// This source comes from the XtreemFS project. It is licensed under the GPLv2 (see COPYING for terms and conditions).

#include "org/xtreemfs/client/file.h"
#include "org/xtreemfs/client/mrc_proxy.h"
#include "org/xtreemfs/client/osd_proxy.h"
#include "org/xtreemfs/client/volume.h"
#include "multi_response_target.h"
using namespace org::xtreemfs::client;

#include <errno.h>

#ifdef _WIN32
#include <windows.h>
#pragma warning( push )
#pragma warning( disable: 4100 )
#else
#include <errno.h>
#endif
#include "org/xtreemfs/interfaces/osd_interface.h"
#ifdef _WIN32
#pragma warning( pop )
#endif

File::File( YIELD::auto_Object<Volume> parent_volume, YIELD::auto_Object<MRCProxy> mrc_proxy, const YIELD::Path& path, const org::xtreemfs::interfaces::FileCredentials& file_credentials )
: parent_volume( parent_volume ), mrc_proxy( mrc_proxy ), path( path ), file_credentials( file_credentials )
{ }

File::~File()
{
  flush();
}

bool File::datasync()
{
  flush();
  return true;
}

bool File::close()
{
  flush();
  return true;
}

bool File::flush()
{
  try
  {
    if ( !latest_osd_write_response.get_new_file_size().empty() )
    {
      mrc_proxy->xtreemfs_update_file_size( file_credentials.get_xcap(), latest_osd_write_response );
      latest_osd_write_response.set_new_file_size( org::xtreemfs::interfaces::NewFileSize() );
    }
    return true;
  }
  catch ( ProxyExceptionResponse& proxy_exception_response )
  {
    YIELD::Exception::set_errno( proxy_exception_response.get_platform_error_code() );
  }
  catch ( std::exception& )
  {
    YIELD::Exception::set_errno( EIO );
  }

  return false;
}

YIELD::auto_Stat File::getattr()
{
  return parent_volume->getattr( path );
}

uint64_t File::get_size()
{
  if ( !latest_osd_write_response.get_new_file_size().empty() )
    return latest_osd_write_response.get_new_file_size()[0].get_size_in_bytes();
  else
    return YIELD::File::get_size();
}

bool File::getxattr( const std::string& name, std::string& out_value )
{
  return parent_volume->getxattr( path, name, out_value );
}

bool File::listxattr( std::vector<std::string>& out_names )
{
  return parent_volume->listxattr( path, out_names );
}

ssize_t File::read( void* rbuf, size_t size, uint64_t offset )
{
  try
  {
    YIELD::auto_Log log = parent_volume->get_log();
    char *rbuf_start = static_cast<char*>( rbuf ), *rbuf_p = static_cast<char*>( rbuf );
#define RBUF_REMAINING ( size - static_cast<size_t>( rbuf_p - rbuf_start ) )
    uint32_t stripe_size = file_credentials.get_xlocs().get_replicas()[0].get_striping_policy().get_stripe_size() * 1024;

    for ( ;; )
    {
      uint64_t object_number = offset / stripe_size;
      uint32_t object_offset = offset % stripe_size;
      uint64_t object_size = RBUF_REMAINING;
      if ( object_offset + object_size > stripe_size )
        object_size = stripe_size - object_offset;

      log->getStream( YIELD::Log::LOG_INFO ) <<
        "org::xtreemfs::client::File: reading " << object_size <<
        " bytes from offset " << object_offset <<
        " in object number " << object_number <<
        " of file " << file_credentials.get_xcap().get_file_id() <<
        ", file offset = " << offset <<
        ", remaining buffer size = " << RBUF_REMAINING <<
        ".";

      org::xtreemfs::interfaces::ObjectData object_data;
      parent_volume->get_osd_proxy_mux()->read( file_credentials, file_credentials.get_xcap().get_file_id(), object_number, 0, object_offset, static_cast<uint32_t>( object_size ), object_data );
      YIELD::auto_Buffer data = object_data.get_data();
      uint32_t zero_padding = object_data.get_zero_padding();

      log->getStream( YIELD::Log::LOG_INFO ) <<
        "org::xtreemfs::client::File: read " << data->size() <<
        " bytes from file " << file_credentials.get_xcap().get_file_id() <<
        " with " << zero_padding << " bytes of zero padding.";

      if ( !data->empty() )
      {
        if ( data->size() <= RBUF_REMAINING )
        {
          memcpy_s( rbuf_p, RBUF_REMAINING, static_cast<void*>( *data ), data->size() );
          rbuf_p += data->size();
          offset += data->size();
        }
        else
        {
          log->getStream( YIELD::Log::LOG_ERR ) << "org::xtreemfs::client::File: received data (data size=" << data->size() << ", zero_padding=" << zero_padding << ") larger than available buffer space (" << RBUF_REMAINING << ")";
          YIELD::ExceptionResponse::set_errno( EIO );
          return -1;
        }
      }

      if ( zero_padding > 0 )
      {
        if ( zero_padding <= RBUF_REMAINING )
        {
          memset( rbuf_p, 0, zero_padding );
          rbuf_p += zero_padding;
          offset += zero_padding;
        }
        else
        {
          log->getStream( YIELD::Log::LOG_ERR ) << "org::xtreemfs::client::File: received zero_padding (data size=" << data->size() << ", zero_padding=" << zero_padding << ") larger than available buffer space (" << RBUF_REMAINING << ")";
          YIELD::ExceptionResponse::set_errno( EIO );
          return -1;
        }
      }

      if ( data->size() < object_size || RBUF_REMAINING == 0 )
        break;
    }

#ifdef _DEBUG
    if ( static_cast<size_t>( rbuf_p - rbuf_start ) > size ) YIELD::DebugBreak();
#endif
    return static_cast<ssize_t>( rbuf_p - rbuf_start );
  }
  catch ( ProxyExceptionResponse& proxy_exception_response )
  {
    YIELD::Exception::set_errno( proxy_exception_response.get_platform_error_code() );
  }
  catch ( std::exception& )
  {
    YIELD::Exception::set_errno( EIO );
  }

  return -1;
}

bool File::removexattr( const std::string& name )
{
  return parent_volume->removexattr( path, name );
}

bool File::setxattr( const std::string& name, const std::string& value, int flags )
{
  return parent_volume->setxattr( path, name, value, flags );
}

bool File::sync()
{
  flush();
  return true;
}

bool File::truncate( uint64_t new_size )
{
  try
  {
    org::xtreemfs::interfaces::XCap truncate_xcap;
    mrc_proxy->ftruncate( file_credentials.get_xcap(), truncate_xcap );
    file_credentials.set_xcap( truncate_xcap );
    org::xtreemfs::interfaces::OSDWriteResponse osd_write_response;
    parent_volume->get_osd_proxy_mux()->truncate( file_credentials, file_credentials.get_xcap().get_file_id(), new_size, osd_write_response );
    if ( osd_write_response > latest_osd_write_response )
      latest_osd_write_response = osd_write_response;
    if ( ( parent_volume->get_flags() & Volume::VOLUME_FLAG_CACHE_METADATA ) != Volume::VOLUME_FLAG_CACHE_METADATA )
      flush();
    return true;
  }
  catch ( ProxyExceptionResponse& proxy_exception_response )
  {
    YIELD::Exception::set_errno( proxy_exception_response.get_platform_error_code() );
  }
  catch ( std::exception& )
  {
    YIELD::Exception::set_errno( EIO );
  }

  return false;
}

ssize_t File::write( const void* buffer, size_t buffer_len, uint64_t offset )
{
  try
  {
    YIELD::auto_Object< YIELD::OneSignalEventQueue<> > write_response_queue( new YIELD::OneSignalEventQueue<> );

    const char* wbuf_p = static_cast<const char*>( buffer );
    uint64_t file_offset = offset, file_offset_max = offset + buffer_len;
    uint32_t stripe_size = file_credentials.get_xlocs().get_replicas()[0].get_striping_policy().get_stripe_size() * 1024;
    size_t expected_write_response_count = 0;

    while ( file_offset < file_offset_max )
    {
      uint64_t object_number = file_offset / stripe_size;
      uint32_t object_offset = file_offset % stripe_size;
      uint64_t object_size = file_offset_max - file_offset;
      if ( object_offset + object_size > stripe_size )
        object_size = stripe_size - object_offset;
      org::xtreemfs::interfaces::ObjectData object_data( 0, false, 0, new YIELD::StringLiteralBuffer( wbuf_p, static_cast<uint32_t>( object_size ) ) );
      org::xtreemfs::interfaces::OSDInterface::writeRequest* write_request = new org::xtreemfs::interfaces::OSDInterface::writeRequest( file_credentials, file_credentials.get_xcap().get_file_id(), object_number, 0, object_offset, 0, object_data );

      write_request->set_response_target( write_response_queue->incRef() );
      parent_volume->get_osd_proxy_mux()->send( *write_request );
      expected_write_response_count++;

      wbuf_p += object_size;
      file_offset += object_size;
    }

    for ( size_t write_response_i = 0; write_response_i < expected_write_response_count; write_response_i++ )
    {
      org::xtreemfs::interfaces::OSDInterface::writeResponse& write_response = write_response_queue->dequeue_typed<org::xtreemfs::interfaces::OSDInterface::writeResponse>();
      if ( write_response.get_osd_write_response() > latest_osd_write_response )
        latest_osd_write_response = write_response.get_osd_write_response();
      YIELD::Object::decRef( write_response );
    }

    if ( ( parent_volume->get_flags() & Volume::VOLUME_FLAG_CACHE_METADATA ) != Volume::VOLUME_FLAG_CACHE_METADATA )
      flush();

    return static_cast<ssize_t>( file_offset - offset );
  }
  catch ( ProxyExceptionResponse& proxy_exception_response )
  {
    YIELD::Exception::set_errno( proxy_exception_response.get_platform_error_code() );
  }
  catch ( std::exception& )
  {
    YIELD::Exception::set_errno( EIO );
  }

  return -1;
}

ssize_t File::writev( const struct iovec* buffers, uint32_t buffers_count, uint64_t offset )
{
  if ( buffers_count == 1 )
    return write( buffers[0].iov_base, buffers[0].iov_len, offset );
  else
  {
#ifdef _WIN32
    ::SetLastError( ERROR_NOT_SUPPORTED );
#else
    errno = ENOTSUP;
#endif
    return -1;
  }
}
<|endoftext|>


<commit_before>//=======================================================================
// Copyright Baptiste Wicht 2011-2012.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#include "ltac/Operator.hpp"

using namespace eddic;
using eddic::ltac::Operator;

bool ltac::erase_result(ltac::Operator op){
   return op == Operator::MOV
       || op == Operator::FMOV
       || op == Operator::MUL3
       || op == Operator::XOR
       || op == Operator::OR
       || (op >= Operator::LEA && op <= Operator::CMOVLE);
}

bool ltac::erase_result_complete(ltac::Operator op){
    return op == Operator::MOV
        || op == Operator::FMOV
        || op == Operator::LEA
        || op == Operator::MUL3;
}
<commit_msg>DIV does not erase the result<commit_after>//=======================================================================
// Copyright Baptiste Wicht 2011-2012.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#include "ltac/Operator.hpp"

using namespace eddic;
using eddic::ltac::Operator;

bool ltac::erase_result(ltac::Operator op){
    return op != Operator::DIV
        && (
                   op == Operator::MOV
                || op == Operator::FMOV
                || op == Operator::MUL3
                || op == Operator::XOR
                || op == Operator::OR
                || (op >= Operator::LEA && op <= Operator::CMOVLE)
         );
}

bool ltac::erase_result_complete(ltac::Operator op){
    return op == Operator::MOV
        || op == Operator::FMOV
        || op == Operator::LEA
        || op == Operator::MUL3;
}
<|endoftext|>


<commit_before>// Copyright 2019 Global Phasing Ltd.
//
// MTZ info

#include <gemmi/mtz.hpp>
#include <gemmi/fileutil.hpp> // for file_open
#include <gemmi/gz.hpp>       // for MaybeGzipped
#include <gemmi/input.hpp>    // for FileStream, MemoryStream
#define GEMMI_PROG mtz
#include "options.h"
#include <stdio.h>

using gemmi::Mtz;

enum OptionIndex { Headers=4, Dump, PrintTsv, PrintStats,
                   CheckAsu, ToggleEndian, NoIsym };

static const option::Descriptor Usage[] = {
  { NoOp, 0, "", "", Arg::None,
    "Usage:\n " EXE_NAME " [options] MTZ_FILE[...]"
    "\nPrint informations from an mtz file."},
  CommonUsage[Help],
  CommonUsage[Version],
  CommonUsage[Verbose],
  { Headers, 0, "H", "headers", Arg::None,
    "  -H, --headers  \tPrint raw headers, until the END record." },
  { Dump, 0, "d", "dump", Arg::None,
    "  -d, --dump  \tPrint a subset of CCP4 mtzdmp informations." },
  { PrintTsv, 0, "", "tsv", Arg::None,
    "  --tsv  \tPrint all the data as tab-separated values." },
  { PrintStats, 0, "", "stats", Arg::None,
    "  --stats  \tPrint column statistics (completeness, mean, etc)." },
  { CheckAsu, 0, "", "check-asu", Arg::None,
    "  --check-asu  \tCheck if reflections are in conventional ASU." },
  { ToggleEndian, 0, "", "toggle-endian", Arg::None,
    "  --toggle-endian  \tToggle assumed endiannes (little <-> big)." },
  { NoIsym, 0, "", "no-isym", Arg::None,
    "  --no-isym  \tDo not apply symmetry from M/ISYM column." },
  { 0, 0, 0, 0, 0, 0 }
};

static void dump(const Mtz& mtz) {
  printf("Title: %s\n", mtz.title.c_str());
  printf("Number of Datasets = %zu\n\n", mtz.datasets.size());
  for (const Mtz::Dataset& ds : mtz.datasets) {
    printf("Dataset %4d   %s > %s > %s:\n",
           ds.id, ds.project_name.c_str(),
           ds.crystal_name.c_str(), ds.dataset_name.c_str());
    printf("        cell  %g %7g %7g  %6g %6g %6g\n",
           ds.cell.a, ds.cell.b, ds.cell.c,
           ds.cell.alpha, ds.cell.beta, ds.cell.gamma);
    printf("  wavelength  %g\n", ds.wavelength);
  }
  printf("\nNumber of Columns = %zu\n", mtz.columns.size());
  printf("Number of Reflections = %d\n", mtz.nreflections);
  printf("Number of Batches = %zu\n", mtz.batches.size());
  printf("Missing values marked as: %g\n", mtz.valm);
  printf("Global Cell (obsolete):  %7.3f %7.3f %7.3f  %g %g %g\n",
         mtz.cell.a, mtz.cell.b, mtz.cell.c,
         mtz.cell.alpha, mtz.cell.beta, mtz.cell.gamma);
  printf("Resolution: %.2f - %.2f A\n",
         mtz.resolution_high(), mtz.resolution_low());
  printf("Sort Order: %d %d %d %d %d\n",
         mtz.sort_order[0], mtz.sort_order[1], mtz.sort_order[2],
         mtz.sort_order[3], mtz.sort_order[4]);
  printf("Space Group: %s\n", mtz.spacegroup_name.c_str());
  printf("Space Group Number: %d\n", mtz.spacegroup_number);
  printf("\nColumn    Type  Dataset    Min        Max\n");
  for (const Mtz::Column& col : mtz.columns)
    printf("%-12s %c %2d %12.6g %10.6g\n",
           col.label.c_str(), col.type, col.dataset_id,
           col.min_value, col.max_value);
  if (mtz.history.empty()) {
    printf("\nNo history in the file.\n");
  } else {
    printf("\nHistory (%zu lines):\n", mtz.history.size());
    for (const std::string& hline : mtz.history)
      printf("%s\n", hline.c_str());
  }
}

static void print_tsv(const Mtz& mtz) {
  size_t ncol = mtz.columns.size();
  for (size_t i = 0; i < ncol; ++i)
    printf("%s%c", mtz.columns[i].label.c_str(), i + 1 != ncol ? '\t' : '\n');
  for (size_t i = 0; i < mtz.nreflections * ncol; ++i)
    printf("%g%c", mtz.data[i], (i + 1) % ncol != 0 ? '\t' : '\n');
}

struct ColumnStats {
  float min_value = INFINITY;
  float max_value = -INFINITY;
  gemmi::Variance var;
};

static void print_stats(const Mtz& mtz) {
  std::vector<ColumnStats> column_stats(mtz.columns.size());
  for (size_t i = 0; i != mtz.data.size(); ++i) {
    float v = mtz.data[i];
    if (!std::isnan(v)) {
      ColumnStats& stat = column_stats[i % column_stats.size()];
      if (v < stat.min_value)
        stat.min_value = v;
      if (v > stat.max_value)
        stat.max_value = v;
      stat.var.add_point(v);
    }
  }
  printf("column type @dataset  completeness        min       max"
         "       mean   stddev\n");
  for (size_t i = 0; i != column_stats.size(); ++i) {
    const Mtz::Column& col = mtz.columns[i];
    const ColumnStats& stat = column_stats[i];
    printf("%-14s %c @%d  %d (%6.2f%%) %9.5g %9.5g  %9.5g %8.4g\n",
           col.label.c_str(), col.type, col.dataset_id,
           stat.var.n, 100.0 * stat.var.n / mtz.nreflections,
           stat.min_value, stat.max_value,
           stat.var.mean_x, std::sqrt(stat.var.for_population()));
  }
}

static void check_asu(const Mtz& mtz) {
  size_t ncol = mtz.columns.size();
  const gemmi::SpaceGroup* sg = mtz.spacegroup;
  if (!sg)
    gemmi::fail("no spacegroup in the MTZ file.");
  int counter = 0;
  gemmi::HklAsuChecker hkl_asu(sg);
  for (int i = 0; i < mtz.nreflections; ++i) {
    int h = (int) mtz.data[i * ncol + 0];
    int k = (int) mtz.data[i * ncol + 1];
    int l = (int) mtz.data[i * ncol + 2];
    if (hkl_asu.is_in(h, k, l))
      ++counter;
  }
  printf("spacegroup: %s\n", sg->xhm().c_str());
  printf("ccp4 ASU convention wrt. standard setting: %s\n",
         hkl_asu.condition_str());
  printf("inside / outside of ASU: %d / %d\n",
         counter, mtz.nreflections - counter);
}

template<typename Stream>
void print_mtz_info(Stream&& stream, const char* path,
                    const std::vector<option::Option>& options) {
  Mtz mtz;
  try {
    mtz.read_first_bytes(stream);
    if (options[ToggleEndian])
      mtz.toggle_endiannes();
  } catch (std::runtime_error& e) {
    gemmi::fail(std::string(e.what()) + ": " + path);
  }
  if (options[Headers]) {
    char buf[81] = {0};
    mtz.seek_headers(stream);
    while (stream.read(buf, 80)) {
      printf("%s\n", gemmi::rtrim_str(buf).c_str());
      if (gemmi::ialpha3_id(buf) == gemmi::ialpha3_id("END"))
        break;
    }
  }
  if (options[Verbose])
    mtz.warnings = stderr;
  mtz.read_main_headers(stream);
  mtz.read_history_and_batch_headers(stream);
  mtz.setup_spacegroup();
  if (options[Dump])
    dump(mtz);
  if (options[PrintTsv] || options[PrintStats] || options[CheckAsu]) {
    mtz.read_raw_data(stream);
    if (!options[NoIsym])
      mtz.apply_isym();
  }
  if (options[PrintTsv])
    print_tsv(mtz);
  if (options[PrintStats])
    print_stats(mtz);
  if (options[CheckAsu])
    check_asu(mtz);
}

int GEMMI_MAIN(int argc, char **argv) {
  OptParser p(EXE_NAME);
  p.simple_parse(argc, argv, Usage);
  p.require_input_files_as_args();
  try {
    for (int i = 0; i < p.nonOptionsCount(); ++i) {
      const char* path = p.nonOption(i);
      if (i != 0)
        printf("\n\n");
      if (p.options[Verbose])
        fprintf(stderr, "Reading %s ...\n", path);
      gemmi::MaybeGzipped input(path);
      if (input.is_stdin()) {
        print_mtz_info(gemmi::FileStream{stdin}, path, p.options);
      } else if (std::unique_ptr<char[]> mem = input.memory()) {
        gemmi::MemoryStream stream(mem.get(), mem.get() + input.memory_size());
        print_mtz_info(std::move(stream), path, p.options);
      } else {
        gemmi::fileptr_t f = gemmi::file_open(input.path().c_str(), "rb");
        print_mtz_info(gemmi::FileStream{f.get()}, path, p.options);
      }
    }
  } catch (std::runtime_error& e) {
    fprintf(stderr, "ERROR: %s\n", e.what());
    return 1;
  }
  return 0;
}

// vim:sw=2:ts=2:et:path^=../include,../third_party
<commit_msg>gemmi-mtz: when invoked without options print the same as with --dump<commit_after>// Copyright 2019 Global Phasing Ltd.
//
// MTZ info

#include <gemmi/mtz.hpp>
#include <gemmi/fileutil.hpp> // for file_open
#include <gemmi/gz.hpp>       // for MaybeGzipped
#include <gemmi/input.hpp>    // for FileStream, MemoryStream
#define GEMMI_PROG mtz
#include "options.h"
#include <stdio.h>

using gemmi::Mtz;

enum OptionIndex { Headers=4, Dump, PrintTsv, PrintStats,
                   CheckAsu, ToggleEndian, NoIsym };

static const option::Descriptor Usage[] = {
  { NoOp, 0, "", "", Arg::None,
    "Usage:\n " EXE_NAME " [options] MTZ_FILE[...]"
    "\nPrint informations from an mtz file."},
  CommonUsage[Help],
  CommonUsage[Version],
  CommonUsage[Verbose],
  { Headers, 0, "H", "headers", Arg::None,
    "  -H, --headers  \tPrint raw headers, until the END record." },
  { Dump, 0, "d", "dump", Arg::None,
    "  -d, --dump  \tPrint a subset of CCP4 mtzdmp informations." },
  { PrintTsv, 0, "", "tsv", Arg::None,
    "  --tsv  \tPrint all the data as tab-separated values." },
  { PrintStats, 0, "", "stats", Arg::None,
    "  --stats  \tPrint column statistics (completeness, mean, etc)." },
  { CheckAsu, 0, "", "check-asu", Arg::None,
    "  --check-asu  \tCheck if reflections are in conventional ASU." },
  { ToggleEndian, 0, "", "toggle-endian", Arg::None,
    "  --toggle-endian  \tToggle assumed endiannes (little <-> big)." },
  { NoIsym, 0, "", "no-isym", Arg::None,
    "  --no-isym  \tDo not apply symmetry from M/ISYM column." },
  { 0, 0, 0, 0, 0, 0 }
};

static void dump(const Mtz& mtz) {
  printf("Title: %s\n", mtz.title.c_str());
  printf("Number of Datasets = %zu\n\n", mtz.datasets.size());
  for (const Mtz::Dataset& ds : mtz.datasets) {
    printf("Dataset %4d   %s > %s > %s:\n",
           ds.id, ds.project_name.c_str(),
           ds.crystal_name.c_str(), ds.dataset_name.c_str());
    printf("        cell  %g %7g %7g  %6g %6g %6g\n",
           ds.cell.a, ds.cell.b, ds.cell.c,
           ds.cell.alpha, ds.cell.beta, ds.cell.gamma);
    printf("  wavelength  %g\n", ds.wavelength);
  }
  printf("\nNumber of Columns = %zu\n", mtz.columns.size());
  printf("Number of Reflections = %d\n", mtz.nreflections);
  printf("Number of Batches = %zu\n", mtz.batches.size());
  printf("Missing values marked as: %g\n", mtz.valm);
  printf("Global Cell (obsolete):  %7.3f %7.3f %7.3f  %g %g %g\n",
         mtz.cell.a, mtz.cell.b, mtz.cell.c,
         mtz.cell.alpha, mtz.cell.beta, mtz.cell.gamma);
  printf("Resolution: %.2f - %.2f A\n",
         mtz.resolution_high(), mtz.resolution_low());
  printf("Sort Order: %d %d %d %d %d\n",
         mtz.sort_order[0], mtz.sort_order[1], mtz.sort_order[2],
         mtz.sort_order[3], mtz.sort_order[4]);
  printf("Space Group: %s\n", mtz.spacegroup_name.c_str());
  printf("Space Group Number: %d\n", mtz.spacegroup_number);
  printf("\nColumn    Type  Dataset    Min        Max\n");
  for (const Mtz::Column& col : mtz.columns)
    printf("%-12s %c %2d %12.6g %10.6g\n",
           col.label.c_str(), col.type, col.dataset_id,
           col.min_value, col.max_value);
  if (mtz.history.empty()) {
    printf("\nNo history in the file.\n");
  } else {
    printf("\nHistory (%zu lines):\n", mtz.history.size());
    for (const std::string& hline : mtz.history)
      printf("%s\n", hline.c_str());
  }
}

static void print_tsv(const Mtz& mtz) {
  size_t ncol = mtz.columns.size();
  for (size_t i = 0; i < ncol; ++i)
    printf("%s%c", mtz.columns[i].label.c_str(), i + 1 != ncol ? '\t' : '\n');
  for (size_t i = 0; i < mtz.nreflections * ncol; ++i)
    printf("%g%c", mtz.data[i], (i + 1) % ncol != 0 ? '\t' : '\n');
}

struct ColumnStats {
  float min_value = INFINITY;
  float max_value = -INFINITY;
  gemmi::Variance var;
};

static void print_stats(const Mtz& mtz) {
  std::vector<ColumnStats> column_stats(mtz.columns.size());
  for (size_t i = 0; i != mtz.data.size(); ++i) {
    float v = mtz.data[i];
    if (!std::isnan(v)) {
      ColumnStats& stat = column_stats[i % column_stats.size()];
      if (v < stat.min_value)
        stat.min_value = v;
      if (v > stat.max_value)
        stat.max_value = v;
      stat.var.add_point(v);
    }
  }
  printf("column type @dataset  completeness        min       max"
         "       mean   stddev\n");
  for (size_t i = 0; i != column_stats.size(); ++i) {
    const Mtz::Column& col = mtz.columns[i];
    const ColumnStats& stat = column_stats[i];
    printf("%-14s %c @%d  %d (%6.2f%%) %9.5g %9.5g  %9.5g %8.4g\n",
           col.label.c_str(), col.type, col.dataset_id,
           stat.var.n, 100.0 * stat.var.n / mtz.nreflections,
           stat.min_value, stat.max_value,
           stat.var.mean_x, std::sqrt(stat.var.for_population()));
  }
}

static void check_asu(const Mtz& mtz) {
  size_t ncol = mtz.columns.size();
  const gemmi::SpaceGroup* sg = mtz.spacegroup;
  if (!sg)
    gemmi::fail("no spacegroup in the MTZ file.");
  int counter = 0;
  gemmi::HklAsuChecker hkl_asu(sg);
  for (int i = 0; i < mtz.nreflections; ++i) {
    int h = (int) mtz.data[i * ncol + 0];
    int k = (int) mtz.data[i * ncol + 1];
    int l = (int) mtz.data[i * ncol + 2];
    if (hkl_asu.is_in(h, k, l))
      ++counter;
  }
  printf("spacegroup: %s\n", sg->xhm().c_str());
  printf("ccp4 ASU convention wrt. standard setting: %s\n",
         hkl_asu.condition_str());
  printf("inside / outside of ASU: %d / %d\n",
         counter, mtz.nreflections - counter);
}

template<typename Stream>
void print_mtz_info(Stream&& stream, const char* path,
                    const std::vector<option::Option>& options) {
  Mtz mtz;
  try {
    mtz.read_first_bytes(stream);
    if (options[ToggleEndian])
      mtz.toggle_endiannes();
  } catch (std::runtime_error& e) {
    gemmi::fail(std::string(e.what()) + ": " + path);
  }
  if (options[Headers]) {
    char buf[81] = {0};
    mtz.seek_headers(stream);
    while (stream.read(buf, 80)) {
      printf("%s\n", gemmi::rtrim_str(buf).c_str());
      if (gemmi::ialpha3_id(buf) == gemmi::ialpha3_id("END"))
        break;
    }
  }
  if (options[Verbose])
    mtz.warnings = stderr;
  mtz.read_main_headers(stream);
  mtz.read_history_and_batch_headers(stream);
  mtz.setup_spacegroup();
  if (options[Dump] ||
      !(options[PrintTsv] || options[PrintStats] || options[CheckAsu] ||
        options[Headers]))
    dump(mtz);
  if (options[PrintTsv] || options[PrintStats] || options[CheckAsu]) {
    mtz.read_raw_data(stream);
    if (!options[NoIsym])
      mtz.apply_isym();
  }
  if (options[PrintTsv])
    print_tsv(mtz);
  if (options[PrintStats])
    print_stats(mtz);
  if (options[CheckAsu])
    check_asu(mtz);
}

int GEMMI_MAIN(int argc, char **argv) {
  OptParser p(EXE_NAME);
  p.simple_parse(argc, argv, Usage);
  p.require_input_files_as_args();
  try {
    for (int i = 0; i < p.nonOptionsCount(); ++i) {
      const char* path = p.nonOption(i);
      if (i != 0)
        printf("\n\n");
      if (p.options[Verbose])
        fprintf(stderr, "Reading %s ...\n", path);
      gemmi::MaybeGzipped input(path);
      if (input.is_stdin()) {
        print_mtz_info(gemmi::FileStream{stdin}, path, p.options);
      } else if (std::unique_ptr<char[]> mem = input.memory()) {
        gemmi::MemoryStream stream(mem.get(), mem.get() + input.memory_size());
        print_mtz_info(std::move(stream), path, p.options);
      } else {
        gemmi::fileptr_t f = gemmi::file_open(input.path().c_str(), "rb");
        print_mtz_info(gemmi::FileStream{f.get()}, path, p.options);
      }
    }
  } catch (std::runtime_error& e) {
    fprintf(stderr, "ERROR: %s\n", e.what());
    return 1;
  }
  return 0;
}

// vim:sw=2:ts=2:et:path^=../include,../third_party
<|endoftext|>


<commit_before>/*
 * Copyright (C) 2013-2014 Phokham Nonava
 *
 * Use of this source code is governed by the MIT license that can be
 * found in the LICENSE file.
 */

#include "pulse.h"
#include "perft.h"

#include <string>
#include <iostream>

int main(int argc, char* argv[]) {
  try {
    if (argc == 2) {
      std::string token(argv[1]);
      if (token == "perft") {
        std::unique_ptr<pulse::Perft> perft(new pulse::Perft());
        perft->run();
      }
    } else {
      std::unique_ptr<pulse::Pulse> uci(new pulse::Pulse());
      uci->run();
    }
  } catch (std::exception e) {
    std::cout << "Exiting Pulse due to an exception: " << e.what() << std::endl;
    return 1;
  }
}
<commit_msg>Fix variable name<commit_after>/*
 * Copyright (C) 2013-2014 Phokham Nonava
 *
 * Use of this source code is governed by the MIT license that can be
 * found in the LICENSE file.
 */

#include "pulse.h"
#include "perft.h"

#include <string>
#include <iostream>

int main(int argc, char* argv[]) {
  try {
    if (argc == 2) {
      std::string token(argv[1]);
      if (token == "perft") {
        std::unique_ptr<pulse::Perft> perft(new pulse::Perft());
        perft->run();
      }
    } else {
      std::unique_ptr<pulse::Pulse> pulse(new pulse::Pulse());
      pulse->run();
    }
  } catch (std::exception e) {
    std::cout << "Exiting Pulse due to an exception: " << e.what() << std::endl;
    return 1;
  }
}
<|endoftext|>


<commit_before>/*
 * Copyright (C) 2016 Fondazione Istituto Italiano di Tecnologia
 * Authors: Silvio Traversaro
 * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
 *
 */

#include <iDynTree/Core/EigenHelpers.h>

#include <iDynTree/Estimation/SimpleLeggedOdometry.h>

#include <iDynTree/Model/ForwardKinematics.h>
#include <iDynTree/Model/Indeces.h>
#include <iDynTree/Model/ModelTransformers.h>

#include <iDynTree/ModelIO/URDFModelImport.h>
#include <iDynTree/ModelIO/URDFGenericSensorsImport.h>

#include <sstream>

namespace iDynTree
{

SimpleLeggedOdometry::SimpleLeggedOdometry(): m_model(),
                                              m_traversal(),
                                              m_isModelValid(false),
                                              m_kinematicsUpdated(false),
                                              m_isOdometryInitialized(false),
                                              m_fixedLinkIndex(iDynTree::LINK_INVALID_INDEX),
                                              m_world_H_fixedLink(Transform::Identity())
{
}

SimpleLeggedOdometry::~SimpleLeggedOdometry()
{
}

bool SimpleLeggedOdometry::init(const std::string& initialFixedFrame,
                                const Transform& world_H_initialFixedFrame)
{
    FrameIndex initialFixedFrameIndex = this->m_model.getFrameIndex(initialFixedFrame);

    return init(initialFixedFrameIndex,world_H_initialFixedFrame);
}

bool SimpleLeggedOdometry::init(const FrameIndex initialFixedFrameIndex,
                                const Transform& world_H_initialFixedFrame)
{
    if( !m_isModelValid )
    {
         reportError("SimpleLeggedOdometry",
                     "init",
                     "Model not initialised.");
         return false;
    }

    if( !m_model.isValidFrameIndex(initialFixedFrameIndex) )
    {
        reportError("SimpleLeggedOdometry",
                    "init","invalid frame passed");
        return false;
    }

    m_fixedLinkIndex = m_model.getFrameLink(initialFixedFrameIndex);
    Transform initialFixedFrame_H_fixedLink = m_model.getFrameTransform(initialFixedFrameIndex).inverse();
    m_world_H_fixedLink = world_H_initialFixedFrame*initialFixedFrame_H_fixedLink;

    m_isOdometryInitialized = true;

    return true;
}

bool SimpleLeggedOdometry::init(const std::string& initialFixedFrame,
                                const std::string& initialWorldFrame)
{
    return this->init(m_model.getFrameIndex(initialFixedFrame),
                      m_model.getFrameIndex(initialWorldFrame));
}

bool SimpleLeggedOdometry::init(const FrameIndex initialFixedFrameIndex,
                                const FrameIndex initialWorldFrameIndex)
{
    if( !m_isModelValid )
    {
         reportError("SimpleLeggedOdometry",
                     "init",
                     "Model not initialised.");
         return false;
    }

    if( !m_model.isValidFrameIndex(initialFixedFrameIndex) ||
        !m_model.isValidFrameIndex(initialWorldFrameIndex)
    )
    {
        reportError("SimpleLeggedOdometry",
                    "init","invalid frame passed");
        return false;
    }

    if( ! m_kinematicsUpdated )
    {
        reportError("SimpleLeggedOdometry",
                    "init","updateKinematics never called");
        return false;
    }

    // Set the fixed link
    m_fixedLinkIndex = m_model.getFrameLink(initialFixedFrameIndex);

    // TODO : we need a simple class to get arbitrary transform in a model,
    //        something even simpler then KinDynComputations to address this
    //        kind of computation that appear from time to time
    // Set the world_H_fixedLink transform
    LinkIndex linkAttachedToWorldFrameIndex = m_model.getFrameLink(initialWorldFrameIndex);
    Transform worldFrame_H_linkAttachedToWorldFrame = m_model.getFrameTransform(initialWorldFrameIndex).inverse();
    Transform linkAttachedToWorldFrameIndex_H_floatingBase = m_base_H_link(linkAttachedToWorldFrameIndex).inverse();
    Transform floatingBase_H_fixedLink = m_base_H_link(m_fixedLinkIndex);

    m_world_H_fixedLink = worldFrame_H_linkAttachedToWorldFrame*linkAttachedToWorldFrameIndex_H_floatingBase*floatingBase_H_fixedLink;

    m_isOdometryInitialized = true;

    return true;
}

bool SimpleLeggedOdometry::updateKinematics(JointPosDoubleArray& jointPos)
{
    if( !m_isModelValid )
    {
        reportError("SimpleLeggedOdometry",
                    "updateKinematics","model not valid");
        return false;
    }

    if( !jointPos.isConsistent(m_model) )
    {
        reportError("SimpleLeggedOdometry",
                    "updateKinematics","error in size of input jointPos");
        return false;
    }

    bool ok = ForwardPositionKinematics(m_model,m_traversal,
                                        Transform::Identity(),jointPos,
                                        m_base_H_link);

    m_kinematicsUpdated = ok;

    return ok;
}

const Model& SimpleLeggedOdometry::model() const
{
    return m_model;
}

bool SimpleLeggedOdometry::setModel(const Model& _model)
{
    m_model = _model;

    // resize the data structures
    m_model.computeFullTreeTraversal(m_traversal);

    // set that the model is valid
    m_isModelValid = true;

    // Set the kinematics and the odometry is not initialized
    m_isOdometryInitialized          = false;
    m_kinematicsUpdated              = false;

    // Resize the linkPositions
    m_base_H_link.resize(m_model);

    return true;
}

bool SimpleLeggedOdometry::loadModelFromFile(const std::string filename,
                                             const std::string /*filetype*/)
{
    Model _model;

    bool parsingCorrect = false;

    parsingCorrect = modelFromURDF(filename,_model);

    if( !parsingCorrect )
    {
        reportError("SimpleLeggedOdometry",
                    "loadModelFromFile",
                    "Error in parsing model from URDF.");
        return false;
    }

    return setModel(_model);
}

bool SimpleLeggedOdometry::loadModelFromFileWithSpecifiedDOFs(const std::string filename,
                                                              const std::vector< std::string >& consideredDOFs,
                                                              const std::string filetype)
{
    Model _modelFull;

    bool parsingCorrect = false;

    parsingCorrect = modelFromURDF(filename,_modelFull);

    if( !parsingCorrect )
    {
        reportError("SimpleLeggedOdometry",
                    "loadModelFromFileWithSpecifiedDOFs",
                    "Error in parsing model from URDF.");
        return false;
    }

    Model _modelReduced;

    // Create a reduced model: this will lump all not considered joints
    iDynTree::createReducedModel(_modelFull,consideredDOFs,_modelReduced);

    return setModel(_modelReduced);
}

std::string SimpleLeggedOdometry::getCurrentFixedLink()
{
    if( this->m_isModelValid && this->m_isOdometryInitialized )
    {
        return this->m_model.getLinkName(this->m_fixedLinkIndex);
    }
    else
    {
        reportError("SimpleLeggedOdometry",
                    "getCurrentFixedLink",
                    "getCurrentFixedLink was called, but no model is available or the odometry is not initialized.");
        return "";
    }
}

bool SimpleLeggedOdometry::changeFixedFrame(const FrameIndex newFixedFrame)
{
    if( !this->m_kinematicsUpdated )
    {
        reportError("SimpleLeggedOdometry",
                    "changeFixedFrame",
                    "changeFixedFrame was called, but the kinematics info was never setted.");
        return false;
    }

    LinkIndex newFixedLink = this->m_model.getFrameLink(newFixedFrame);

    if( newFixedLink == LINK_INVALID_INDEX )
    {
        reportError("SimpleLeggedOdometry",
                    "changeFixedFrame",
                    "changeFixedFrame was called, but the provided new fixed frame is unknown.");
        return false;
    }

    Transform world_H_oldFixed = this->m_world_H_fixedLink;
    LinkIndex oldFixedLink = m_fixedLinkIndex;
    Transform oldFixed_H_newFixed = m_base_H_link(oldFixedLink).inverse()*m_base_H_link(newFixedLink);
    Transform world_H_newFixed = world_H_oldFixed*oldFixed_H_newFixed;
    this->m_world_H_fixedLink = world_H_newFixed;
    this->m_fixedLinkIndex = newFixedLink;

    return true;
}

bool SimpleLeggedOdometry::changeFixedFrame(const std::string& newFixedFrame)
{
    iDynTree::FrameIndex newFixedFrameIndex = this->m_model.getFrameIndex(newFixedFrame);

    if( newFixedFrameIndex == FRAME_INVALID_INDEX )
    {
        reportError("SimpleLeggedOdometry",
                    "changeFixedFrame",
                    "changeFixedFrame was called, but the provided new fixed frame is unknown.");
        return false;
    }

    return this->changeFixedFrame(newFixedFrameIndex);
}

Transform SimpleLeggedOdometry::getWorldLinkTransform(const LinkIndex link_index)
{
    if( !this->m_kinematicsUpdated || !this->m_isOdometryInitialized  )
    {
        reportError("SimpleLeggedOdometry",
                    "getWorldLinkTransform",
                    "getWorldLinkTransform was called, but the kinematics update or the odometry init was never setted.");
        return Transform::Identity();
    }

    if( !this->m_model.isValidLinkIndex(link_index) )
    {
        reportError("SimpleLeggedOdometry",
                    "getWorldLinkTransform",
                    "getWorldLinkTransform was called, but the request linkindex is not part of the model");
        return Transform::Identity();
    }

    std::cerr << m_fixedLinkIndex << std::endl;
    std::cerr << m_base_H_link.getNrOfLinks() << std::endl;

    assert(m_fixedLinkIndex < m_base_H_link.getNrOfLinks());
    Transform base_H_fixed = m_base_H_link(m_fixedLinkIndex);
    Transform base_H_link =  m_base_H_link(link_index);

    std::cerr << "base_H_fixed " << base_H_fixed.toString() << std::endl;
    std::cerr << "base_H_link " << base_H_link.toString() << std::endl;
    std::cerr << "m_world_H_fixedLink " << m_world_H_fixedLink.toString() << std::endl;

    return m_world_H_fixedLink*base_H_fixed.inverse()*base_H_link;
}

}

<commit_msg>[estimation] Removed spurious prints<commit_after>/*
 * Copyright (C) 2016 Fondazione Istituto Italiano di Tecnologia
 * Authors: Silvio Traversaro
 * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
 *
 */

#include <iDynTree/Core/EigenHelpers.h>

#include <iDynTree/Estimation/SimpleLeggedOdometry.h>

#include <iDynTree/Model/ForwardKinematics.h>
#include <iDynTree/Model/Indeces.h>
#include <iDynTree/Model/ModelTransformers.h>

#include <iDynTree/ModelIO/URDFModelImport.h>
#include <iDynTree/ModelIO/URDFGenericSensorsImport.h>

#include <sstream>

namespace iDynTree
{

SimpleLeggedOdometry::SimpleLeggedOdometry(): m_model(),
                                              m_traversal(),
                                              m_isModelValid(false),
                                              m_kinematicsUpdated(false),
                                              m_isOdometryInitialized(false),
                                              m_fixedLinkIndex(iDynTree::LINK_INVALID_INDEX),
                                              m_world_H_fixedLink(Transform::Identity())
{
}

SimpleLeggedOdometry::~SimpleLeggedOdometry()
{
}

bool SimpleLeggedOdometry::init(const std::string& initialFixedFrame,
                                const Transform& world_H_initialFixedFrame)
{
    FrameIndex initialFixedFrameIndex = this->m_model.getFrameIndex(initialFixedFrame);

    return init(initialFixedFrameIndex,world_H_initialFixedFrame);
}

bool SimpleLeggedOdometry::init(const FrameIndex initialFixedFrameIndex,
                                const Transform& world_H_initialFixedFrame)
{
    if( !m_isModelValid )
    {
         reportError("SimpleLeggedOdometry",
                     "init",
                     "Model not initialised.");
         return false;
    }

    if( !m_model.isValidFrameIndex(initialFixedFrameIndex) )
    {
        reportError("SimpleLeggedOdometry",
                    "init","invalid frame passed");
        return false;
    }

    m_fixedLinkIndex = m_model.getFrameLink(initialFixedFrameIndex);
    Transform initialFixedFrame_H_fixedLink = m_model.getFrameTransform(initialFixedFrameIndex).inverse();
    m_world_H_fixedLink = world_H_initialFixedFrame*initialFixedFrame_H_fixedLink;

    m_isOdometryInitialized = true;

    return true;
}

bool SimpleLeggedOdometry::init(const std::string& initialFixedFrame,
                                const std::string& initialWorldFrame)
{
    return this->init(m_model.getFrameIndex(initialFixedFrame),
                      m_model.getFrameIndex(initialWorldFrame));
}

bool SimpleLeggedOdometry::init(const FrameIndex initialFixedFrameIndex,
                                const FrameIndex initialWorldFrameIndex)
{
    if( !m_isModelValid )
    {
         reportError("SimpleLeggedOdometry",
                     "init",
                     "Model not initialised.");
         return false;
    }

    if( !m_model.isValidFrameIndex(initialFixedFrameIndex) ||
        !m_model.isValidFrameIndex(initialWorldFrameIndex)
    )
    {
        reportError("SimpleLeggedOdometry",
                    "init","invalid frame passed");
        return false;
    }

    if( ! m_kinematicsUpdated )
    {
        reportError("SimpleLeggedOdometry",
                    "init","updateKinematics never called");
        return false;
    }

    // Set the fixed link
    m_fixedLinkIndex = m_model.getFrameLink(initialFixedFrameIndex);

    // TODO : we need a simple class to get arbitrary transform in a model,
    //        something even simpler then KinDynComputations to address this
    //        kind of computation that appear from time to time
    // Set the world_H_fixedLink transform
    LinkIndex linkAttachedToWorldFrameIndex = m_model.getFrameLink(initialWorldFrameIndex);
    Transform worldFrame_H_linkAttachedToWorldFrame = m_model.getFrameTransform(initialWorldFrameIndex).inverse();
    Transform linkAttachedToWorldFrameIndex_H_floatingBase = m_base_H_link(linkAttachedToWorldFrameIndex).inverse();
    Transform floatingBase_H_fixedLink = m_base_H_link(m_fixedLinkIndex);

    m_world_H_fixedLink = worldFrame_H_linkAttachedToWorldFrame*linkAttachedToWorldFrameIndex_H_floatingBase*floatingBase_H_fixedLink;

    m_isOdometryInitialized = true;

    return true;
}

bool SimpleLeggedOdometry::updateKinematics(JointPosDoubleArray& jointPos)
{
    if( !m_isModelValid )
    {
        reportError("SimpleLeggedOdometry",
                    "updateKinematics","model not valid");
        return false;
    }

    if( !jointPos.isConsistent(m_model) )
    {
        reportError("SimpleLeggedOdometry",
                    "updateKinematics","error in size of input jointPos");
        return false;
    }

    bool ok = ForwardPositionKinematics(m_model,m_traversal,
                                        Transform::Identity(),jointPos,
                                        m_base_H_link);

    m_kinematicsUpdated = ok;

    return ok;
}

const Model& SimpleLeggedOdometry::model() const
{
    return m_model;
}

bool SimpleLeggedOdometry::setModel(const Model& _model)
{
    m_model = _model;

    // resize the data structures
    m_model.computeFullTreeTraversal(m_traversal);

    // set that the model is valid
    m_isModelValid = true;

    // Set the kinematics and the odometry is not initialized
    m_isOdometryInitialized          = false;
    m_kinematicsUpdated              = false;

    // Resize the linkPositions
    m_base_H_link.resize(m_model);

    return true;
}

bool SimpleLeggedOdometry::loadModelFromFile(const std::string filename,
                                             const std::string /*filetype*/)
{
    Model _model;

    bool parsingCorrect = false;

    parsingCorrect = modelFromURDF(filename,_model);

    if( !parsingCorrect )
    {
        reportError("SimpleLeggedOdometry",
                    "loadModelFromFile",
                    "Error in parsing model from URDF.");
        return false;
    }

    return setModel(_model);
}

bool SimpleLeggedOdometry::loadModelFromFileWithSpecifiedDOFs(const std::string filename,
                                                              const std::vector< std::string >& consideredDOFs,
                                                              const std::string filetype)
{
    Model _modelFull;

    bool parsingCorrect = false;

    parsingCorrect = modelFromURDF(filename,_modelFull);

    if( !parsingCorrect )
    {
        reportError("SimpleLeggedOdometry",
                    "loadModelFromFileWithSpecifiedDOFs",
                    "Error in parsing model from URDF.");
        return false;
    }

    Model _modelReduced;

    // Create a reduced model: this will lump all not considered joints
    iDynTree::createReducedModel(_modelFull,consideredDOFs,_modelReduced);

    return setModel(_modelReduced);
}

std::string SimpleLeggedOdometry::getCurrentFixedLink()
{
    if( this->m_isModelValid && this->m_isOdometryInitialized )
    {
        return this->m_model.getLinkName(this->m_fixedLinkIndex);
    }
    else
    {
        reportError("SimpleLeggedOdometry",
                    "getCurrentFixedLink",
                    "getCurrentFixedLink was called, but no model is available or the odometry is not initialized.");
        return "";
    }
}

bool SimpleLeggedOdometry::changeFixedFrame(const FrameIndex newFixedFrame)
{
    if( !this->m_kinematicsUpdated )
    {
        reportError("SimpleLeggedOdometry",
                    "changeFixedFrame",
                    "changeFixedFrame was called, but the kinematics info was never setted.");
        return false;
    }

    LinkIndex newFixedLink = this->m_model.getFrameLink(newFixedFrame);

    if( newFixedLink == LINK_INVALID_INDEX )
    {
        reportError("SimpleLeggedOdometry",
                    "changeFixedFrame",
                    "changeFixedFrame was called, but the provided new fixed frame is unknown.");
        return false;
    }

    Transform world_H_oldFixed = this->m_world_H_fixedLink;
    LinkIndex oldFixedLink = m_fixedLinkIndex;
    Transform oldFixed_H_newFixed = m_base_H_link(oldFixedLink).inverse()*m_base_H_link(newFixedLink);
    Transform world_H_newFixed = world_H_oldFixed*oldFixed_H_newFixed;
    this->m_world_H_fixedLink = world_H_newFixed;
    this->m_fixedLinkIndex = newFixedLink;

    return true;
}

bool SimpleLeggedOdometry::changeFixedFrame(const std::string& newFixedFrame)
{
    iDynTree::FrameIndex newFixedFrameIndex = this->m_model.getFrameIndex(newFixedFrame);

    if( newFixedFrameIndex == FRAME_INVALID_INDEX )
    {
        reportError("SimpleLeggedOdometry",
                    "changeFixedFrame",
                    "changeFixedFrame was called, but the provided new fixed frame is unknown.");
        return false;
    }

    return this->changeFixedFrame(newFixedFrameIndex);
}

Transform SimpleLeggedOdometry::getWorldLinkTransform(const LinkIndex link_index)
{
    if( !this->m_kinematicsUpdated || !this->m_isOdometryInitialized  )
    {
        reportError("SimpleLeggedOdometry",
                    "getWorldLinkTransform",
                    "getWorldLinkTransform was called, but the kinematics update or the odometry init was never setted.");
        return Transform::Identity();
    }

    if( !this->m_model.isValidLinkIndex(link_index) )
    {
        reportError("SimpleLeggedOdometry",
                    "getWorldLinkTransform",
                    "getWorldLinkTransform was called, but the request linkindex is not part of the model");
        return Transform::Identity();
    }

    assert(m_fixedLinkIndex < m_base_H_link.getNrOfLinks());
    Transform base_H_fixed = m_base_H_link(m_fixedLinkIndex);
    Transform base_H_link =  m_base_H_link(link_index);

    return m_world_H_fixedLink*base_H_fixed.inverse()*base_H_link;
}

}

<|endoftext|>


<commit_before>/** \copyright
 * Copyright (c) 2013, Balazs Racz
 * 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.
 *
 * \file mbed_usbserial.cpp
 * This file implements an USB-Serial driver on top of the mbed
 * library. Currently it is tested on the LPC23xx processors.
 *
 * @author Balazs Racz
 * @date 4 May 2013
 */

#include "mbed.h"
#include "USBSerial.h"
#include "serial.h"
#include "os/os.h"

#ifdef TARGET_LPC2368
#endif

#define TX_DATA_SIZE 64
#define RX_DATA_SIZE 64

#include <stdlib.h>

void * operator new(size_t size);
void * operator new[](size_t size);
void operator delete(void * ptr);
void operator delete[](void * ptr);

__extension__ typedef int __guard __attribute__((mode (__DI__)));

extern "C" int __cxa_guard_acquire(__guard *);
extern "C" void __cxa_guard_release (__guard *);
extern "C" void __cxa_guard_abort (__guard *);

extern "C" void __cxa_pure_virtual(void);

/** This class is an empty wrapper around MBed's USB CDC class. The difference
    between this and mbed::USBSerial is that this class does not have any
    buffering and no interaction with stdio, whereas mbed::USBSerial has the
    following buffering:
    * it has a 128-byte receive buffer.
    * it has an fd
    * it has a FILE* with a default-sized send/receive buffer
    * it requires mbed's custom glue code in the open(2) method, without which
      it crashes.
 */
class MbedRawUSBSerial : public USBCDC
{
public:
    MbedRawUSBSerial(SerialPriv* serialPriv, uint16_t vendor_id = 0x1f00, uint16_t product_id = 0x2012, uint16_t product_release = 0x0001): USBCDC(vendor_id, product_id, product_release), serialPriv(serialPriv), txPending(false)
    {
	os_sem_init(&rxSem, 0);
	os_thread_t thread;
	os_thread_create(&thread, "usbserial.rx", 2, 1024, &_RxThread, this);
    }

    ~MbedRawUSBSerial()
    {
	os_sem_destroy(&rxSem);
    }

    void Transmit()
    {
	if (txPending) return;
	txPending = true;
	// At this point we know there is no outstading send, thus there can't
	// be an incoming TX interrupt either. Thus we don't need a critical
	// section.
	int count;
	for (count = 0; count < TX_DATA_SIZE; count++)
	{
	    if (os_mq_timedreceive(serialPriv->txQ, txData+count, 0) != OS_MQ_NONE)
	    {
		/* no more data left to transmit */
		break;
	    }
	}
	TxHelper(count);
    }

protected:
    virtual bool EP2_OUT_callback()
    {
	// we read the packet received to our assembly buffer
	readEP(rxData, &rxSize);
	// and wake up the RX thread.
	os_sem_post_from_isr(&rxSem);
	return true;
    }

    virtual bool EP2_IN_callback()
    {
	int count;
	for (count = 0; count < MAX_TX_PACKET_LENGTH; count++)
	{
	    if (os_mq_receive_from_isr(serialPriv->txQ, &txData[count]) != OS_MQ_NONE)
	    {
		/* no more data left to transmit */
		break;
	    }
	}
	TxHelper(count);
	return true;
    }

private:
    static const int MAX_TX_PACKET_LENGTH = 64;
    static const int MAX_RX_PACKET_LENGTH = 64;

    /** Transmits count bytes from the txData buffer. Sets txPending and
	bytesLost as needed. */
    void TxHelper(int count)
    {
	if (!count)
	{
	    txPending = false;
	    return;
	}
	if ((!configured()) ||
	    (!terminal_connected))
	{
	    // An error occured, data was lost.
	    txPending = false;
	    serialPriv->overrunCount += count;
	    return;
	}
	sendNB(txData, count);
	txPending = true;
    }

    void RxThread()
    {
	while(1)
	{
	    os_sem_wait(&rxSem);
	    for (uint32_t i = 0; i < rxSize; i++)
	    {
		os_mq_send(serialPriv->rxQ, rxData+i);
	    }
	    // We reactivate the endpoint to receive next characters
	    readStart(EPBULK_OUT, MAX_PACKET_SIZE_EPBULK);
	}
    }

    static void* _RxThread(void* arg)
    {
	((MbedRawUSBSerial*)arg)->RxThread();
	return NULL;
    }

    uint8_t txData[MAX_TX_PACKET_LENGTH]; /**< packet assemby buffer to host */
    uint32_t rxSize; /**< number of valis characters in rxData */
    uint8_t rxData[MAX_RX_PACKET_LENGTH]; /**< packet assembly buffer from host */
    SerialPriv* serialPriv;
    bool txPending; /**< transmission currently pending */
    os_sem_t rxSem;
};

/** Private data for this implementation of Serial port
 */
class MbedSerialPriv
{
public:
    MbedSerialPriv() : serial(NULL) {}
    SerialPriv serialPriv; /**< common private data */
    MbedRawUSBSerial* serial; /*< USB implementation object */
};

/** private data for the can device */
static MbedSerialPriv serial_private[1];

static int mbed_usbserial_init(devtab_t *dev);
static void ignore_dev_function(devtab_t *dev);
static void mbed_usbserial_tx_msg(devtab_t *dev);

void * operator new(size_t size)
{
  return malloc(size);
}

void * operator new[](size_t size)
{
  return malloc(size);
}

void operator delete(void * ptr)
{
  free(ptr);
}

void operator delete[](void * ptr)
{
  free(ptr);
}

int __cxa_guard_acquire(__guard *g) {return !*(char *)(g);};
void __cxa_guard_release (__guard *g) {*(char *)g = 1;};
void __cxa_guard_abort (__guard *) {};

void __cxa_pure_virtual(void) {};

/** initialize the device
 * @param dev device to initialize
 * @return 0 upon success
 */
static int mbed_usbserial_init(devtab_t *dev)
{
    MbedSerialPriv *priv = (MbedSerialPriv*)dev->priv;
    int r = serial_init(dev);
    if (r) return r;
    priv->serial = new MbedRawUSBSerial(&priv->serialPriv);
    priv->serialPriv.enable = ignore_dev_function;
    priv->serialPriv.disable = ignore_dev_function;
    priv->serialPriv.tx_char = mbed_usbserial_tx_msg;
    //priv->serial->attach(priv, &MbedSerialPriv::RxCallback);
    //priv->serial->txattach(priv, &MbedSerialPriv::TxCallback);
    return r;
}

/** Empty device function. Does nothing.
 * @param dev device
 */
static void ignore_dev_function(devtab_t *dev) {}

/** Try and transmit a message. Does nothing if there is no message to transmit
 *  or no write buffers to transmit via.
 * @param dev device to transmit message on
 */
static void mbed_usbserial_tx_msg(devtab_t *dev)
{
    MbedSerialPriv *priv = (MbedSerialPriv*)dev->priv;
    priv->serial->Transmit();
}

/** Device table entry for serial device */
static SERIAL_DEVTAB_ENTRY(serUSB0, "/dev/serUSB0", mbed_usbserial_init, &serial_private[0]);
<commit_msg>Fixes a race condition in the MBED usbserial implementation.<commit_after>/** \copyright
 * Copyright (c) 2013, Balazs Racz
 * 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.
 *
 * \file mbed_usbserial.cpp
 * This file implements an USB-Serial driver on top of the mbed
 * library. Currently it is tested on the LPC23xx processors.
 *
 * @author Balazs Racz
 * @date 4 May 2013
 */

#include "mbed.h"
#include "USBSerial.h"
#include "serial.h"
#include "os/os.h"

#ifdef TARGET_LPC2368
#endif

#define TX_DATA_SIZE 64
#define RX_DATA_SIZE 64

#include <stdlib.h>

void * operator new(size_t size);
void * operator new[](size_t size);
void operator delete(void * ptr);
void operator delete[](void * ptr);

__extension__ typedef int __guard __attribute__((mode (__DI__)));

extern "C" int __cxa_guard_acquire(__guard *);
extern "C" void __cxa_guard_release (__guard *);
extern "C" void __cxa_guard_abort (__guard *);

extern "C" void __cxa_pure_virtual(void);

/** This class is an empty wrapper around MBed's USB CDC class. The difference
    between this and mbed::USBSerial is that this class does not have any
    buffering and no interaction with stdio, whereas mbed::USBSerial has the
    following buffering:
    * it has a 128-byte receive buffer.
    * it has an fd
    * it has a FILE* with a default-sized send/receive buffer
    * it requires mbed's custom glue code in the open(2) method, without which
      it crashes.
 */
class MbedRawUSBSerial : public USBCDC
{
public:
    MbedRawUSBSerial(SerialPriv* serialPriv, uint16_t vendor_id = 0x1f00, uint16_t product_id = 0x2012, uint16_t product_release = 0x0001): USBCDC(vendor_id, product_id, product_release), serialPriv(serialPriv), txPending(false)
    {
	os_sem_init(&rxSem, 0);
	os_thread_t thread;
	os_thread_create(&thread, "usbserial.rx", 2, 1024, &_RxThread, this);
    }

    ~MbedRawUSBSerial()
    {
	os_sem_destroy(&rxSem);
    }

    void Transmit()
    {
	// Without this critical section there were cases when we deadlocked
	// with txPending == true but no interrupt coming in to clear it.
	taskENTER_CRITICAL();
	if (txPending) {
	    taskEXIT_CRITICAL();
	    return;
	}
	txPending = true;
	int count;
	for (count = 0; count < TX_DATA_SIZE; count++)
	{
	    if (os_mq_timedreceive(serialPriv->txQ, txData+count, 0) != OS_MQ_NONE)
	    {
		/* no more data left to transmit */
		break;
	    }
	}
	TxHelper(count);
	taskEXIT_CRITICAL();
    }

protected:
    virtual bool EP2_OUT_callback()
    {
	// we read the packet received to our assembly buffer
	readEP(rxData, &rxSize);
	// and wake up the RX thread.
	os_sem_post_from_isr(&rxSem);
	return true;
    }

    virtual bool EP2_IN_callback()
    {
	int count;
	configASSERT(txPending);
	for (count = 0; count < MAX_TX_PACKET_LENGTH; count++)
	{
	    if (os_mq_receive_from_isr(serialPriv->txQ, &txData[count]) != OS_MQ_NONE)
	    {
		/* no more data left to transmit */
		break;
	    }
	}
	TxHelper(count);
	return true;
    }

private:
    static const int MAX_TX_PACKET_LENGTH = 64;
    static const int MAX_RX_PACKET_LENGTH = 64;

    /** Transmits count bytes from the txData buffer. Sets txPending and
	bytesLost as needed. */
    void TxHelper(int count)
    {
	if (!count)
	{
	    txPending = false;
	    return;
	}
	if ((!configured()) ||
	    (!terminal_connected))
	{
	    // An error occured, data was lost.
	    txPending = false;
	    serialPriv->overrunCount += count;
	    return;
	}
	txPending = true;
	sendNB(txData, count);
    }

    void RxThread()
    {
	while(1)
	{
	    os_sem_wait(&rxSem);
	    for (uint32_t i = 0; i < rxSize; i++)
	    {
		os_mq_send(serialPriv->rxQ, rxData+i);
	    }
	    // We reactivate the endpoint to receive next characters
	    readStart(EPBULK_OUT, MAX_PACKET_SIZE_EPBULK);
	}
    }

    static void* _RxThread(void* arg)
    {
	((MbedRawUSBSerial*)arg)->RxThread();
	return NULL;
    }

    uint8_t txData[MAX_TX_PACKET_LENGTH]; /**< packet assemby buffer to host */
    uint32_t rxSize; /**< number of valis characters in rxData */
    uint8_t rxData[MAX_RX_PACKET_LENGTH]; /**< packet assembly buffer from host */
    SerialPriv* serialPriv;
    bool txPending; /**< transmission currently pending */
    os_sem_t rxSem;
};

/** Private data for this implementation of Serial port
 */
class MbedSerialPriv
{
public:
    MbedSerialPriv() : serial(NULL) {}
    SerialPriv serialPriv; /**< common private data */
    MbedRawUSBSerial* serial; /*< USB implementation object */
};

/** private data for the can device */
static MbedSerialPriv serial_private[1];

static int mbed_usbserial_init(devtab_t *dev);
static void ignore_dev_function(devtab_t *dev);
static void mbed_usbserial_tx_msg(devtab_t *dev);

void * operator new(size_t size)
{
  return malloc(size);
}

void * operator new[](size_t size)
{
  return malloc(size);
}

void operator delete(void * ptr)
{
  free(ptr);
}

void operator delete[](void * ptr)
{
  free(ptr);
}

int __cxa_guard_acquire(__guard *g) {return !*(char *)(g);};
void __cxa_guard_release (__guard *g) {*(char *)g = 1;};
void __cxa_guard_abort (__guard *) {};

void __cxa_pure_virtual(void) {};

/** initialize the device
 * @param dev device to initialize
 * @return 0 upon success
 */
static int mbed_usbserial_init(devtab_t *dev)
{
    MbedSerialPriv *priv = (MbedSerialPriv*)dev->priv;
    int r = serial_init(dev);
    if (r) return r;
    priv->serial = new MbedRawUSBSerial(&priv->serialPriv);
    priv->serialPriv.enable = ignore_dev_function;
    priv->serialPriv.disable = ignore_dev_function;
    priv->serialPriv.tx_char = mbed_usbserial_tx_msg;
    //priv->serial->attach(priv, &MbedSerialPriv::RxCallback);
    //priv->serial->txattach(priv, &MbedSerialPriv::TxCallback);
    return r;
}

/** Empty device function. Does nothing.
 * @param dev device
 */
static void ignore_dev_function(devtab_t *dev) {}

/** Try and transmit a message. Does nothing if there is no message to transmit
 *  or no write buffers to transmit via.
 * @param dev device to transmit message on
 */
static void mbed_usbserial_tx_msg(devtab_t *dev)
{
    MbedSerialPriv *priv = (MbedSerialPriv*)dev->priv;
    priv->serial->Transmit();
}

/** Device table entry for serial device */
static SERIAL_DEVTAB_ENTRY(serUSB0, "/dev/serUSB0", mbed_usbserial_init, &serial_private[0]);
<|endoftext|>


<commit_before>// See: CodinGame demo interview question

#include <iostream>
#include <vector>
#include <algorithm> // std::min_element

int main()
{
    std::vector<double> v = {1.0, 2.0, 3.0};
    auto min_value = *std::min_element(std::begin(v), std::end(v));
    std::cout << "Minimal value: " << min_value << "\n";

    return 0;
}

<commit_msg>Use uniform initialization syntax.<commit_after>// See: CodinGame demo interview question

#include <iostream>
#include <vector>
#include <algorithm> // std::min_element

int main()
{
    std::vector<double> v{1.0, 2.0, 3.0};
    auto min_value = *std::min_element(std::begin(v), std::end(v));
    std::cout << "Minimal value: " << min_value << "\n";

    return 0;
}
<|endoftext|>


<commit_before>//===--------------------------- new.cpp ----------------------------------===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is dual licensed under the MIT and the University of Illinois Open
// Source Licenses. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//

#define _LIBCPP_BUILDING_NEW

#include <stdlib.h>

#include "new"

#if defined(__APPLE__) && !defined(LIBCXXRT) && \
    !defined(_LIBCPP_BUILDING_HAS_NO_ABI_LIBRARY)
    #include <cxxabi.h>

    #ifndef _LIBCPPABI_VERSION
        // On Darwin, there are two STL shared libraries and a lower level ABI
        // shared library.  The global holding the current new handler is
        // in the ABI library and named __cxa_new_handler.
        #define __new_handler __cxxabiapple::__cxa_new_handler
    #endif
#else  // __APPLE__
    #if defined(LIBCXXRT) || defined(LIBCXX_BUILDING_LIBCXXABI)
        #include <cxxabi.h>
    #endif  // defined(LIBCXX_BUILDING_LIBCXXABI)
    #if defined(_LIBCPP_BUILDING_HAS_NO_ABI_LIBRARY) || \
        (!defined(_LIBCPPABI_VERSION) && !defined(__GLIBCXX__))
        static std::new_handler __new_handler;
    #endif  // _LIBCPPABI_VERSION
#endif

#ifndef __GLIBCXX__

// Implement all new and delete operators as weak definitions
// in this shared library, so that they can be overridden by programs
// that define non-weak copies of the functions.

_LIBCPP_WEAK
void *
operator new(std::size_t size) _THROW_BAD_ALLOC
{
    if (size == 0)
        size = 1;
    void* p;
    while ((p = ::malloc(size)) == 0)
    {
        // If malloc fails and there is a new_handler,
        // call it to try free up memory.
        std::new_handler nh = std::get_new_handler();
        if (nh)
            nh();
        else
#ifndef _LIBCPP_NO_EXCEPTIONS
            throw std::bad_alloc();
#else
            break;
#endif
    }
    return p;
}

_LIBCPP_WEAK
void *
operator new(std::size_t size, std::align_val_t alignment) _THROW_BAD_ALLOC
{
    if (size == 0)
        size = 1;
    if (static_cast<size_t>(alignment) < sizeof(void*))
      alignment = std::align_val_t(sizeof(void*));
    void* p;
#if defined(_LIBCPP_MSVCRT)
    while ((p = _aligned_malloc(size, static_cast<size_t>(alignment))) == nullptr)
#else
    while (::posix_memalign(&p, static_cast<size_t>(alignment), size) != 0)
#endif
    {
        // If posix_memalign fails and there is a new_handler,
        // call it to try free up memory.
        std::new_handler nh = std::get_new_handler();
        if (nh)
            nh();
        else {
#ifndef _LIBCPP_NO_EXCEPTIONS
            throw std::bad_alloc();
#else
            p = nullptr; // posix_memalign doesn't initialize 'p' on failure
            break;
#endif
        }
    }
    return p;
}

_LIBCPP_WEAK
void*
operator new(size_t size, const std::nothrow_t&) _NOEXCEPT
{
    void* p = 0;
#ifndef _LIBCPP_NO_EXCEPTIONS
    try
    {
#endif  // _LIBCPP_NO_EXCEPTIONS
        p = ::operator new(size);
#ifndef _LIBCPP_NO_EXCEPTIONS
    }
    catch (...)
    {
    }
#endif  // _LIBCPP_NO_EXCEPTIONS
    return p;
}

_LIBCPP_WEAK
void*
operator new(size_t size, std::align_val_t alignment, const std::nothrow_t&) _NOEXCEPT
{
    void* p = 0;
#ifndef _LIBCPP_NO_EXCEPTIONS
    try
    {
#endif  // _LIBCPP_NO_EXCEPTIONS
        p = ::operator new(size, alignment);
#ifndef _LIBCPP_NO_EXCEPTIONS
    }
    catch (...)
    {
    }
#endif  // _LIBCPP_NO_EXCEPTIONS
    return p;
}

_LIBCPP_WEAK
void*
operator new[](size_t size) _THROW_BAD_ALLOC
{
    return ::operator new(size);
}

_LIBCPP_WEAK
void*
operator new[](size_t size, std::align_val_t alignment) _THROW_BAD_ALLOC
{
    return ::operator new(size, alignment);
}

_LIBCPP_WEAK
void*
operator new[](size_t size, const std::nothrow_t&) _NOEXCEPT
{
    void* p = 0;
#ifndef _LIBCPP_NO_EXCEPTIONS
    try
    {
#endif  // _LIBCPP_NO_EXCEPTIONS
        p = ::operator new[](size);
#ifndef _LIBCPP_NO_EXCEPTIONS
    }
    catch (...)
    {
    }
#endif  // _LIBCPP_NO_EXCEPTIONS
    return p;
}

_LIBCPP_WEAK
void*
operator new[](size_t size, std::align_val_t alignment, const std::nothrow_t&) _NOEXCEPT
{
    void* p = 0;
#ifndef _LIBCPP_NO_EXCEPTIONS
    try
    {
#endif  // _LIBCPP_NO_EXCEPTIONS
        p = ::operator new[](size, alignment);
#ifndef _LIBCPP_NO_EXCEPTIONS
    }
    catch (...)
    {
    }
#endif  // _LIBCPP_NO_EXCEPTIONS
    return p;
}

_LIBCPP_WEAK
void
operator delete(void* ptr) _NOEXCEPT
{
    if (ptr)
        ::free(ptr);
}

_LIBCPP_WEAK
void
operator delete(void* ptr, std::align_val_t) _NOEXCEPT
{
    if (ptr)
#if defined(_LIBCPP_MSVCRT)
        ::_aligned_free(ptr);
#else
        ::free(ptr);
#endif
}

_LIBCPP_WEAK
void
operator delete(void* ptr, const std::nothrow_t&) _NOEXCEPT
{
    ::operator delete(ptr);
}

_LIBCPP_WEAK
void
operator delete(void* ptr, std::align_val_t alignment, const std::nothrow_t&) _NOEXCEPT
{
    ::operator delete(ptr, alignment);
}

_LIBCPP_WEAK
void
operator delete(void* ptr, size_t) _NOEXCEPT
{
    ::operator delete(ptr);
}

_LIBCPP_WEAK
void
operator delete(void* ptr, size_t, std::align_val_t alignment) _NOEXCEPT
{
    ::operator delete(ptr, alignment);
}

_LIBCPP_WEAK
void
operator delete[] (void* ptr) _NOEXCEPT
{
    ::operator delete(ptr);
}

_LIBCPP_WEAK
void
operator delete[] (void* ptr, std::align_val_t alignment) _NOEXCEPT
{
    ::operator delete(ptr, alignment);
}

_LIBCPP_WEAK
void
operator delete[] (void* ptr, const std::nothrow_t&) _NOEXCEPT
{
    ::operator delete[](ptr);
}

_LIBCPP_WEAK
void
operator delete[] (void* ptr, std::align_val_t alignment, const std::nothrow_t&) _NOEXCEPT
{
    ::operator delete[](ptr, alignment);
}

_LIBCPP_WEAK
void
operator delete[] (void* ptr, size_t) _NOEXCEPT
{
    ::operator delete[](ptr);
}

_LIBCPP_WEAK
void
operator delete[] (void* ptr, size_t, std::align_val_t alignment) _NOEXCEPT
{
    ::operator delete[](ptr, alignment);
}

#endif // !__GLIBCXX__

namespace std
{

#ifndef __GLIBCXX__
const nothrow_t nothrow = {};
#endif

#ifndef _LIBCPPABI_VERSION

#ifndef __GLIBCXX__

new_handler
set_new_handler(new_handler handler) _NOEXCEPT
{
    return __sync_lock_test_and_set(&__new_handler, handler);
}

new_handler
get_new_handler() _NOEXCEPT
{
    return __sync_fetch_and_add(&__new_handler, nullptr);
}

#endif // !__GLIBCXX__

#ifndef LIBCXXRT

bad_alloc::bad_alloc() _NOEXCEPT
{
}

#ifndef __GLIBCXX__

bad_alloc::~bad_alloc() _NOEXCEPT
{
}

const char*
bad_alloc::what() const _NOEXCEPT
{
    return "std::bad_alloc";
}

#endif // !__GLIBCXX__

bad_array_new_length::bad_array_new_length() _NOEXCEPT
{
}

#ifndef __GLIBCXX__

bad_array_new_length::~bad_array_new_length() _NOEXCEPT
{
}

const char*
bad_array_new_length::what() const _NOEXCEPT
{
    return "bad_array_new_length";
}

#endif // !__GLIBCXX__

#endif //LIBCXXRT

bad_array_length::bad_array_length() _NOEXCEPT
{
}

#ifndef __GLIBCXX__

bad_array_length::~bad_array_length() _NOEXCEPT
{
}

const char*
bad_array_length::what() const _NOEXCEPT
{
    return "bad_array_length";
}

#endif // !__GLIBCXX__

#endif // _LIBCPPABI_VERSION

#ifndef LIBSTDCXX

void
__throw_bad_alloc()
{
#ifndef _LIBCPP_NO_EXCEPTIONS
    throw bad_alloc();
#else
    _VSTD::abort();
#endif
}

#endif // !LIBSTDCXX

}  // std
<commit_msg>[NFC] Group aligned new/delete definitions together in new.cpp<commit_after>//===--------------------------- new.cpp ----------------------------------===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is dual licensed under the MIT and the University of Illinois Open
// Source Licenses. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//

#define _LIBCPP_BUILDING_NEW

#include <stdlib.h>

#include "new"

#if defined(__APPLE__) && !defined(LIBCXXRT) && \
    !defined(_LIBCPP_BUILDING_HAS_NO_ABI_LIBRARY)
    #include <cxxabi.h>

    #ifndef _LIBCPPABI_VERSION
        // On Darwin, there are two STL shared libraries and a lower level ABI
        // shared library.  The global holding the current new handler is
        // in the ABI library and named __cxa_new_handler.
        #define __new_handler __cxxabiapple::__cxa_new_handler
    #endif
#else  // __APPLE__
    #if defined(LIBCXXRT) || defined(LIBCXX_BUILDING_LIBCXXABI)
        #include <cxxabi.h>
    #endif  // defined(LIBCXX_BUILDING_LIBCXXABI)
    #if defined(_LIBCPP_BUILDING_HAS_NO_ABI_LIBRARY) || \
        (!defined(_LIBCPPABI_VERSION) && !defined(__GLIBCXX__))
        static std::new_handler __new_handler;
    #endif  // _LIBCPPABI_VERSION
#endif

#ifndef __GLIBCXX__

// Implement all new and delete operators as weak definitions
// in this shared library, so that they can be overridden by programs
// that define non-weak copies of the functions.

_LIBCPP_WEAK
void *
operator new(std::size_t size) _THROW_BAD_ALLOC
{
    if (size == 0)
        size = 1;
    void* p;
    while ((p = ::malloc(size)) == 0)
    {
        // If malloc fails and there is a new_handler,
        // call it to try free up memory.
        std::new_handler nh = std::get_new_handler();
        if (nh)
            nh();
        else
#ifndef _LIBCPP_NO_EXCEPTIONS
            throw std::bad_alloc();
#else
            break;
#endif
    }
    return p;
}

_LIBCPP_WEAK
void*
operator new(size_t size, const std::nothrow_t&) _NOEXCEPT
{
    void* p = 0;
#ifndef _LIBCPP_NO_EXCEPTIONS
    try
    {
#endif  // _LIBCPP_NO_EXCEPTIONS
        p = ::operator new(size);
#ifndef _LIBCPP_NO_EXCEPTIONS
    }
    catch (...)
    {
    }
#endif  // _LIBCPP_NO_EXCEPTIONS
    return p;
}

_LIBCPP_WEAK
void*
operator new[](size_t size) _THROW_BAD_ALLOC
{
    return ::operator new(size);
}

_LIBCPP_WEAK
void*
operator new[](size_t size, const std::nothrow_t&) _NOEXCEPT
{
    void* p = 0;
#ifndef _LIBCPP_NO_EXCEPTIONS
    try
    {
#endif  // _LIBCPP_NO_EXCEPTIONS
        p = ::operator new[](size);
#ifndef _LIBCPP_NO_EXCEPTIONS
    }
    catch (...)
    {
    }
#endif  // _LIBCPP_NO_EXCEPTIONS
    return p;
}

_LIBCPP_WEAK
void
operator delete(void* ptr) _NOEXCEPT
{
    if (ptr)
        ::free(ptr);
}

_LIBCPP_WEAK
void
operator delete(void* ptr, const std::nothrow_t&) _NOEXCEPT
{
    ::operator delete(ptr);
}

_LIBCPP_WEAK
void
operator delete(void* ptr, size_t) _NOEXCEPT
{
    ::operator delete(ptr);
}

_LIBCPP_WEAK
void
operator delete[] (void* ptr) _NOEXCEPT
{
    ::operator delete(ptr);
}

_LIBCPP_WEAK
void
operator delete[] (void* ptr, const std::nothrow_t&) _NOEXCEPT
{
    ::operator delete[](ptr);
}

_LIBCPP_WEAK
void
operator delete[] (void* ptr, size_t) _NOEXCEPT
{
    ::operator delete[](ptr);
}

_LIBCPP_WEAK
void *
operator new(std::size_t size, std::align_val_t alignment) _THROW_BAD_ALLOC
{
    if (size == 0)
        size = 1;
    if (static_cast<size_t>(alignment) < sizeof(void*))
      alignment = std::align_val_t(sizeof(void*));
    void* p;
#if defined(_LIBCPP_MSVCRT)
    while ((p = _aligned_malloc(size, static_cast<size_t>(alignment))) == nullptr)
#else
    while (::posix_memalign(&p, static_cast<size_t>(alignment), size) != 0)
#endif
    {
        // If posix_memalign fails and there is a new_handler,
        // call it to try free up memory.
        std::new_handler nh = std::get_new_handler();
        if (nh)
            nh();
        else {
#ifndef _LIBCPP_NO_EXCEPTIONS
            throw std::bad_alloc();
#else
            p = nullptr; // posix_memalign doesn't initialize 'p' on failure
            break;
#endif
        }
    }
    return p;
}

_LIBCPP_WEAK
void*
operator new(size_t size, std::align_val_t alignment, const std::nothrow_t&) _NOEXCEPT
{
    void* p = 0;
#ifndef _LIBCPP_NO_EXCEPTIONS
    try
    {
#endif  // _LIBCPP_NO_EXCEPTIONS
        p = ::operator new(size, alignment);
#ifndef _LIBCPP_NO_EXCEPTIONS
    }
    catch (...)
    {
    }
#endif  // _LIBCPP_NO_EXCEPTIONS
    return p;
}

_LIBCPP_WEAK
void*
operator new[](size_t size, std::align_val_t alignment) _THROW_BAD_ALLOC
{
    return ::operator new(size, alignment);
}

_LIBCPP_WEAK
void*
operator new[](size_t size, std::align_val_t alignment, const std::nothrow_t&) _NOEXCEPT
{
    void* p = 0;
#ifndef _LIBCPP_NO_EXCEPTIONS
    try
    {
#endif  // _LIBCPP_NO_EXCEPTIONS
        p = ::operator new[](size, alignment);
#ifndef _LIBCPP_NO_EXCEPTIONS
    }
    catch (...)
    {
    }
#endif  // _LIBCPP_NO_EXCEPTIONS
    return p;
}

_LIBCPP_WEAK
void
operator delete(void* ptr, std::align_val_t) _NOEXCEPT
{
    if (ptr)
#if defined(_LIBCPP_MSVCRT)
        ::_aligned_free(ptr);
#else
        ::free(ptr);
#endif
}

_LIBCPP_WEAK
void
operator delete(void* ptr, std::align_val_t alignment, const std::nothrow_t&) _NOEXCEPT
{
    ::operator delete(ptr, alignment);
}

_LIBCPP_WEAK
void
operator delete(void* ptr, size_t, std::align_val_t alignment) _NOEXCEPT
{
    ::operator delete(ptr, alignment);
}

_LIBCPP_WEAK
void
operator delete[] (void* ptr, std::align_val_t alignment) _NOEXCEPT
{
    ::operator delete(ptr, alignment);
}

_LIBCPP_WEAK
void
operator delete[] (void* ptr, std::align_val_t alignment, const std::nothrow_t&) _NOEXCEPT
{
    ::operator delete[](ptr, alignment);
}

_LIBCPP_WEAK
void
operator delete[] (void* ptr, size_t, std::align_val_t alignment) _NOEXCEPT
{
    ::operator delete[](ptr, alignment);
}

#endif // !__GLIBCXX__

namespace std
{

#ifndef __GLIBCXX__
const nothrow_t nothrow = {};
#endif

#ifndef _LIBCPPABI_VERSION

#ifndef __GLIBCXX__

new_handler
set_new_handler(new_handler handler) _NOEXCEPT
{
    return __sync_lock_test_and_set(&__new_handler, handler);
}

new_handler
get_new_handler() _NOEXCEPT
{
    return __sync_fetch_and_add(&__new_handler, nullptr);
}

#endif // !__GLIBCXX__

#ifndef LIBCXXRT

bad_alloc::bad_alloc() _NOEXCEPT
{
}

#ifndef __GLIBCXX__

bad_alloc::~bad_alloc() _NOEXCEPT
{
}

const char*
bad_alloc::what() const _NOEXCEPT
{
    return "std::bad_alloc";
}

#endif // !__GLIBCXX__

bad_array_new_length::bad_array_new_length() _NOEXCEPT
{
}

#ifndef __GLIBCXX__

bad_array_new_length::~bad_array_new_length() _NOEXCEPT
{
}

const char*
bad_array_new_length::what() const _NOEXCEPT
{
    return "bad_array_new_length";
}

#endif // !__GLIBCXX__

#endif //LIBCXXRT

bad_array_length::bad_array_length() _NOEXCEPT
{
}

#ifndef __GLIBCXX__

bad_array_length::~bad_array_length() _NOEXCEPT
{
}

const char*
bad_array_length::what() const _NOEXCEPT
{
    return "bad_array_length";
}

#endif // !__GLIBCXX__

#endif // _LIBCPPABI_VERSION

#ifndef LIBSTDCXX

void
__throw_bad_alloc()
{
#ifndef _LIBCPP_NO_EXCEPTIONS
    throw bad_alloc();
#else
    _VSTD::abort();
#endif
}

#endif // !LIBSTDCXX

}  // std
<|endoftext|>