Twist Twist::operator-(const Twist& other) const
{
#ifdef IDYNTREE_DONT_USE_SEMANTICS
    return efficient6ddifference(*this,other);
#else
    return compose(*this,inverse(other));
#endif
}

Twist Twist::operator+(const Twist& other) const
{
#ifdef IDYNTREE_DONT_USE_SEMANTICS
    return efficient6dSum(*this,other);
#else
    return compose(*this,(other));
#endif
}

/** Make a path from a d2 sbasis.
 \param p the d2 Symmetric basis polynomial
 \returns a Path

  If only_cubicbeziers is true, the resulting path may only contain CubicBezier curves.
*/
void build_from_sbasis(Geom::PathBuilder &pb, D2<SBasis> const &B, double tol, bool only_cubicbeziers) {
    if (!B.isFinite()) {
        THROW_EXCEPTION("assertion failed: B.isFinite()");
    }
    if(tail_error(B, 2) < tol || sbasis_size(B) == 2) { // nearly cubic enough
        if( !only_cubicbeziers && (sbasis_size(B) <= 1) ) {
            pb.lineTo(B.at1());
        } else {
            std::vector<Geom::Point> bez;
            sbasis_to_bezier(bez, B, 4);
            pb.curveTo(bez[1], bez[2], bez[3]);
        }
    } else {
        build_from_sbasis(pb, compose(B, Linear(0, 0.5)), tol, only_cubicbeziers);
        build_from_sbasis(pb, compose(B, Linear(0.5, 1)), tol, only_cubicbeziers);
    }
}

format pp_def_type_mismatch(formatter const & fmt, environment const & env, options const & opts, name const & n,
                            expr const & expected_type, expr const & given_type) {
    format r("type mismatch at definition '");
    r += format(n);
    r += format("', expected type");
    r += pp_indent_expr(fmt, env, opts, expected_type);
    r += compose(line(), format("given type:"));
    r += pp_indent_expr(fmt, env, opts, given_type);
    return r;
}

int main(int argc, char *argv[])
{
    compositor *comp;
    char *dispatch;
    char *rest;
    char *comment;
    prim_source *prim;
    base_prim_source *base;
    char *output_file;
    int args_offset = 0;
    int i;

    if (argc < ARGS_START) {
        fail("usage: wfcomp output_file base:+HH+WW#RRGGBB [@comment] [args]");
    }

    output_file = argv[OUTPUT_FILENAME_ARG];

    /* Read the base */
    dispatch = arg_read_dispatch(argv[BASE_ARG], &rest);
    if (strcmp(dispatch, "base") != 0) {
        fail("bad base arg");
    }

    base = arg_read_base(rest);

    if (argv[ARGS_START][0] == '@') {
        comment = argv[ARGS_START] + 1;
        args_offset = 1;
    } else {
        comment = "";
    }

    comp = make_compositor(output_file,
                           comment,
                           base->width, base->height,
                           base->red, base->green, base->blue,
                           argc);
    if (argc >= ARGS_START + args_offset) {
        for (i = ARGS_START + args_offset; i < argc; i++) {
            dispatch = arg_read_dispatch(argv[i], &rest);
            prim = invoke_dispatch(dispatch, rest);

            add_source(comp, prim->source,
                       prim->x, prim->y,
                       prim->width, prim->height);
        }
    } else {
        fail("usage: args dispatch");
    }

    compose(comp);
    exit(0);
}

void Path::setResource( const String &res )
{
   if ( res != m_device )
   {
      m_device = res;
      if ( m_device.find( ":" ) != String::npos || m_device.find( "/" ) != String::npos )
         m_bValid = false;

      compose();
   }
}

BOOST_FIXTURE_TEST_CASE( math2d__transformations, Some2dPoints )
{
    move2 m0(r0,p0);
    move2 m1(r1,p1);
    BOOMST_CHECK_APPROX_EQUAL( p2 + p1, leftOrthogonal(p2) >> m1 );
    BOOMST_CHECK_APPROX_EQUAL( m0, inverse(inverse(m0)) );
    BOOMST_CHECK_APPROX_EQUAL( move2_id, compose(inverse(m0),m0) );
    BOOMST_CHECK_APPROX_EQUAL( p1, p1 >> m0 >> inverse(m0) );
    BOOMST_CHECK_APPROX_EQUAL( p0, zero2 >> m0 );
    BOOMST_CHECK_APPROX_EQUAL( p3, move2_id.map(p3) );
}

int main()
{
    // print sin(abs(-0.5))
    std::cout << compose(Abs(),Sine())(0.5) << "\n\n";

    // print abs() of some values
    print_values(Abs());
    std::cout << '\n';

    // print sin() of some values
    print_values(Sine());
    std::cout << '\n';

    // print sin(abs()) of some values
    print_values(compose(Abs(),Sine()));
    std::cout << '\n';

    // print abs(sin()) of some values
    print_values(compose(Sine(),Abs()));
}

 Real GeneralStatistics::variance() const {
     Size N = samples();
     QL_REQUIRE(N > 1,
                "sample number <=1, unsufficient");
     // Subtract the mean and square. Repeat on the whole range.
     // Hopefully, the whole thing will be inlined in a single loop.
     Real s2 = expectationValue(compose(square<Real>(),
                                        subtract<Real>(mean())),
                                everywhere()).first;
     return s2*N/(N-1.0);
 }

int main(void)
{
    init_IO();
    init_interrupts();
    oledInit();
    _delay_ms(200);

    oledSetCursor(cursX, cursY);
    putChar(66,1);
    advanceCursor(6);

    compose();

    initMenu();

    while(1)
    {
        static uint16_t butCounter = 0;
        if (butCounter++ > 65000) {
            //FIXME: Proper button debounce and handling
            butCounter = 0;
            uint8_t readButtons = BUT_PIN;
            if (~readButtons & BUT_LEFT) {
                ++goLeft;
            }
            if (~readButtons & BUT_SEL) {
                ++goSel;
            }
        }

        if (knobChange) {
            if (knobChange > 0) {
                //menuUp();
                knobLeft();
            }
            else {
                //menuDn();
                knobRight();
            }
            knobChange = 0;
        }

        if (goSel) {
            //Lookup and execute action
            doSelect[optionIndex]();
            goSel = 0;
        }
        else if (goLeft) {
            doBack();
            goLeft = 0;
        }
    }
}

void StaticText::update()
{
    m_messages.pop_front();
    if(m_messages.empty()) {
        // schedule removal
        auto self = asStaticText();
        g_dispatcher.addEvent([self]() { g_map.removeThing(self); });
    } else {
        compose();
        scheduleUpdate();
    }
}

tactic change_goal_tactic(elaborate_fn const & elab, expr const & e) {
    return tactic([=](environment const & env, io_state const & ios, proof_state const & s) {
            proof_state new_s = s;
            goals const & gs  = new_s.get_goals();
            if (!gs) {
                throw_no_goal_if_enabled(s);
                return proof_state_seq();
            }
            expr t            = head(gs).get_type();
            bool report_unassigned = true;
            if (auto new_e = elaborate_with_respect_to(env, ios, elab, new_s, e, none_expr(), report_unassigned)) {
                goals const & gs    = new_s.get_goals();
                goal const & g      = head(gs);
                substitution subst  = new_s.get_subst();
                auto tc             = mk_type_checker(env);
                constraint_seq cs;
                if (tc->is_def_eq(t, *new_e, justification(), cs)) {
                    if (cs) {
                        unifier_config cfg(ios.get_options());
                        buffer<constraint> cs_buf;
                        cs.linearize(cs_buf);
                        to_buffer(new_s.get_postponed(), cs_buf);
                        unify_result_seq rseq = unify(env, cs_buf.size(), cs_buf.data(), subst, cfg);
                        return map2<proof_state>(rseq, [=](pair<substitution, constraints> const & p) -> proof_state {
                                substitution const & subst    = p.first;
                                constraints const & postponed = p.second;
                                substitution new_subst = subst;
                                expr final_e = new_subst.instantiate_all(*new_e);
                                expr M       = g.mk_meta(mk_fresh_name(), final_e);
                                goal new_g(M, final_e);
                                assign(new_subst, g, M);
                                return proof_state(new_s, cons(new_g, tail(gs)), new_subst, postponed);
                            });
                    }
                    expr M   = g.mk_meta(mk_fresh_name(), *new_e);
                    goal new_g(M, *new_e);
                    assign(subst, g, M);
                    return proof_state_seq(proof_state(new_s, cons(new_g, tail(gs)), subst));
                } else {
                    throw_tactic_exception_if_enabled(new_s, [=](formatter const & fmt) {
                            format r = format("invalid 'change' tactic, the given type");
                            r += pp_indent_expr(fmt, *new_e);
                            r += compose(line(), format("does not match the goal type"));
                            r += pp_indent_expr(fmt, t);
                            return r;
                        });
                    return proof_state_seq();
                }
            }
            return proof_state_seq();
        });
}

//-Sqrt----------------------------------------------------------
static Piecewise<SBasis> sqrt_internal(SBasis const &f, 
                                    double tol, 
                                    int order){
    SBasis sqrtf;
    if(f.isZero() || order == 0){
        return Piecewise<SBasis>(sqrtf);
    }
    if (f.at0()<-tol*tol && f.at1()<-tol*tol){
        return sqrt_internal(-f,tol,order);
    }else if (f.at0()>tol*tol && f.at1()>tol*tol){
        sqrtf.resize(order+1, Linear(0,0));
        sqrtf[0] = Linear(std::sqrt(f[0][0]), std::sqrt(f[0][1]));
        SBasis r = f - multiply(sqrtf, sqrtf); // remainder    
        for(unsigned i = 1; int(i) <= order && i<r.size(); i++) {
            Linear ci(r[i][0]/(2*sqrtf[0][0]), r[i][1]/(2*sqrtf[0][1]));
            SBasis cisi = shift(ci, i);
            r -= multiply(shift((sqrtf*2 + cisi), i), SBasis(ci));
            r.truncate(order+1);
            sqrtf[i] = ci;
            if(r.tailError(i) == 0) // if exact
                break;
        }
    }else{
        sqrtf = Linear(std::sqrt(fabs(f.at0())), std::sqrt(fabs(f.at1())));
    }

    double err = (f - multiply(sqrtf, sqrtf)).tailError(0);
    if (err<tol){
        return Piecewise<SBasis>(sqrtf);
    }

    Piecewise<SBasis> sqrtf0,sqrtf1;
    sqrtf0 = sqrt_internal(compose(f,Linear(0.,.5)),tol,order);
    sqrtf1 = sqrt_internal(compose(f,Linear(.5,1.)),tol,order);
    sqrtf0.setDomain(Interval(0.,.5));
    sqrtf1.setDomain(Interval(.5,1.));
    sqrtf0.concat(sqrtf1);
    return sqrtf0;
}

void solve() {
    for( int i = 1; i <= n; i++ )
        inv[perm[i]] = i;

    int base[81];
    for( int i = 1; i <= n; i++ )
        base[i] = i;
    /* We will repeatedly square inv
     * and put the intermediade results in base.
     */

    while( m != 0 ) {
        if( m % 2 == 1 )
            compose( base, inv );
        compose( inv, inv );
        m >>= 1;
    }

    for( int i = 1; i <= n; i++ )
        base_solution[i] = base_str[base[i]];
    base_solution[n+1] = '\0';
}

int main(int argc, char** argv)
{
  double (*input)[N] = (double (*)[N])new double[M*N];
  double (*output)[N] = (double (*)[N])new double[2*M*N];
  double (*output_ref)[N] = (double (*)[N])new double[2*M*N];

  for( int i = 0 ; i < M ; i++ )
    for( int j = 0 ; j < N ; j++ )
      input[i][j] = (double)(rand()) / (double)(RAND_MAX-1);
      
  for( int i = 0 ; i < 2*M ; i++ ) 
    for( int j = 0 ; j < N ; j++ ){
      output[i][j] = 0.0;
      output_ref[i][j] = 0.0;
    }

  compose((double*)input,M,N,(double*)output);
  ref_output(input,output_ref);
  
  printf("Input :\n");
  for( int i = 0 ; i < M ; i++ ){
    for( int j = 0 ; j < N ; j++ )
      printf(" %f",input[i][j]);
    printf("\n");
  }
  printf("Output[%d,%d] :\n",2*M,N);
  for( int i = 0 ; i < 2*M ; i++ ){
    for( int j = 0 ; j < N ; j++ )
      printf(" %f",output[i][j]);
    printf("\n");
  }
  printf("OutputRef[%d,%d] :\n",2*M,N);
  for( int i = 0 ; i < 2*M ; i++ ){
    for( int j = 0 ; j < N ; j++ )
      printf(" %f",output_ref[i][j]);
    printf("\n");
  }
  double diff = 0.0;
  for( int i = 0 ; i < 2*M ; i++ )
    for( int j = 0 ; j < N ; j++ )
      diff += fabs(output_ref[i][j] - output[i][j]);
  printf("Diff : %e\n",diff);
  if( diff > 1e-6 ){
    printf("Incorrect result\n");
    exit(1);
  }
  delete[] input;
  delete[] output;
  delete[] output_ref;
  return 0;
}

ModelAddFunctionModule::ModelAddFunctionModule()
{
  mp_ModelAvailFctMVP = new ModelAvailFctComponent();
  mp_ModelFctDetailMVP = new ModelFctDetailComponent();

  mp_Coordinator = new ModelAddFunctionCoordinator(
      *mp_ModelAvailFctMVP->getModel(), *mp_ModelFctDetailMVP->getModel());

  mp_Coordinator->signal_AvailFctSelectionChanged().connect(sigc::mem_fun(
      *this, &ModelAddFunctionModule::whenAvailFctSelectionChanged));

  compose();

}

//-----------------------------------------------------------------------------
    std::string Value::to_string() const
    {
        switch (type)
        {
            case VT_EMPTY:
                return "";
            case VT_BOOL:
                return ::config::to_string(bool_value);
            case VT_INT:
                return ::config::to_string(int_value);
            case VT_UNSIGNED_INT:
                return ::config::to_string(unsigned_int_value);
            case VT_FLOAT:
                return ::config::to_string(float_value);
            case VT_DOUBLE:
                return ::config::to_string(double_value);
            case VT_STRING:
                return compose("\"%0\"", string_value);
            default:
                throw std::invalid_argument(compose("unknown value type %0", type));
                return "";
        }
    }

Noise2CVGUI::Noise2CVGUI(const std::string& URI)
{
	EventBox *p_background = manage (new EventBox());
	Gdk::Color* color = new  Gdk::Color();
	color->set_rgb(7710, 8738, 9252);
	p_background->modify_bg(Gtk::STATE_NORMAL, *color);

	VBox *p_mainWidget = manage (new VBox(false, 5));

	Label *p_labelWaveForm = manage (new Label("Noise Type"));
	p_mainWidget->pack_start(*p_labelWaveForm);

	m_comboNoiseForm = manage (new ComboBoxText());
	m_comboNoiseForm->append_text("White");
	m_comboNoiseForm->append_text("Random");
	m_comboNoiseForm->append_text("Pink");

	slot<void> p_slotNoiseForm = compose(bind<0> (mem_fun(*this, &Noise2CVGUI::write_control), p_noiseType), mem_fun(*m_comboNoiseForm, &ComboBoxText::get_active_row_number));
	m_comboNoiseForm->signal_changed().connect(p_slotNoiseForm);

	p_mainWidget->pack_start(*m_comboNoiseForm);

	slot<void> p_slotRandomRate = compose(bind<0>(mem_fun(*this, &Noise2CVGUI::write_control), p_rate), mem_fun(*this,  &Noise2CVGUI::get_randomRate));
	m_dialRandomRate = new LabeledDial("Random Rate", p_slotRandomRate, p_rate, 0, 10, NORMAL, 0.01, 2);
	p_mainWidget->pack_start(*m_dialRandomRate);

	slot<void> p_slotRandomLevel = compose(bind<0>(mem_fun(*this, &Noise2CVGUI::write_control), p_level), mem_fun(*this, &Noise2CVGUI::get_randomLevel));
	m_dialRandomLevel = new LabeledDial("Random Level", p_slotRandomLevel, p_level, 0, 1, LOG, 0.0001, 4);
	p_mainWidget->pack_start(*m_dialRandomLevel);

	p_mainWidget->set_size_request(150, 200);

	p_background->add(*p_mainWidget);
	add(*p_background);

	Gtk::manage(p_mainWidget);
}

int main() {
    auto f = [](int x, int y) {
        return x * y;
    };
    auto f2 = [](int x) {
        return x * 2;
    };
    auto f3 = [](int y) {
        return y * y;
    };
    auto f4 = [](int x) {
        return x + 11;
    };
    std::cout << compose(f4, f3, f2, f)(10, 11);
}

/*
* This version works by inverting a reasonable upper bound on the error term after subdividing the
* curve at $a$.  We keep biting off pieces until there is no more curve left.
*
* Derivation: The tail of the power series is $a_ks^k + a_{k+1}s^{k+1} + \ldots = e$.  A
* subdivision at $a$ results in a tail error of $e*A^k, A = (1-a)a$.  Let this be the desired
* tolerance tol $= e*A^k$ and invert getting $A = e^{1/k}$ and $a = 1/2 - \sqrt{1/4 - A}$
*/
void
subpath_from_sbasis_incremental(Geom::OldPathSetBuilder &pb, D2<SBasis> B, double tol, bool initial) {
    const unsigned k = 2; // cubic bezier
    double te = B.tail_error(k);
    assert(B[0].IS_FINITE());
    assert(B[1].IS_FINITE());

    //std::cout << "tol = " << tol << std::endl;
    while(1) {
        double A = std::sqrt(tol/te); // pow(te, 1./k)
        double a = A;
        if(A < 1) {
            A = std::min(A, 0.25);
            a = 0.5 - std::sqrt(0.25 - A); // quadratic formula
            if(a > 1) a = 1; // clamp to the end of the segment
        } else
            a = 1;
        assert(a > 0);
        //std::cout << "te = " << te << std::endl;
        //std::cout << "A = " << A << "; a=" << a << std::endl;
        D2<SBasis> Bs = compose(B, Linear(0, a));
        assert(Bs.tail_error(k));
        std::vector<Geom::Point> bez = sbasis_to_bezier(Bs, 2);
        reverse(bez.begin(), bez.end());
        if (initial) {
          pb.start_subpath(bez[0]);
          initial = false;
        }
        pb.push_cubic(bez[1], bez[2], bez[3]);

// move to next piece of curve
        if(a >= 1) break;
        B = compose(B, Linear(a, 1));
        te = B.tail_error(k);
    }
}