text:
/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2012 Artem Pavlenko
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

#ifndef MAPNIK_OFFSET_CONVERTER_HPP
#define MAPNIK_OFFSET_CONVERTER_HPP

#ifdef MAPNIK_LOG
#include <mapnik/debug.hpp>
#endif
#include <mapnik/config.hpp>
#include <mapnik/box2d.hpp>
#include <mapnik/vertex.hpp>
#include <mapnik/proj_transform.hpp>

// boost
#include <boost/math/constants/constants.hpp>

// stl
#include <cmath>

namespace mapnik
{

const double pi = boost::math::constants::pi<double>();

template <typename Geometry>
struct MAPNIK_DECL offset_converter
{
    typedef std::size_t size_type;
    //typedef typename Geometry::value_type value_type;

    offset_converter(Geometry & geom)
        : geom_(geom)
        , offset_(0.0)
        , threshold_(8.0)
        , half_turn_segments_(16)
        , status_(initial)
        , pre_first_(vertex2d::no_init)
        , pre_(vertex2d::no_init)
        , cur_(vertex2d::no_init)
        {}

    enum status
    {
        initial,
        process,
        last_vertex,
        angle_joint,
        end
    };

    double get_offset() const
    {
        return offset_;
    }

    double get_threshold() const
    {
        return threshold_;
    }

    void set_offset(double value)
    {
        if (offset_ != value)
        {
            offset_ = value;
            reset();
        }
    }

    void set_threshold(double value)
    {
        threshold_ = value;
        // no need to reset(), since threshold doesn't affect
        // offset vertices' computation, it only controls how
        // far will we be looking for self-intersections
    }

    unsigned vertex(double * x, double * y)
    {
        if (offset_ == 0.0)
            return geom_.vertex(x, y);

        if (status_ == initial)
            init_vertices();

        if (pos_ >= vertices_.size())
            return SEG_END;

        pre_ = (pos_ ? cur_ : pre_first_);
        cur_ = vertices_[pos_++];

        if (pos_ == vertices_.size())
            return output_vertex(x, y);

        double const check_dist = offset_ * threshold_;
        double const check_dist2 = check_dist * check_dist;
        double t = 1.0;
        double vt, ut;

        for (size_t i = pos_; i+1 < vertices_.size(); ++i)
        {
            //break; // uncomment this to see all the curls

            vertex2d const& u0 = vertices_[i];
            vertex2d const& u1 = vertices_[i+1];
            double const dx = u0.x - cur_.x;
            double const dy = u0.y - cur_.y;

            if (dx*dx + dy*dy > check_dist2)
                break;

            if (!intersection(pre_, cur_, &vt, u0, u1, &ut))
                continue;

            if (vt < 0.0 || vt > t || ut < 0.0 || ut > 1.0)
                continue;

            t = vt;
            pos_ = i+1;
        }

        cur_.x = pre_.x + t * (cur_.x - pre_.x);
        cur_.y = pre_.y + t * (cur_.y - pre_.y);
        return output_vertex(x, y);
    }

    void reset()
    {
        geom_.rewind(0);
        vertices_.clear();
        status_ = initial;
        pos_ = 0;
    }

    void rewind(unsigned)
    {
        pos_ = 0;
    }

private:

    static double explement_reflex_angle(double angle)
    {
        if (angle > pi)
            return angle - 2 * pi;
        else if (angle < -pi)
            return angle + 2 * pi;
        else
            return angle;
    }

    static bool intersection(vertex2d const& u1, vertex2d const& u2, double* ut,
                             vertex2d const& v1, vertex2d const& v2, double* vt)
    {
        double const dx = v1.x - u1.x;
        double const dy = v1.y - u1.y;
        double const ux = u2.x - u1.x;
        double const uy = u2.y - u1.y;
        double const vx = v2.x - v1.x;
        double const vy = v2.y - v1.y;

        // the first line is not vertical
        if (ux < -1e-6 || ux > 1e-6)
        {
            double const up = ux * dy - dx * uy;
            double const dn = vx * uy - ux * vy;

            if (dn > -1e-6 && dn < 1e-6)
                return false; // they are parallel

            *vt = up / dn;
            *ut = (*vt * vx + dx) / ux;
            return true;
        }

        // the first line is not horizontal
        if (uy < -1e-6 || uy > 1e-6)
        {
            double const up = uy * dx - dy * ux;
            double const dn = vy * ux - uy * vx;

            if (dn > -1e-6 && dn < 1e-6)
                return false; // they are parallel

            *vt = up / dn;
            *ut = (*vt * vy + dy) / uy;
            return true;
        }

        // the first line is too short
        return false;
    }

    /**
     *  @brief  Translate (vx, vy) by rotated (dx, dy).
     */
    static void displace(vertex2d & v, double dx, double dy, double a)
    {
        v.x += dx * std::cos(a) - dy * std::sin(a);
        v.y += dx * std::sin(a) + dy * std::cos(a);
    }

    /**
     *  @brief  Translate (vx, vy) by rotated (0, -offset).
     */
    void displace(vertex2d & v, double a) const
    {
        v.x += offset_ * std::sin(a);
        v.y -= offset_ * std::cos(a);
    }

    /**
     *  @brief  (vx, vy) := (ux, uy) + rotated (0, -offset)
     */
    void displace(vertex2d & v, vertex2d const& u, double a) const
    {
        v.x = u.x + offset_ * std::sin(a);
        v.y = u.y - offset_ * std::cos(a);
        v.cmd = u.cmd;
    }

    void displace2(vertex2d & v, double a, double b) const
    {
        double sa = offset_ * std::sin(a);
        double ca = offset_ * std::cos(a);
        double h = std::tan(0.5 * (b - a));
        v.x = v.x + sa + h * ca;
        v.y = v.y - ca + h * sa;
    }

    status init_vertices()
    {
        if (status_ != initial) // already initialized
            return status_;

        vertex2d v1(vertex2d::no_init);
        vertex2d v2(vertex2d::no_init);
        vertex2d w(vertex2d::no_init);

        v1.cmd = geom_.vertex(&v1.x, &v1.y);
        v2.cmd = geom_.vertex(&v2.x, &v2.y);

        if (v2.cmd == SEG_END) // not enough vertices in source
            return status_ = process;

        double angle_a = 0;
        double angle_b = std::atan2((v2.y - v1.y), (v2.x - v1.x));
        double joint_angle;

        // first vertex
        displace(v1, angle_b);
        push_vertex(v1);

        // Sometimes when the first segment is too short, it causes ugly
        // curls at the beginning of the line. To avoid this, we make up
        // a fake vertex two offset-lengths before the first, and expect
        // intersection detection smoothes it out.
        pre_first_ = v1;
        displace(pre_first_, -2 * std::fabs(offset_), 0, angle_b);

        while ((v1 = v2, v2.cmd = geom_.vertex(&v2.x, &v2.y)) != SEG_END)
        {
            angle_a = angle_b;
            angle_b = std::atan2((v2.y - v1.y), (v2.x - v1.x));
            joint_angle = explement_reflex_angle(angle_b - angle_a);

            double half_turns = half_turn_segments_ * std::fabs(joint_angle);
            int bulge_steps = 0;

            if (offset_ < 0.0)
            {
                if (joint_angle > 0.0)
                    joint_angle = joint_angle - 2 * pi;
                else
                    bulge_steps = 1 + int(std::floor(half_turns / pi));
            }
            else
            {
                if (joint_angle < 0.0)
                    joint_angle = joint_angle + 2 * pi;
                else
                    bulge_steps = 1 + int(floor(half_turns / pi));
            }

            #ifdef MAPNIK_LOG
            if (bulge_steps == 0)
            {
                // inside turn (sharp/obtuse angle)
                MAPNIK_LOG_DEBUG(ctrans) << "offset_converter:"
                    << " Sharp joint [<< inside turn " << int(joint_angle*180/pi)
                    << " degrees >>]";
            }
            else
            {
                // outside turn (reflex angle)
                MAPNIK_LOG_DEBUG(ctrans) << "offset_converter:"
                    << " Bulge joint >)) outside turn " << int(joint_angle*180/pi)
                    << " degrees ((< with " << bulge_steps << " segments";
            }
            #endif

            displace(w, v1, angle_a);
            push_vertex(w);

            for (int s = 0; ++s < bulge_steps; )
            {
                displace(w, v1, angle_a + (joint_angle * s) / bulge_steps);
                push_vertex(w);
            }

            displace(v1, angle_b);
            push_vertex(v1);
        }

        // last vertex
        displace(v1, angle_b);
        push_vertex(v1);

        // initialization finished
        return status_ = process;
    }

    unsigned output_vertex(double* px, double* py)
    {
        *px = cur_.x;
        *py = cur_.y;
        return cur_.cmd;
    }

    unsigned output_vertex(double* px, double* py, status st)
    {
        status_ = st;
        return output_vertex(px, py);
    }

    void push_vertex(vertex2d const& v)
    {
        #ifdef MAPNIK_LOG
        MAPNIK_LOG_DEBUG(ctrans) << "offset_converter: " << v;
        #endif

        vertices_.push_back(v);
    }

    Geometry &              geom_;
    double                  offset_;
    double                  threshold_;
    unsigned                half_turn_segments_;
    status                  status_;
    size_t                  pos_;
    std::vector<vertex2d>   vertices_;
    vertex2d                pre_first_;
    vertex2d                pre_;
    vertex2d                cur_;
};

}

#endif // MAPNIK_OFFSET_CONVERTER_HPP

max_stars_repo_name: Wujingli/OpenWebGlobeDataProcessing

max_issues_repo_name: Wujingli/OpenWebGlobeDataProcessing

max_forks_repo_name: Wujingli/OpenWebGlobeDataProcessing


text: #include <boost/geometry/util/promote_integral.hpp>

ext: hpp

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text:
#include <NTL/RR.h>

#include <NTL/new.h>

NTL_START_IMPL

// FIXME: I just converted all the static RR's to thread local static RR's.
// Perhaps I should at some point make the equivalent of an RR Register.
// But be careful: certain computations, like ComputePi, actually cache
// results, so that will take more work.  In any case, RR is not a high
// priority right now.

NTL_CHEAP_THREAD_LOCAL
long RR::prec = 150;

void RR::SetPrecision(long p)
{
   if (p < 53)
      p = 53;

   if (NTL_OVERFLOW(p, 1, 0))
      ResourceError("RR: precision too high");

   prec = p;
}

NTL_CHEAP_THREAD_LOCAL
long RR::oprec = 10;

void RR::SetOutputPrecision(long p)
{
   if (p < 1)
      p = 1;

   if (NTL_OVERFLOW(p, 1, 0))
      ResourceError("RR: output precision too high");

   oprec = p;
}

static
void normalize1(RR& z, const ZZ& y_x, long y_e, long prec, long residual)
{
   long len = NumBits(y_x);

   if (len > prec) {
      long correction = ZZ_RoundCorrection(y_x, len - prec, residual);

      RightShift(z.x, y_x, len - prec);

      if (correction)
         add(z.x, z.x, correction);

      z.e = y_e + len - prec;
   }
   else if (len == 0) {
      clear(z.x);
      z.e = 0;
   }
   else {
      z.x = y_x;
      z.e = y_e;
   }

   if (!IsOdd(z.x))
      z.e += MakeOdd(z.x);

   if (z.e >= NTL_OVFBND)
      ResourceError("RR: overflow");

   if (z.e <= -NTL_OVFBND)
      ResourceError("RR: underflow");
}

void normalize(RR& z, const RR& y, long residual = 0)
{
   normalize1(z, y.x, y.e, RR::prec, residual);
}

void MakeRR(RR& z, const ZZ& a,  long e)
{
   if (e >= NTL_OVFBND)
      ResourceError("MakeRR: e too big");

   if (e <= -NTL_OVFBND)
      ResourceError("MakeRR: e too small");

   normalize1(z, a, e, RR::prec, 0);
}

void MakeRRPrec(RR& x, const ZZ& a, long e, long p)
{
   if (p < 1)
      LogicError("MakeRRPrec: bad precsion");
   if (NTL_OVERFLOW(p, 1, 0))
      ResourceError("MakeRRPrec: precsion too big");

   RRPush push;
   RR::prec = p;
   MakeRR(x, a, e);
}

void random(RR& z)
{
   NTL_TLS_LOCAL(RR, t);
   RandomBits(t.x, RR::prec);
   t.e = -RR::prec;
   normalize(z, t);
}

static inline
void xcopy(RR& x, const RR& a)
   { normalize(x, a); }

// xcopy emulates old assignment semantics...
// many routines here implicitly assume assignment normalizes,
// but that is no longer the case as of v3.0.

void ConvPrec(RR& x, const RR& a, long p)
{
   if (p < 1)
      LogicError("ConvPrec: bad precsion");
   if (NTL_OVERFLOW(p, 1, 0))
      ResourceError("ConvPrec: precsion too big");

   RRPush push;
   RR::prec = p;
   normalize(x, a);
}

void RoundToPrecision(RR& x, const RR& a, long p)
{
   ConvPrec(x, a, p);
}

void conv(RR& x, const RR& a)
{
   normalize(x, a);
}

long IsZero(const RR& a)
{
   return IsZero(a.x);
}

long IsOne(const RR& a)
{
   return a.e == 0 && IsOne(a.x);
}

long sign(const RR& a)
{
   return sign(a.x);
}

void clear(RR& z)
{
   z.e = 0;
   clear(z.x);
}

void set(RR& z)
{
   z.e = 0;
   set(z.x);
}

void add(RR& z, const RR& a, const RR& b)
{
   NTL_TLS_LOCAL(RR, t);

   if (IsZero(a.x)) {
      xcopy(z, b);
      return;
   }

   if (IsZero(b.x)) {
      xcopy(z, a);
      return;
   }

   if (a.e > b.e) {
      if (a.e-b.e - max(RR::prec-NumBits(a.x),0) >= NumBits(b.x) + 2)
         normalize(z, a, sign(b));
      else {
         LeftShift(t.x, a.x, a.e-b.e);
         add(t.x, t.x, b.x);
         t.e = b.e;
         normalize(z, t);
      }
   }
   else if (a.e < b.e) {
      if (b.e-a.e - max(RR::prec-NumBits(b.x),0) >= NumBits(a.x) + 2)
         normalize(z, b, sign(a));
      else {
         LeftShift(t.x, b.x, b.e-a.e);
         add(t.x, t.x, a.x);
         t.e = a.e;
         normalize(z, t);
      }
   }
   else {
      add(t.x, a.x, b.x);
      t.e = a.e;
      normalize(z, t);
   }
}

void AddPrec(RR& x, const RR& a, const RR& b, long p)
{
   if (p < 1)
      LogicError("AddPrec: bad precsion");
   if (NTL_OVERFLOW(p, 1, 0))
      ResourceError("AddPrec: bad precsion");

   RRPush push;
   RR::prec = p;
   add(x, a, b);
}

void sub(RR& z, const RR& a, const RR& b)
{
   NTL_TLS_LOCAL(RR, t);

   if (IsZero(a.x)) {
      negate(z, b);
      return;
   }

   if (IsZero(b.x)) {
      xcopy(z, a);
      return;
   }

   if (a.e > b.e) {
      if (a.e-b.e - max(RR::prec-NumBits(a.x),0) >= NumBits(b.x) + 2)
         normalize(z, a, -sign(b));
      else {
         LeftShift(t.x, a.x, a.e-b.e);
         sub(t.x, t.x, b.x);
         t.e = b.e;
         xcopy(z, t);
      }
   }
   else if (a.e < b.e) {
      if (b.e-a.e - max(RR::prec-NumBits(b.x),0) >= NumBits(a.x) + 2) {
         normalize(z, b, -sign(a));
         negate(z.x, z.x);
      }
      else {
         LeftShift(t.x, b.x, b.e-a.e);
         sub(t.x, a.x, t.x);
         t.e = a.e;
         xcopy(z, t);
      }
   }
   else {
      sub(t.x, a.x, b.x);
      t.e = a.e;
      normalize(z, t);
   }
}

void SubPrec(RR& x, const RR& a, const RR& b, long p)
{
   if (p < 1)
      LogicError("SubPrec: bad precsion");
   if (NTL_OVERFLOW(p, 1, 0))
      ResourceError("SubPrec: bad precsion");

   RRPush push;
   RR::prec = p;
   sub(x, a, b);
}

void negate(RR& z, const RR& a)
{
   xcopy(z, a);
   negate(z.x, z.x);
}

void NegatePrec(RR& x, const RR& a, long p)
{
   if (p < 1)
      LogicError("NegatePrec: bad precsion");
   if (NTL_OVERFLOW(p, 1, 0))
      ResourceError("NegatePrec: bad precsion");

   RRPush push;
   RR::prec = p;
   negate(x, a);
}

void abs(RR& z, const RR& a)
{
   xcopy(z, a);
   abs(z.x, z.x);
}

void AbsPrec(RR& x, const RR& a, long p)
{
   if (p < 1)
      LogicError("AbsPrec: bad precsion");
   if (NTL_OVERFLOW(p, 1, 0))
      ResourceError("AbsPrec: bad precsion");

   RRPush push;
   RR::prec = p;
   abs(x, a);
}

void mul(RR& z, const RR& a, const RR& b)
{
   NTL_TLS_LOCAL(RR, t);

   mul(t.x, a.x, b.x);
   t.e = a.e + b.e;
   xcopy(z, t);
}

void MulPrec(RR& x, const RR& a, const RR& b, long p)
{
   if (p < 1)
      LogicError("MulPrec: bad precsion");
   if (NTL_OVERFLOW(p, 1, 0))
      ResourceError("MulPrec: bad precsion");

   RRPush push;
   RR::prec = p;
   mul(x, a, b);
}

void sqr(RR& z, const RR& a)
{
   NTL_TLS_LOCAL(RR, t);

   sqr(t.x, a.x);
   t.e = a.e + a.e;
   xcopy(z, t);
}

void SqrPrec(RR& x, const RR& a, long p)
{
   if (p < 1)
      LogicError("SqrPrec: bad precsion");
   if (NTL_OVERFLOW(p, 1, 0))
      ResourceError("SqrPrec: bad precsion");

   RRPush push;
   RR::prec = p;
   sqr(x, a);
}

void div(RR& z, const RR& a, const RR& b)
{
   if (IsZero(b))
      ArithmeticError("RR: division by zero");

   if (IsZero(a)) {
      clear(z);
      return;
   }

   long la = NumBits(a.x);
   long lb = NumBits(b.x);

   long neg = (sign(a) != sign(b));

   long k = RR::prec - la + lb + 1;
   if (k < 0) k = 0;

   NTL_TLS_LOCAL(RR, t);
   NTL_ZZRegister(A);
   NTL_ZZRegister(B);
   NTL_ZZRegister(R);

   abs(A, a.x);
   LeftShift(A, A, k);

   abs(B, b.x);
   DivRem(t.x, R, A, B);

   t.e = a.e - b.e - k;

   normalize(z, t, !IsZero(R));

   if (neg)
      negate(z.x, z.x);
}

void DivPrec(RR& x, const RR& a, const RR& b, long p)
{
   if (p < 1)
      LogicError("DivPrec: bad precsion");
   if (NTL_OVERFLOW(p, 1, 0))
      ResourceError("DivPrec: bad precsion");

   RRPush push;
   RR::prec = p;
   div(x, a, b);
}

void SqrRoot(RR& z, const RR& a)
{
   if (sign(a) < 0)
      ArithmeticError("RR: attempt to take square root of negative number");

   if (IsZero(a)) {
      clear(z);
      return;
   }

   RR t;
   ZZ T1, T2;
   long k;

   k = 2*RR::prec - NumBits(a.x) + 1;

   if (k < 0) k = 0;

   if ((a.e - k) & 1) k++;

   LeftShift(T1, a.x, k);
   // since k >= 2*prec - bits(a) + 1, T1 has at least 2*prec+1 bits,
   // thus T1 >= 2^(2*prec)

   SqrRoot(t.x, T1); // t.x >= 2^prec thus t.x contains the round bit
   t.e = (a.e - k)/2;
   sqr(T2, t.x);

   // T1-T2 is the (lower part of the) sticky bit
   normalize(z, t, T2 < T1);
}

void SqrRootPrec(RR& x, const RR& a, long p)
{
   if (p < 1)
      LogicError("SqrRootPrec: bad precsion");
   if (NTL_OVERFLOW(p, 1, 0))
      ResourceError("SqrRootPrec: bad precsion");

   RRPush push;
   RR::prec = p;
   SqrRoot(x, a);
}

long compare(const RR& a, const RR& b)
{
   NTL_TLS_LOCAL(RR, t);

   SubPrec(t, a, b, 1);
   return sign(t);
}

long operator==(const RR& a, const RR& b)
{
   return a.e == b.e && a.x == b.x;
}

void trunc(RR& z, const RR& a)
{
   NTL_TLS_LOCAL(RR, t);

   if (a.e >= 0)
      xcopy(z, a);
   else {
      RightShift(t.x, a.x, -a.e);
      t.e = 0;
      xcopy(z, t);
   }
}

void TruncPrec(RR& x, const RR& a, long p)
{
   if (p < 1)
      LogicError("TruncPrec: bad precsion");
   if (NTL_OVERFLOW(p, 1, 0))
      ResourceError("TruncPrec: bad precsion");

   RRPush push;
   RR::prec = p;
   trunc(x, a);
}

void floor(RR& z, const RR& a)
{
   NTL_TLS_LOCAL(RR, t);

   if (a.e >= 0)
      xcopy(z, a);
   else {
      RightShift(t.x, a.x, -a.e);
      if (sign(a.x) < 0)
         add(t.x, t.x, -1);
      t.e = 0;
      xcopy(z, t);
   }
}

void FloorPrec(RR& x, const RR& a, long p)
{
   if (p < 1)
      LogicError("FloorPrec: bad precsion");
   if (NTL_OVERFLOW(p, 1, 0))
      ResourceError("FloorPrec: bad precsion");

   RRPush push;
   RR::prec = p;
   floor(x, a);
}

void ceil(RR& z, const RR& a)
{
   NTL_TLS_LOCAL(RR, t);

   if (a.e >= 0)
      xcopy(z, a);
   else {
      RightShift(t.x, a.x, -a.e);
      if (sign(a.x) > 0)
         add(t.x, t.x, 1);
      t.e = 0;
      xcopy(z, t);
   }
}

void CeilPrec(RR& x, const RR& a, long p)
{
   if (p < 1)
      LogicError("CeilPrec: bad precsion");
   if (NTL_OVERFLOW(p, 1, 0))
      ResourceError("CeilPrec: bad precsion");

   RRPush push;
   RR::prec = p;
   ceil(x, a);
}

void round(RR& z, const RR& a)
{
   if (a.e >= 0) {
      xcopy(z, a);
      return;
   }

   long len = NumBits(a.x);

   if (-a.e > len) {
      z = 0;
      return;
   }

   if (-a.e == len) {
      if (len == 1)
         z = 0;
      else
         z = sign(a.x);

      return;
   }

   NTL_TLS_LOCAL(RR, t);
   ConvPrec(t, a, len+a.e);
   xcopy(z, t);
}

void RoundPrec(RR& x, const RR& a, long p)
{
   if (p < 1)
      LogicError("RoundPrec: bad precsion");
   if (NTL_OVERFLOW(p, 1, 0))
      ResourceError("RoundPrec: bad precsion");

   RRPush push;
   RR::prec = p;
   round(x, a);
}

void conv(RR& z, const ZZ& a)
{
   normalize1(z, a, 0, RR::prec, 0);
}

void ConvPrec(RR& x, const ZZ& a, long p)
{
   if (p < 1)
      LogicError("ConvPrec: bad precsion");
   if (NTL_OVERFLOW(p, 1, 0))
      ResourceError("ConvPrec: bad precsion");

   RRPush push;
   RR::prec = p;
   conv(x, a);
}

void conv(RR& z, long a)
{
   if (a == 0) {
      clear(z);
      return;
   }

   if (a == 1) {
      set(z);
      return;
   }

   NTL_ZZRegister(t);
   t = a;
   conv(z, t);
}

void ConvPrec(RR& x, long a, long p)
{
   if (p < 1)
      LogicError("ConvPrec: bad precsion");
   if (NTL_OVERFLOW(p, 1, 0))
      ResourceError("ConvPrec: bad precsion");

   RRPush push;
   RR::prec = p;
   conv(x, a);
}

void conv(RR& z, unsigned long a)
{
   if (a == 0) {
      clear(z);
      return;
   }

   if (a == 1) {
      set(z);
      return;
   }

   NTL_ZZRegister(t);
   conv(t, a);
   conv(z, t);
}

void ConvPrec(RR& x, unsigned long a, long p)
{
   if (p < 1)
      LogicError("ConvPrec: bad precsion");
   if (NTL_OVERFLOW(p, 1, 0))
      ResourceError("ConvPrec: bad precsion");

   RRPush push;
   RR::prec = p;
   conv(x, a);
}

void conv(RR& z, double a)
{
   if (a == 0) {
      clear(z);
      return;
   }

   if (a == 1) {
      set(z);
      return;
   }

   if (!IsFinite(&a))
      ArithmeticError("RR: conversion of a non-finite double");

   int e;
   double f;
   NTL_TLS_LOCAL(RR, t);

   f = frexp(a, &e);

   f = f * NTL_FDOUBLE_PRECISION;
   f = f * 4;

   conv(t.x, f);
   t.e = e - (NTL_DOUBLE_PRECISION + 1);

   xcopy(z, t);
}

void ConvPrec(RR& x, double a, long p)
{
   if (p < 1)
      LogicError("ConvPrec: bad precsion");
   if (NTL_OVERFLOW(p, 1, 0))
      ResourceError("ConvPrec: bad precsion");

   RRPush push;
   RR::prec = p;
   conv(x, a);
}

void conv(ZZ& z, const RR& a)
{
   if (a.e >= 0)
      LeftShift(z, a.x, a.e);
   else {
      long sgn = sign(a.x);
      RightShift(z, a.x, -a.e);
      if (sgn < 0)
         sub(z, z, 1);
   }
}

void CeilToZZ(ZZ& z, const RR& a)
{
   if (a.e >= 0)
      LeftShift(z, a.x, a.e);
   else {
      long sgn = sign(a.x);
      RightShift(z, a.x, -a.e);
      if (sgn > 0)
         add(z, z, 1);
   }
}

void TruncToZZ(ZZ& z, const RR& a)
{
   if (a.e >= 0)
      LeftShift(z, a.x, a.e);
   else
      RightShift(z, a.x, -a.e);
}

void RoundToZZ(ZZ& z, const RR& a)
{
   if (a.e >= 0) {
      LeftShift(z, a.x, a.e);
      return;
   }

   long len = NumBits(a.x);

   if (-a.e > len) {
      z = 0;
      return;
   }

   if (-a.e == len) {
      if (len == 1)
         z = 0;
      else
         z = sign(a.x);

      return;
   }

   NTL_TLS_LOCAL(RR, t);

   ConvPrec(t, a, len+a.e);

   LeftShift(z, t.x, t.e);
}

void conv(long& z, const RR& a)
{
   ZZ t;
   if (a.e >= NTL_BITS_PER_LONG)
      z = 0;
   else {
      conv(t, a);
      conv(z, t);
   }
}

void conv(double& z, const RR& aa)
{
   double x;
   NTL_TLS_LOCAL(RR, a);

   ConvPrec(a, aa, NTL_DOUBLE_PRECISION);
   // round to NTL_DOUBLE_PRECISION bits to avoid double overflow

   conv(x, a.x);
   z = _ntl_ldexp(x, a.e);
}

void add(RR& z, const RR& a, double b)
{
   NTL_TLS_LOCAL(RR, B);
   B = b;
   add(z, a, B);
}

void sub(RR& z, const RR& a, double b)
{
   NTL_TLS_LOCAL(RR, B);
   B = b;
   sub(z, a, B);
}

void sub(RR& z, double a, const RR& b)
{
   NTL_TLS_LOCAL(RR, A);
   A = a;
   sub(z, A, b);
}

void mul(RR& z, const RR& a, double b)
{
   NTL_TLS_LOCAL(RR, B);
   B = b;
   mul(z, a, B);
}

void div(RR& z, const RR& a, double b)
{
   NTL_TLS_LOCAL(RR, B);
   B = b;
   div(z, a, B);
}

void div(RR& z, double a, const RR& b)
{
   NTL_TLS_LOCAL(RR, A);
   A = a;
   div(z, A, b);
}

void inv(RR& z, const RR& a)
{
   NTL_TLS_LOCAL_INIT(RR, one, (to_RR(1)));
   div(z, one, a);
}

void InvPrec(RR& x, const RR& a, long p)
{
   if (p < 1)
      LogicError("InvPrec: bad precsion");
   if (NTL_OVERFLOW(p, 1, 0))
      ResourceError("InvPrec: bad precsion");

   RRPush push;
   RR::prec = p;
   inv(x, a);
}

long compare(const RR& a, double b)
{
   if (b == 0) return sign(a);

   NTL_TLS_LOCAL(RR, B);
   B = b;
   return compare(a, B);
}

long operator==(const RR& a, double b)
{
   if (b == 0) return IsZero(a);
   if (b == 1) return IsOne(a);

   NTL_TLS_LOCAL(RR, B);
   B = b;
   return a == B;
}

void power(RR& z, const RR& a, long e)
{
   RR b, res;

   long n = NumBits(e);

   RRPush push;
   long p = RR::precision();
   RR::SetPrecision(p + n + 10);

   xcopy(b, a);

   set(res);
   long i;

   for (i = n-1; i >= 0; i--) {
      sqr(res, res);
      if (bit(e, i))
         mul(res, res, b);
   }

   RR::SetPrecision(p);

   if (e < 0)
      inv(z, res);
   else
      xcopy(z, res);
}

istream& operator>>(istream& s, RR& x)
{
   RR v;

   {
      RRPush push;

      long c;
      long cval;
      long sign;
      ZZ a, b;

      if (!s) NTL_INPUT_ERROR(s, "bad RR input");

      c = s.peek();
      while (IsWhiteSpace(c)) {
         s.get();
         c = s.peek();
      }

      if (c == '-') {
         sign = -1;
         s.get();
         c = s.peek();
      }
      else
         sign = 1;

      long got1 = 0;
      long got_dot = 0;
      long got2 = 0;

      a = 0;
      b = 1;

      cval = CharToIntVal(c);

      if (cval >= 0 && cval <= 9) {
         got1 = 1;

         while (cval >= 0 && cval <= 9) {
            mul(a, a, 10);
            add(a, a, cval);
            s.get();
            c = s.peek();
            cval = CharToIntVal(c);
         }
      }

      if (c == '.') {
         got_dot = 1;

         s.get();
         c = s.peek();
         cval = CharToIntVal(c);

         if (cval >= 0 && cval <= 9) {
            got2 = 1;

            while (cval >= 0 && cval <= 9) {
               mul(a, a, 10);
               add(a, a, cval);
               mul(b, b, 10);
               s.get();
               c = s.peek();
               cval = CharToIntVal(c);
            }
         }
      }

      if (got_dot && !got1 && !got2)  NTL_INPUT_ERROR(s, "bad RR input");

      ZZ e;

      long got_e = 0;
      long e_sign;

      if (c == 'e' || c == 'E') {
         got_e = 1;

         s.get();
         c = s.peek();

         if (c == '-') {
            e_sign = -1;
            s.get();
            c = s.peek();
         }
         else if (c == '+') {
            e_sign = 1;
            s.get();
            c = s.peek();
         }
         else
            e_sign = 1;

         cval = CharToIntVal(c);

         if (cval < 0 || cval > 9) NTL_INPUT_ERROR(s, "bad RR input");

         e = 0;
         while (cval >= 0 && cval <= 9) {
            mul(e, e, 10);
            add(e, e, cval);
            s.get();
            c = s.peek();
            cval = CharToIntVal(c);
         }
      }

      if (!got1 && !got2 && !got_e) NTL_INPUT_ERROR(s, "bad RR input");

      RR t1, t2;

      long old_p = RR::precision();

      if (got1 || got2) {
         ConvPrec(t1, a, max(NumBits(a), 1));
         ConvPrec(t2, b, NumBits(b));
         if (got_e)
            RR::SetPrecision(old_p + 10);

         div(v, t1, t2);
      }
      else
         set(v);

      if (sign < 0)
         negate(v, v);

      if (got_e) {
         if (e >= NTL_OVFBND) ResourceError("RR input overflow");
         long E;
         conv(E, e);
         if (e_sign < 0) E = -E;
         RR::SetPrecision(old_p + 10);
         power(t1, to_RR(10), E);
         mul(v, v, t1);
      }
   }

   xcopy(x, v);
   return s;
}

istream& InputPrec(RR& x, istream& s, long p)
{
   if (p < 1)
      LogicError("InputPrec: bad precsion");
   if (NTL_OVERFLOW(p, 1, 0))
      ResourceError("InputPrec: bad precsion");

   RRPush push;
   RR::prec = p;
   s >> x;
   return s;
}

void conv(RR& z, const xdouble& a)
{
   conv(z, a.mantissa());

   if (a.exponent() >  ((2*NTL_OVFBND)/(2*NTL_XD_HBOUND_LOG)))
      ResourceError("RR: overlow");

   if (a.exponent() < -((2*NTL_OVFBND)/(2*NTL_XD_HBOUND_LOG)))
      ResourceError("RR: underflow");

   z.e += a.exponent()*(2*NTL_XD_HBOUND_LOG);

   if (z.e >= NTL_OVFBND)
      ResourceError("RR: overflow");

   if (z.e <= -NTL_OVFBND)
      ResourceError("RR: underflow");
}

void ConvPrec(RR& x, const xdouble& a, long p)
{
   if (p < 1)
      LogicError("ConvPrec: bad precsion");
   if (NTL_OVERFLOW(p, 1, 0))
      ResourceError("ConvPrec: bad precsion");

   RRPush push;
   RR::prec = p;
   conv(x, a);
}

void conv(xdouble& z, const RR& a)
{
   xdouble x;
   xdouble y;

   conv(x, a.x);
   power2(y, a.e);
   z = x*y;
}

void power2(RR& z, long e)
{
   if (e >= NTL_OVFBND)
      ResourceError("RR: overflow");

   if (e <= -NTL_OVFBND)
      ResourceError("RR: underflow");

   set(z.x);
   z.e = e;
}

void conv(RR& z, const quad_float& a)
{
   NTL_TLS_LOCAL(RR, hi);
   NTL_TLS_LOCAL(RR, lo);
   NTL_TLS_LOCAL(RR, res);

   ConvPrec(hi, a.hi, NTL_DOUBLE_PRECISION);
   ConvPrec(lo, a.lo, NTL_DOUBLE_PRECISION);

   add(res, hi, lo);

   z = res;
}

void ConvPrec(RR& x, const quad_float& a, long p)
{
   if (p < 1)
      LogicError("ConvPrec: bad precsion");
   if (NTL_OVERFLOW(p, 1, 0))
      ResourceError("ConvPrec: bad precsion");

   RRPush push;
   RR::prec = p;
   conv(x, a);
}

void conv(quad_float& z, const RR& a)
{
   NTL_TLS_LOCAL(RR, a_hi);
   NTL_TLS_LOCAL(RR, a_lo);

   ConvPrec(a_hi, a, NTL_DOUBLE_PRECISION);  // high order bits
   SubPrec(a_lo, a, a_hi, NTL_DOUBLE_PRECISION);  // low order bits

   z = to_quad_float(a_hi.x)*power2_quad_float(a_hi.e) +
       to_quad_float(a_lo.x)*power2_quad_float(a_lo.e);
}

void ConvPrec(RR& x, const char *s, long p)
{
   if (p < 1)
      LogicError("ConvPrec: bad precsion");
   if (NTL_OVERFLOW(p, 1, 0))
      ResourceError("ConvPrec: bad precsion");

   RRPush push;
   RR::prec = p;
   conv(x, s);
}

void ReallyComputeE(RR& res)
{
   RRPush push;
   long p = RR::precision();
   RR::SetPrecision(p + NumBits(p) + 10);

   RR s, s1, t;

   s = 1;
   t = 1;

   long i;

   for (i = 2; ; i++) {
      add(s1, s, t);
      if (s == s1) break;
      xcopy(s, s1);
      div(t, t, i);
   }

   RR::SetPrecision(p);
   xcopy(res, s);
}

void ComputeE(RR& res)
{
   static NTL_CHEAP_THREAD_LOCAL long prec = 0;

   NTL_TLS_LOCAL(RR, e);

   RRPush push;
   long p = RR::precision();

   if (prec <= p + 10) {
      prec = p + 20;
      RR::SetPrecision(prec);
      ReallyComputeE(e);
      RR::SetPrecision(p);
   }

   xcopy(res, e);
}

void exp(RR& res, const RR& x)
{
   if (x >= NTL_OVFBND || x <= -NTL_OVFBND)
      ResourceError("RR: overflow");

   RRPush push;
   long p = RR::precision();

   // step 0: write x = n + f, n an integer and |f| <= 1/2
   //    careful -- we want to compute f to > p bits of precision

   RR f, nn;
   RR::SetPrecision(NTL_BITS_PER_LONG);
   round(nn, x);
   RR::SetPrecision(p + 10);
   sub(f, x, nn);
   long n = to_long(nn);

   // step 1: calculate t1 = e^n by repeated squaring

   RR::SetPrecision(p + NumBits(n) + 10);

   RR e;
   ComputeE(e);

   RR::SetPrecision(p + 10);

   RR t1;
   power(t1, e, n);

   // step 2: calculate t2 = e^f using Taylor series expansion

   RR::SetPrecision(p + NumBits(p) + 10);

   RR t2, s, s1, t;
   long i;

   s = 0;
   t = 1;

   for (i = 1; ; i++) {
      add(s1, s, t);
      if (s == s1) break;
      xcopy(s, s1);
      mul(t, t, f);
      div(t, t, i);
   }

   xcopy(t2, s);

   RR::SetPrecision(p);

   mul(res, t1, t2);
}

void ReallyComputeLn2(RR& res)
{
   RRPush push;
   long p = RR::precision();
   RR::SetPrecision(p + NumBits(p) + 10);

   RR s, s1, t, t1;

   s = 0;
   t = 0.5;
   t1 = 0.5;

   long i;

   for (i = 2; ; i++) {
      add(s1, s, t);
      if (s == s1) break;
      xcopy(s, s1);
      mul(t1, t1, 0.5);
      div(t, t1, i);
   }

   RR::SetPrecision(p);
   xcopy(res, s);
}

void ComputeLn2(RR& res)
{
   static NTL_CHEAP_THREAD_LOCAL long prec = 0;

   NTL_TLS_LOCAL(RR, ln2);

   RRPush push;
   long p = RR::precision();

   if (prec <= p + 10) {
      prec = p + 20;
      RR::SetPrecision(prec);
      ReallyComputeLn2(ln2);
      RR::SetPrecision(p);
   }

   xcopy(res, ln2);
}

long Lg2(const RR& x)
{
   return NumBits(x.mantissa()) + x.exponent();
}

void log(RR& res, const RR& x)
{
   if (x <= 0) ArithmeticError("argument to log must be positive");

   RRPush push;
   long p = RR::precision();

   RR::SetPrecision(p + NumBits(p) + 10);

   RR y;
   long n;

   // re-write x = 2^n * (1 - y), where -1/2 < y < 1/4  (so 3/4 < 1-y < 3/2)

   if (x > 0.75 && x < 1.5) {
      n = 0;
      sub(y, 1, x);
   }
   else {
      n = Lg2(x) - 1;
      RR t;
      power2(t, -n);
      mul(t, t, x);
      while (t > 1.5) {
         mul(t, t, 0.5);
         n++;
      }

      sub(y, 1, t);
   }

   // compute s = - ln(1-y) by power series expansion

   RR s, s1, t, t1;

   s = 0;
   xcopy(t, y);
   xcopy(t1, y);

   long i;

   for (i = 2; ; i++) {
      add(s1, s, t);
      if (s == s1) break;
      xcopy(s, s1);
      mul(t1, t1, y);
      div(t, t1, i);
   }

   if (n == 0)
      t = 0;
   else {
      ComputeLn2(t);
      mul(t, t, n);
   }

   RR::SetPrecision(p);

   sub(res, t, s);
}

void ComputeLn10(RR& res)
{
   static NTL_CHEAP_THREAD_LOCAL long prec = 0;

   NTL_TLS_LOCAL(RR, ln10);

   RRPush push;
   long p = RR::precision();

   if (prec <= p + 10) {
      prec = p + 20;
      RR::SetPrecision(prec);
      log(ln10, to_RR(10));
      RR::SetPrecision(p);
   }

   xcopy(res, ln10);
}

void log10(RR& res, const RR& x)
{
   RRPush push;
   long p = RR::precision();
   RR::SetPrecision(p + 10);

   RR ln10, t1, t2;
   ComputeLn10(ln10);

   log(t1, x);
   div(t2, t1, ln10);

   RR::SetPrecision(p);

   xcopy(res, t2);
}

void expm1(RR& res, const RR& x)
{
   RRPush push;
   long p = RR::precision();

   if (x < -0.5 || x > 0.5) {
      RR t;
      RR::SetPrecision(p + 10);
      exp(t, x);
      RR::SetPrecision(p);
      sub(res, t, 1);
      return;
   }

   RR::SetPrecision(p + NumBits(p) + 10);

   RR f;

   xcopy(f, x);

   RR s, s1, t;
   long i;

   s = 0;
   xcopy(t, f);

   for (i = 2; ; i++) {
      add(s1, s, t);
      if (s == s1) break;
      xcopy(s, s1);
      mul(t, t, f);
      div(t, t, i);
   }

   RR::SetPrecision(p);

   xcopy(res, s);
}

void log1p(RR& res, const RR& x)
{
   RRPush push;
   long p = RR::precision();
   RR y;

   if (x < -0.5 || x > 0.5) {
      RR::SetPrecision(p + 10);
      log(y, 1 + x);
      RR::SetPrecision(p);
      xcopy(res, y);
      return;
   }

   RR::SetPrecision(p + NumBits(p) + 10);

   negate(y, x);

   // compute s = - ln(1-y) by power series expansion

   RR s, s1, t, t1;

   s = 0;
   xcopy(t, y);
   xcopy(t1, y);

   long i;

   for (i = 2; ; i++) {
      add(s1, s, t);
      if (s == s1) break;
      xcopy(s, s1);
      mul(t1, t1, y);
      div(t, t1, i);
   }

   RR::SetPrecision(p);

   negate(res, s);

}

void pow(RR& res, const RR& x, const RR& y)
{

   if (y == 0) {
      res = 1;
      return;
   }

   if (x == 0) {
      res = 0;
      return;
   }

   if (x == 1) {
      res = 1;
      return;
   }

   if (x < 0) {
      ArithmeticError("pow: sorry...first argument to pow must be nonnegative");
   }

   RRPush push;
   long p = RR::precision();

   // calculate working precison...one could use p + NTL_BITS_PER_LONG + 10,
   // for example, but we want the behaviour to be machine independent.
   // so we calculate instead a rough approximation to log |y log(x)|

   RR t1, t2;
   long k;

   if (x > 0.5 && x < 1.5) {
      xcopy(t1, x - 1);
      k = Lg2(t1);
   }
   else {
      k = NumBits(Lg2(x));
   }

   k += Lg2(y);

   if (k > NTL_BITS_PER_LONG+10) ResourceError("RR: overflow");

   if (k < 0) k = 0;

   RR::SetPrecision(p + k + 10);

   t1 = y*log(x);

   RR::SetPrecision(p);

   t2 = exp(t1);

   res = t2;
}

void ReallyComputePi(RR& res)
{
   RRPush push;
   long p = RR::precision();
   RR::SetPrecision(p + NumBits(p) + 10);

   RR sum1;

   RR s, s1, t, t1;

   s = 0;
   t = 0.5;
   t1 = 0.5;

   long i;

   for (i = 3; ; i+=2) {
      add(s1, s, t);
      if (s == s1) break;
      xcopy(s, s1);
      mul(t1, t1, -0.25);
      div(t, t1, i);
   }

   xcopy(sum1, s);

   RR g;

   inv(g, to_RR(3)); // g = 1/3

   s = 0;
   xcopy(t, g);
   xcopy(t1, g);

   sqr(g, g);
   negate(g, g); // g = -1/9

   for (i = 3; ; i+=2) {
      add(s1, s, t);
      if (s == s1) break;
      xcopy(s, s1);
      mul(t1, t1, g);
      div(t, t1, i);
   }

   add(s, s, sum1);
   mul(s, s, 4);

   RR::SetPrecision(p);
   xcopy(res, s);
}

void ComputePi(RR& res)
{
   static NTL_CHEAP_THREAD_LOCAL long prec = 0;

   NTL_TLS_LOCAL(RR, pi);

   RRPush push;
   long p = RR::precision();

   if (prec <= p + 10) {
      prec = p + 20;
      RR::SetPrecision(prec);
      ReallyComputePi(pi);
      RR::SetPrecision(p);
   }

   xcopy(res, pi);
}

void sin(RR& res, const RR& x)
{
   if (x == 0) {
      res = 0;
      return;
   }

   if (Lg2(x) > 1000)
      ResourceError("sin: sorry...argument too large in absolute value");

   RRPush push;
   long p = RR::precision();

   RR pi, t1, f;
   RR n;

   // we want to make x^2 < 3, so that the series for sin(x)
   // converges nicely, without any nasty cancellations in the
   // first terms of the series.

   RR::SetPrecision(p + NumBits(p) + 10);

   if (x*x < 3) {
      xcopy(f, x);
   }
   else {

      // we want to write x/pi = n + f, |f| < 1/2....
      // but we have to do *this* very carefully, so that f is computed
      // to precision > p.  I know, this is sick!

      long p1;

      p1 = p + Lg2(x) + 20;

      for (;;) {
         RR::SetPrecision(p1);
         ComputePi(pi);
         xcopy(t1, x/pi);
         xcopy(n, floor(t1));
         xcopy(f, t1 - n);
         if (f > 0.5) {
            n++;
            xcopy(f, t1 - n);
         }

         if (f == 0 || p1 < p - Lg2(f) + Lg2(n) + 10) {
            // we don't have enough bits of f...increase p1 and continue

            p1 = p1 + max(20, p1/10);
         }
         else
            break;
      }

      RR::SetPrecision(p + NumBits(p) + 10);
      ComputePi(pi);

      xcopy(f, pi * f);

      if (n != 0 && n.exponent() == 0) {
         // n is odd, so we negate f, which negates sin(f)

         xcopy(f, -f);
      }

   }

   // Boy, that was painful, but now its over, and we can simply apply
   // the series for sin(f)

   RR t2, s, s1, t;
   long i;

   s = 0;
   xcopy(t, f);

   for (i = 3; ; i=i+2) {
      add(s1, s, t);
      if (s == s1) break;
      xcopy(s, s1);
      mul(t, t, f);
      mul(t, t, f);
      div(t, t, i-1);
      div(t, t, i);
      negate(t, t);
   }

   RR::SetPrecision(p);

   xcopy(res, s);

}

void cos(RR& res, const RR& x)
{
   if (x == 0) {
      res = 1;
      return;
   }

   if (Lg2(x) > 1000)
      ResourceError("cos: sorry...argument too large in absolute value");

   RRPush push;
   long p = RR::precision();

   RR pi, t1, f;
   RR n;

   // we want to write x/pi = (n+1/2) + f, |f| < 1/2....
   // but we have to do *this* very carefully, so that f is computed
   // to precision > p.  I know, this is sick!

   long p1;

   p1 = p + Lg2(x) + 20;

   for (;;) {
      RR::SetPrecision(p1);
      ComputePi(pi);
      xcopy(t1, x/pi);
      xcopy(n, floor(t1));
      xcopy(f, t1 - (n + 0.5));

      if (f == 0 || p1 < p - Lg2(f) + Lg2(n) + 10) {
         // we don't have enough bits of f...increase p1 and continue

         p1 = p1 + max(20, p1/10);
      }
      else
         break;
   }

   RR::SetPrecision(p + NumBits(p) + 10);
   ComputePi(pi);

   xcopy(f, pi * f);

   if (n == 0 || n.exponent() != 0) {
      // n is even, so we negate f, which negates sin(f)

      xcopy(f, -f);
   }

   // Boy, that was painful, but now its over, and we can simply apply
   // the series for sin(f)

   RR t2, s, s1, t;
   long i;

   s = 0;
   xcopy(t, f);

   for (i = 3; ; i=i+2) {
      add(s1, s, t);
      if (s == s1) break;
      xcopy(s, s1);
      mul(t, t, f);
      mul(t, t, f);
      div(t, t, i-1);
      div(t, t, i);
      negate(t, t);
   }

   RR::SetPrecision(p);

   xcopy(res, s);

}

ostream& operator<<(ostream& s, const RR& a)
{
   if (IsZero(a)) {
      s << "0";
      return s;
   }

   RRPush push;

   // we compute new_p and log_10_a precisely using sufficient
   // precision---this is necessary to achieve accuracy and
   // platform independent behaviour

   long temp_p = max(NumBits(RR::OutputPrecision()),
                     NumBits(Lg2(a))) + 10;

   RR::SetPrecision(temp_p);

   RR ln2, ln10, log_2_10;
   ComputeLn2(ln2);
   ComputeLn10(ln10);
   log_2_10 = ln10/ln2;
   long new_p = to_long(RR::OutputPrecision()*log_2_10) + 20;
   long log_10_a = to_long(Lg2(a)/log_2_10);

   RR::SetPrecision(new_p);

   RR b;
   long neg;

   if (a < 0) {
      negate(b, a);
      neg = 1;
   }
   else {
      xcopy(b, a);
      neg = 0;
   }

   long k = RR::OutputPrecision() - log_10_a;

   RR c, d;

   power(c, to_RR(10), RR::OutputPrecision());
   power(d, to_RR(10), log_10_a);

   div(b, b, d);
   mul(b, b, c);

   while (b < c) {
      mul(b, b, 10);
      k++;
   }

   while (b >= c) {
      div(b, b, 10);
      k--;
   }

   add(b, b, 0.5);
   k = -k;

   ZZ B;
   conv(B, b);

   long bp_len = RR::OutputPrecision()+10;

   UniqueArray<char> bp_store;
   bp_store.SetLength(bp_len);
   char *bp = bp_store.get();

   long len, i;

   len = 0;
   do {
      if (len >= bp_len) LogicError("RR output: buffer overflow");
      bp[len] = IntValToChar(DivRem(B, B, 10));
      len++;
   } while (B > 0);

   for (i = 0; i < len/2; i++) {
      char tmp;
      tmp = bp[i];
      bp[i] = bp[len-1-i];
      bp[len-1-i] = tmp;
   }

   i = len-1;
   while (bp[i] == '0') i--;

   k += (len-1-i);
   len = i+1;

   bp[len] = '\0';

   if (k > 3 || k < -len - 3) {
      // use scientific notation

      if (neg) s << "-";
      s << "0." << bp << "e" << (k + len);
   }
   else if (k >= 0) {
      if (neg) s << "-";
      s << bp;
      for (i = 0; i < k; i++)
         s << "0";
   }
   else if (k <= -len) {
      if (neg) s << "-";
      s << "0.";
      for (i = 0; i < -len-k; i++)
         s << "0";
      s << bp;
   }
   else {
      if (neg) s << "-";
      for (i = 0; i < len+k; i++)
         s << bp[i];

      s << ".";

      for (i = len+k; i < len; i++)
         s << bp[i];
   }

   return s;
}

NTL_END_IMPL

ext: cpp

max_stars_repo_name: AnthonyTudorov/PALISADE-SizeOf-Fork

max_issues_repo_name: AnthonyTudorov/PALISADE-SizeOf-Fork

max_forks_repo_name: AnthonyTudorov/PALISADE-SizeOf-Fork


text:
#include "CorePch.h"
#include <rtp++/network/UdpSocketWrapper.h>
#include <boost/bind.hpp>
#include <boost/asio/ip/multicast.hpp>
#include <boost/asio/ip/udp.hpp>
#include <boost/asio/placeholders.hpp>
#include <boost/make_shared.hpp>
#include <boost/system/system_error.hpp>
#include <boost/throw_exception.hpp>
#include <rtp++/RtpTime.h>

using namespace boost::asio::ip;

namespace rtp_plus_plus
{

UdpSocketWrapper::ptr UdpSocketWrapper::create(boost::asio::io_service& ioService, const std::string& sBindIp, unsigned short uiBindPort)
{
  return boost::make_shared<UdpSocketWrapper>(boost::ref(ioService), boost::ref(sBindIp), uiBindPort);
}

UdpSocketWrapper::ptr UdpSocketWrapper::create(boost::asio::io_service& ioService, const std::string& sBindIp, unsigned short uiBindPort, boost::system::error_code& ec)
{
  return boost::make_shared<UdpSocketWrapper>(boost::ref(ioService), boost::ref(sBindIp), uiBindPort, boost::ref(ec));
}

UdpSocketWrapper::ptr UdpSocketWrapper::create(boost::asio::io_service& ioService, const std::string& sBindIp, unsigned short uiBindPort, std::unique_ptr<boost::asio::ip::udp::socket> pSocket)
{
  return boost::make_shared<UdpSocketWrapper>(boost::ref(ioService), boost::ref(sBindIp), uiBindPort, std::move(pSocket));
}

UdpSocketWrapper::UdpSocketWrapper(boost::asio::io_service& ioService, const std::string& sBindIp, unsigned short uiBindPort)
  :m_rIoService(ioService),
  m_timer(ioService),
  m_sIpAddress(sBindIp),
  m_uiPort(uiBindPort),
  m_address(boost::asio::ip::address::from_string(sBindIp)),
  m_endpoint(m_address, uiBindPort),
  m_pSocket(new udp::socket(m_rIoService)),
  m_bTimeOut(false),
  m_uiTimeoutMs(1000)
{
  boost::system::error_code ec = initialise();
  if (ec)
  {
    boost::system::system_error e(ec);
    boost::throw_exception(e);
  }
}

UdpSocketWrapper::UdpSocketWrapper(boost::asio::io_service& ioService, const std::string& sBindIp, unsigned short uiBindPort, boost::system::error_code& ec)
  :m_rIoService(ioService),
  m_timer(ioService),
  m_sIpAddress(sBindIp),
  m_uiPort(uiBindPort),
  m_address(boost::asio::ip::address::from_string(sBindIp)),
  m_endpoint(m_address, uiBindPort),
  m_pSocket(new udp::socket(m_rIoService)),
  m_bTimeOut(false),
  m_uiTimeoutMs(1000)
{
  ec = initialise();
}

UdpSocketWrapper::UdpSocketWrapper(boost::asio::io_service& ioService, const std::string& sBindIp, unsigned short uiBindPort, std::unique_ptr<boost::asio::ip::udp::socket> pSocket)
  :m_rIoService(ioService),
  m_timer(ioService),
  m_sIpAddress(sBindIp),
  m_uiPort(uiBindPort),
  m_address(boost::asio::ip::address::from_string(sBindIp)),
  m_endpoint(m_address, uiBindPort),
  m_pSocket(std::move(pSocket)),
  m_bTimeOut(false),
  m_uiTimeoutMs(1000)
{

}

UdpSocketWrapper::~UdpSocketWrapper()
{
  VLOG(10) << "[" << this << "] Destructor";
}

void UdpSocketWrapper::send(Buffer networkPacket, const EndPoint& endpoint)
{
  boost::mutex::scoped_lock l(m_lock);
  bool bBusyWriting = !m_vDeliveryQueue.empty();
  m_vDeliveryQueue.push_back( std::make_pair(networkPacket, endpoint) );
  if (!bBusyWriting)
  {
    boost::asio::ip::udp::endpoint destination(boost::asio::ip::address::from_string(endpoint.getAddress()), endpoint.getPort());
    m_pSocket->async_send_to( boost::asio::buffer(networkPacket.data(), networkPacket.getSize()),
      destination,
      boost::bind(&UdpSocketWrapper::writeCompletionHandler,
      shared_from_this(),
      boost::asio::placeholders::error,
      boost::asio::placeholders::bytes_transferred)
      );
  }
  else
  {
#if 0
    DLOG(INFO) << "Packet queued, write in progress";
#endif
  }
}

void UdpSocketWrapper::close()
{
  VLOG(10) << "[" << this << "] Closing socket [" << m_sIpAddress << ":" << m_uiPort << "]";
  m_pSocket->close();
}

void UdpSocketWrapper::recv()
{
  m_pSocket->async_receive_from(
    boost::asio::buffer(m_data, max_length), m_lastSenderEndpoint,
    boost::bind(&UdpSocketWrapper::readCompletionHandler, shared_from_this(),
    boost::asio::placeholders::error,
    boost::asio::placeholders::bytes_transferred));

  if (m_bTimeOut)
  {
    /* Create a task that will be called if we wait more than 300ms */
    m_timer.expires_from_now(boost::posix_time::milliseconds(m_uiTimeoutMs));
    m_timer.async_wait(boost::bind(&UdpSocketWrapper::timeoutHandler, shared_from_this(), boost::asio::placeholders::error));
  }
}

boost::system::error_code UdpSocketWrapper::initialise()
{
  if (m_address.is_multicast())
  {
    // HACK FOR MULTICAST: to be re-thought?
    LOG(INFO) << "Address is multicast " << m_sIpAddress << ":" << m_uiPort;
    boost::asio::ip::udp::endpoint listen_endpoint(boost::asio::ip::address::from_string("0.0.0.0"), m_uiPort);
    m_pSocket->open(listen_endpoint.protocol());
    m_pSocket->set_option(boost::asio::ip::udp::socket::reuse_address(true));
    VLOG(10) << "[" << this << "] Binding socket [" << m_sIpAddress << ":" << m_uiPort << "]";
    boost::system::error_code ec;
    m_pSocket->bind(listen_endpoint, ec);
    if (ec)
    {
      LOG(WARNING) << "Failed to bind socket to port " << m_uiPort;
      return ec;
    }
    // Join the multicast group to receive data
    m_pSocket->set_option(
      boost::asio::ip::multicast::join_group(m_address));

  }
  else
  {
    m_pSocket->open(m_endpoint.protocol());
    // Binding to port 0 should result in the OS selecting the port
    VLOG(10) << "[" << this << "] Binding socket [" << m_sIpAddress << ":" << m_uiPort << "]";
    boost::system::error_code ec;
    //m_pSocket->bind(m_endpoint, ec);
    //m_pSocket->bind(boost::asio::ip::udp::endpoint(boost::asio::ip::udp::v4(), m_uiPort), ec);
    m_pSocket->bind(m_endpoint, ec);
    if (ec)
    {
      LOG(WARNING) << "Failed to bind socket to port " << m_uiPort;
      return ec;
    }
  }

  boost::asio::socket_base::receive_buffer_size getOption;
  boost::system::error_code ecDummy;
  m_pSocket->get_option(getOption, ecDummy);
  VLOG(10) << "UDP socket buffer size before: " << getOption.value();

  // it helps to increase the buffer size to lessen packet loss
  boost::asio::socket_base::receive_buffer_size option(udp_receiver_buffer_size_kb);

  m_pSocket->set_option(option);

  m_pSocket->get_option(getOption, ecDummy);
  VLOG(10) << "UDP socket buffer size after: " << getOption.value();
  return boost::system::error_code();
}

void UdpSocketWrapper::writeCompletionHandler(const boost::system::error_code& ec, std::size_t bytes_transferred)
{
  boost::mutex::scoped_lock l(m_lock);

  NetworkPackage_t package = m_vDeliveryQueue.front();
  m_vDeliveryQueue.pop_front();
  if (m_fnOnSend)
    m_fnOnSend(ec, shared_from_this(), package.first, package.second);
  else
    VLOG(2) << "Send callback has not been set";

  if (!ec)
  {
    VLOG(10) << "[" << this << "][" << m_sIpAddress << ":" << m_uiPort << "] Sent " << bytes_transferred << " to " << package.second.getAddress() << ":" << package.second.getPort();

    // send next sample if one was queued in the meantime
    if ( !m_vDeliveryQueue.empty() )
    {
#if 0
      DLOG(INFO) << "Sending queued packet Queue size: " << m_vDeliveryQueue.size();
#else
      size_t size = m_vDeliveryQueue.size();
      VLOG_IF(5, size > 1) << "Send queue: " << size;
#endif
      NetworkPackage_t package = m_vDeliveryQueue.front();
      Buffer networkPacket = package.first;
      EndPoint ep = package.second;
      boost::asio::ip::udp::endpoint endPoint(boost::asio::ip::address::from_string(ep.getAddress()), ep.getPort());
      m_pSocket->async_send_to( boost::asio::buffer(networkPacket.data(), networkPacket.getSize()),
        endPoint,
        boost::bind(&UdpSocketWrapper::writeCompletionHandler,
        shared_from_this(),
        boost::asio::placeholders::error,
        boost::asio::placeholders::bytes_transferred)
        );
    }
  }
  else
  {
    LOG(WARNING) << "Send failed from " << m_sIpAddress << ":" << m_uiPort << " to " << package.second << " Ec: " << ec.message();
  }
}

void UdpSocketWrapper::timeoutHandler(const boost::system::error_code& ec)
{
  if (ec != boost::asio::error::operation_aborted)
  {
    // socket read timed out
    if (m_fnOnTimeout) m_fnOnTimeout(shared_from_this());
    else
      VLOG(2) << "Timeout callback has not been set";
  }
}

void UdpSocketWrapper::readCompletionHandler(const boost::system::error_code& ec, std::size_t bytes_received)
{
// #define DEBUG_NTP
#ifdef DEBUG_NTP
  DLOG(INFO) << "NTP arrival time: " << convertNtpTimestampToPosixTime(getNTPTimeStamp());
#endif

  NetworkPacket networkPacket(RtpTime::getNTPTimeStamp());
  if (m_bTimeOut)
  {
    // cancel timeout
    m_timer.cancel();
  }

  EndPoint ep;
  if (!ec )
  {
    VLOG(10) << "[" << this << "][" << m_sIpAddress << ":" << m_uiPort  << "] Received " << bytes_received << " from " << m_lastSenderEndpoint.address().to_string() << ":" << m_lastSenderEndpoint.port();

    uint8_t* pData = new uint8_t[bytes_received];
    memcpy(pData, m_data, bytes_received);
    networkPacket.setData(pData, bytes_received);
    ep.setAddress(m_lastSenderEndpoint.address().to_string());
    ep.setPort(m_lastSenderEndpoint.port());
  }
  // let calling clas handle error and logging
#if 0
  else
  {
    if (ec != boost::asio::error::operation_aborted)
    {
      LOG(WARNING) << "Receive failed: " << ec.message();
    }
  }
#endif
  if (m_fnOnRecv)
    m_fnOnRecv(ec, shared_from_this(), networkPacket, ep);
  else
    VLOG(2) << "Recv callback has not been set";
}

} // rtp_plus_plus


text:
//
// Copyright (c) 2018 Vinnie Falco (vinnie dot falco at gmail dot com)
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// Official repository: https://github.com/vinniefalco/CppCon2018
//

//------------------------------------------------------------------------------
/*
    WebSocket chat server

    This implements a multi-user chat room using WebSocket.
*/
//------------------------------------------------------------------------------

#include "listener.hpp"
#include "shared_state.hpp"
#include <boost/asio/signal_set.hpp>
#include <iostream>

int
main(int argc, char* argv[])
{
    // Check command line arguments.
    if (argc != 4)
    {
        std::cerr <<
            "Usage: websocket-chat-server <address> <port> <doc_root>\n" <<
            "Example:\n" <<
            "    websocket-chat-server 0.0.0.0 8080 .\n";
        return EXIT_FAILURE;
    }
    auto address = net::ip::make_address(argv[1]);
    auto port = static_cast<unsigned short>(std::atoi(argv[2]));
    auto doc_root = argv[3];

    // The io_context is required for all I/O
    net::io_context ioc;

    // Create and launch a listening port
    std::make_shared<listener>(
        ioc,
        tcp::endpoint{address, port},
        std::make_shared<shared_state>(doc_root))->run();

    // Capture SIGINT and SIGTERM to perform a clean shutdown
    net::signal_set signals(ioc, SIGINT, SIGTERM);
    signals.async_wait(
        [&ioc](boost::system::error_code const&, int)
        {
            // Stop the io_context. This will cause run()
            // to return immediately, eventually destroying the
            // io_context and any remaining handlers in it.
            ioc.stop();
        });

    // Run the I/O service on the main thread
    ioc.run();

    // (If we get here, it means we got a SIGINT or SIGTERM)

    return EXIT_SUCCESS;
}

max_stars_repo_name: Jackarain/tinyrpc

max_issues_repo_name: avplayer/cxxrpc

max_forks_repo_name: avplayer/cxxrpc


text:
/*
 *          Copyright Andrey Semashev 2007 - 2010.
 * Distributed under the Boost Software License, Version 1.0.
 *    (See accompanying file LICENSE_1_0.txt or copy at
 *          http://www.boost.org/LICENSE_1_0.txt)
 */
/*!
 * \file   formatter_parser.hpp
 * \author Andrey Semashev
 * \date   07.04.2008
 *
 * The header contains definition of a formatter parser function, along with facilities to
 * add support for custom formatters.
 */

#if (defined(_MSC_VER) && _MSC_VER > 1000)
#pragma once
#endif // _MSC_VER > 1000

#ifndef BOOST_LOG_UTILITY_INIT_FORMATTER_PARSER_HPP_INCLUDED_
#define BOOST_LOG_UTILITY_INIT_FORMATTER_PARSER_HPP_INCLUDED_

#include <new> // std::nothrow
#include <iosfwd>
#include <map>
#include <string>
#include <boost/function/function2.hpp>
#include <boost/log/detail/setup_prologue.hpp>
#include <boost/log/core/record.hpp>
#include <boost/log/formatters/attr.hpp>

#ifdef _MSC_VER
#pragma warning(push)
// 'm_A' : class 'A' needs to have dll-interface to be used by clients of class 'B'
#pragma warning(disable: 4251)
// non dll-interface class 'A' used as base for dll-interface class 'B'
#pragma warning(disable: 4275)
#endif // _MSC_VER

namespace boost {

namespace BOOST_LOG_NAMESPACE {

/*!
 * \brief Auxiliary formatter traits
 *
 * The structure generates commonly used types related to formatters and formatter factories.
 */
template< typename CharT >
struct formatter_types
{
    //! Character type
    typedef CharT char_type;
    //! String type
    typedef std::basic_string< char_type > string_type;
    //! Output stream type
    typedef std::basic_ostream< char_type > ostream_type;
    //! Log record type
    typedef basic_record< char_type > record_type;
    //! The formatter function object
    typedef function2< void, ostream_type&, record_type const& > formatter_type;

    /*!
     * Type of the map of formatter factory arguments [argument name -> argument value].
     * This type of maps will be passed to formatter factories on attempt to create a formatter.
     */
    typedef std::map< string_type, string_type > formatter_factory_args;
    /*!
     * \brief The type of a function object that constructs formatter instance
     * \param name Attribute name
     * \param args Formatter arguments
     * \return The constructed formatter. The formatter must not be empty.
     *
     * \b Throws: An <tt>std::exception</tt>-based If an exception is thrown from the method,
     *        the exception is propagated to the parse_formatter caller
     */
    typedef function2< formatter_type, string_type const&, formatter_factory_args const& > formatter_factory;
    //! Map of formatter factory function objects
    typedef std::map< string_type, formatter_factory > factories_map;
};

namespace aux {

    /*!
     * A simple formatter factory function. Provides an easy way to add user-defined types support to
     * the formatter parser. The factory does not consider any formatter arguments, if specified,
     * but produces the formatter that uses the native \c operator<< to format the attribute value.
     *
     * \param attr_name Attribute name to create formatter for.
     */
    template< typename CharT, typename AttributeValueT >
    typename formatter_types< CharT >::formatter_type make_simple_formatter(
        typename formatter_types< CharT >::string_type const& attr_name,
        typename formatter_types< CharT >::formatter_factory_args const&)
    {
        typedef typename formatter_types< CharT >::formatter_type formatter_type;
        return formatter_type(boost::log::formatters::attr< AttributeValueT >(attr_name, std::nothrow));
    }

} // namespace aux

/*!
 * \brief The function registers a user-defined formatter factory
 *
 * The function registers a user-defined formatter factory. The registered factory function will be
 * called when the formatter parser detects the specified attribute name in the formatter string.
 *
 * \pre <tt>attr_name != NULL && !factory.empty()</tt>, \c attr_name must point to a zero-terminated sequence of characters.
 *
 * \param attr_name Attribute name
 * \param factory Formatter factory function
 */
template< typename CharT >
BOOST_LOG_SETUP_EXPORT void register_formatter_factory(
    const CharT* attr_name,
#ifndef BOOST_LOG_BROKEN_TEMPLATE_DEFINITION_MATCHING
    typename formatter_types< CharT >::formatter_factory const& factory
#else
    function2<
        function2< void, std::basic_ostream< CharT >&, basic_record< CharT > const& >,
        std::basic_string< CharT > const&,
        std::map< std::basic_string< CharT >, std::basic_string< CharT > > const&
    > const& factory
#endif // BOOST_LOG_BROKEN_TEMPLATE_DEFINITION_MATCHING
    );

/*!
 * \brief The function registers a user-defined formatter factory
 *
 * The function registers a user-defined formatter factory. The registered factory function will be
 * called when the formatter parser detects the specified attribute name in the formatter string.
 *
 * \pre <tt>!factory.empty()</tt>
 *
 * \param attr_name Attribute name
 * \param factory Formatter factory function
 */
template< typename CharT, typename TraitsT, typename AllocatorT >
inline void register_formatter_factory(
    std::basic_string< CharT, TraitsT, AllocatorT > const& attr_name,
    typename formatter_types< CharT >::formatter_factory const& factory)
{
    register_formatter_factory(attr_name.c_str(), factory);
}

/*!
 * \brief The function registers a simple formatter factory
 *
 * The function registers a simple formatter factory. The registered factory will generate formatters
 * that will be equivalent to the <tt>log::formatters::attr</tt> formatter (i.e. that will use the
 * native \c operator<< to format the attribute value). The factory does not use any arguments,
 * if specified.
 *
 * \pre <tt>attr_name != NULL</tt>, \c attr_name must point to a zero-terminated sequence of characters.
 *
 * \param attr_name Attribute name
 */
template< typename AttributeValueT, typename CharT >
inline void register_simple_formatter_factory(const CharT* attr_name)
{
    register_formatter_factory(attr_name, &boost::log::aux::make_simple_formatter< CharT, AttributeValueT >);
}

/*!
 * \brief The function registers a simple formatter factory
 *
 * The function registers a simple formatter factory. The registered factory will generate formatters
 * that will be equivalent to the <tt>log::formatters::attr</tt> formatter (i.e. that will use the
 * native \c operator<< to format the attribute value). The factory does not use any arguments,
 * if specified.
 *
 * \param attr_name Attribute name
 */
template< typename AttributeValueT, typename CharT, typename TraitsT, typename AllocatorT >
inline void register_simple_formatter_factory(std::basic_string< CharT, TraitsT, AllocatorT > const& attr_name)
{
    register_formatter_factory(attr_name.c_str(), &boost::log::aux::make_simple_formatter< CharT, AttributeValueT >);
}

/*!
 * The function parses a formatter from the sequence of characters
 *
 * \pre <tt>begin <= end</tt>, both pointers must not be NULL
 * \param begin Pointer to the first character of the sequence
 * \param end Pointer to the after-the-last character of the sequence
 * \return A function object that can be used as a formatter.
 *
 * \b Throws: An <tt>std::exception</tt>-based exception, if a formatter cannot be recognized in the character sequence.
 */
template< typename CharT >
BOOST_LOG_SETUP_EXPORT
#ifndef BOOST_LOG_BROKEN_TEMPLATE_DEFINITION_MATCHING
typename formatter_types< CharT >::formatter_type
#else
function2< void, std::basic_ostream< CharT >&, basic_record< CharT > const& >
#endif
parse_formatter(const CharT* begin, const CharT* end);

/*!
 * The function parses a formatter from the string
 *
 * \param str A string that contains format description
 * \return A function object that can be used as a formatter.
 *
 * \b Throws: An <tt>std::exception</tt>-based exception, if a formatter cannot be recognized in the character sequence.
 */
template< typename CharT, typename TraitsT, typename AllocatorT >
inline typename formatter_types< CharT >::formatter_type
parse_formatter(std::basic_string< CharT, TraitsT, AllocatorT > const& str)
{
    const CharT* p = str.c_str();
    return parse_formatter(p, p + str.size());
}

/*!
 * The function parses a formatter from the string
 *
 * \pre <tt>str != NULL</tt>, <tt>str</tt> points to a zero-terminated string
 * \param str A string that contains format description.
 * \return A function object that can be used as a formatter.
 *
 * \b Throws: An <tt>std::exception</tt>-based exception, if a formatter cannot be recognized in the character sequence.
 */
template< typename CharT >
inline typename formatter_types< CharT >::formatter_type
parse_formatter(const CharT* str)
{
    return parse_formatter(str, str + std::char_traits< CharT >::length(str));
}

} // namespace log

} // namespace boost

#ifdef _MSC_VER
#pragma warning(pop)
#endif // _MSC_VER

#endif // BOOST_LOG_UTILITY_INIT_FORMATTER_PARSER_HPP_INCLUDED_


text:
#include <boost/test/unit_test.hpp>
#include "algorithms/data_structures/array/max_subarray_product.hpp"

BOOST_AUTO_TEST_SUITE(TestMaxSubarrayProduct)

BOOST_AUTO_TEST_CASE(empty_array)
{
    BOOST_CHECK(0 == Algo::DS::Array::MaxSubarrayProduct::Calc({}));
}

BOOST_AUTO_TEST_CASE(array_with_one_elem)
{
    BOOST_CHECK(0 == Algo::DS::Array::MaxSubarrayProduct::Calc({0}));
    BOOST_CHECK(1 == Algo::DS::Array::MaxSubarrayProduct::Calc({1}));
    BOOST_CHECK(-1 == Algo::DS::Array::MaxSubarrayProduct::Calc({-1}));
}

BOOST_AUTO_TEST_CASE(array_with_several_elements)
{
    BOOST_CHECK(1 == Algo::DS::Array::MaxSubarrayProduct::Calc({-2, 1}));
    BOOST_CHECK(2 == Algo::DS::Array::MaxSubarrayProduct::Calc({-2, -1}));
    BOOST_CHECK(0 == Algo::DS::Array::MaxSubarrayProduct::Calc({-2, 0, -1}));
    BOOST_CHECK(6 == Algo::DS::Array::MaxSubarrayProduct::Calc({2, 3, -2, 4}));
    BOOST_CHECK(8 == Algo::DS::Array::MaxSubarrayProduct::Calc({0, 2, 4, -1, -1}));
    BOOST_CHECK(360 == Algo::DS::Array::MaxSubarrayProduct::Calc(
            {-2, -3, 4, -1, -2, 1, 5, -3}));
    BOOST_CHECK(960 == Algo::DS::Array::MaxSubarrayProduct::Calc(
            {-2, 1, -3, 4, -1, 2, 1, -5, 4}));
    BOOST_CHECK(60 == Algo::DS::Array::MaxSubarrayProduct::Calc(
            {-1, -3, -10, 0, 60}));
}

BOOST_AUTO_TEST_SUITE_END()

max_stars_repo_name: iamantony/CppNotes

max_issues_repo_name: iamantony/CppNotes

max_forks_repo_name: iamantony/CppNotes


text:
/***************************************************************************************************************************
 * aruco_det_v2.cpp
 * Author: Jario
 * Update Time: 2020.12.11
 *
 * 说明: 单个二维码识别程序，可识别的二维码在Prometheus/Modules/object_detection/config/aruco_images文件夹中
 *      视野里只允许存在一个二维码 且二维码的字典类型要对应
 *      默认二维码的边长为0.2m
 *      1. 【订阅】图像话题 (默认来自web_cam)
 *         /prometheus/camera/rgb/image_raw
 *      2. 【发布】目标位置，发布话题见 Prometheus/Modules/msgs/msg/DetectionInfo.msg
 *         /prometheus/object_detection/aruco_det
 *      3. 【发布】检测结果的可视化图像话题
 *         /prometheus/camera/rgb/image_aruco_det
***************************************************************************************************************************/
#include <pthread.h>
#include <thread>
#include <chrono>
#include <numeric>
#include <boost/thread/mutex.hpp>
#include <boost/thread/shared_mutex.hpp>

#include <ros/ros.h>
#include <ros/package.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <yaml-cpp/yaml.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <geometry_msgs/PoseStamped.h>
#include <std_msgs/Bool.h>
#include <std_msgs/String.h>
#include <prometheus_msgs/DetectionInfo.h>
#include <prometheus_msgs/DroneState.h>
#include <prometheus_msgs/ArucoInfo.h>
#include <prometheus_msgs/IndoorSearch.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/aruco.hpp>
#include <opencv2/aruco/dictionary.hpp>
#include <opencv2/aruco/charuco.hpp>
#include <opencv2/calib3d.hpp>

#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>

// #include "message_utils.h"

using namespace std;
using namespace cv;

//【订阅】运行状态转换开关
ros::Subscriber switch_subscriber;
//【订阅】输入图像
image_transport::Subscriber image_subscriber;
//【订阅】无人机状态
ros::Subscriber drone_state_subscriber;

//【发布】检测得到的位置与姿态信息
ros::Publisher pose_pub;
//【发布】检测结果图像
image_transport::Publisher aruco_pub;
//【发布】融合DroneState的二维码全局位置
ros::Publisher indoor_search_pub;

// 使用cout打印消息
bool local_print = true;
// 无人机状态量
prometheus_msgs::DroneState _drone_state;

// 相机话题中的图像同步相关变量
int frame_width, frame_height;
std_msgs::Header image_header;
cv::Mat cam_image_copy;
boost::shared_mutex mutex_image_callback;
bool image_status = false;
boost::shared_mutex mutex_image_status;

// 运行状态
// 0: 正常检测Aruco码，输出位姿
// 1: 世界坐标系标定，标定后，检测结果被转换到世界坐标系下
// 2: 世界坐标系标定，标定后，run_state会自动变为2，在世界坐标系下输出tf位置
// 3: 使用DroneState信息进行全局位置估计
int run_state(0);

void switchCallback(const std_msgs::String::ConstPtr& msg)
{
    ROS_INFO("receiving [%s]", msg->data.c_str());
    if ("calibrate" == msg->data)
    {
        ROS_INFO("run_state = 1");
        run_state = 1;
    }
}

// 图像接收回调函数，接收web_cam的话题，并将图像保存在cam_image_copy中
void cameraCallback(const sensor_msgs::ImageConstPtr& msg)
{
    if (local_print) ROS_DEBUG("[ArucoDetector] USB image received.");

    cv_bridge::CvImagePtr cam_image;

    try {
        cam_image = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
        image_header = msg->header;
    } catch (cv_bridge::Exception& e) {
        if (local_print) ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    if (cam_image) {
        {
            boost::unique_lock<boost::shared_mutex> lockImageCallback(mutex_image_callback);
            cam_image_copy = cam_image->image.clone();
        }
        {
            boost::unique_lock<boost::shared_mutex> lockImageStatus(mutex_image_status);
            image_status = true;
        }
        frame_width = cam_image->image.size().width;
        frame_height = cam_image->image.size().height;
    }
    return;
}

void droneStateCallbackb(const prometheus_msgs::DroneState::ConstPtr& msg)
{
    _drone_state = *msg;
}

// 用此函数查看是否收到图像话题
bool getImageStatus(void)
{
    boost::shared_lock<boost::shared_mutex> lock(mutex_image_status);
    return image_status;
}

static bool readCameraParameters(string filename, Mat &camMatrix, Mat &distCoeffs) {
    FileStorage fs(filename, FileStorage::READ);
    if(!fs.isOpened())
        return false;
    fs["camera_matrix"] >> camMatrix;
    fs["distortion_coefficients"] >> distCoeffs;
    return true;
}

float _vector_stdev(std::vector<float>& x)
{
    float sum, mean, accum, stdev;
    sum = std::accumulate(std::begin(x), std::end(x), 0.0);
    mean = sum / x.size();
    accum = 0.0;
    std::for_each (std::begin(x), std::end(x), [&](const float d) {
        accum += (d - mean) * (d - mean);
    });
    stdev = sqrt(accum / (x.size() - 1));
    return stdev;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "aruco_det_v2");
    ros::NodeHandle nh("~");
    image_transport::ImageTransport it(nh);
    // 更新频率为60HZ
    ros::Rate loop_rate(60);

    std::string camera_topic = "/prometheus/camera/rgb/image_raw";
    std::string camera_params_yaml;
    std::string output_topic = "/prometheus/camera/rgb/image_aruco_det";

    int dictionaryId(2);
    float targetMarkerLength(0.0207);
    float calibMarkerLength(0.0207);
    float calibSquareLength(0.0345);  // Square side length (in meters)

    if (nh.getParam("camera_topic", camera_topic)) {
        if (local_print) ROS_INFO("camera_topic is %s", camera_topic.c_str());
    } else {
        if (local_print) ROS_WARN("didn't find parameter camera_topic");
    }
    if (nh.getParam("camera_parameters", camera_params_yaml)) {
        if (local_print) ROS_INFO("camera_parameters is %s", camera_params_yaml.c_str());
    } else {
        if (local_print) ROS_WARN("didn't find camera_parameters");
    }
    if (nh.getParam("output_topic", output_topic)) {
        if (local_print) ROS_INFO("output_topic is %s", output_topic.c_str());
    } else {
        if (local_print) ROS_WARN("didn't find parameter output_topic");
    }

    if (nh.getParam("dictionary_type", dictionaryId)) {
        if (local_print) ROS_INFO("dictionary_type is %d", dictionaryId);
    } else {
        if (local_print) ROS_WARN("didn't find parameter dictionary_type");
    }
    if (nh.getParam("target_marker_length", targetMarkerLength)) {
        if (local_print) ROS_INFO("target_marker_length is %f", targetMarkerLength);
    } else {
        if (local_print) ROS_WARN("didn't find parameter target_marker_length");
    }
    if (nh.getParam("calib_marker_length", calibMarkerLength)) {
        if (local_print) ROS_INFO("calib_marker_length is %f", calibMarkerLength);
    } else {
        if (local_print) ROS_WARN("didn't find parameter calib_marker_length");
    }
    if (nh.getParam("calib_square_length", calibSquareLength)) {
        if (local_print) ROS_INFO("calib_square_length is %f", calibSquareLength);
    } else {
        if (local_print) ROS_WARN("didn't find parameter calib_square_length");
    }
    int run_state_init;
    if (nh.getParam("run_state", run_state_init)) {
        if (local_print) ROS_INFO("run_state is %d", run_state_init);
        run_state = run_state_init;
    } else {
        if (local_print) ROS_WARN("didn't find parameter run_state");
    }

    //【订阅】运行状态转换开关
    switch_subscriber = nh.subscribe("/prometheus/object_detection/aruco_navigation_switch", 1, switchCallback);
    //【订阅】输入图像
    image_subscriber = it.subscribe(camera_topic.c_str(), 1, cameraCallback);
    //【订阅】无人接自身位置信息
    drone_state_subscriber = nh.subscribe<prometheus_msgs::DroneState>("/prometheus/drone_state", 1, droneStateCallbackb);

    //【发布】检测得到的位置与姿态信息
    pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/prometheus/object_detection/aruco_det_v2", 1);
    //【发布】检测结果图像
    aruco_pub = it.advertise(output_topic.c_str(), 1);

    if (3 == run_state){
        //【发布】融合DroneState的二维码全局位置
        indoor_search_pub = nh.advertise<prometheus_msgs::IndoorSearch>("/prometheus/indoor_search/detection_result", 1);
    }

    std::string ros_path = ros::package::getPath("prometheus_detection");
    if (local_print) ROS_INFO("DETECTION_PATH: %s", ros_path.c_str());

    cv::Mat camMatrix, distCoeffs;
    bool readOk = readCameraParameters(camera_params_yaml.c_str(), camMatrix, distCoeffs);
    if (!readOk) {
        cerr << "Invalid camera file" << endl;
        return 0;
    }

    if (local_print) {
        cout << "[camMatrix]:" << endl;
        cout << camMatrix << endl;
        cout << "[distCoeffs]:" << endl;
        cout << distCoeffs << endl;
    }

    // <pose>0 0 -0.1 0 1.5707963 0</pose>
    Vec3d cam2drn_tvecs, cam2drn_rvecs1, cam2drn_rvecs2;
    cv::Mat cam2drn_rmat1, cam2drn_rmat2;
    Eigen::Matrix3d cam2drn_rmat_eigen;
    Eigen::Quaterniond cam2drn_q;

    prometheus_msgs::IndoorSearch _indoor_search_msg;
    vector< vector< Vec3d > > aruco_pos(9);

    if (3 == run_state)
    {
        cam2drn_tvecs[0] = 0.;
        cam2drn_tvecs[1] = 0.;
        cam2drn_tvecs[2] = -0.1;
        cam2drn_rvecs1[0] = 1.5707963 * 2.;
        cam2drn_rvecs1[1] = 0.;
        cam2drn_rvecs1[2] = 0.;

        cv::Rodrigues(cam2drn_rvecs1, cam2drn_rmat1);
        cam2drn_rvecs2[0] = 0.;
        cam2drn_rvecs2[1] = 0.;
        cam2drn_rvecs2[2] = -1.5707963;
        cv::Rodrigues(cam2drn_rvecs2, cam2drn_rmat2);

        cv::cv2eigen(cam2drn_rmat2 * cam2drn_rmat1, cam2drn_rmat_eigen);
        cam2drn_q = Eigen::Quaterniond(cam2drn_rmat_eigen);
        cam2drn_q.normalize();

        vector< prometheus_msgs::ArucoInfo > aruco_infos;
        for (int i=0; i<9; i++)
        {
            prometheus_msgs::ArucoInfo aruco_info;
            aruco_info.aruco_num = i + 1;
            aruco_info.detected = false;
            aruco_info.position[0] = 0;
            aruco_info.position[1] = 0;
            aruco_info.position[2] = 0;
            aruco_infos.push_back(aruco_info);
        }
        _indoor_search_msg.Aruco1 = aruco_infos[0];
        _indoor_search_msg.Aruco2 = aruco_infos[1];
        _indoor_search_msg.Aruco3 = aruco_infos[2];
        _indoor_search_msg.Aruco4 = aruco_infos[3];
        _indoor_search_msg.Aruco5 = aruco_infos[4];
        _indoor_search_msg.Aruco6 = aruco_infos[5];
        _indoor_search_msg.Aruco7 = aruco_infos[6];
        _indoor_search_msg.Aruco8 = aruco_infos[7];
        _indoor_search_msg.Aruco9 = aruco_infos[8];
    }

    Ptr<aruco::Dictionary> dictionary =
        aruco::getPredefinedDictionary(aruco::PREDEFINED_DICTIONARY_NAME(dictionaryId));
    Ptr<aruco::DetectorParameters> detectorParams = aruco::DetectorParameters::create();

    std::vector<float> collected_mtx, collected_mty, collected_mtz;
    std::vector<float> collected_mqx, collected_mqy, collected_mqz, collected_mqw;
    float mtx_calib, mty_calib, mtz_calib, mqx_calib, mqy_calib, mqz_calib, mqw_calib;

    cv::Mat frame, frameCopy;
    const auto wait_duration = std::chrono::milliseconds(1000);
    while (ros::ok())
	{
        while (!getImageStatus() && ros::ok())
        {
            if (local_print) cout << "Waiting for image." << endl;
            std::this_thread::sleep_for(wait_duration);
            ros::spinOnce();
        }

        {
            boost::unique_lock<boost::shared_mutex> lockImageCallback(mutex_image_callback);
            frame = cam_image_copy.clone();
        }

        if (!frame.empty())
        {
            vector< int > ids;
            vector< vector< Point2f > > corners, rejected;
            vector< Vec3d > rvecs, tvecs;

            float markerLength = targetMarkerLength;
            float squareLength = calibSquareLength;
            if (0 == run_state || 1 == run_state)
                markerLength = calibMarkerLength;
            if (2 == run_state)
                markerLength = targetMarkerLength;
            if (3 == run_state)
                markerLength = targetMarkerLength;

            // detect markers and estimate pose
            aruco::detectMarkers(frame, dictionary, corners, ids, detectorParams, rejected);
            if (ids.size() > 0)
                aruco::estimatePoseSingleMarkers(corners, markerLength, camMatrix, distCoeffs, rvecs, tvecs);

            frame.copyTo(frameCopy);
            if(ids.size() > 0) {
                aruco::drawDetectedMarkers(frameCopy, corners, ids);

                std::vector<float> collected_tx, collected_ty, collected_tz;
                std::vector<float> collected_qx, collected_qy, collected_qz, collected_qw;

                for(unsigned int i = 0; i < ids.size(); i++) {
                    aruco::drawAxis(frameCopy, camMatrix, distCoeffs, rvecs[i], tvecs[i], markerLength * 0.5f);
                    cv::Mat rotation_matrix;
                    cv::Rodrigues(rvecs[i], rotation_matrix);
                    Eigen::Matrix3d rotation_matrix_eigen;
                    cv::cv2eigen(rotation_matrix, rotation_matrix_eigen);
                    Eigen::Quaterniond q = Eigen::Quaterniond(rotation_matrix_eigen);
                    q.normalize();

                    geometry_msgs::PoseStamped pose;
                    pose.header.frame_id = "camera";
                    pose.pose.position.x = tvecs[i][0];
                    pose.pose.position.y = tvecs[i][1];
                    pose.pose.position.z = tvecs[i][2];
                    pose.pose.orientation.x = q.x();
                    pose.pose.orientation.y = q.y();
                    pose.pose.orientation.z = q.z();
                    pose.pose.orientation.w = q.w();
                    pose_pub.publish(pose);

                    if (3 == run_state) // 使用DroneState信息进行全局位置估计
                    {
                        static tf::TransformBroadcaster br;
                        tf::Transform aruco2camera = tf::Transform(tf::Quaternion(q.x(), q.y(), q.z(), q.w()), tf::Vector3(tvecs[i][0], tvecs[i][1], tvecs[i][2]));
                        char obj_str[16];
                        sprintf(obj_str, "aruco-%d", ids[i]);
                        tf::StampedTransform trans_aruco2camera = tf::StampedTransform(aruco2camera, ros::Time(pose.header.stamp), "camera", obj_str);
                        // br.sendTransform(trans_aruco2camera);

                        tf::Transform camera2drone = tf::Transform(
                            tf::Quaternion(cam2drn_q.x(), cam2drn_q.y(), cam2drn_q.z(), cam2drn_q.w()),
                            tf::Vector3(cam2drn_tvecs[0], cam2drn_tvecs[1], cam2drn_tvecs[2])
                            );
                        tf::StampedTransform trans_camera2drone = tf::StampedTransform(camera2drone, ros::Time(pose.header.stamp), "drone", "camera");
                        // br.sendTransform(trans_camera2drone);

                        tf::Transform drone2world = tf::Transform(
                            tf::Quaternion(_drone_state.attitude_q.x, _drone_state.attitude_q.y, _drone_state.attitude_q.z, _drone_state.attitude_q.w),
                            tf::Vector3(_drone_state.position[0], _drone_state.position[1], _drone_state.position[2])
                            );
                        tf::StampedTransform trans_drone2world = tf::StampedTransform(drone2world, ros::Time(pose.header.stamp), "world", "drone");
                        // br.sendTransform(trans_drone2world);

                        tf::Transform aruco2world;
                        aruco2world = drone2world * camera2drone * aruco2camera;
                        tf::StampedTransform trans_aruco2world = tf::StampedTransform(aruco2world, ros::Time(pose.header.stamp), "world", obj_str);
                        br.sendTransform(trans_aruco2world);

                        vector< float > collected_ax, collected_ay, collected_az;
                        int collected_id;
                        if (16 == ids[i])
                        {
                            collected_id = 1;
                        }
                        else if (15 == ids[i])
                        {
                            collected_id = 2;
                        }
                        else if (14 == ids[i])
                        {
                            collected_id = 3;
                        }
                        else if (13 == ids[i])
                        {
                            collected_id = 4;
                        }
                        else if (12 == ids[i])
                        {
                            collected_id = 5;
                        }
                        else if (11 == ids[i])
                        {
                            collected_id = 6;
                        }
                        else if (10 == ids[i])
                        {
                            collected_id = 7;
                        }
                        else if (9 == ids[i])
                        {
                            collected_id = 8;
                        }
                        else if (8 == ids[i])
                        {
                            collected_id = 9;
                        }

                        aruco_pos[collected_id - 1].push_back(Vec3d(aruco2world.getOrigin().x(), aruco2world.getOrigin().y(), aruco2world.getOrigin().z()));
                        for (Vec3d v : aruco_pos[collected_id - 1])
                        {
                            collected_ax.push_back(v[0]);
                            collected_ay.push_back(v[1]);
                            collected_az.push_back(v[2]);
                        }

                        if (collected_ax.size() > 0)
                        {
                            prometheus_msgs::ArucoInfo aruco_info;
                            aruco_info.aruco_num = collected_id;
                            aruco_info.detected = true;
                            float ax_sum = std::accumulate(std::begin(collected_ax), std::end(collected_ax), 0.0);
                            float ax_mean =  ax_sum / collected_ax.size();
                            float ay_sum = std::accumulate(std::begin(collected_ay), std::end(collected_ay), 0.0);
                            float ay_mean =  ay_sum / collected_ay.size();
                            float az_sum = std::accumulate(std::begin(collected_az), std::end(collected_az), 0.0);
                            float az_mean =  az_sum / collected_az.size();
                            aruco_info.position[0] = ax_mean;
                            aruco_info.position[1] = ay_mean;
                            aruco_info.position[2] = az_mean;
                            if (1 == collected_id)
                            {
                                _indoor_search_msg.Aruco1 = aruco_info;
                            }
                            else if (2 == collected_id)
                            {
                                _indoor_search_msg.Aruco2 = aruco_info;
                            }
                            else if (3 == collected_id)
                            {
                                _indoor_search_msg.Aruco3 = aruco_info;
                            }
                            else if (4 == collected_id)
                            {
                                _indoor_search_msg.Aruco4 = aruco_info;
                            }
                            else if (5 == collected_id)
                            {
                                _indoor_search_msg.Aruco5 = aruco_info;
                            }
                            else if (6 == collected_id)
                            {
                                _indoor_search_msg.Aruco6 = aruco_info;
                            }
                            else if (7 == collected_id)
                            {
                                _indoor_search_msg.Aruco7 = aruco_info;
                            }
                            else if (8 == collected_id)
                            {
                                _indoor_search_msg.Aruco8 = aruco_info;
                            }
                            else if (9 == collected_id)
                            {
                                _indoor_search_msg.Aruco9 = aruco_info;
                            }
                        }
                    }

                    if (2 == run_state)
                    {
                        static tf::TransformBroadcaster br;
                        tf::Transform world2camera = tf::Transform(tf::Quaternion(q.x(), q.y(), q.z(), q.w()), tf::Vector3(tvecs[i][0], tvecs[i][1], tvecs[i][2]));
                        char obj_str[16];
                        sprintf(obj_str, "object-%d", ids[i]);
                        tf::StampedTransform trans_world2camera = tf::StampedTransform(world2camera, ros::Time(pose.header.stamp), "camera", obj_str);
                        br.sendTransform(trans_world2camera);
                    }

                    if (1 == run_state)
                    {
                        if (ids[i] >= 0 && ids[i] <= 16)
                        {
                            std::vector<double> vec_t{tvecs[i][0], tvecs[i][1], tvecs[i][2]};
	                        cv::Mat vec_t_mat{vec_t};
                            vec_t_mat = vec_t_mat;
                            vec_t_mat.convertTo(vec_t_mat, CV_32FC1);
                            // cout << "vec_t_mat.size():" << vec_t_mat.size() << endl;
                            // cout << "vec_t_mat.type():" << vec_t_mat.type() <<endl;
                            std::vector<double> id_to8_t(3);
                            if (ids[i] == 0)
                            {
                                id_to8_t[0] = squareLength;
                                id_to8_t[1] = -squareLength*3;
                                id_to8_t[2] = 0.;
                            }
                            else if (ids[i] == 1)
                            {
                                id_to8_t[0] = -squareLength;
                                id_to8_t[1] = -squareLength*3;
                                id_to8_t[2] = 0.;
                            }
                            else if (ids[i] == 2)
                            {
                                id_to8_t[0] = squareLength*2;
                                id_to8_t[1] = -squareLength*2;
                                id_to8_t[2] = 0.;
                            }
                            else if (ids[i] == 3)
                            {
                                id_to8_t[0] = 0.;
                                id_to8_t[1] = -squareLength*2;
                                id_to8_t[2] = 0.;
                            }
                            else if (ids[i] == 4)
                            {
                                id_to8_t[0] = -squareLength*2;
                                id_to8_t[1] = -squareLength*2;
                                id_to8_t[2] = 0.;
                            }
                            else if (ids[i] == 5)
                            {
                                id_to8_t[0] = squareLength;
                                id_to8_t[1] = -squareLength;
                                id_to8_t[2] = 0.;
                            }
                            else if (ids[i] == 6)
                            {
                                id_to8_t[0] = -squareLength;
                                id_to8_t[1] = -squareLength;
                                id_to8_t[2] = 0.;
                            }
                            else if (ids[i] == 7)
                            {
                                id_to8_t[0] = squareLength*2;
                                id_to8_t[1] = 0.;
                                id_to8_t[2] = 0.;
                            }
                            else if (ids[i] == 8)
                            {
                                collected_tx.push_back(tvecs[i][0]);
                                collected_ty.push_back(tvecs[i][1]);
                                collected_tz.push_back(tvecs[i][2]);

                                collected_qx.push_back(q.x());
                                collected_qy.push_back(q.y());
                                collected_qz.push_back(q.z());
                                collected_qw.push_back(q.w());
                                continue;
                            }
                            else if (ids[i] == 9)
                            {
                                id_to8_t[0] = -squareLength*2;
                                id_to8_t[1] = 0.;
                                id_to8_t[2] = 0.;
                            }
                            else if (ids[i] == 10)
                            {
                                id_to8_t[0] = squareLength;
                                id_to8_t[1] = squareLength;
                                id_to8_t[2] = 0.;
                            }
                            else if (ids[i] == 11)
                            {
                                id_to8_t[0] = -squareLength;
                                id_to8_t[1] = squareLength;
                                id_to8_t[2] = 0.;
                            }
                            else if (ids[i] == 12)
                            {
                                id_to8_t[0] = squareLength*2;
                                id_to8_t[1] = squareLength*2;
                                id_to8_t[2] = 0.;
                            }
                            else if (ids[i] == 13)
                            {
                                id_to8_t[0] = 0.;
                                id_to8_t[1] = squareLength*2;
                                id_to8_t[2] = 0.;
                            }
                            else if (ids[i] == 14)
                            {
                                id_to8_t[0] = -squareLength*2;
                                id_to8_t[1] = squareLength*2;
                                id_to8_t[2] = 0.;
                            }
                            else if (ids[i] == 15)
                            {
                                id_to8_t[0] = squareLength;
                                id_to8_t[1] = squareLength*3;
                                id_to8_t[2] = 0.;
                            }
                            else if (ids[i] == 16)
                            {
                                id_to8_t[0] = -squareLength;
                                id_to8_t[1] = squareLength*3;
                                id_to8_t[2] = 0.;
                            }
                            cv::Mat id_to8_t_mat{id_to8_t};
                            id_to8_t_mat.convertTo(id_to8_t_mat, CV_32FC1);

                            rotation_matrix.convertTo(rotation_matrix, CV_32FC1);
                            // cv::invert(rotation_matrix, rotation_matrix);
                            cv::Mat id_8_t = rotation_matrix * id_to8_t_mat + vec_t_mat;

                            collected_tx.push_back(id_8_t.at<float>(0));
                            collected_ty.push_back(id_8_t.at<float>(1));
                            collected_tz.push_back(id_8_t.at<float>(2));

                            collected_qx.push_back(q.x());
                            collected_qy.push_back(q.y());
                            collected_qz.push_back(q.z());
                            collected_qw.push_back(q.w());

                            // static tf::TransformBroadcaster br;
                            // tf::Transform world2camera = tf::Transform(tf::Quaternion(q.x(), q.y(), q.z(), q.w()),
                            //     tf::Vector3(id_8_t.at<float>(0), id_8_t.at<float>(1), id_8_t.at<float>(2)));
                            // char obj_str[16];
                            // sprintf(obj_str, "object-%d-8", ids[i]);
                            // tf::StampedTransform trans_world2camera = tf::StampedTransform(world2camera, ros::Time(pose.header.stamp), "camera", obj_str);
                            // br.sendTransform(trans_world2camera);
                        }
                    }
                }

                if (1 == run_state && collected_tx.size() > 8)
                {
                    float tx_sum = std::accumulate(std::begin(collected_tx), std::end(collected_tx), 0.0);
                    float tx_mean =  tx_sum / collected_tx.size();
                    float ty_sum = std::accumulate(std::begin(collected_ty), std::end(collected_ty), 0.0);
                    float ty_mean =  ty_sum / collected_ty.size();
                    float tz_sum = std::accumulate(std::begin(collected_tz), std::end(collected_tz), 0.0);
                    float tz_mean =  tz_sum / collected_tz.size();

                    float qx_sum = std::accumulate(std::begin(collected_qx), std::end(collected_qx), 0.0);
                    float qx_mean =  qx_sum / collected_qx.size();
                    float qy_sum = std::accumulate(std::begin(collected_qy), std::end(collected_qy), 0.0);
                    float qy_mean =  qy_sum / collected_qy.size();
                    float qz_sum = std::accumulate(std::begin(collected_qz), std::end(collected_qz), 0.0);
                    float qz_mean =  qz_sum / collected_qz.size();
                    float qw_sum = std::accumulate(std::begin(collected_qw), std::end(collected_qw), 0.0);
                    float qw_mean =  qw_sum / collected_qw.size();

                    static tf::TransformBroadcaster br;
                    tf::Transform world2camera = tf::Transform(tf::Quaternion(qx_mean, qy_mean, qz_mean, qw_mean), tf::Vector3(tx_mean, ty_mean, tz_mean));
                    tf::StampedTransform trans_world2camera = tf::StampedTransform(world2camera, ros::Time(), "camera", "calib-MEAN");
                    br.sendTransform(trans_world2camera);

                    collected_mtx.push_back(tx_mean);
                    collected_mty.push_back(ty_mean);
                    collected_mtz.push_back(tz_mean);
                    collected_mqx.push_back(qx_mean);
                    collected_mqy.push_back(qy_mean);
                    collected_mqz.push_back(qz_mean);
                    collected_mqw.push_back(qw_mean);
                }
            }

            if (1 == run_state && collected_mtx.size() >= 10)
            {

                float mtx_std = _vector_stdev(collected_mtx);
                float mty_std = _vector_stdev(collected_mty);
                float mtz_std = _vector_stdev(collected_mtz);

                float mqx_std = _vector_stdev(collected_mqx);
                float mqy_std = _vector_stdev(collected_mqy);
                float mqz_std = _vector_stdev(collected_mqz);
                float mqw_std = _vector_stdev(collected_mqw);

                // cout<<mtx_std<<", "<<mty_std<<", "<<mtz_std<<", "<<mqx_std<<", "<<mqy_std<<", "<<mqz_std<<", "<<mqw_std<<endl;
                if (mtx_std < 0.01 && mty_std < 0.01 && mtz_std < 0.01 && mqx_std < 0.01 && mqy_std < 0.01 && mqz_std < 0.01 && mqw_std < 0.01)
                {
                    ROS_INFO("Calibration completed!");
                    run_state = 2;

                    mtx_calib = std::accumulate(std::begin(collected_mtx), std::end(collected_mtx), 0.0) / collected_mtx.size();
                    mty_calib = std::accumulate(std::begin(collected_mty), std::end(collected_mty), 0.0) / collected_mty.size();
                    mtz_calib = std::accumulate(std::begin(collected_mtz), std::end(collected_mtz), 0.0) / collected_mtz.size();
                    mqx_calib = std::accumulate(std::begin(collected_mqx), std::end(collected_mqx), 0.0) / collected_mqx.size();
                    mqy_calib = std::accumulate(std::begin(collected_mqy), std::end(collected_mqy), 0.0) / collected_mqy.size();
                    mqz_calib = std::accumulate(std::begin(collected_mqz), std::end(collected_mqz), 0.0) / collected_mqz.size();
                    mqw_calib = std::accumulate(std::begin(collected_mqw), std::end(collected_mqw), 0.0) / collected_mqw.size();
                }

                collected_mtx.clear();
                collected_mty.clear();
                collected_mtz.clear();

                collected_mqx.clear();
                collected_mqy.clear();
                collected_mqz.clear();
                collected_mqw.clear();
            }

            if (2 == run_state)
            {
                static tf::TransformBroadcaster br;
                tf::Transform world2camera = tf::Transform(tf::Quaternion(mqx_calib, mqy_calib, mqz_calib, mqw_calib), tf::Vector3(mtx_calib, mty_calib, mtz_calib));
                tf::StampedTransform trans_world2camera = tf::StampedTransform(world2camera, ros::Time(), "camera", "map");
                br.sendTransform(trans_world2camera);

            }
            if (3 == run_state)
            {
                indoor_search_pub.publish(_indoor_search_msg);
            }

            sensor_msgs::ImagePtr det_output_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frameCopy).toImageMsg();
            aruco_pub.publish(det_output_msg);
            // cv::imshow("frame", frame);
            // cv::waitKey(10);
        }

        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}