<commit_before>/* ===========================
 *
 * Copyright (c) 2013 Philippe Tillet - National Chiao Tung University
 *
 * FMinCL - Unconstrained Function Minimization on OpenCL
 *
 * License : MIT X11 - See the LICENSE file in the root folder
 * ===========================*/

#ifndef FMINCL_DIRECTIONS_HPP_
#define FMINCL_DIRECTIONS_HPP_

#include <vector>
#include <cmath>

#include "fmincl/utils.hpp"
#include "fmincl/tools/shared_ptr.hpp"
#include "fmincl/tools/typelist.hpp"
#include "fmincl/mapping.hpp"

namespace fmincl{

struct direction_tag{ virtual ~direction_tag(){ } };
template<class BackendType>
struct direction_implementation{
    virtual void operator()(void) = 0;
    virtual ~direction_implementation(){ }
};

/* =========================== *
 * CONJUGATE GRADIENTS
 * ===========================*/

//UPDATES
struct cg_update_tag{ virtual ~cg_update_tag(){ } };
template<class BackendType>
struct cg_update_implementation{
    virtual typename BackendType::ScalarType operator()(void) = 0;
    virtual ~cg_update_implementation(){ }
};

struct polak_ribiere_tag : public cg_update_tag{ };

template<class BackendType>
struct polak_ribiere_implementation : public cg_update_implementation<BackendType>{
private:
    typedef typename BackendType::ScalarType ScalarType;
    typedef typename BackendType::VectorType VectorType;
public:
    polak_ribiere_implementation(polak_ribiere_tag const &, detail::optimization_context<BackendType> & context) : context_(context){
        N_ = context_.dim();
        tmp_ = BackendType::create_vector(N_);
    }

    ScalarType operator()(){
        VectorType & g = context_.g();
        VectorType & gm1 = context_.gm1();

        //tmp_ = g - gm1;
        BackendType::copy(N_,g, tmp_);
        BackendType::axpy(N_,-1,gm1,tmp_);
        return BackendType::dot(N_,g,tmp_)/BackendType::dot(N_,gm1,gm1);
    }

    ~polak_ribiere_implementation(){
        BackendType::delete_if_dynamically_allocated(tmp_);
    }
private:
    detail::optimization_context<BackendType> & context_;

    std::size_t N_;
    VectorType tmp_;
};

template<class BackendType>
struct cg_update_mapping{

    typedef implementation_from_tag<typename make_typelist<FMINCL_CREATE_MAPPING(polak_ribiere)>::type
                               ,cg_update_tag, cg_update_implementation<BackendType> > type;

};

//RESTARTS
struct cg_restart_tag{ virtual ~cg_restart_tag(){ } };

template<class BackendType>
struct cg_restart_implementation{
    virtual bool operator()(void) = 0;
    virtual ~cg_restart_implementation(){ }
};

struct no_restart_tag : public cg_restart_tag{ };
template<class BackendType>
struct no_restart_implementation : public cg_restart_implementation<BackendType>{
    no_restart_implementation(no_restart_tag const & /*tag*/, detail::optimization_context<BackendType> & /*context*/) { }
    bool operator()() { return false; }
};

template<class BackendType>
struct cg_restart_mapping{
    typedef implementation_from_tag<typename make_typelist<FMINCL_CREATE_MAPPING(no_restart)>::type
                                   ,cg_restart_tag, cg_restart_implementation<BackendType> > type;
};

//CG
struct cg_tag : public direction_tag{
    cg_tag(cg_update_tag * _update = new polak_ribiere_tag(), cg_restart_tag * _restart = new no_restart_tag()) : update(_update), restart(_restart){ }
    tools::shared_ptr<cg_update_tag> update;
    tools::shared_ptr<cg_restart_tag> restart;
};
template<class BackendType>
class cg_implementation : public direction_implementation<BackendType>{
    typedef typename BackendType::ScalarType ScalarType;
    typedef typename BackendType::VectorType VectorType;
public:
    cg_implementation(cg_tag const & cg_params, detail::optimization_context<BackendType> & context) : context_(context)
                                                                                ,update_implementation_(cg_update_mapping<BackendType>::type::create(*cg_params.update, context))
                                                                                ,restart_implementation_(cg_restart_mapping<BackendType>::type::create(*cg_params.restart, context)){ }
    void operator()(){
      VectorType const & g = context_.g();
      VectorType & p = context_.p();
      std::size_t N = context_.dim();

      if((*restart_implementation_)()){

        //p = -g;
        BackendType::copy(N,g,p);
        BackendType::scale(N,-1,p);
      }
      else{
        ScalarType beta = (*update_implementation_)();

        //p = -g + beta*p;
        BackendType::scale(N,beta,p);
        BackendType::axpy(N,-1,g,p);
      }
    }
private:
    detail::optimization_context<BackendType> & context_;

    tools::shared_ptr<cg_update_implementation<BackendType> > update_implementation_;
    tools::shared_ptr<cg_restart_implementation<BackendType> > restart_implementation_;
};

/* =========================== *
 * QUASI NEWTON
 * ===========================*/

struct qn_update_tag{ virtual ~qn_update_tag(){ } };
template<class BackendType>
struct qn_update_implementation{
    virtual void operator()(void) = 0;
    virtual ~qn_update_implementation(){ }
};

struct lbfgs_tag : public qn_update_tag{
    lbfgs_tag(unsigned int _m = 4) : m(_m) { }
    unsigned int m;
};
template<class BackendType>
class lbfgs_implementation : public qn_update_implementation<BackendType>{
    typedef typename BackendType::ScalarType ScalarType;
    typedef typename BackendType::VectorType VectorType;
    typedef typename BackendType::MatrixType MatrixType;

    struct storage_pair{
        VectorType s;
        VectorType y;
    };

    VectorType & s(std::size_t i) { return vecs_[i].s; }
    VectorType & y(std::size_t i) { return vecs_[i].y; }

public:
    lbfgs_implementation(lbfgs_tag const & tag, detail::optimization_context<BackendType> & context) : tag_(tag), context_(context), vecs_(tag.m){
        N_ = context_.dim();

        q_ = BackendType::create_vector(N_);
        r_ = BackendType::create_vector(N_);

        for(unsigned int i = 0 ; i < tag_.m ; ++i){
            vecs_[i].s = BackendType::create_vector(N_);
            vecs_[i].y = BackendType::create_vector(N_);
        }
    }

    void operator()(){
        //Initizalization of aliases
        VectorType const & x=context_.x();
        VectorType const & xm1=context_.xm1();
        VectorType const & g=context_.g();
        VectorType const & gm1=context_.gm1();
        VectorType & p=context_.p();

        unsigned int const & iter = context_.iter();
        unsigned int const & m = tag_.m;

        std::vector<ScalarType> rhos(m);
        std::vector<ScalarType> alphas(m);

        //Algorithm

        //Updates storage
        for(unsigned int i = std::min(iter,m)-1 ; i > 0  ; --i){
            BackendType::copy(N_,s(i-1), s(i));
            BackendType::copy(N_,y(i-1), y(i));
        }

        //s(0) = x - xm1;
        BackendType::copy(N_,x,s(0));
        BackendType::axpy(N_,-1,xm1,s(0));

        //y(0) = g - gm1;
        BackendType::copy(N_,g,y(0));
        BackendType::axpy(N_,-1,gm1,y(0));

        BackendType::copy(N_,g,q_);
        int i = 0;
        for(; i < (int)std::min(iter,m) ; ++i){
            rhos[i] = static_cast<ScalarType>(1)/BackendType::dot(N_,y(i),s(i));
            alphas[i] = rhos[i]*BackendType::dot(N_,s(i),q_);
            //q_ = q - alphas[i]*y(i);
            BackendType::axpy(N_,-alphas[i],y(i),q_);
        }
        ScalarType scale = BackendType::dot(N_,s(0),y(0))/BackendType::dot(N_,y(0),y(0));

        //r_ = scale*q_;
        BackendType::copy(N_,q_,r_);
        BackendType::scale(N_,scale,r_);

        --i;
        for(; i >=0 ; --i){
            ScalarType beta = rhos[i]*BackendType::dot(N_,y(i),r_);
            //r_ = r_ + (alphas[i]-beta)*s(i)
            BackendType::axpy(N_,alphas[i]-beta,s(i),r_);
        }

        //p = -r_;
        BackendType::copy(N_,r_,p);
        BackendType::scale(N_,-1,p);
    }

    ~lbfgs_implementation(){
        BackendType::delete_if_dynamically_allocated(q_);
        BackendType::delete_if_dynamically_allocated(r_);

        for(unsigned int i = 0 ; i < tag_.m ; ++i){
            BackendType::delete_if_dynamically_allocated(s(i));
            BackendType::delete_if_dynamically_allocated(y(i));
        }
    }

private:
    lbfgs_tag const & tag_;
    detail::optimization_context<BackendType> & context_;

    std::size_t N_;

    VectorType q_;
    VectorType r_;

    std::vector<storage_pair> vecs_;
};

struct bfgs_tag : public qn_update_tag{ };
template<class BackendType>
class bfgs_implementation : public qn_update_implementation<BackendType>{
    typedef typename BackendType::ScalarType ScalarType;
    typedef typename BackendType::VectorType VectorType;
    typedef typename BackendType::MatrixType MatrixType;
public:
    bfgs_implementation(bfgs_tag const &, detail::optimization_context<BackendType> & context) : context_(context), is_first_update_(true){
        N_ = context_.dim();
        Hy_ = BackendType::create_vector(N_); BackendType::set_to_value(Hy_,0,N_);
        s_ = BackendType::create_vector(N_);
        y_ = BackendType::create_vector(N_);
        H_ = BackendType::create_matrix(N_, N_);
    }

    void operator()(){
      //Aliases initialization
      VectorType & x = context_.x();
      VectorType & xm1 = context_.xm1();
      VectorType & g = context_.g();
      VectorType & gm1 = context_.gm1();
      VectorType & p = context_.p();

      //Algorithm

      //s = x - xm1;
      BackendType::copy(N_,x,s_);
      BackendType::axpy(N_,-1,xm1,s_);

      //y = g - gm1;
      BackendType::copy(N_,g,y_);
      BackendType::axpy(N_,-1,gm1,y_);

      ScalarType ys = BackendType::dot(N_,s_,y_);
      if(is_first_update_==true){
        ScalarType yy = BackendType::dot(N_,y_,y_);
        BackendType::set_to_diagonal(N_,H_,ys/yy);
        is_first_update_=false;
      }

      //Hy_ = H*y
      BackendType::symv(N_,1,H_,y_,0,Hy_);
      ScalarType yHy = BackendType::dot(N_,y_,Hy_);

      //H_ += (-1/ys)*(s_*Hy' + Hy*s_')
      BackendType::syr2(N_,-1/ys,s_,Hy_,H_);

      //H_ += (1/ys + yHy/pow(ys,2))*s_*s_'
      BackendType::syr1(N_,1/ys + yHy/pow(ys,2),s_,H_);

      //p = -H_*g
      BackendType::symv(N_,1,H_,g,0,p);
      BackendType::scale(N_,-1,p);
    }

    ~bfgs_implementation(){
        BackendType::delete_if_dynamically_allocated(Hy_);
        BackendType::delete_if_dynamically_allocated(s_);
        BackendType::delete_if_dynamically_allocated(y_);

        BackendType::delete_if_dynamically_allocated(H_);
    }

private:
    detail::optimization_context<BackendType> & context_;

    std::size_t N_;

    VectorType Hy_;
    VectorType s_;
    VectorType y_;

    MatrixType H_;

    bool is_first_update_;
};

template<class BackendType>
struct qn_update_mapping{
    typedef implementation_from_tag<typename make_typelist<FMINCL_CREATE_MAPPING(lbfgs),FMINCL_CREATE_MAPPING(bfgs)>::type
                                   ,qn_update_tag, qn_update_implementation<BackendType> > type;
};

struct quasi_newton_tag : public direction_tag{
    quasi_newton_tag(qn_update_tag * _update = new lbfgs_tag()) : update(_update){ }
    tools::shared_ptr<qn_update_tag> update;
};
template<class BackendType>
class quasi_newton_implementation : public direction_implementation<BackendType>{
  public:
    quasi_newton_implementation(quasi_newton_tag const & tag, detail::optimization_context<BackendType> & context) : update(qn_update_mapping<BackendType>::type::create(*tag.update, context)){ }
    virtual void operator()(){ (*update)();  }
private:
    tools::shared_ptr<qn_update_implementation<BackendType> > update;
};

template<class BackendType>
struct direction_mapping{
    typedef implementation_from_tag<typename make_typelist<FMINCL_CREATE_MAPPING(cg),FMINCL_CREATE_MAPPING(quasi_newton)>::type
                                   ,direction_tag, direction_implementation<BackendType> > type;
};

}

#endif
<commit_msg>Cleaned a bit BFGS computation<commit_after>/* ===========================
 *
 * Copyright (c) 2013 Philippe Tillet - National Chiao Tung University
 *
 * FMinCL - Unconstrained Function Minimization on OpenCL
 *
 * License : MIT X11 - See the LICENSE file in the root folder
 * ===========================*/

#ifndef FMINCL_DIRECTIONS_HPP_
#define FMINCL_DIRECTIONS_HPP_

#include <vector>
#include <cmath>

#include "fmincl/utils.hpp"
#include "fmincl/tools/shared_ptr.hpp"
#include "fmincl/tools/typelist.hpp"
#include "fmincl/mapping.hpp"

namespace fmincl{

struct direction_tag{ virtual ~direction_tag(){ } };
template<class BackendType>
struct direction_implementation{
    virtual void operator()(void) = 0;
    virtual ~direction_implementation(){ }
};

/* =========================== *
 * CONJUGATE GRADIENTS
 * ===========================*/

//UPDATES
struct cg_update_tag{ virtual ~cg_update_tag(){ } };
template<class BackendType>
struct cg_update_implementation{
    virtual typename BackendType::ScalarType operator()(void) = 0;
    virtual ~cg_update_implementation(){ }
};

struct polak_ribiere_tag : public cg_update_tag{ };

template<class BackendType>
struct polak_ribiere_implementation : public cg_update_implementation<BackendType>{
private:
    typedef typename BackendType::ScalarType ScalarType;
    typedef typename BackendType::VectorType VectorType;
public:
    polak_ribiere_implementation(polak_ribiere_tag const &, detail::optimization_context<BackendType> & context) : context_(context){
        N_ = context_.dim();
        tmp_ = BackendType::create_vector(N_);
    }

    ScalarType operator()(){
        VectorType & g = context_.g();
        VectorType & gm1 = context_.gm1();

        //tmp_ = g - gm1;
        BackendType::copy(N_,g, tmp_);
        BackendType::axpy(N_,-1,gm1,tmp_);
        return BackendType::dot(N_,g,tmp_)/BackendType::dot(N_,gm1,gm1);
    }

    ~polak_ribiere_implementation(){
        BackendType::delete_if_dynamically_allocated(tmp_);
    }
private:
    detail::optimization_context<BackendType> & context_;

    std::size_t N_;
    VectorType tmp_;
};

template<class BackendType>
struct cg_update_mapping{

    typedef implementation_from_tag<typename make_typelist<FMINCL_CREATE_MAPPING(polak_ribiere)>::type
                               ,cg_update_tag, cg_update_implementation<BackendType> > type;

};

//RESTARTS
struct cg_restart_tag{ virtual ~cg_restart_tag(){ } };

template<class BackendType>
struct cg_restart_implementation{
    virtual bool operator()(void) = 0;
    virtual ~cg_restart_implementation(){ }
};

struct no_restart_tag : public cg_restart_tag{ };
template<class BackendType>
struct no_restart_implementation : public cg_restart_implementation<BackendType>{
    no_restart_implementation(no_restart_tag const & /*tag*/, detail::optimization_context<BackendType> & /*context*/) { }
    bool operator()() { return false; }
};

template<class BackendType>
struct cg_restart_mapping{
    typedef implementation_from_tag<typename make_typelist<FMINCL_CREATE_MAPPING(no_restart)>::type
                                   ,cg_restart_tag, cg_restart_implementation<BackendType> > type;
};

//CG
struct cg_tag : public direction_tag{
    cg_tag(cg_update_tag * _update = new polak_ribiere_tag(), cg_restart_tag * _restart = new no_restart_tag()) : update(_update), restart(_restart){ }
    tools::shared_ptr<cg_update_tag> update;
    tools::shared_ptr<cg_restart_tag> restart;
};
template<class BackendType>
class cg_implementation : public direction_implementation<BackendType>{
    typedef typename BackendType::ScalarType ScalarType;
    typedef typename BackendType::VectorType VectorType;
public:
    cg_implementation(cg_tag const & cg_params, detail::optimization_context<BackendType> & context) : context_(context)
                                                                                ,update_implementation_(cg_update_mapping<BackendType>::type::create(*cg_params.update, context))
                                                                                ,restart_implementation_(cg_restart_mapping<BackendType>::type::create(*cg_params.restart, context)){ }
    void operator()(){
      VectorType const & g = context_.g();
      VectorType & p = context_.p();
      std::size_t N = context_.dim();

      if((*restart_implementation_)()){

        //p = -g;
        BackendType::copy(N,g,p);
        BackendType::scale(N,-1,p);
      }
      else{
        ScalarType beta = (*update_implementation_)();

        //p = -g + beta*p;
        BackendType::scale(N,beta,p);
        BackendType::axpy(N,-1,g,p);
      }
    }
private:
    detail::optimization_context<BackendType> & context_;

    tools::shared_ptr<cg_update_implementation<BackendType> > update_implementation_;
    tools::shared_ptr<cg_restart_implementation<BackendType> > restart_implementation_;
};

/* =========================== *
 * QUASI NEWTON
 * ===========================*/

struct qn_update_tag{ virtual ~qn_update_tag(){ } };
template<class BackendType>
struct qn_update_implementation{
    virtual void operator()(void) = 0;
    virtual ~qn_update_implementation(){ }
};

struct lbfgs_tag : public qn_update_tag{
    lbfgs_tag(unsigned int _m = 4) : m(_m) { }
    unsigned int m;
};
template<class BackendType>
class lbfgs_implementation : public qn_update_implementation<BackendType>{
    typedef typename BackendType::ScalarType ScalarType;
    typedef typename BackendType::VectorType VectorType;
    typedef typename BackendType::MatrixType MatrixType;

    struct storage_pair{
        VectorType s;
        VectorType y;
    };

    VectorType & s(std::size_t i) { return vecs_[i].s; }
    VectorType & y(std::size_t i) { return vecs_[i].y; }

public:
    lbfgs_implementation(lbfgs_tag const & tag, detail::optimization_context<BackendType> & context) : tag_(tag), context_(context), vecs_(tag.m){
        N_ = context_.dim();

        q_ = BackendType::create_vector(N_);
        r_ = BackendType::create_vector(N_);

        for(unsigned int i = 0 ; i < tag_.m ; ++i){
            vecs_[i].s = BackendType::create_vector(N_);
            vecs_[i].y = BackendType::create_vector(N_);
        }
    }

    void operator()(){
        //Initizalization of aliases
        VectorType const & x=context_.x();
        VectorType const & xm1=context_.xm1();
        VectorType const & g=context_.g();
        VectorType const & gm1=context_.gm1();
        VectorType & p=context_.p();

        unsigned int const & iter = context_.iter();
        unsigned int const & m = tag_.m;

        std::vector<ScalarType> rhos(m);
        std::vector<ScalarType> alphas(m);

        //Algorithm

        //Updates storage
        for(unsigned int i = std::min(iter,m)-1 ; i > 0  ; --i){
            BackendType::copy(N_,s(i-1), s(i));
            BackendType::copy(N_,y(i-1), y(i));
        }

        //s(0) = x - xm1;
        BackendType::copy(N_,x,s(0));
        BackendType::axpy(N_,-1,xm1,s(0));

        //y(0) = g - gm1;
        BackendType::copy(N_,g,y(0));
        BackendType::axpy(N_,-1,gm1,y(0));

        BackendType::copy(N_,g,q_);
        int i = 0;
        for(; i < (int)std::min(iter,m) ; ++i){
            rhos[i] = static_cast<ScalarType>(1)/BackendType::dot(N_,y(i),s(i));
            alphas[i] = rhos[i]*BackendType::dot(N_,s(i),q_);
            //q_ = q - alphas[i]*y(i);
            BackendType::axpy(N_,-alphas[i],y(i),q_);
        }
        ScalarType scale = BackendType::dot(N_,s(0),y(0))/BackendType::dot(N_,y(0),y(0));

        //r_ = scale*q_;
        BackendType::copy(N_,q_,r_);
        BackendType::scale(N_,scale,r_);

        --i;
        for(; i >=0 ; --i){
            ScalarType beta = rhos[i]*BackendType::dot(N_,y(i),r_);
            //r_ = r_ + (alphas[i]-beta)*s(i)
            BackendType::axpy(N_,alphas[i]-beta,s(i),r_);
        }

        //p = -r_;
        BackendType::copy(N_,r_,p);
        BackendType::scale(N_,-1,p);
    }

    ~lbfgs_implementation(){
        BackendType::delete_if_dynamically_allocated(q_);
        BackendType::delete_if_dynamically_allocated(r_);

        for(unsigned int i = 0 ; i < tag_.m ; ++i){
            BackendType::delete_if_dynamically_allocated(s(i));
            BackendType::delete_if_dynamically_allocated(y(i));
        }
    }

private:
    lbfgs_tag const & tag_;
    detail::optimization_context<BackendType> & context_;

    std::size_t N_;

    VectorType q_;
    VectorType r_;

    std::vector<storage_pair> vecs_;
};

struct bfgs_tag : public qn_update_tag{ };
template<class BackendType>
class bfgs_implementation : public qn_update_implementation<BackendType>{
    typedef typename BackendType::ScalarType ScalarType;
    typedef typename BackendType::VectorType VectorType;
    typedef typename BackendType::MatrixType MatrixType;
public:
    bfgs_implementation(bfgs_tag const &, detail::optimization_context<BackendType> & context) : context_(context), is_first_update_(true){
        N_ = context_.dim();
        Hy_ = BackendType::create_vector(N_);
        s_ = BackendType::create_vector(N_);
        y_ = BackendType::create_vector(N_);
        H_ = BackendType::create_matrix(N_, N_);

        BackendType::set_to_value(Hy_,0,N_);
        BackendType::set_to_value(s_,0,N_);
        BackendType::set_to_value(y_,0,N_);

    }

    void operator()(){
      //Aliases initialization
      VectorType & x = context_.x();
      VectorType & xm1 = context_.xm1();
      VectorType & g = context_.g();
      VectorType & gm1 = context_.gm1();
      VectorType & p = context_.p();

      //Algorithm

      //s = x - xm1;
      BackendType::copy(N_,x,s_);
      BackendType::axpy(N_,-1,xm1,s_);

      //y = g - gm1;
      BackendType::copy(N_,g,y_);
      BackendType::axpy(N_,-1,gm1,y_);

      ScalarType ys = BackendType::dot(N_,s_,y_);
      if(is_first_update_==true){
        ScalarType yy = BackendType::dot(N_,y_,y_);
        BackendType::set_to_diagonal(N_,H_,ys/yy);
        is_first_update_=false;
      }

      //Hy_ = H*y
      BackendType::symv(N_,1,H_,y_,0,Hy_);
      ScalarType yHy = BackendType::dot(N_,y_,Hy_);
      ScalarType alpha = -1/ys;
      ScalarType beta = 1/ys + yHy/pow(ys,2);
      //H_ += alpha*(s_*Hy' + Hy*s_') + beta*s_*s_';
      BackendType::syr2(N_,alpha,s_,Hy_,H_);
      BackendType::syr1(N_,beta,s_,H_);

      //p = -H_*g
      BackendType::symv(N_,-1,H_,g,0,p);
    }

    ~bfgs_implementation(){
        BackendType::delete_if_dynamically_allocated(Hy_);
        BackendType::delete_if_dynamically_allocated(s_);
        BackendType::delete_if_dynamically_allocated(y_);

        BackendType::delete_if_dynamically_allocated(H_);
    }

private:
    detail::optimization_context<BackendType> & context_;

    std::size_t N_;

    VectorType Hy_;
    VectorType s_;
    VectorType y_;

    MatrixType H_;

    bool is_first_update_;
};

template<class BackendType>
struct qn_update_mapping{
    typedef implementation_from_tag<typename make_typelist<FMINCL_CREATE_MAPPING(lbfgs),FMINCL_CREATE_MAPPING(bfgs)>::type
                                   ,qn_update_tag, qn_update_implementation<BackendType> > type;
};

struct quasi_newton_tag : public direction_tag{
    quasi_newton_tag(qn_update_tag * _update = new lbfgs_tag()) : update(_update){ }
    tools::shared_ptr<qn_update_tag> update;
};
template<class BackendType>
class quasi_newton_implementation : public direction_implementation<BackendType>{
  public:
    quasi_newton_implementation(quasi_newton_tag const & tag, detail::optimization_context<BackendType> & context) : update(qn_update_mapping<BackendType>::type::create(*tag.update, context)){ }
    virtual void operator()(){ (*update)();  }
private:
    tools::shared_ptr<qn_update_implementation<BackendType> > update;
};

template<class BackendType>
struct direction_mapping{
    typedef implementation_from_tag<typename make_typelist<FMINCL_CREATE_MAPPING(cg),FMINCL_CREATE_MAPPING(quasi_newton)>::type
                                   ,direction_tag, direction_implementation<BackendType> > type;
};

}

#endif
<|endoftext|>


<commit_before>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: ItemPropertyMap.hxx,v $
 *
 *  $Revision: 1.3 $
 *
 *  last change: $Author: rt $ $Date: 2005-09-08 00:23:30 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/
#ifndef CHART_ITEMPROPERTYMAP_HXX
#define CHART_ITEMPROPERTYMAP_HXX

#include "InlineContainer.hxx"

namespace comphelper
{

typedef ::std::map< USHORT, ::rtl::OUString > ItemPropertyMapType;
typedef ::comphelper::MakeMap< USHORT, ::rtl::OUString > MakeItemPropertyMap;

class FillItemSetFunc : public ::std::unary_function< void, ItemConverter * >
{
public:
    explicit FillItemSetFunc( SfxItemSet & rOutItemSet ) :
            m_rOutItemSet( rOutItemSet )
    {}

    void operator() ( ItemConverter * pConv )
    {
        pConv->FillItemSet( m_rOutItemSet );
    }

private:
    SfxItemSet & m_rOutItemSet;
};

class ApplyItemSetFunc : public ::std::unary_function< void, ItemConverter * >
{
public:
    explicit ApplyItemSetFunc( const SfxItemSet & rItemSet,
                               bool & rOutResult ) :
            m_rItemSet( rItemSet ),
            m_rOutResult( rOutResult )
    {}

    void operator() ( ItemConverter * pConv )
    {
        m_rOutResult = pConv->ApplyItemSet( m_rItemSet ) || m_rOutResult;
    }

private:
    const SfxItemSet & m_rItemSet;
    bool & m_rOutResult;
};

struct DeleteItemConverterPtr : public ::std::unary_function< void, ItemConverter * >
{
    void operator() ( ItemConverter * pConv )
    { delete pConv; }
};

} //  namespace comphelper

// CHART_ITEMPROPERTYMAP_HXX
#endif
<commit_msg>INTEGRATION: CWS chart2mst3 (1.2.4); FILE MERGED 2006/01/27 09:51:20 bm 1.2.4.4: InlineContainer.hxx moved to comphelper 2005/10/07 11:26:31 bm 1.2.4.3: RESYNC: (1.2-1.3); FILE MERGED 2004/05/07 09:14:08 bm 1.2.4.2: enabling member-ids for all items 2004/02/13 16:51:18 bm 1.2.4.1: join from changes on branch bm_post_chart01<commit_after>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: ItemPropertyMap.hxx,v $
 *
 *  $Revision: 1.4 $
 *
 *  last change: $Author: vg $ $Date: 2007-05-22 17:54:14 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/
#ifndef CHART_ITEMPROPERTYMAP_HXX
#define CHART_ITEMPROPERTYMAP_HXX

#ifndef INCLUDED_COMPHELPER_INLINE_CONTAINER_HXX
#include <comphelper/InlineContainer.hxx>
#endif

#ifndef CHART_ITEMCONVERTER_HXX
#include "ItemConverter.hxx"
#endif

#define IPM_MAP_ENTRY(wid,uno,mid) (wid, ::std::make_pair< ::comphelper::ItemConverter::tPropertyNameType, ::comphelper::ItemConverter::tMemberIdType >(\
    ::rtl::OUString(RTL_CONSTASCII_USTRINGPARAM(uno)), mid))

namespace comphelper
{

typedef ::std::map< ItemConverter::tWhichIdType, ::std::pair< ItemConverter::tPropertyNameType, ItemConverter::tMemberIdType > >
    ItemPropertyMapType;
typedef ::comphelper::MakeMap< ItemConverter::tWhichIdType, ::std::pair< ItemConverter::tPropertyNameType, ItemConverter::tMemberIdType > >
    MakeItemPropertyMap;

class FillItemSetFunc : public ::std::unary_function< ItemConverter *, void >
{
public:
    explicit FillItemSetFunc( SfxItemSet & rOutItemSet ) :
            m_rOutItemSet( rOutItemSet )
    {}

    void operator() ( ItemConverter * pConv )
    {
        pConv->FillItemSet( m_rOutItemSet );
    }

private:
    SfxItemSet & m_rOutItemSet;
};

class ApplyItemSetFunc : public ::std::unary_function< ItemConverter *, void >
{
public:
    explicit ApplyItemSetFunc( const SfxItemSet & rItemSet,
                               bool & rOutResult ) :
            m_rItemSet( rItemSet ),
            m_rOutResult( rOutResult )
    {}

    void operator() ( ItemConverter * pConv )
    {
        m_rOutResult = pConv->ApplyItemSet( m_rItemSet ) || m_rOutResult;
    }

private:
    const SfxItemSet & m_rItemSet;
    bool & m_rOutResult;
};

struct DeleteItemConverterPtr : public ::std::unary_function< ItemConverter *, void >
{
    void operator() ( ItemConverter * pConv )
    { delete pConv; }
};

} //  namespace comphelper

// CHART_ITEMPROPERTYMAP_HXX
#endif
<|endoftext|>


<commit_before><commit_msg>fdo#37197: No legend entry for regression curve if not applicable<commit_after><|endoftext|>


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

#include <fstream>

#include <ros/ros.h>
#include <ros/console.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>

static tf::StampedTransform transform;
static bool output_first;

static int sub_point_queue_size;

static double velocity;		// km/h
static std::string output_file;

static void write_clicked_point(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
{
	if (output_first) {
		std::ofstream ofs(output_file.c_str());
		ofs << std::fixed << msg->pose.pose.position.x + transform.getOrigin().x() << ","
		    << std::fixed << msg->pose.pose.position.y + transform.getOrigin().y() << ","
		    << std::fixed << msg->pose.pose.position.z + transform.getOrigin().z() << std::endl;
	} else {
		std::ofstream ofs(output_file.c_str(), std::ios_base::app);
		ofs << std::fixed << msg->pose.pose.position.x + transform.getOrigin().x() << ","
		    << std::fixed << msg->pose.pose.position.y + transform.getOrigin().y() << ","
		    << std::fixed << msg->pose.pose.position.z + transform.getOrigin().z() << ","
		    << std::fixed << velocity << std::endl;
	}

	if (output_first)
		output_first = false;
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "waypoint_clicker");

	ros::NodeHandle n;
	n.param<int>("/waypoint_clicker/sub_point_queue_size", sub_point_queue_size, 1);
	n.param<double>("/waypoint_clicker/velocity", velocity, 40);
	n.param<std::string>("/waypoint_clicker/output_file", output_file, "/tmp/lane_waypoint.csv");

	ros::Subscriber sub_point = n.subscribe("/initialpose", sub_point_queue_size, write_clicked_point);

	tf::TransformListener listener;
	try {
		ros::Time zero = ros::Time(0);
		listener.waitForTransform("map", "world", zero, ros::Duration(10));
		listener.lookupTransform("map", "world", zero, transform);
	} catch (tf::TransformException &ex) {
		ROS_ERROR("%s", ex.what());
	}
	output_first = true;

	ros::spin();

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

#include <fstream>

#include <ros/ros.h>
#include <ros/console.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/PointStamped.h>
#include <visualization_msgs/Marker.h>

#include <vmap_utility.hpp>

static const std::string FRAME_ID = "map";

static ros::Publisher pub_marker;

static VectorMap vmap_all;
static VectorMap vmap_lane;
static VectorMap vmap_coarse;

visualization_msgs::Marker selection;
static tf::StampedTransform transform;
static bool output_first;

static int sub_vmap_queue_size;
static int sub_pose_queue_size;
static int pub_marker_queue_size;

static double velocity;		// km/h
static std::string output_file;
static int waypoint_max;

static bool is_branching(const map_file::Lane& lane)
{
	return (lane.jct >= 1 && lane.jct <= 2);
}

static bool is_merging(const map_file::Lane& lane)
{
	return (lane.jct >= 3 && lane.jct <= 4);
}

static void delete_all_marker()
{
	visualization_msgs::Marker marker;
	marker.header.frame_id = FRAME_ID;
	marker.header.stamp = ros::Time::now();
	marker.id = 0;
	marker.action = visualization_msgs::Marker::DELETE;

	marker.ns = "waypoint";
	pub_marker.publish(marker);
	marker.ns = "branching";
	pub_marker.publish(marker);
	marker.ns = "merging";
	pub_marker.publish(marker);
	marker.ns = "selection";
	pub_marker.publish(marker);
	marker.ns = "route";
	pub_marker.publish(marker);
}

static void publish_waypoint()
{
	visualization_msgs::Marker waypoint;
	waypoint.header.frame_id = FRAME_ID;
	waypoint.header.stamp = ros::Time::now();
	waypoint.ns = "waypoint";
	waypoint.id = 0;
	waypoint.type = visualization_msgs::Marker::SPHERE_LIST;
	waypoint.action = visualization_msgs::Marker::ADD;
	waypoint.scale.x = 0.2;
	waypoint.scale.y = 0.2;
	waypoint.color.r = 1;
	waypoint.color.g = 1;
	waypoint.color.b = 0;
	waypoint.color.a = 1;
	waypoint.frame_locked = true;

	for (const map_file::PointClass& p : vmap_lane.points) {
		// msg's X-Y axis is reversed
		geometry_msgs::Point point;
		point.x = p.ly;
		point.y = p.bx;
		point.z = p.h;
		waypoint.points.push_back(point);
	}

	pub_marker.publish(waypoint);
}

static void publish_branching()
{
	visualization_msgs::Marker branching;
	branching.header.frame_id = FRAME_ID;
	branching.header.stamp = ros::Time::now();
	branching.ns = "branching";
	branching.id = 0;
	branching.type = visualization_msgs::Marker::SPHERE_LIST;
	branching.action = visualization_msgs::Marker::ADD;
	branching.scale.x = 0.2;
	branching.scale.y = 0.2;
	branching.color.r = 0;
	branching.color.g = 1;
	branching.color.b = 0;
	branching.color.a = 1;
	branching.frame_locked = true;

	for (const map_file::Lane& l : vmap_lane.lanes) {
		if (!is_branching(l))
			continue;
		map_file::PointClass p = vmap_find_end_point(vmap_lane, l);
		if (p.pid < 0)
			continue;
		// msg's X-Y axis is reversed
		geometry_msgs::Point point;
		point.x = p.ly;
		point.y = p.bx;
		point.z = p.h;
		branching.points.push_back(point);
	}

	pub_marker.publish(branching);
}

static void publish_merging()
{
	visualization_msgs::Marker merging;
	merging.header.frame_id = FRAME_ID;
	merging.header.stamp = ros::Time::now();
	merging.ns = "merging";
	merging.id = 0;
	merging.type = visualization_msgs::Marker::SPHERE_LIST;
	merging.action = visualization_msgs::Marker::ADD;
	merging.scale.x = 0.2;
	merging.scale.y = 0.2;
	merging.color.r = 1;
	merging.color.g = 0;
	merging.color.b = 0;
	merging.color.a = 1;
	merging.frame_locked = true;

	for (const map_file::Lane& l : vmap_lane.lanes) {
		if (!is_merging(l))
			continue;
		map_file::PointClass p = vmap_find_start_point(vmap_lane, l);
		if (p.pid < 0)
			continue;
		// msg's X-Y axis is reversed
		geometry_msgs::Point point;
		point.x = p.ly;
		point.y = p.bx;
		point.z = p.h;
		merging.points.push_back(point);
	}

	pub_marker.publish(merging);
}

static void publish_selection(const geometry_msgs::PointStamped& pose)
{
	selection.header.stamp = ros::Time::now();

	geometry_msgs::Point point;
	point.x = pose.point.x + transform.getOrigin().x();
	point.y = pose.point.y + transform.getOrigin().y();
	point.z = pose.point.z + transform.getOrigin().z();
	selection.points.push_back(point);

	pub_marker.publish(selection);
}

static void cache_vmap_lane()
{
	VectorMap vmap;
	for (const map_file::Lane& l : vmap_all.lanes) {
		vmap.lanes.push_back(l);
		for (const map_file::Node& n : vmap_all.nodes) {
			if (n.nid != l.bnid && n.nid != l.fnid)
				continue;
			vmap.nodes.push_back(n);
			for (const map_file::PointClass& p : vmap_all.points) {
				if (p.pid != n.pid)
					continue;
				vmap.points.push_back(p);
			}
		}
	}
	vmap_lane = vmap;
}

static void cache_lane(const map_file::LaneArray& msg)
{
	vmap_all.lanes = msg.lanes;

	if (vmap_all.lanes.empty() || vmap_all.nodes.empty() || vmap_all.points.empty())
		return;

	cache_vmap_lane();
	delete_all_marker();
	publish_waypoint();
	publish_branching();
	publish_merging();
}

static void cache_node(const map_file::NodeArray& msg)
{
	vmap_all.nodes = msg.nodes;

	if (vmap_all.lanes.empty() || vmap_all.nodes.empty() || vmap_all.points.empty())
		return;

	cache_vmap_lane();
	delete_all_marker();
	publish_waypoint();
	publish_branching();
	publish_merging();
}

static void cache_point(const map_file::PointClassArray& msg)
{
	vmap_all.points = msg.point_classes;

	if (vmap_all.lanes.empty() || vmap_all.nodes.empty() || vmap_all.points.empty())
		return;

	cache_vmap_lane();
	delete_all_marker();
	publish_waypoint();
	publish_branching();
	publish_merging();
}

static void write_route(const geometry_msgs::Point& point, bool first)
{
	if (first) {
		std::ofstream ofs(output_file.c_str());
		ofs << std::fixed << point.x << ","
		    << std::fixed << point.y << ","
		    << std::fixed << point.z << std::endl;
	} else {
		std::ofstream ofs(output_file.c_str(), std::ios_base::app);
		ofs << std::fixed << point.x << ","
		    << std::fixed << point.y << ","
		    << std::fixed << point.z << ","
		    << std::fixed << velocity << std::endl;
	}
}

static void create_route(const geometry_msgs::PointStamped& msg)
{
	if (vmap_all.lanes.empty() || vmap_all.nodes.empty() || vmap_all.points.empty())
		return;

	publish_selection(msg);

	// msg's X-Y axis is reversed
	map_file::PointClass coarse;
	coarse.bx = msg.point.y + transform.getOrigin().y();
	coarse.ly = msg.point.x + transform.getOrigin().x();
	coarse.h = msg.point.z + transform.getOrigin().z();
	vmap_coarse.points.push_back(coarse);

	if (vmap_coarse.points.size() < 2)
		return;

	map_file::PointClass start_point = vmap_find_nearest_point(vmap_lane, vmap_coarse.points.front());
	if (start_point.pid < 0) {
		ROS_ERROR("no start point");
		return;
	}
	map_file::PointClass end_point = vmap_find_nearest_point(vmap_lane, vmap_coarse.points.back());
	if (end_point.pid < 0) {
		ROS_ERROR("no end point");
		return;
	}
	map_file::PointClass point = start_point;
	map_file::Lane lane = vmap_find_lane(vmap_lane, point);
	if (lane.lnid < 0) {
		ROS_ERROR("no start lane");
		return;
	}

	visualization_msgs::Marker route;
	route.header.frame_id = FRAME_ID;
	route.header.stamp = ros::Time::now();
	route.ns = "route";
	route.id = 0;
	route.type = visualization_msgs::Marker::LINE_STRIP;
	route.action = visualization_msgs::Marker::ADD;
	route.scale.x = 0.2;
	route.scale.y = 0.2;
	route.color.r = 1;
	route.color.g = 1;
	route.color.b = 0;
	route.color.a = 1;
	route.frame_locked = true;

	bool finish = false;
	for (int i = 0; i < waypoint_max; ++i) {
		// msg's X-Y axis is reversed
		geometry_msgs::Point route_point;
		route_point.x = point.ly;
		route_point.y = point.bx;
		route_point.z = point.h;
		write_route(route_point, (i == 0));
		route.points.push_back(route_point);

		if (finish)
			break;

		point = vmap_find_end_point(vmap_lane, lane);
		if (point.pid < 0) {
			ROS_ERROR("no next point");
			return;
		}

		if (point.bx == end_point.bx && point.ly == end_point.ly) {
			finish = true;
			continue;
		}

		if (is_branching(lane)) {
			map_file::PointClass p1 = vmap_find_end_point(vmap_lane, lane);
			if (p1.pid < 0) {
				ROS_ERROR("no branching start point");
				return;
			}

			p1 = vmap_find_nearest_point(vmap_coarse, p1);
			if (p1.pid < 0) {
				ROS_ERROR("no coarse branching start point");
				return;
			}

			map_file::PointClass p2;
			double distance = -1;
			for (const map_file::PointClass& p : vmap_coarse.points) {
				if (distance == -1) {
					if (p.bx == p1.bx && p.ly == p1.ly)
						distance = 0;
					continue;
				}
				p2 = p;
				distance = hypot(p1.bx - p2.bx, p1.ly - p2.ly);
				if (distance > VMAP_JUNCTION_DISTANCE_MAX)
					break;
			}
			if (distance <= 0) {
				ROS_ERROR("no coarse branching end point");
				return;
			}

			lane = vmap_find_junction_lane(vmap_lane, lane, vmap_compute_direction_angle(p1, p2));
		} else
			lane = vmap_find_next_lane(vmap_lane, lane);
		if (lane.lnid < 0) {
			ROS_ERROR("no next lane");
			return;
		}
	}

	if (!finish) {
		ROS_ERROR("route not found");
		return;
	}

	pub_marker.publish(route);
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "waypoint_clicker");

	ros::NodeHandle n;
	n.param<int>("/waypoint_clicker/sub_vmap_queue_size", sub_vmap_queue_size, 1);
	n.param<int>("/waypoint_clicker/sub_pose_queue_size", sub_pose_queue_size, 1);
	n.param<int>("/waypoint_clicker/pub_marker_queue_size", pub_marker_queue_size, 10);
	n.param<double>("/waypoint_clicker/velocity", velocity, 40);
	n.param<std::string>("/waypoint_clicker/output_file", output_file, "/tmp/lane_waypoint.csv");
	n.param<int>("/waypoint_clicker/waypoint_max", waypoint_max, 10000);

	ros::Subscriber sub_lane = n.subscribe("/vector_map_info/lane", sub_vmap_queue_size, cache_lane);
	ros::Subscriber sub_node = n.subscribe("/vector_map_info/node", sub_vmap_queue_size, cache_node);
	ros::Subscriber sub_point = n.subscribe("/vector_map_info/point_class", sub_vmap_queue_size, cache_point);
	ros::Subscriber sub_pose = n.subscribe("/clicked_point", sub_pose_queue_size, create_route);

	pub_marker = n.advertise<visualization_msgs::Marker>("/waypoint_guide", pub_marker_queue_size, true);

	selection.header.frame_id = FRAME_ID;
	selection.ns = "selection";
	selection.id = 0;
	selection.type = visualization_msgs::Marker::SPHERE_LIST;
	selection.action = visualization_msgs::Marker::ADD;
	selection.scale.x = 0.4;
	selection.scale.y = 0.4;
	selection.color.r = 1;
	selection.color.g = 1;
	selection.color.b = 0;
	selection.color.a = 1;
	selection.frame_locked = true;

	tf::TransformListener listener;
	try {
		ros::Time zero = ros::Time(0);
		listener.waitForTransform("map", "world", zero, ros::Duration(10));
		listener.lookupTransform("map", "world", zero, transform);
	} catch (tf::TransformException &ex) {
		ROS_ERROR("%s", ex.what());
	}
	output_first = true;

	ros::spin();

	return 0;
}
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2014 Eran Pe'er.
 *
 * This program is made available under the terms of the MIT License.
 *
 * Created on Mar 10, 2014
 */

#include <string>
#include <iosfwd>
#include <stdexcept>
#include <tuple>
#include <iosfwd>
#include <type_traits>

#include "tpunit++.hpp"
#include "fakeit.hpp"
#include "mockutils/Formatter.hpp"

using namespace fakeit;

struct TypeInfoTests : tpunit::TestFixture {

	TypeInfoTests() :
	tpunit::TestFixture(
	//
	TEST(TypeInfoTests::mock_should_use_same_typeid_as_moked_class), //
	TEST(TypeInfoTests::simple_inheritance_upcast), //
	TEST(TypeInfoTests::dynamic_cast_to_same_type__with_concrete_type),
	TEST(TypeInfoTests::dynamic_cast_to_same_type__with_abstract_type),
	TEST(TypeInfoTests::simple_inheritance_dynamic_down_cast) //
	)  //
	{
	}

	struct SomeInterface {
		virtual int func(int) = 0;
	};

	void mock_should_use_same_typeid_as_moked_class() {
		Mock<SomeInterface> mock;
		ASSERT_EQUAL(typeid(mock.get()), typeid(SomeInterface));
	}

	struct ConcreteType {
		int demo;
		virtual int f() { return 0; };
	};

	struct TopLeft {
		int topLeft;
		virtual int l() = 0;
	};

	struct Left : public TopLeft {
		int left;
		virtual int l() override = 0;
	};

	struct TopRight {
		int topRight;
		virtual int r() = 0;
	};

	struct Right : public TopRight {
		int right;
		virtual int r() override = 0;
	};

	struct A : public Left
	//, public Right
	{
		int a;
		virtual int l() override { return 0; };
		//virtual int r() override { return 0; };
	};

	void simple_inheritance_upcast() {

		Mock<A> aMock;
		A& a = aMock.get();
		Fake(Method(aMock,l));

		Left& left = a;
		TopLeft& topLeft = left;

		ASSERT_EQUAL(0, a.l());
		ASSERT_EQUAL(0, left.l());
		ASSERT_EQUAL(0, topLeft.l());
	}

	void simple_inheritance_dynamic_down_cast() {
		Mock<A, Left, TopLeft> aMock;
		Fake(Method(aMock,l));
		A& a = aMock.get();
		Left* left = &a;
		TopLeft* topLeft = &a;

		A* aPtr = dynamic_cast<A*>(left);
 		ASSERT_EQUAL(0, aPtr->l());

 		aPtr = dynamic_cast<A*>(topLeft);
  		ASSERT_EQUAL(0, aPtr->l());

  		left = dynamic_cast<Left*>(topLeft);
  		ASSERT_EQUAL(0, left->l());
	}

	void dynamic_cast_to_same_type__with_concrete_type()
	{
		Mock<ConcreteType> mockDemo;
		ConcreteType* mockDemoPtr = &mockDemo.get();
		ConcreteType* mockDemoPtr2 = dynamic_cast<ConcreteType*>(mockDemoPtr);
		ASSERT_EQUAL(mockDemoPtr, mockDemoPtr2);
	}

	void dynamic_cast_to_same_type__with_abstract_type()
	{
		Mock<TopLeft> mock;
		TopLeft* ptr = &mock.get();
		TopLeft* ptr2 = dynamic_cast<TopLeft*>(ptr);
		ASSERT_EQUAL(ptr, ptr2);
	}

} __TypeInfoTests;
<commit_msg>fix typo<commit_after>/*
 * Copyright (c) 2014 Eran Pe'er.
 *
 * This program is made available under the terms of the MIT License.
 *
 * Created on Mar 10, 2014
 */

#include <string>
#include <iosfwd>
#include <stdexcept>
#include <tuple>
#include <iosfwd>
#include <type_traits>

#include "tpunit++.hpp"
#include "fakeit.hpp"
#include "mockutils/Formatter.hpp"

using namespace fakeit;

struct TypeInfoTests : tpunit::TestFixture {

	TypeInfoTests() :
	tpunit::TestFixture(
	//
	TEST(TypeInfoTests::mock_should_use_same_typeid_as_mocked_class), //
	TEST(TypeInfoTests::simple_inheritance_upcast), //
	TEST(TypeInfoTests::dynamic_cast_to_same_type__with_concrete_type),
	TEST(TypeInfoTests::dynamic_cast_to_same_type__with_abstract_type),
	TEST(TypeInfoTests::simple_inheritance_dynamic_down_cast) //
	)  //
	{
	}

	struct SomeInterface {
		virtual int func(int) = 0;
	};

	void mock_should_use_same_typeid_as_mocked_class() {
		Mock<SomeInterface> mock;
		ASSERT_EQUAL(typeid(mock.get()), typeid(SomeInterface));
	}

	struct ConcreteType {
		int demo;
		virtual int f() { return 0; };
	};

	struct TopLeft {
		int topLeft;
		virtual int l() = 0;
	};

	struct Left : public TopLeft {
		int left;
		virtual int l() override = 0;
	};

	struct TopRight {
		int topRight;
		virtual int r() = 0;
	};

	struct Right : public TopRight {
		int right;
		virtual int r() override = 0;
	};

	struct A : public Left
	//, public Right
	{
		int a;
		virtual int l() override { return 0; };
		//virtual int r() override { return 0; };
	};

	void simple_inheritance_upcast() {

		Mock<A> aMock;
		A& a = aMock.get();
		Fake(Method(aMock,l));

		Left& left = a;
		TopLeft& topLeft = left;

		ASSERT_EQUAL(0, a.l());
		ASSERT_EQUAL(0, left.l());
		ASSERT_EQUAL(0, topLeft.l());
	}

	void simple_inheritance_dynamic_down_cast() {
		Mock<A, Left, TopLeft> aMock;
		Fake(Method(aMock,l));
		A& a = aMock.get();
		Left* left = &a;
		TopLeft* topLeft = &a;

		A* aPtr = dynamic_cast<A*>(left);
 		ASSERT_EQUAL(0, aPtr->l());

 		aPtr = dynamic_cast<A*>(topLeft);
  		ASSERT_EQUAL(0, aPtr->l());

  		left = dynamic_cast<Left*>(topLeft);
  		ASSERT_EQUAL(0, left->l());
	}

	void dynamic_cast_to_same_type__with_concrete_type()
	{
		Mock<ConcreteType> mockDemo;
		ConcreteType* mockDemoPtr = &mockDemo.get();
		ConcreteType* mockDemoPtr2 = dynamic_cast<ConcreteType*>(mockDemoPtr);
		ASSERT_EQUAL(mockDemoPtr, mockDemoPtr2);
	}

	void dynamic_cast_to_same_type__with_abstract_type()
	{
		Mock<TopLeft> mock;
		TopLeft* ptr = &mock.get();
		TopLeft* ptr2 = dynamic_cast<TopLeft*>(ptr);
		ASSERT_EQUAL(ptr, ptr2);
	}

} __TypeInfoTests;
<|endoftext|>


<commit_before><commit_msg>AimGate for kick angle error margin dependant on gate distance.<commit_after><|endoftext|>


<commit_before><commit_msg>Change the size of media cache Setting media cache too large caused creation of cache backend failed because rvargas is performing a experiment that limits cache size to ~240MB, respect such change.<commit_after><|endoftext|>


<commit_before>#define CATCH_CONFIG_MAIN
#include "catch.hpp"

#include "scene/sceneLayer.h"

using namespace Tangram;

using Context = StyleContext;

// Functions to initialize SceneLayer instances

SceneLayer instance_a() {

    Filter f = Filter(); // passes everything

    DrawRule rule = { "dg0", "style_A", { { StyleParamKey::order, "value_a" } } };

    return { "layer_a", f, { rule }, {} };
}

SceneLayer instance_b() {

    Filter f = Filter::MatchAny({}); // passes nothing

    DrawRule rule = { "dg1", "style_B", { { StyleParamKey::order, "value_b" } } };

    return { "layer_b", f, { rule }, {} };
}

SceneLayer instance_c() {

    Filter f = Filter(); // passes everything

    DrawRule rule = { "dg2", "style_C", { { StyleParamKey::order, "value_c" } } };

    return { "layer_c", f, { rule }, { instance_a(), instance_b() } };
}

SceneLayer instance_d() {

    Filter f = Filter(); // passes everything

    DrawRule rule = { "dg0", "style_A", { { StyleParamKey::order, "value_d" } } };

    return { "layer_d", f, { rule }, {} };
}

SceneLayer instance_e() {

    Filter f = Filter(); // passes everything

    DrawRule rule = { "dg2", "style_C", { { StyleParamKey::order, "value_e" } } };

    return { "layer_e", f, { rule }, { instance_c(), instance_d() } };
}

TEST_CASE("SceneLayer matches correct rules for a feature and context", "[SceneLayer][Filter]") {

    Feature feat;
    Context ctx;

    auto layer_a = instance_a();

    std::vector<DrawRule> matches_a;
    layer_a.match(feat, ctx, matches_a);

    REQUIRE(matches_a.size() == 1);
    REQUIRE(matches_a[0].style == "style_A");

    auto layer_b = instance_b();

    std::vector<DrawRule> matches_b;
    layer_b.match(feat, ctx, matches_b);

    REQUIRE(matches_b.size() == 0);

}

TEST_CASE("SceneLayer matches correct sublayer rules for a feature and context", "[SceneLayer][Filter]") {

    Feature feat;
    Context ctx;

    auto layer_c = instance_c();

    std::vector<DrawRule> matches;
    layer_c.match(feat, ctx, matches);

    REQUIRE(matches.size() == 2);

    // matches should be in lexicographic order by style
    REQUIRE(matches[0].style == "style_A");
    REQUIRE(matches[1].style == "style_C");

}

TEST_CASE("SceneLayer correctly merges rules matched from sublayer", "[SceneLayer][Filter]") {

    Feature feat;
    Context ctx;

    auto layer_e = instance_e();

    std::vector<DrawRule> matches;
    layer_e.match(feat, ctx, matches);

    REQUIRE(matches.size() == 2);

    // deeper match from layer_a should override parameters in same style from layer_d
    REQUIRE(matches[0].style == "style_A");
    REQUIRE(matches[0].parameters[0].key == StyleParamKey::order);
    REQUIRE(matches[0].parameters[0].value.get<std::string>() == "value_a");

    // deeper match from layer_c should override parameters in same style from layer_e
    REQUIRE(matches[1].style == "style_C");
    REQUIRE(matches[1].parameters[0].key == StyleParamKey::order);
    REQUIRE(matches[1].parameters[0].value.get<std::string>() == "value_c");

}
<commit_msg>Adds THE test case<commit_after>#define CATCH_CONFIG_MAIN
#include "catch.hpp"

#include "scene/sceneLayer.h"

using namespace Tangram;

using Context = StyleContext;

// Functions to initialize SceneLayer instances

SceneLayer instance_a() {

    Filter f = Filter(); // passes everything

    DrawRule rule = { "dg0", "style_A", { { StyleParamKey::order, "value_a" } } };

    return { "layer_a", f, { rule }, {} };
}

SceneLayer instance_b() {

    Filter f = Filter::MatchAny({}); // passes nothing

    DrawRule rule = { "dg1", "style_B", { { StyleParamKey::order, "value_b" } } };

    return { "layer_b", f, { rule }, {} };
}

SceneLayer instance_c() {

    Filter f = Filter(); // passes everything

    DrawRule rule = { "dg2", "style_C", { { StyleParamKey::order, "value_c" } } };

    return { "layer_c", f, { rule }, { instance_a(), instance_b() } };
}

SceneLayer instance_d() {

    Filter f = Filter(); // passes everything

    DrawRule rule = { "dg0", "style_A", { { StyleParamKey::order, "value_d" } } };

    return { "layer_d", f, { rule }, {} };
}

SceneLayer instance_e() {

    Filter f = Filter(); // passes everything

    DrawRule rule = { "dg2", "style_C", { { StyleParamKey::order, "value_e" } } };

    return { "layer_e", f, { rule }, { instance_c(), instance_d() } };
}

SceneLayer instance_2() {

    Filter f = Filter("two", true);

    DrawRule rule = { "group2", "styleA", {} };

    return { "subLayer2", f, { rule }, {} };
}

SceneLayer instance_1() {

    Filter f = Filter("one", true);

    DrawRule rule = { "group1", "styleB", {} };

    return { "subLayer1", f, { rule }, {} };
}

SceneLayer instance() {

    Filter f = Filter("base", true);

    DrawRule rule = { "group1", "styleA", { {StyleParamKey::order, "a" } } };

    return { "layer", f, { rule }, { instance_1(), instance_2() } };
}

TEST_CASE("SceneLayer", "[SceneLayer][Filter][DrawRule][Match][Merge]") {

    Feature f1;
    Feature f2;
    Feature f3;
    Feature f4;
    Context ctx;

    auto layer = instance();
    std::vector<DrawRule> matches;

    {
        f1.props.add("base", "blah"); // Should match Base Layer
        layer.match(f1, ctx, matches);

        REQUIRE(matches.size() == 1);
        REQUIRE(matches[0].style == "styleA");
    }

    {
        matches.clear();
        f2.props.add("one", "blah"); // Should match Base and subLayer1
        f2.props.add("base", "blah");
        layer.match(f2, ctx, matches);

        REQUIRE(matches.size() == 1);
        REQUIRE(matches[0].style == "styleB");
        REQUIRE(matches[0].parameters[0].key == StyleParamKey::order);
        REQUIRE(matches[0].parameters[0].value.get<std::string>() == "a");
    }

    {
        matches.clear();
        f3.props.add("two", "blah"); // Should not match anything as uber layer will not be satisfied
        layer.match(f3, ctx, matches);

        REQUIRE(matches.size() == 0);
    }

    {
        matches.clear();
        f4.props.add("two", "blah");
        f4.props.add("base", "blah"); // Should match Base and subLayer2
        layer.match(f4, ctx, matches);

        REQUIRE(matches.size() == 2);
        REQUIRE(matches[0].style == "styleA");
        REQUIRE(matches[0].parameters[0].key == StyleParamKey::order);
        REQUIRE(matches[0].parameters[0].value.get<std::string>() == "a");
        REQUIRE(matches[1].style == "styleA");
    }

}

TEST_CASE("SceneLayer matches correct rules for a feature and context", "[SceneLayer][Filter]") {

    Feature feat;
    Context ctx;

    auto layer_a = instance_a();

    std::vector<DrawRule> matches_a;
    layer_a.match(feat, ctx, matches_a);

    REQUIRE(matches_a.size() == 1);
    REQUIRE(matches_a[0].style == "style_A");

    auto layer_b = instance_b();

    std::vector<DrawRule> matches_b;
    layer_b.match(feat, ctx, matches_b);

    REQUIRE(matches_b.size() == 0);

}

TEST_CASE("SceneLayer matches correct sublayer rules for a feature and context", "[SceneLayer][Filter]") {

    Feature feat;
    Context ctx;

    auto layer_c = instance_c();

    std::vector<DrawRule> matches;
    layer_c.match(feat, ctx, matches);

    REQUIRE(matches.size() == 2);

    // matches should be in lexicographic order by style
    REQUIRE(matches[0].style == "style_A");
    REQUIRE(matches[1].style == "style_C");

}

TEST_CASE("SceneLayer correctly merges rules matched from sublayer", "[SceneLayer][Filter]") {

    Feature feat;
    Context ctx;

    auto layer_e = instance_e();

    std::vector<DrawRule> matches;
    layer_e.match(feat, ctx, matches);

    REQUIRE(matches.size() == 2);

    // deeper match from layer_a should override parameters in same style from layer_d
    REQUIRE(matches[0].style == "style_A");
    REQUIRE(matches[0].parameters[0].key == StyleParamKey::order);
    REQUIRE(matches[0].parameters[0].value.get<std::string>() == "value_a");

    // deeper match from layer_c should override parameters in same style from layer_e
    REQUIRE(matches[1].style == "style_C");
    REQUIRE(matches[1].parameters[0].key == StyleParamKey::order);
    REQUIRE(matches[1].parameters[0].value.get<std::string>() == "value_c");

}
<|endoftext|>


<commit_before>//
//  TapePRG.cpp
//  Clock Signal
//
//  Created by Thomas Harte on 14/08/2016.
//  Copyright © 2016 Thomas Harte. All rights reserved.
//

#include "TapePRG.hpp"

#include <sys/stat.h>

using namespace Storage;

TapePRG::TapePRG(const char *file_name) : _file(nullptr), _bitPhase(3), _filePhase(FilePhaseLeadIn), _phaseOffset(0)
{
	struct stat file_stats;
	stat(file_name, &file_stats);

	// There's really no way to validate other than that if this file is larger than 64kb,
	// of if load address + length > 65536 then it's broken.
	if(file_stats.st_size >= 65538 || file_stats.st_size < 3)
		throw ErrorBadFormat;

	_file = fopen(file_name, "rb");
	if(!_file) throw ErrorBadFormat;

	_load_address = (uint16_t)fgetc(_file);
	_load_address |= (uint16_t)fgetc(_file) << 8;
	_length = (uint16_t)(file_stats.st_size - 2);

	if (_load_address + _length >= 65536)
		throw ErrorBadFormat;
}

TapePRG::~TapePRG()
{
	if(_file) fclose(_file);
}

Tape::Pulse TapePRG::get_next_pulse()
{
	// these are all microseconds per pole
	static const unsigned int leader_zero_length = 179;
	static const unsigned int zero_length = 169;
	static const unsigned int one_length = 247;
	static const unsigned int marker_length = 328;

	_bitPhase = (_bitPhase+1)&3;
	if(!_bitPhase) get_next_output_token();

	Tape::Pulse pulse;
	switch(_outputToken)
	{
		case Leader:		pulse.length.length = leader_zero_length;							break;
		case Zero:			pulse.length.length = (_bitPhase&2) ? one_length : zero_length;		break;
		case One:			pulse.length.length = (_bitPhase&2) ? zero_length : one_length;		break;
		case WordMarker:	pulse.length.length = (_bitPhase&2) ? one_length : marker_length;	break;
		case EndOfBlock:	pulse.length.length = (_bitPhase&2) ? zero_length : marker_length;	break;
	}
	pulse.length.clock_rate = 1000000;
	pulse.type = (_bitPhase&1) ? Pulse::High : Pulse::Low;
	return pulse;
}

void TapePRG::reset()
{
	_bitPhase = 3;
	fseek(_file, 2, SEEK_SET);
	_filePhase = FilePhaseLeadIn;
	_phaseOffset = 0;
}

void TapePRG::get_next_output_token()
{
	static const int block_length = 192;	// not counting the checksum
	static const int countdown_bytes = 9;

	// the lead-in is 20,000 instances of the lead-in pair; every other phase begins with 5000
	// before doing whatever it should be doing
	if(_filePhase == FilePhaseLeadIn || _phaseOffset < 5000)
	{
		_outputToken = Leader;
		_phaseOffset++;
		if(_filePhase == FilePhaseLeadIn && _phaseOffset == 20000)
		{
			_phaseOffset = 0;
			_filePhase = FilePhaseHeader;
		}
		return;
	}

	// determine whether a new byte needs to be queued up
	int block_offset = _phaseOffset - 5000;
	int bit_offset = block_offset % 10;
	int byte_offset = block_offset / 10;
	_phaseOffset++;

	if(byte_offset == block_length + countdown_bytes + 1) // i.e. after the checksum
	{
		_phaseOffset = 0;
		_filePhase = FilePhaseData;	// TODO
		_outputToken = EndOfBlock;
		printf("\n===\n");
		return;
	}

	if(bit_offset == 0)
	{
		// the first nine bytes are countdown; the high bit is set if this is a header
		if(byte_offset < countdown_bytes)
		{
			_output_byte = (uint8_t)(countdown_bytes - byte_offset) | 0x80;
		}
		else if(byte_offset == countdown_bytes + block_length)
		{
			_output_byte = _check_digit;
		}
		else
		{
			if(byte_offset == countdown_bytes) _check_digit = 0;
			if(_filePhase == FilePhaseHeader)
			{
				switch(byte_offset - countdown_bytes)
				{
					case 0:	_output_byte = 0x03;										break;
					case 1: _output_byte = _load_address & 0xff;						break;
					case 2: _output_byte = (_load_address >> 8)&0xff;					break;
					case 3: _output_byte = (_load_address + _length) & 0xff;			break;
					case 4: _output_byte = ((_load_address + _length) >> 8) & 0xff;		break;

					case 5: _output_byte = 0x50;	break; // P
					case 6: _output_byte = 0x52;	break; // R
					case 7: _output_byte = 0x47;	break; // G
					default:
						_output_byte = 0x20;
					break;
				}
			}
			else
			{
				_output_byte = (uint8_t)fgetc(_file);
				if(feof(_file)) _output_byte = 0x20;
			}

			_check_digit ^= _output_byte;
		}

		printf("%02x ", _output_byte);
	}

	switch(bit_offset)
	{
		case 0:
			_outputToken = WordMarker;
		break;
		default:	// i.e. 1–8
			_outputToken = (_output_byte & (1 << (bit_offset - 1))) ? One : Zero;
		break;
		case 9:
		{
			uint8_t parity = _outputToken;
			parity ^= (parity >> 4);
			parity ^= (parity >> 2);
			parity ^= (parity >> 1);
			_outputToken = (parity&1) ? One : Zero;
		}
		break;
	}
}
<commit_msg>Fixed parity: now calculated from the actual byte and works the other way around. The Vic now believes it is loading the actual program. So I guess bytes, headers and the lead-in is working.<commit_after>//
//  TapePRG.cpp
//  Clock Signal
//
//  Created by Thomas Harte on 14/08/2016.
//  Copyright © 2016 Thomas Harte. All rights reserved.
//

#include "TapePRG.hpp"

#include <sys/stat.h>

using namespace Storage;

TapePRG::TapePRG(const char *file_name) : _file(nullptr), _bitPhase(3), _filePhase(FilePhaseLeadIn), _phaseOffset(0)
{
	struct stat file_stats;
	stat(file_name, &file_stats);

	// There's really no way to validate other than that if this file is larger than 64kb,
	// of if load address + length > 65536 then it's broken.
	if(file_stats.st_size >= 65538 || file_stats.st_size < 3)
		throw ErrorBadFormat;

	_file = fopen(file_name, "rb");
	if(!_file) throw ErrorBadFormat;

	_load_address = (uint16_t)fgetc(_file);
	_load_address |= (uint16_t)fgetc(_file) << 8;
	_length = (uint16_t)(file_stats.st_size - 2);

	if (_load_address + _length >= 65536)
		throw ErrorBadFormat;
}

TapePRG::~TapePRG()
{
	if(_file) fclose(_file);
}

Tape::Pulse TapePRG::get_next_pulse()
{
	// these are all microseconds per pole
	static const unsigned int leader_zero_length = 179;
	static const unsigned int zero_length = 169;
	static const unsigned int one_length = 247;
	static const unsigned int marker_length = 328;

	_bitPhase = (_bitPhase+1)&3;
	if(!_bitPhase) get_next_output_token();

	Tape::Pulse pulse;
	switch(_outputToken)
	{
		case Leader:		pulse.length.length = leader_zero_length;							break;
		case Zero:			pulse.length.length = (_bitPhase&2) ? one_length : zero_length;		break;
		case One:			pulse.length.length = (_bitPhase&2) ? zero_length : one_length;		break;
		case WordMarker:	pulse.length.length = (_bitPhase&2) ? one_length : marker_length;	break;
		case EndOfBlock:	pulse.length.length = (_bitPhase&2) ? zero_length : marker_length;	break;
	}
	pulse.length.clock_rate = 1000000;
	pulse.type = (_bitPhase&1) ? Pulse::High : Pulse::Low;
	return pulse;
}

void TapePRG::reset()
{
	_bitPhase = 3;
	fseek(_file, 2, SEEK_SET);
	_filePhase = FilePhaseLeadIn;
	_phaseOffset = 0;
}

void TapePRG::get_next_output_token()
{
	static const int block_length = 192;	// not counting the checksum
	static const int countdown_bytes = 9;

	// the lead-in is 20,000 instances of the lead-in pair; every other phase begins with 5000
	// before doing whatever it should be doing
	if(_filePhase == FilePhaseLeadIn || _phaseOffset < 5000)
	{
		_outputToken = Leader;
		_phaseOffset++;
		if(_filePhase == FilePhaseLeadIn && _phaseOffset == 20000)
		{
			_phaseOffset = 0;
			_filePhase = FilePhaseHeader;
		}
		return;
	}

	// determine whether a new byte needs to be queued up
	int block_offset = _phaseOffset - 5000;
	int bit_offset = block_offset % 10;
	int byte_offset = block_offset / 10;
	_phaseOffset++;

	if(byte_offset == block_length + countdown_bytes + 1) // i.e. after the checksum
	{
		_phaseOffset = 0;
		_filePhase = FilePhaseData;	// TODO
		_outputToken = EndOfBlock;
		printf("\n===\n");
		return;
	}

	if(bit_offset == 0)
	{
		// the first nine bytes are countdown; the high bit is set if this is a header
		if(byte_offset < countdown_bytes)
		{
			_output_byte = (uint8_t)(countdown_bytes - byte_offset) | 0x80;
		}
		else if(byte_offset == countdown_bytes + block_length)
		{
			_output_byte = _check_digit;
		}
		else
		{
			if(byte_offset == countdown_bytes) _check_digit = 0;
			if(_filePhase == FilePhaseHeader)
			{
				switch(byte_offset - countdown_bytes)
				{
					case 0:	_output_byte = 0x03;										break;
					case 1: _output_byte = _load_address & 0xff;						break;
					case 2: _output_byte = (_load_address >> 8)&0xff;					break;
					case 3: _output_byte = (_load_address + _length) & 0xff;			break;
					case 4: _output_byte = ((_load_address + _length) >> 8) & 0xff;		break;

					case 5: _output_byte = 0x50;	break; // P
					case 6: _output_byte = 0x52;	break; // R
					case 7: _output_byte = 0x47;	break; // G
					default:
						_output_byte = 0x20;
					break;
				}
			}
			else
			{
				_output_byte = (uint8_t)fgetc(_file);
				if(feof(_file)) _output_byte = 0x20;
			}

			_check_digit ^= _output_byte;
		}

		printf(" %02x", _output_byte);
	}

	switch(bit_offset)
	{
		case 0:
			_outputToken = WordMarker;
		break;
		default:	// i.e. 1–8
			_outputToken = (_output_byte & (1 << (bit_offset - 1))) ? One : Zero;
		break;
		case 9:
		{
			uint8_t parity = _output_byte;
			parity ^= (parity >> 4);
			parity ^= (parity >> 2);
			parity ^= (parity >> 1);
			_outputToken = (parity&1) ? Zero : One;
			printf("[%d]", parity&1);
		}
		break;
	}
}
<|endoftext|>


<commit_before>#include "apitrace.h"

#include "loaderthread.h"
#include "saverthread.h"

#include <QDir>

ApiTrace::ApiTrace()
    : m_frameMarker(ApiTrace::FrameMarker_SwapBuffers),
      m_needsSaving(false)
{
    m_loader = new LoaderThread(this);
    connect(m_loader, SIGNAL(parsedFrames(const QList<ApiTraceFrame*>)),
            this, SLOT(addFrames(const QList<ApiTraceFrame*>)));
    connect(m_loader, SIGNAL(started()),
            this, SIGNAL(startedLoadingTrace()));
    connect(m_loader, SIGNAL(finished()),
            this, SIGNAL(finishedLoadingTrace()));

    m_saver = new SaverThread(this);
    connect(m_saver, SIGNAL(traceSaved()),
            this, SLOT(slotSaved()));
    connect(m_saver, SIGNAL(traceSaved()),
            this, SIGNAL(saved()));
}

ApiTrace::~ApiTrace()
{
    qDeleteAll(m_calls);
    qDeleteAll(m_frames);
    delete m_loader;
    delete m_saver;
}

bool ApiTrace::isCallAFrameMarker(const ApiTraceCall *call,
                                  ApiTrace::FrameMarker marker)
{
    if (!call)
        return false;

    switch (marker) {
    case FrameMarker_SwapBuffers:
        return call->name().contains(QLatin1String("SwapBuffers")) ||
               call->name() == QLatin1String("CGLFlushDrawable") ||
               call->name() == QLatin1String("glFrameTerminatorGREMEDY");
    case FrameMarker_Flush:
        return call->name() == QLatin1String("glFlush");
    case FrameMarker_Finish:
        return call->name() == QLatin1String("glFinish");
    case FrameMarker_Clear:
        return call->name() == QLatin1String("glClear");
    }

    Q_ASSERT(!"unknown frame marker");

    return false;
}

bool ApiTrace::isEmpty() const
{
    return m_calls.isEmpty();
}

QString ApiTrace::fileName() const
{
    if (edited())
        return m_tempFileName;

    return m_fileName;
}

ApiTrace::FrameMarker ApiTrace::frameMarker() const
{
    return m_frameMarker;
}

QList<ApiTraceCall*> ApiTrace::calls() const
{
    return m_calls;
}

ApiTraceCall * ApiTrace::callAt(int idx) const
{
    return m_calls.value(idx);
}

int ApiTrace::numCalls() const
{
    return m_calls.count();
}

QList<ApiTraceFrame*> ApiTrace::frames() const
{
    return m_frames;
}

ApiTraceFrame * ApiTrace::frameAt(int idx) const
{
    return m_frames.value(idx);
}

int ApiTrace::numFrames() const
{
    return m_frames.count();
}

int ApiTrace::numCallsInFrame(int idx) const
{
    const ApiTraceFrame *frame = frameAt(idx);
    if (frame)
        return frame->numChildren();
    else
        return 0;
}

void ApiTrace::setFileName(const QString &name)
{
    if (m_fileName != name) {
        m_fileName = name;

        if (m_loader->isRunning()) {
            m_loader->terminate();
            m_loader->wait();
        }
        m_frames.clear();
        m_calls.clear();
        m_errors.clear();
        m_editedCalls.clear();
        m_needsSaving = false;
        emit invalidated();

        m_loader->loadFile(m_fileName);
    }
}

void ApiTrace::setFrameMarker(FrameMarker marker)
{
    if (m_frameMarker != marker) {
        emit framesInvalidated();

        qDeleteAll(m_frames);
        m_frames.clear();
        detectFrames();
    }
}

void ApiTrace::addFrames(const QList<ApiTraceFrame*> &frames)
{
    QList<ApiTraceCall*> calls;
    int currentFrames = m_frames.count();
    int numNewFrames = frames.count();

    emit beginAddingFrames(currentFrames, numNewFrames);

    m_frames += frames;

    int currentCalls = m_calls.count();
    int numNewCalls = 0;
    foreach(ApiTraceFrame *frame, frames) {
        Q_ASSERT(this == frame->parentTrace());
        numNewCalls += frame->numChildren();
        calls += frame->calls();
    }
    m_calls.reserve(m_calls.count() + calls.count());
    m_calls += calls;

    emit endAddingFrames();
    emit callsAdded(currentCalls, numNewCalls);
}

void ApiTrace::detectFrames()
{
    if (m_calls.isEmpty())
        return;

    emit beginAddingFrames(0, m_frames.count());

    ApiTraceFrame *currentFrame = 0;
    foreach(ApiTraceCall *apiCall, m_calls) {
        if (!currentFrame) {
            currentFrame = new ApiTraceFrame(this);
            currentFrame->number = m_frames.count();
        }
        apiCall->setParentFrame(currentFrame);
        currentFrame->addCall(apiCall);
        if (ApiTrace::isCallAFrameMarker(apiCall,
                                         m_frameMarker)) {
            m_frames.append(currentFrame);
            currentFrame = 0;
        }
    }
    //last frames won't have markers
    //  it's just a bunch of Delete calls for every object
    //  after the last SwapBuffers
    if (currentFrame) {
        m_frames.append(currentFrame);
        currentFrame = 0;
    }
    emit endAddingFrames();
}

ApiTraceCall * ApiTrace::callWithIndex(int idx) const
{
    for (int i = 0; i < m_calls.count(); ++i) {
        ApiTraceCall *call = m_calls[i];
        if (call->index() == idx)
            return call;
    }
    return NULL;
}

ApiTraceState ApiTrace::defaultState() const
{
    ApiTraceFrame *frame = frameAt(0);
    if (!frame || !frame->hasState())
        return ApiTraceState();

    return *frame->state();
}

void ApiTrace::callEdited(ApiTraceCall *call)
{
    if (!m_editedCalls.contains(call)) {
        //lets generate a temp filename
        QString tempPath = QDir::tempPath();
        m_tempFileName = QString::fromLatin1("%1/%2.edited")
                         .arg(tempPath)
                         .arg(m_fileName);
    }
    m_editedCalls.insert(call);
    m_needsSaving = true;

    emit changed(call);
}

void ApiTrace::callReverted(ApiTraceCall *call)
{
    m_editedCalls.remove(call);

    if (m_editedCalls.isEmpty()) {
        m_needsSaving = false;
    }
    emit changed(call);
}

bool ApiTrace::edited() const
{
    return !m_editedCalls.isEmpty();
}

bool ApiTrace::needsSaving() const
{
    return m_needsSaving;
}

void ApiTrace::save()
{
    QFileInfo fi(m_tempFileName);
    QDir dir;
    emit startedSaving();
    dir.mkpath(fi.absolutePath());
    m_saver->saveFile(m_tempFileName, m_calls);
}

void ApiTrace::slotSaved()
{
    m_needsSaving = false;
}

bool ApiTrace::isSaving() const
{
    return m_saver->isRunning();
}

void ApiTrace::callError(ApiTraceCall *call)
{
    Q_ASSERT(call);

    if (call->hasError())
        m_errors.insert(call);
    else
        m_errors.remove(call);

    emit changed(call);
}

bool ApiTrace::hasErrors() const
{
    return !m_errors.isEmpty();
}

ApiTraceCallSignature * ApiTrace::signature(unsigned id)
{
    if (id >= m_signatures.count()) {
        m_signatures.resize(id + 1);
        return NULL;
    } else {
        return m_signatures[id];
    }
}

void ApiTrace::addSignature(unsigned id, ApiTraceCallSignature *signature)
{
    m_signatures[id] = signature;
}

ApiTraceEnumSignature * ApiTrace::enumSignature(unsigned id)
{
    if (id >= m_signatures.count()) {
        m_enumSignatures.resize(id + 1);
        return NULL;
    } else {
        return m_enumSignatures[id];
    }
}

void ApiTrace::addEnumSignature(unsigned id, ApiTraceEnumSignature *signature)
{
    m_enumSignatures[id] = signature;
}

#include "apitrace.moc"
<commit_msg>Fix a silly mistake.<commit_after>#include "apitrace.h"

#include "loaderthread.h"
#include "saverthread.h"

#include <QDir>

ApiTrace::ApiTrace()
    : m_frameMarker(ApiTrace::FrameMarker_SwapBuffers),
      m_needsSaving(false)
{
    m_loader = new LoaderThread(this);
    connect(m_loader, SIGNAL(parsedFrames(const QList<ApiTraceFrame*>)),
            this, SLOT(addFrames(const QList<ApiTraceFrame*>)));
    connect(m_loader, SIGNAL(started()),
            this, SIGNAL(startedLoadingTrace()));
    connect(m_loader, SIGNAL(finished()),
            this, SIGNAL(finishedLoadingTrace()));

    m_saver = new SaverThread(this);
    connect(m_saver, SIGNAL(traceSaved()),
            this, SLOT(slotSaved()));
    connect(m_saver, SIGNAL(traceSaved()),
            this, SIGNAL(saved()));
}

ApiTrace::~ApiTrace()
{
    qDeleteAll(m_calls);
    qDeleteAll(m_frames);
    delete m_loader;
    delete m_saver;
}

bool ApiTrace::isCallAFrameMarker(const ApiTraceCall *call,
                                  ApiTrace::FrameMarker marker)
{
    if (!call)
        return false;

    switch (marker) {
    case FrameMarker_SwapBuffers:
        return call->name().contains(QLatin1String("SwapBuffers")) ||
               call->name() == QLatin1String("CGLFlushDrawable") ||
               call->name() == QLatin1String("glFrameTerminatorGREMEDY");
    case FrameMarker_Flush:
        return call->name() == QLatin1String("glFlush");
    case FrameMarker_Finish:
        return call->name() == QLatin1String("glFinish");
    case FrameMarker_Clear:
        return call->name() == QLatin1String("glClear");
    }

    Q_ASSERT(!"unknown frame marker");

    return false;
}

bool ApiTrace::isEmpty() const
{
    return m_calls.isEmpty();
}

QString ApiTrace::fileName() const
{
    if (edited())
        return m_tempFileName;

    return m_fileName;
}

ApiTrace::FrameMarker ApiTrace::frameMarker() const
{
    return m_frameMarker;
}

QList<ApiTraceCall*> ApiTrace::calls() const
{
    return m_calls;
}

ApiTraceCall * ApiTrace::callAt(int idx) const
{
    return m_calls.value(idx);
}

int ApiTrace::numCalls() const
{
    return m_calls.count();
}

QList<ApiTraceFrame*> ApiTrace::frames() const
{
    return m_frames;
}

ApiTraceFrame * ApiTrace::frameAt(int idx) const
{
    return m_frames.value(idx);
}

int ApiTrace::numFrames() const
{
    return m_frames.count();
}

int ApiTrace::numCallsInFrame(int idx) const
{
    const ApiTraceFrame *frame = frameAt(idx);
    if (frame)
        return frame->numChildren();
    else
        return 0;
}

void ApiTrace::setFileName(const QString &name)
{
    if (m_fileName != name) {
        m_fileName = name;

        if (m_loader->isRunning()) {
            m_loader->terminate();
            m_loader->wait();
        }
        m_frames.clear();
        m_calls.clear();
        m_errors.clear();
        m_editedCalls.clear();
        m_needsSaving = false;
        emit invalidated();

        m_loader->loadFile(m_fileName);
    }
}

void ApiTrace::setFrameMarker(FrameMarker marker)
{
    if (m_frameMarker != marker) {
        emit framesInvalidated();

        qDeleteAll(m_frames);
        m_frames.clear();
        detectFrames();
    }
}

void ApiTrace::addFrames(const QList<ApiTraceFrame*> &frames)
{
    QList<ApiTraceCall*> calls;
    int currentFrames = m_frames.count();
    int numNewFrames = frames.count();

    emit beginAddingFrames(currentFrames, numNewFrames);

    m_frames += frames;

    int currentCalls = m_calls.count();
    int numNewCalls = 0;
    foreach(ApiTraceFrame *frame, frames) {
        Q_ASSERT(this == frame->parentTrace());
        numNewCalls += frame->numChildren();
        calls += frame->calls();
    }
    m_calls.reserve(m_calls.count() + calls.count());
    m_calls += calls;

    emit endAddingFrames();
    emit callsAdded(currentCalls, numNewCalls);
}

void ApiTrace::detectFrames()
{
    if (m_calls.isEmpty())
        return;

    emit beginAddingFrames(0, m_frames.count());

    ApiTraceFrame *currentFrame = 0;
    foreach(ApiTraceCall *apiCall, m_calls) {
        if (!currentFrame) {
            currentFrame = new ApiTraceFrame(this);
            currentFrame->number = m_frames.count();
        }
        apiCall->setParentFrame(currentFrame);
        currentFrame->addCall(apiCall);
        if (ApiTrace::isCallAFrameMarker(apiCall,
                                         m_frameMarker)) {
            m_frames.append(currentFrame);
            currentFrame = 0;
        }
    }
    //last frames won't have markers
    //  it's just a bunch of Delete calls for every object
    //  after the last SwapBuffers
    if (currentFrame) {
        m_frames.append(currentFrame);
        currentFrame = 0;
    }
    emit endAddingFrames();
}

ApiTraceCall * ApiTrace::callWithIndex(int idx) const
{
    for (int i = 0; i < m_calls.count(); ++i) {
        ApiTraceCall *call = m_calls[i];
        if (call->index() == idx)
            return call;
    }
    return NULL;
}

ApiTraceState ApiTrace::defaultState() const
{
    ApiTraceFrame *frame = frameAt(0);
    if (!frame || !frame->hasState())
        return ApiTraceState();

    return *frame->state();
}

void ApiTrace::callEdited(ApiTraceCall *call)
{
    if (!m_editedCalls.contains(call)) {
        //lets generate a temp filename
        QString tempPath = QDir::tempPath();
        m_tempFileName = QString::fromLatin1("%1/%2.edited")
                         .arg(tempPath)
                         .arg(m_fileName);
    }
    m_editedCalls.insert(call);
    m_needsSaving = true;

    emit changed(call);
}

void ApiTrace::callReverted(ApiTraceCall *call)
{
    m_editedCalls.remove(call);

    if (m_editedCalls.isEmpty()) {
        m_needsSaving = false;
    }
    emit changed(call);
}

bool ApiTrace::edited() const
{
    return !m_editedCalls.isEmpty();
}

bool ApiTrace::needsSaving() const
{
    return m_needsSaving;
}

void ApiTrace::save()
{
    QFileInfo fi(m_tempFileName);
    QDir dir;
    emit startedSaving();
    dir.mkpath(fi.absolutePath());
    m_saver->saveFile(m_tempFileName, m_calls);
}

void ApiTrace::slotSaved()
{
    m_needsSaving = false;
}

bool ApiTrace::isSaving() const
{
    return m_saver->isRunning();
}

void ApiTrace::callError(ApiTraceCall *call)
{
    Q_ASSERT(call);

    if (call->hasError())
        m_errors.insert(call);
    else
        m_errors.remove(call);

    emit changed(call);
}

bool ApiTrace::hasErrors() const
{
    return !m_errors.isEmpty();
}

ApiTraceCallSignature * ApiTrace::signature(unsigned id)
{
    if (id >= m_signatures.count()) {
        m_signatures.resize(id + 1);
        return NULL;
    } else {
        return m_signatures[id];
    }
}

void ApiTrace::addSignature(unsigned id, ApiTraceCallSignature *signature)
{
    m_signatures[id] = signature;
}

ApiTraceEnumSignature * ApiTrace::enumSignature(unsigned id)
{
    if (id >= m_enumSignatures.count()) {
        m_enumSignatures.resize(id + 1);
        return NULL;
    } else {
        return m_enumSignatures[id];
    }
}

void ApiTrace::addEnumSignature(unsigned id, ApiTraceEnumSignature *signature)
{
    m_enumSignatures[id] = signature;
}

#include "apitrace.moc"
<|endoftext|>