channel( unsigned int index, const channel::options& options )
     : index_( index )
     , scaled_( options.scale )
     , row_( 0, block_size )
     , row_count_( 0 )
     , angle_step_( ( M_PI * 2 ) / block_size )
     , options_( options )
 {
     if( options.colourmap == "green" ) { colourmap_ = color_map::constant( 0, 255, 0 ); }
     else if( options.colourmap == "red" ) { colourmap_ = color_map::constant( 255, 0, 0 ); }
     else if( options.colourmap == "hot" ) { colourmap_ = color_map::temperature( 96, 96 ); }
     else if( options.colourmap == "jet" ) { colourmap_ = color_map::jet(); }
     else
     { 
         std::vector< std::string > v = comma::split( options.colourmap, ',' );
         if( v.size() != 3 ) { COMMA_THROW( comma::exception, "image-accumulate: expected colourmap, got '" << options.colourmap << "'" ); }
         colourmap_ = color_map::constant( boost::lexical_cast< unsigned int >( v[0] ), boost::lexical_cast< unsigned int >( v[1] ), boost::lexical_cast< unsigned int >( v[2] ) );
     }
     if( options.dial_colour == "white" ) { dial_colour_ = cv::Scalar( 255, 255, 255 ); }
     else if( options.dial_colour == "white" ) { dial_colour_ = cv::Scalar( 255, 255, 255 ); }
     else if( options.dial_colour == "black" ) { dial_colour_ = cv::Scalar( 0, 0, 0 ); }
     else if( options.dial_colour == "red" ) { dial_colour_ = cv::Scalar( 0, 0, 255 ); }
     else if( options.dial_colour == "green" ) { dial_colour_ = cv::Scalar( 0, 255, 0 ); }
     else if( options.dial_colour == "blue" ) { dial_colour_ = cv::Scalar( 255, 0, 0 ); }
     else if( options.dial_colour == "yellow" ) { dial_colour_ = cv::Scalar( 0, 255, 255 ); }
     else
     { 
         std::vector< std::string > v = comma::split( options.dial_colour, ',' );
         if( v.size() != 3 ) { COMMA_THROW( comma::exception, "image-accumulate: expected colour, got '" << options.dial_colour << "'" ); }
         dial_colour_ = cv::Scalar( boost::lexical_cast< unsigned int >( v[2] ), boost::lexical_cast< unsigned int >( v[1] ), boost::lexical_cast< unsigned int >( v[0] ) );
     }
 }

turn_laser_on::turn_laser_on(stream_base& ios, laser_device& device, bool reboot_on_error):ios(ios)
{
    state_command start( "BM" ); // starts transmission
    state_reply start_reply;

    ios.write( start.data(), state_command::size  );
    ios.flush();
    
    comma::io::select select;
    select.read().add( ios.native() );
    
    select.wait( 1 ); // wait one select for reply, it can be much smaller
    if( !select.read().ready( ios.native() ) ) { 
        COMMA_THROW( comma::exception, "no reply received from laser scanner after a startup (BM) command: " << std::string( start.data(), state_command::size ) ); 
    }
    ios.read( start_reply.data(), state_reply::size  );
    
    if( start_reply.status() != 0 && 
        start_reply.status() != 10 &&
        start_reply.status() != 2 ) // 0 = success, 2 seems to be returned when it is already in scanning mode but idle
    {
        if( reboot_on_error )
        {
            device.reboot(ios);
        }
        COMMA_THROW( comma::exception, std::string("Starting laser with BM command failed, status: ") + std::string( start_reply.status.data(), 2 ) ); 
    }
}

int dc1394color_coding_to_cv_type(dc1394color_coding_t color_coding)
{
    //std::cerr << "Color coding: " << color_coding_to_string( color_coding ) << std::endl;
    switch( color_coding )
    {
        case DC1394_COLOR_CODING_MONO8:
            return CV_8UC1;
        case DC1394_COLOR_CODING_RGB8:
            return CV_8UC3;
        case DC1394_COLOR_CODING_MONO16:
            return CV_16UC1;
        case DC1394_COLOR_CODING_RGB16:
            return CV_16UC3;
        case DC1394_COLOR_CODING_MONO16S:
            return CV_16SC1;
        case DC1394_COLOR_CODING_RGB16S:
            return CV_16SC3;
        case DC1394_COLOR_CODING_RAW8:
            return CV_8UC1;
        case DC1394_COLOR_CODING_RAW16:
            return CV_16UC1;
        case DC1394_COLOR_CODING_YUV411:
        case DC1394_COLOR_CODING_YUV422:
        case DC1394_COLOR_CODING_YUV444:
            COMMA_THROW( comma::exception, "unsupported color coding: " << color_coding);
        default:
            COMMA_THROW( comma::exception, "invalid color coding: " << color_coding);
    }
}

attribute::attribute( AVT::VmbAPI::FeaturePtr feature )
    : feature_( feature )
    , type_( VmbFeatureDataUnknown )
{
    VmbErrorType status = feature_->GetName( name_ );
    if( status != VmbErrorSuccess )
    {
        COMMA_THROW( comma::exception, error_msg( "GetName() failed", status ));
    }

    status = feature_->GetDataType( type_ );
    if( status != VmbErrorSuccess )
    {
        COMMA_THROW( comma::exception, error_msg( "GetDataType() failed", status ));
    }

    status = feature_->GetDescription( description_ );
    if( status != VmbErrorSuccess )
    {
        COMMA_THROW( comma::exception, error_msg( "GetDescription() failed", status ));
    }

    init_value();
    init_allowed_values();
}

 void sendcommand( const command& c )
 {
     if( !m_ostream ) { COMMA_THROW( comma::exception, "cannot write to read-only stream" ); }
     if( m_commandId ) { COMMA_THROW( comma::exception, "got a new command (0x" << std::hex << command::id << "), while waiting for response to 0x" << *m_commandId << std::dec ); }
     commands::packet< command > packet( c );
     m_ostream->write( packet.data(), commands::packet< command >::size );
     m_ostream->flush();
     if( m_ostream->bad() ) { COMMA_THROW( comma::exception, "failed to send command (0x" << std::hex << command::id << std::dec ); }
 }

 static boost::posix_time::time_duration time_delay( unsigned int block, unsigned int laser )
 {
     if(laser<0 ||laser>=lasers_per_block) {
         COMMA_THROW( comma::exception, "laser id out of range" << laser );
     }
     if(block<0||block>=block_count) {
         COMMA_THROW(comma::exception, "block id out of range"<<block );
     }
     double delay = (time_table[block][laser] ) + timestamps::ethernetOutputDuration * 1e6;
     return boost::posix_time::microseconds( delay);
 }

 udp_stream( const std::string& address ) : stream( address ), socket_( service_ )
 {
     const std::vector< std::string >& v = comma::split( address, ':' );
     if( v.size() != 2 ) { COMMA_THROW( comma::exception, "io-cat: expected udp:<port>, e.g. udp:12345, got" << address ); }
     unsigned short port = boost::lexical_cast< unsigned short >( v[1] );
     socket_.open( boost::asio::ip::udp::v4() );
     boost::system::error_code error;
     socket_.set_option( boost::asio::ip::udp::socket::broadcast( true ), error );
     if( error ) { COMMA_THROW( comma::exception, "io-cat: udp failed to set broadcast option on port " << port ); }
     socket_.bind( boost::asio::ip::udp::endpoint( boost::asio::ip::udp::v4(), port ), error );
     if( error ) { COMMA_THROW( comma::exception, "io-cat: udp failed to bind port " << port ); }
 }

dc1394speed_t iso_speed_from_string ( const std::string& speed )
{
    if( speed == "DC1394_ISO_SPEED_100" )
    {
        return DC1394_ISO_SPEED_100;
    }
    else if( speed == "DC1394_ISO_SPEED_200" )
    {
        return DC1394_ISO_SPEED_200;
    }
    else if( speed == "DC1394_ISO_SPEED_400" )
    {
        return DC1394_ISO_SPEED_400;
    }
    else if( speed == "DC1394_ISO_SPEED_800" )
    {
        return DC1394_ISO_SPEED_800;
    }
    else if( speed == "DC1394_ISO_SPEED_1600" )
    {
        return DC1394_ISO_SPEED_1600;
    }
    else if( speed == "DC1394_ISO_SPEED_3200" )
    {
        return DC1394_ISO_SPEED_3200;
    }
    else
    {
        COMMA_THROW( comma::exception, "invalid iso speed: " << speed );
    }
}

std::string color_coding_to_string( dc1394color_coding_t color_coding )
{
    switch( color_coding )
    {
        case DC1394_COLOR_CODING_MONO8:
            return "DC1394_COLOR_CODING_MONO8";
        case DC1394_COLOR_CODING_YUV411:
            return "DC1394_COLOR_CODING_YUV411";
        case DC1394_COLOR_CODING_YUV422:
            return "DC1394_COLOR_CODING_YUV422";
        case DC1394_COLOR_CODING_YUV444:
            return "DC1394_COLOR_CODING_YUV444";
        case DC1394_COLOR_CODING_RGB8:
            return "DC1394_COLOR_CODING_RGB8";
        case DC1394_COLOR_CODING_MONO16:
            return "DC1394_COLOR_CODING_MONO16";
        case DC1394_COLOR_CODING_RGB16:
            return "DC1394_COLOR_CODING_RGB16";
        case DC1394_COLOR_CODING_MONO16S:
            return "DC1394_COLOR_CODING_MONO16S";
        case DC1394_COLOR_CODING_RGB16S:
            return "DC1394_COLOR_CODING_RGB16S";
        case DC1394_COLOR_CODING_RAW8:
            return "DC1394_COLOR_CODING_RAW8";
        case DC1394_COLOR_CODING_RAW16:
            return "DC1394_COLOR_CODING_RAW16";
        default:
            COMMA_THROW( comma::exception, "invalid color coding: " << color_coding );
    }
}

    static inline boost::property_tree::ptree& parse( boost::property_tree::ptree& ptree
            , const std::string& s
            , char equal_sign
            , char delimiter
            , bool use_index ) // todo? make using index default?
    {
        const std::vector< std::string >& v = comma::split( s, delimiter );
        impl::path_filter< check_type > c( ptree );

        for( std::size_t i = 0; i < v.size(); ++i )
        {
            if( v[i].empty() ) {
                continue;
            }
            std::string::size_type p = v[i].find_first_of( equal_sign );
            if( p == std::string::npos ) {
                COMMA_THROW( comma::exception, "expected '" << delimiter << "'-separated xpath" << equal_sign << "value pairs; got \"" << v[i] << "\"" );
            }
            const std::string& path = comma::strip( v[i].substr( 0, p ), '"' );
            const std::string& value = comma::strip( v[i].substr( p + 1, std::string::npos ), '"' );
            if( c.put_allowed( path, use_index ) )
            {
                if( use_index ) {
                    property_tree::put( ptree, path, value, use_index );    // quick and dirty
                }
                else {
                    ptree.put( boost::property_tree::ptree::path_type( path, '/' ), value );
                }
            }
        }
        return ptree;
    }

static void set_pixel( cv::Mat& m, const input_t& v, const std::pair< double, double >& offset ) // quick and dirty; reimplement as templates
{
    int x = std::floor( v.x + 0.5 - offset.first );
    int y = std::floor( v.y + 0.5 - offset.second );
    if( x < 0 || x >= m.cols ) { return; }
    if( y < 0 || y >= m.rows ) { return; }
    switch( m.type() )
    {
        case CV_8UC1:
            m.at< unsigned char >( y, x ) = v.channels[0];
            break;
        case CV_32FC1:
            m.at< float >( y, x ) = v.channels[0];
            break;
        case CV_8UC3:
        {
            cv::Vec3b& p = m.at< cv::Vec3b >( y, x );
            p[0] = v.channels[0];
            p[1] = v.channels[1];
            p[2] = v.channels[2];
            break;
        }
        case CV_32FC3:
        {
            cv::Vec3f& p = m.at< cv::Vec3f >( y, x );
            p[0] = v.channels[0];
            p[1] = v.channels[1];
            p[2] = v.channels[2];
            break;
        }
        default:
            COMMA_THROW( comma::exception, "unsupported cv mat type " << m.type() );
    }   
}

void gaussian_process::evaluate( const Eigen::MatrixXd& domains, Eigen::VectorXd& means, Eigen::VectorXd& variances ) const
{
    if( domains.cols() != domains_.cols() ) { COMMA_THROW( comma::exception, "expected " << domains_.cols() << " column(s) in domains, got " << domains.cols() << std::endl ); }
    Eigen::MatrixXd Kxsx = Eigen::MatrixXd::Zero( domains.rows(), domains_.rows() );
    for( std::size_t r = 0; r < std::size_t( domains.rows() ); ++r )
    {
        const Eigen::VectorXd& row = domains.row( r );
        for( std::size_t c = 0; c < std::size_t( domains_.rows() ); ++c )
        {
            Kxsx( r, c ) = covariance_( row, domains_.row( c ) );
        }
    }
    means = Kxsx * alpha_;
    means.array() += offset_;
    Eigen::MatrixXd Kxxs = Kxsx.transpose();
    L_.matrixL().solveInPlace( Kxxs );
    Eigen::MatrixXd& variance = Kxxs;
    variance = variance.array() * variance.array();
    variances = variance.colwise().sum();
    // for each diagonal variance, set v(r) = -v(r,r) + Kxsxs
    for( std::size_t r = 0; r < std::size_t( domains.rows() ); ++r )
    {
        variances( r ) = -variances( r ) + self_covariance_;
    }
}

gaussian_process::gaussian_process( const Eigen::MatrixXd& domains
                                , const Eigen::VectorXd& targets
                                , const gaussian_process::covariance& covariance
                                , double self_covariance )
    : domains_( domains )
    , targets_( targets )
    , covariance_( covariance )
    , self_covariance_( self_covariance )
    , offset_( targets.sum() / targets.rows() )
    , K_( domains.rows(), domains.rows() )
{
    if( domains.rows() != targets.rows() ) { COMMA_THROW( comma::exception, "expected " << domains.rows() << " row(s) in targets, got " << targets.rows() << " row(s)" ); }
    targets_.array() -= offset_; // normalise
    //use m_K as Kxx + variance*I, then invert it
    //fill Kxx with values from covariance function
    //for elements r,c in upper triangle
    for( std::size_t r = 0; r < std::size_t( domains.rows() ); ++r )
    {
        K_( r, r ) = self_covariance_;
        const Eigen::VectorXd& row = domains.row( r );
        for( std::size_t c = r + 1; c < std::size_t( domains.rows() ); ++c )
        {
            K_( c, r ) = K_( r, c ) = covariance_( row, domains.row( c ) );
        }
    }
    L_.compute( K_ ); // invert Kxx + variance * I to become (by definition) B
    alpha_ = L_.solve( targets_ );
}

 key_press_t_( bool interactive = false, const std::string& tty = "/dev/tty" ) : interactive_( interactive )
 {
     if( !interactive_ ) { return; }
     fd_ = ::open( &tty[0], O_RDONLY | O_NONBLOCK | O_NOCTTY );
     if( !isatty( fd_ ) ) { COMMA_THROW( comma::exception, "'" << tty << "' is not tty" ); }
     if( fd_ == -1 ) { COMMA_THROW( comma::exception, "failed to open '" << tty << "'" ); }
     struct termios new_termios;
     ::tcgetattr( fd_, &old_termios_ );
     new_termios = old_termios_;
     new_termios.c_lflag &= ~( ICANON | ECHO );
     new_termios.c_iflag &= ~( BRKINT | ICRNL | INPCK | ISTRIP | IXON );
     if( ::tcsetattr( fd_, TCSANOW, &new_termios ) < 0 ) { COMMA_THROW( comma::exception, "failed to set '" << tty << "'" ); }
     std::cerr << "csv-play: running in interactive mode" << std::endl;
     std::cerr << "          press <whitespace> to pause or resume" << std::endl;
     std::cerr << "          press left or down arrow key: output one record at a time" << std::endl;
 }

static void on_frame_( const Pair& p ) // quick and dirty
{
    if( p.second.size().width == 0 )
    {
        emptyFrameCounter++;
        if( emptyFrameCounter > 20 )
        {
            COMMA_THROW( comma::exception, "got lots of empty frames, check that the packet size in the camera matches the mtu on your machine" );
        }
        if( verbose )
        {
            std::cerr << "gige-cat: got empty frame" << std::endl;
        }
        return;
    }
    emptyFrameCounter = 0;
    Pair q;
    if( is_shutdown || !running ) { return queue.push( q ); } // to force read exit
    q.first = p.first;
    p.second.copyTo( q.second ); // quick and dirty: full copy; todo: implement circular queue in gige::callback?
    queue.push( q );
    
    if( verbose ) { spin_(); }
    if( discard_more_than > 0 )
    {
        int size = queue.size();
        if( size > 1 )
        {
            int size_to_discard = size - discard_more_than;
            Pair p;
            for( int i = 0; i < size_to_discard; ++i ) { queue.pop( p ); } // clear() is not thread-safe
            if( verbose && size_to_discard > 0 ) { std::cerr << "gige-cat: discarded " << size_to_discard << " frames" << std::endl; }
        }
    }    
}

 typename command::response write( const command& c )
 {
     if( m_fault ) { COMMA_THROW( comma::exception, "got command, while having uncleared fault" ); }
     sendcommand( c );
     m_commandId = static_cast< commands::types >( command::id );
     return readresponse< command >();
 }

T* read_packet(std::istream& is, T& t)
{
    is.read(t.data(),t.size);
    std::streamsize read_count=is.gcount();
    if(read_count==0&&!is.good()){return NULL;}
    if(read_count != t.size) { COMMA_THROW(comma::exception, "read count mismatch, expected: " << t.size << " bytes; got: " << read_count );}
    return &t;
}

std::pair< double, double > gaussian_process::evaluate( const Eigen::MatrixXd& domain ) const
{
    if( domain.rows() != 1 ) { COMMA_THROW( comma::exception, "expected 1 row in domain, got " << domain.rows() << " rows" ); }
    Eigen::VectorXd means( 1 );
    Eigen::VectorXd variances( 1 );
    evaluate( domain, means, variances );
    return std::make_pair( means( 0 ), variances( 0 ) );
}

 const scan_packet* readscan()
 {
     if( m_fault ) { COMMA_THROW( comma::exception, "asked to read scan, while having uncleared fault" ); }
     if( m_commandId ) { COMMA_THROW( comma::exception, "cannot read scan, while waiting for response to 0x" << std::hex << *m_commandId << std::dec ); }
     if( !readpacket() ) { return NULL; }
     switch( m_header->type() )
     {
         case header::scan_type:
             return reinterpret_cast< const scan_packet* >( &m_buf[0] );
         case header::fault_type:
             throw faultException(); // COMMA_THROW( comma::exception, "received fault, while reading scan" );
         case header::response_type:
             COMMA_THROW( comma::exception, "expected scan data, got command response of type 0x" << std::hex << ( reinterpret_cast< const commands::response_header* >( m_payload )->id() & 0x3fff ) << std::dec );
         default:
             COMMA_THROW( comma::exception, "expected scan data, got packet of unknown type (0x" << std::hex << m_header->type() << std::dec );
     }
 }

 bool put_allowed( const std::string& p, bool use_index ) const
 {
     if( use_index )
     {
         if( property_tree::get( ptree_, p, use_index ) ) {
             COMMA_THROW( comma::exception, "input path '" << p << "' already in the tree" );
         }
     }
     else
     {
         boost::optional< std::string > old_v = ptree_.get_optional< std::string >( boost::property_tree::ptree::path_type( p, '/' ) );
         if( old_v ) {
             COMMA_THROW( comma::exception, "input path '" << p << "' already in the tree" );
         }
     }
     return true;
 }