<commit_before>//
// Copyright (c) 2015-2016 CNRS
//
// This file is part of Pinocchio
// Pinocchio 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
// 3 of the License, or (at your option) any later version.
//
// Pinocchio 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
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.

#ifndef __se3_se3_hpp__
#define __se3_se3_hpp__

#include <Eigen/Geometry>
#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/spatial/skew.hpp"

namespace se3
{

  /* Type returned by the "se3Action" and "se3ActionInverse" functions. */
  namespace internal
  {
    template<typename D>
    struct ActionReturn    { typedef D Type; };
  }

  /** The rigid transform aMb can be seen in two ways:
   *
   * - given a point p expressed in frame B by its coordinate vector Bp, aMb
   * computes its coordinates in frame A by Ap = aMb Bp.
   * - aMb displaces a solid S centered at frame A into the solid centered in
   * B. In particular, the origin of A is displaced at the origin of B: $^aM_b
   * ^aA = ^aB$.

   * The rigid displacement is stored as a rotation matrix and translation vector by:
   * aMb (x) =  aRb*x + aAB
   * where aAB is the vector from origin A to origin B expressed in coordinates A.
   */
  template< class Derived>
  class SE3Base
  {
  protected:

    typedef Derived  Derived_t;
    SPATIAL_TYPEDEF_TEMPLATE(Derived_t);

  public:
      Derived_t & derived() { return *static_cast<Derived_t*>(this); }
      const Derived_t& derived() const { return *static_cast<const Derived_t*>(this); }

      const Angular_t & rotation() const  { return derived().rotation_impl(); }
      const Linear_t & translation() const  { return derived().translation_impl(); }
      Angular_t & rotation()  { return derived().rotation_impl(); }
      Linear_t & translation()   { return derived().translation_impl(); }
      void rotation(const Angular_t & R) { derived().rotation_impl(R); }
      void translation(const Linear_t & R) { derived().translation_impl(R); }

      Matrix4 toHomogeneousMatrix() const
      {
        return derived().toHomogeneousMatrix_impl();
      }
      operator Matrix4() const { return toHomogeneousMatrix(); }

      Matrix6 toActionMatrix() const
      {
        return derived().toActionMatrix_impl();
      }
      operator Matrix6() const { return toActionMatrix(); }

      void disp(std::ostream & os) const
      {
        static_cast<const Derived_t*>(this)->disp_impl(os);
      }

      Derived_t operator*(const Derived_t & m2) const    { return derived().__mult__(m2); }

      /// ay = aXb.act(by)
      template<typename D>
      typename internal::ActionReturn<D>::Type act   (const D & d) const
      {
        return derived().act_impl(d);
      }

      /// by = aXb.actInv(ay)
      template<typename D> typename internal::ActionReturn<D>::Type actInv(const D & d) const
      {
        return derived().actInv_impl(d);
      }

      Derived_t act   (const Derived_t& m2) const { return derived().act_impl(m2); }
      Derived_t actInv(const Derived_t& m2) const { return derived().actInv_impl(m2); }

      bool operator == (const Derived_t & other) const
      {
        return derived().__equal__(other);
      }

      bool isApprox (const Derived_t & other, const Scalar_t & prec = Eigen::NumTraits<Scalar_t>::dummy_precision()) const
      {
        return derived().isApprox_impl(other, prec);
      }

      friend std::ostream & operator << (std::ostream & os,const SE3Base<Derived> & X)
      {
        X.disp(os);
        return os;
      }

  }; // class SE3Base

  template<typename T, int U>
  struct traits< SE3Tpl<T, U> >
  {
    typedef T Scalar_t;
    typedef Eigen::Matrix<T,3,1,U> Vector3;
    typedef Eigen::Matrix<T,4,1,U> Vector4;
    typedef Eigen::Matrix<T,6,1,U> Vector6;
    typedef Eigen::Matrix<T,3,3,U> Matrix3;
    typedef Eigen::Matrix<T,4,4,U> Matrix4;
    typedef Eigen::Matrix<T,6,6,U> Matrix6;
    typedef Matrix3 Angular_t;
    typedef Vector3 Linear_t;
    typedef Matrix6 ActionMatrix_t;
    typedef Eigen::Quaternion<T,U> Quaternion_t;
    typedef SE3Tpl<T,U> SE3;
    typedef ForceTpl<T,U> Force;
    typedef MotionTpl<T,U> Motion;
    typedef Symmetric3Tpl<T,U> Symmetric3;
    enum {
      LINEAR = 0,
      ANGULAR = 3
    };
  }; // traits SE3Tpl

  template<typename _Scalar, int _Options>
  class SE3Tpl : public SE3Base< SE3Tpl< _Scalar, _Options > >
  {

  public:
    friend class SE3Base< SE3Tpl< _Scalar, _Options > >;
    SPATIAL_TYPEDEF_TEMPLATE(SE3Tpl);
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    SE3Tpl(): rot(), trans() {};

    template<typename M3,typename v3>
    SE3Tpl(const Eigen::MatrixBase<M3> & R, const Eigen::MatrixBase<v3> & p)
    : rot(R), trans(p)
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(v3,3)
      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M3,3,3)
    }

    template<typename M4>
    SE3Tpl(const Eigen::MatrixBase<M4> & m)
    : rot(m.template block<3,3>(LINEAR,LINEAR)), trans(m.template block<3,1>(LINEAR,ANGULAR))
    {
      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M4,4,4);
    }

    SE3Tpl(int) : rot(Matrix3::Identity()), trans(Vector3::Zero()) {}

    template<typename S2, int O2>
    SE3Tpl( const SE3Tpl<S2,O2> & clone )
    : rot(clone.rotation()),trans(clone.translation()) {}

    template<typename S2, int O2>
    SE3Tpl & operator= (const SE3Tpl<S2,O2> & other)
    {
      rot = other.rotation ();
      trans = other.translation ();
      return *this;
    }

    static SE3Tpl Identity()
    {
      return SE3Tpl(1);
    }

    SE3Tpl & setIdentity () { rot.setIdentity (); trans.setZero (); return *this;}

    /// aXb = bXa.inverse()
    SE3Tpl inverse() const
    {
      return SE3Tpl(rot.transpose(), -rot.transpose()*trans);
    }

    static SE3Tpl Random()
    {
      Quaternion_t q(Vector4::Random());
      q.normalize();
      return SE3Tpl(q.matrix(),Vector3::Random());
    }

    SE3Tpl & setRandom ()
    {
      Quaternion_t q(Vector4::Random());
      q.normalize ();
      rot = q.matrix ();
      trans.setRandom ();

      return *this;
    }

  public:
    Matrix4 toHomogeneousMatrix_impl() const
    {
      Matrix4 M;
      M.template block<3,3>(LINEAR,LINEAR) = rot;
      M.template block<3,1>(LINEAR,ANGULAR) = trans;
      M.template block<1,3>(ANGULAR,LINEAR).setZero();
      M(3,3) = 1;
      return M;
    }

    /// Vb.toVector() = bXa.toMatrix() * Va.toVector()
    Matrix6 toActionMatrix_impl() const
    {
      typedef Eigen::Block<Matrix6,3,3> Block3;
      Matrix6 M;
      M.template block<3,3>(ANGULAR,ANGULAR)
      = M.template block<3,3>(LINEAR,LINEAR) = rot;
      M.template block<3,3>(ANGULAR,LINEAR).setZero();
      Block3 B = M.template block<3,3>(LINEAR,ANGULAR);

      B.col(0) = trans.cross(rot.col(0));
      B.col(1) = trans.cross(rot.col(1));
      B.col(2) = trans.cross(rot.col(2));
      return M;
    }

    void disp_impl(std::ostream & os) const
    {
      os << "  R =\n" << rot << std::endl
      << "  p = " << trans.transpose() << std::endl;
    }

    /// --- GROUP ACTIONS ON M6, F6 and I6 ---

    /// ay = aXb.act(by)
    template<typename D>
    typename internal::ActionReturn<D>::Type act_impl   (const D & d) const
    {
      return d.se3Action(*this);
    }
    /// by = aXb.actInv(ay)
    template<typename D> typename internal::ActionReturn<D>::Type actInv_impl(const D & d) const
    {
      return d.se3ActionInverse(*this);
    }

    Vector3 act_impl   (const Vector3& p) const { return (rot*p+trans).eval(); }
    Vector3 actInv_impl(const Vector3& p) const { return (rot.transpose()*(p-trans)).eval(); }

    SE3Tpl act_impl    (const SE3Tpl& m2) const { return SE3Tpl( rot*m2.rot,trans+rot*m2.trans);}
    SE3Tpl actInv_impl (const SE3Tpl& m2) const { return SE3Tpl( rot.transpose()*m2.rot, rot.transpose()*(m2.trans-trans));}

    SE3Tpl __mult__(const SE3Tpl & m2) const { return this->act(m2);}

    bool __equal__( const SE3Tpl & m2 ) const
    {
      return (rotation_impl() == m2.rotation() && translation_impl() == m2.translation());
    }

    bool isApprox_impl (const SE3Tpl & m2, const Scalar_t & prec = Eigen::NumTraits<Scalar_t>::dummy_precision()) const
    {
      return rot.isApprox(m2.rot, prec) && trans.isApprox(m2.trans, prec);
    }

  public:
    const Angular_t & rotation_impl() const { return rot; }
    Angular_t & rotation_impl() { return rot; }
    void rotation_impl(const Angular_t & R) { rot = R; }
    const Linear_t & translation_impl() const { return trans;}
    Linear_t & translation_impl() { return trans;}
    void translation_impl(const Linear_t & p) { trans=p; }

  protected:
    Angular_t rot;
    Linear_t trans;

  }; // class SE3Tpl

  typedef SE3Tpl<double,0> SE3;

} // namespace se3

#endif // ifndef __se3_se3_hpp__

<commit_msg>[C++] Remove useless eval operator<commit_after>//
// Copyright (c) 2015-2016 CNRS
//
// This file is part of Pinocchio
// Pinocchio 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
// 3 of the License, or (at your option) any later version.
//
// Pinocchio 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
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.

#ifndef __se3_se3_hpp__
#define __se3_se3_hpp__

#include <Eigen/Geometry>
#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/spatial/skew.hpp"

namespace se3
{

  /* Type returned by the "se3Action" and "se3ActionInverse" functions. */
  namespace internal
  {
    template<typename D>
    struct ActionReturn    { typedef D Type; };
  }

  /** The rigid transform aMb can be seen in two ways:
   *
   * - given a point p expressed in frame B by its coordinate vector Bp, aMb
   * computes its coordinates in frame A by Ap = aMb Bp.
   * - aMb displaces a solid S centered at frame A into the solid centered in
   * B. In particular, the origin of A is displaced at the origin of B: $^aM_b
   * ^aA = ^aB$.

   * The rigid displacement is stored as a rotation matrix and translation vector by:
   * aMb (x) =  aRb*x + aAB
   * where aAB is the vector from origin A to origin B expressed in coordinates A.
   */
  template< class Derived>
  class SE3Base
  {
  protected:

    typedef Derived  Derived_t;
    SPATIAL_TYPEDEF_TEMPLATE(Derived_t);

  public:
      Derived_t & derived() { return *static_cast<Derived_t*>(this); }
      const Derived_t& derived() const { return *static_cast<const Derived_t*>(this); }

      const Angular_t & rotation() const  { return derived().rotation_impl(); }
      const Linear_t & translation() const  { return derived().translation_impl(); }
      Angular_t & rotation()  { return derived().rotation_impl(); }
      Linear_t & translation()   { return derived().translation_impl(); }
      void rotation(const Angular_t & R) { derived().rotation_impl(R); }
      void translation(const Linear_t & R) { derived().translation_impl(R); }

      Matrix4 toHomogeneousMatrix() const
      {
        return derived().toHomogeneousMatrix_impl();
      }
      operator Matrix4() const { return toHomogeneousMatrix(); }

      Matrix6 toActionMatrix() const
      {
        return derived().toActionMatrix_impl();
      }
      operator Matrix6() const { return toActionMatrix(); }

      void disp(std::ostream & os) const
      {
        static_cast<const Derived_t*>(this)->disp_impl(os);
      }

      Derived_t operator*(const Derived_t & m2) const    { return derived().__mult__(m2); }

      /// ay = aXb.act(by)
      template<typename D>
      typename internal::ActionReturn<D>::Type act   (const D & d) const
      {
        return derived().act_impl(d);
      }

      /// by = aXb.actInv(ay)
      template<typename D> typename internal::ActionReturn<D>::Type actInv(const D & d) const
      {
        return derived().actInv_impl(d);
      }

      Derived_t act   (const Derived_t& m2) const { return derived().act_impl(m2); }
      Derived_t actInv(const Derived_t& m2) const { return derived().actInv_impl(m2); }

      bool operator == (const Derived_t & other) const
      {
        return derived().__equal__(other);
      }

      bool isApprox (const Derived_t & other, const Scalar_t & prec = Eigen::NumTraits<Scalar_t>::dummy_precision()) const
      {
        return derived().isApprox_impl(other, prec);
      }

      friend std::ostream & operator << (std::ostream & os,const SE3Base<Derived> & X)
      {
        X.disp(os);
        return os;
      }

  }; // class SE3Base

  template<typename T, int U>
  struct traits< SE3Tpl<T, U> >
  {
    typedef T Scalar_t;
    typedef Eigen::Matrix<T,3,1,U> Vector3;
    typedef Eigen::Matrix<T,4,1,U> Vector4;
    typedef Eigen::Matrix<T,6,1,U> Vector6;
    typedef Eigen::Matrix<T,3,3,U> Matrix3;
    typedef Eigen::Matrix<T,4,4,U> Matrix4;
    typedef Eigen::Matrix<T,6,6,U> Matrix6;
    typedef Matrix3 Angular_t;
    typedef Vector3 Linear_t;
    typedef Matrix6 ActionMatrix_t;
    typedef Eigen::Quaternion<T,U> Quaternion_t;
    typedef SE3Tpl<T,U> SE3;
    typedef ForceTpl<T,U> Force;
    typedef MotionTpl<T,U> Motion;
    typedef Symmetric3Tpl<T,U> Symmetric3;
    enum {
      LINEAR = 0,
      ANGULAR = 3
    };
  }; // traits SE3Tpl

  template<typename _Scalar, int _Options>
  class SE3Tpl : public SE3Base< SE3Tpl< _Scalar, _Options > >
  {

  public:
    friend class SE3Base< SE3Tpl< _Scalar, _Options > >;
    SPATIAL_TYPEDEF_TEMPLATE(SE3Tpl);
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    SE3Tpl(): rot(), trans() {};

    template<typename M3,typename v3>
    SE3Tpl(const Eigen::MatrixBase<M3> & R, const Eigen::MatrixBase<v3> & p)
    : rot(R), trans(p)
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(v3,3)
      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M3,3,3)
    }

    template<typename M4>
    SE3Tpl(const Eigen::MatrixBase<M4> & m)
    : rot(m.template block<3,3>(LINEAR,LINEAR)), trans(m.template block<3,1>(LINEAR,ANGULAR))
    {
      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M4,4,4);
    }

    SE3Tpl(int) : rot(Matrix3::Identity()), trans(Vector3::Zero()) {}

    template<typename S2, int O2>
    SE3Tpl( const SE3Tpl<S2,O2> & clone )
    : rot(clone.rotation()),trans(clone.translation()) {}

    template<typename S2, int O2>
    SE3Tpl & operator= (const SE3Tpl<S2,O2> & other)
    {
      rot = other.rotation ();
      trans = other.translation ();
      return *this;
    }

    static SE3Tpl Identity()
    {
      return SE3Tpl(1);
    }

    SE3Tpl & setIdentity () { rot.setIdentity (); trans.setZero (); return *this;}

    /// aXb = bXa.inverse()
    SE3Tpl inverse() const
    {
      return SE3Tpl(rot.transpose(), -rot.transpose()*trans);
    }

    static SE3Tpl Random()
    {
      Quaternion_t q(Vector4::Random());
      q.normalize();
      return SE3Tpl(q.matrix(),Vector3::Random());
    }

    SE3Tpl & setRandom ()
    {
      Quaternion_t q(Vector4::Random());
      q.normalize ();
      rot = q.matrix ();
      trans.setRandom ();

      return *this;
    }

    Matrix4 toHomogeneousMatrix_impl() const
    {
      Matrix4 M;
      M.template block<3,3>(LINEAR,LINEAR) = rot;
      M.template block<3,1>(LINEAR,ANGULAR) = trans;
      M.template block<1,3>(ANGULAR,LINEAR).setZero();
      M(3,3) = 1;
      return M;
    }

    /// Vb.toVector() = bXa.toMatrix() * Va.toVector()
    Matrix6 toActionMatrix_impl() const
    {
      typedef Eigen::Block<Matrix6,3,3> Block3;
      Matrix6 M;
      M.template block<3,3>(ANGULAR,ANGULAR)
      = M.template block<3,3>(LINEAR,LINEAR) = rot;
      M.template block<3,3>(ANGULAR,LINEAR).setZero();
      Block3 B = M.template block<3,3>(LINEAR,ANGULAR);

      B.col(0) = trans.cross(rot.col(0));
      B.col(1) = trans.cross(rot.col(1));
      B.col(2) = trans.cross(rot.col(2));
      return M;
    }

    void disp_impl(std::ostream & os) const
    {
      os << "  R =\n" << rot << std::endl
      << "  p = " << trans.transpose() << std::endl;
    }

    /// --- GROUP ACTIONS ON M6, F6 and I6 ---

    /// ay = aXb.act(by)
    template<typename D>
    typename internal::ActionReturn<D>::Type act_impl   (const D & d) const
    {
      return d.se3Action(*this);
    }
    /// by = aXb.actInv(ay)
    template<typename D> typename internal::ActionReturn<D>::Type actInv_impl(const D & d) const
    {
      return d.se3ActionInverse(*this);
    }

    Vector3 act_impl   (const Vector3& p) const { return rot*p+trans; }
    Vector3 actInv_impl(const Vector3& p) const { return rot.transpose()*(p-trans); }

    SE3Tpl act_impl    (const SE3Tpl& m2) const { return SE3Tpl( rot*m2.rot,trans+rot*m2.trans);}
    SE3Tpl actInv_impl (const SE3Tpl& m2) const { return SE3Tpl( rot.transpose()*m2.rot, rot.transpose()*(m2.trans-trans));}

    SE3Tpl __mult__(const SE3Tpl & m2) const { return this->act(m2);}

    bool __equal__( const SE3Tpl & m2 ) const
    {
      return (rotation_impl() == m2.rotation() && translation_impl() == m2.translation());
    }

    bool isApprox_impl (const SE3Tpl & m2, const Scalar_t & prec = Eigen::NumTraits<Scalar_t>::dummy_precision()) const
    {
      return rot.isApprox(m2.rot, prec) && trans.isApprox(m2.trans, prec);
    }

    const Angular_t & rotation_impl() const { return rot; }
    Angular_t & rotation_impl() { return rot; }
    void rotation_impl(const Angular_t & R) { rot = R; }
    const Linear_t & translation_impl() const { return trans;}
    Linear_t & translation_impl() { return trans;}
    void translation_impl(const Linear_t & p) { trans=p; }

  protected:
    Angular_t rot;
    Linear_t trans;

  }; // class SE3Tpl

  typedef SE3Tpl<double,0> SE3;

} // namespace se3

#endif // ifndef __se3_se3_hpp__

<|endoftext|>


<commit_before>/*
 * The MIT License (MIT)
 *
 * Copyright (c) 2017 Nathan Osman
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to
 * deal in the Software without restriction, including without limitation the
 * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
 * sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 * IN THE SOFTWARE.
 */

#include <qmdnsengine/abstractserver.h>
#include <qmdnsengine/browser.h>
#include <qmdnsengine/cache.h>
#include <qmdnsengine/dns.h>
#include <qmdnsengine/mdns.h>
#include <qmdnsengine/message.h>
#include <qmdnsengine/query.h>
#include <qmdnsengine/record.h>

#include "browser_p.h"

using namespace QMdnsEngine;

BrowserPrivate::BrowserPrivate(Browser *browser, AbstractServer *server, const QByteArray &type, Cache *existingCache)
    : QObject(browser),
      q(browser),
      server(server),
      type(type),
      cache(existingCache ? existingCache : new Cache(this))
{
    connect(server, &AbstractServer::messageReceived, this, &BrowserPrivate::onMessageReceived);
    connect(cache, &Cache::shouldQuery, this, &BrowserPrivate::onShouldQuery);
    connect(cache, &Cache::recordExpired, this, &BrowserPrivate::onRecordExpired);
    connect(&queryTimer, &QTimer::timeout, this, &BrowserPrivate::onQueryTimeout);
    connect(&serviceTimer, &QTimer::timeout, this, &BrowserPrivate::onServiceTimeout);

    queryTimer.setInterval(60 * 1000);
    queryTimer.setSingleShot(true);

    serviceTimer.setInterval(100);
    serviceTimer.setSingleShot(true);

    // Immediately begin browsing for services
    onQueryTimeout();
}

// TODO: multiple SRV records not supported

bool BrowserPrivate::updateService(const QByteArray &fqName)
{
    // Split the FQDN into service name and type
    int index = fqName.indexOf('.');
    QByteArray serviceName = fqName.left(index);
    QByteArray serviceType = fqName.mid(index + 1);

    // Immediately return if a PTR record does not exist
    Record ptrRecord;
    if (!cache->lookupRecord(serviceType, PTR, ptrRecord)) {
        return false;
    }

    // If a SRV record is missing, query for it (by returning true)
    Record srvRecord;
    if (!cache->lookupRecord(fqName, SRV, srvRecord)) {
        return true;
    }

    Service service;
    service.setName(serviceName);
    service.setType(serviceType);
    service.setHostname(srvRecord.target());
    service.setPort(srvRecord.port());

    // If TXT records are available for the service, add their values
    QList<Record> txtRecords;
    if (cache->lookupRecords(fqName, TXT, txtRecords)) {
        QMap<QByteArray, QByteArray> attributes;
        foreach (Record record, txtRecords) {
            for (auto i = record.attributes().constBegin();
                    i != record.attributes().constEnd(); ++i) {
                attributes.insert(i.key(), i.value());
            }
        }
        service.setAttributes(attributes);
    }

    // If the service existed, this is an update; otherwise it is a new
    // addition; emit the appropriate signal
    if (!services.contains(fqName)) {
        emit q->serviceAdded(service);
    } else if(services.value(fqName) != service) {
        emit q->serviceUpdated(service);
    }

    services.insert(fqName, service);

    return false;
}

void BrowserPrivate::onMessageReceived(const Message &message)
{
    if (!message.isResponse()) {
        return;
    }

    // Use a set to track all services that are updated in the message - this
    // avoids extraneous signals being emitted if SRV and TXT are provided
    QSet<QByteArray> updateNames;
    foreach (Record record, message.records()) {
        bool any = type == MdnsBrowseType;
        if (record.type() == PTR && record.name() == MdnsBrowseType) {
            cache->addRecord(record);
            ptrTargets.insert(record.target());
            serviceTimer.stop();
            serviceTimer.start();
        } else if ((record.type() == PTR && (any || record.name() == type)) ||
                (record.type() == SRV && (any || record.name().endsWith("." + type))) ||
                (record.type() == TXT && (any || record.name().endsWith("." + type)))) {
            cache->addRecord(record);
            switch (record.type()) {
            case PTR:
                updateNames.insert(record.target());
                break;
            case SRV:
            case TXT:
                updateNames.insert(record.name());
                break;
            }
        }
    }

    // For each of the services marked to be updated, perform the update and
    // make a list of all missing SRV records
    QSet<QByteArray> queryNames;
    foreach (QByteArray name, updateNames) {
        if (updateService(name)) {
            queryNames.insert(name);
        }
    }

    // Build and send a query for all of the SRV records
    if (queryNames.count()) {
        Message queryMessage;
        foreach (QByteArray name, queryNames) {
            Query query;
            query.setName(name);
            query.setType(SRV);
            queryMessage.addQuery(query);
            query.setType(TXT);
            queryMessage.addQuery(query);
        }
        server->sendMessageToAll(queryMessage);
    }
}

void BrowserPrivate::onShouldQuery(const Record &record)
{
    // Assume that all messages in the cache are still in use (by the browser)
    // and attempt to renew them immediately

    Query query;
    query.setName(record.name());
    query.setType(record.type());
    Message message;
    message.addQuery(query);
    server->sendMessageToAll(message);
}

void BrowserPrivate::onRecordExpired(const Record &record)
{
    // If the PTR or SRV record has expired for a service, then it must be
    // removed - TXT records on the other hand, cause an update

    QByteArray serviceName;
    switch (record.type()) {
    case PTR:
        serviceName = record.target();
        break;
    case SRV:
        serviceName = record.name();
        break;
    case TXT:
        updateService(record.name());
        return;
    default:
        return;
    }
    Service service = services.value(serviceName);
    if (!service.name().isNull()) {
        emit q->serviceRemoved(service);
        services.remove(serviceName);
    }
}

void BrowserPrivate::onQueryTimeout()
{
    Query query;
    query.setName(type);
    query.setType(PTR);
    Message message;
    message.addQuery(query);

    // TODO: including too many records could cause problems

    // Include all currently valid PTR records
    QList<Record> records;
    if (cache->lookupRecords(QByteArray(), PTR, records)) {
        foreach (Record record, records) {
            message.addRecord(record);
        }
    }

    server->sendMessageToAll(message);
    queryTimer.start();
}

void BrowserPrivate::onServiceTimeout()
{
    if (ptrTargets.count()) {
        Message message;
        foreach (QByteArray target, ptrTargets) {

            // Add a query for PTR records
            Query query;
            query.setName(target);
            query.setType(PTR);
            message.addQuery(query);

            // Include PTR records for the target that are already known
            QList<Record> records;
            if (cache->lookupRecords(target, PTR, records)) {
                foreach (Record record, records) {
                    message.addRecord(record);
                }
            }
        }

        server->sendMessageToAll(message);
        ptrTargets.clear();
    }
}

Browser::Browser(AbstractServer *server, const QByteArray &type, Cache *cache, QObject *parent)
    : QObject(parent),
      d(new BrowserPrivate(this, server, type, cache))
{
}
<commit_msg>Refactor onMessageReceived() in Browser.<commit_after>/*
 * The MIT License (MIT)
 *
 * Copyright (c) 2017 Nathan Osman
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to
 * deal in the Software without restriction, including without limitation the
 * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
 * sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 * IN THE SOFTWARE.
 */

#include <qmdnsengine/abstractserver.h>
#include <qmdnsengine/browser.h>
#include <qmdnsengine/cache.h>
#include <qmdnsengine/dns.h>
#include <qmdnsengine/mdns.h>
#include <qmdnsengine/message.h>
#include <qmdnsengine/query.h>
#include <qmdnsengine/record.h>

#include "browser_p.h"

using namespace QMdnsEngine;

BrowserPrivate::BrowserPrivate(Browser *browser, AbstractServer *server, const QByteArray &type, Cache *existingCache)
    : QObject(browser),
      q(browser),
      server(server),
      type(type),
      cache(existingCache ? existingCache : new Cache(this))
{
    connect(server, &AbstractServer::messageReceived, this, &BrowserPrivate::onMessageReceived);
    connect(cache, &Cache::shouldQuery, this, &BrowserPrivate::onShouldQuery);
    connect(cache, &Cache::recordExpired, this, &BrowserPrivate::onRecordExpired);
    connect(&queryTimer, &QTimer::timeout, this, &BrowserPrivate::onQueryTimeout);
    connect(&serviceTimer, &QTimer::timeout, this, &BrowserPrivate::onServiceTimeout);

    queryTimer.setInterval(60 * 1000);
    queryTimer.setSingleShot(true);

    serviceTimer.setInterval(100);
    serviceTimer.setSingleShot(true);

    // Immediately begin browsing for services
    onQueryTimeout();
}

// TODO: multiple SRV records not supported

bool BrowserPrivate::updateService(const QByteArray &fqName)
{
    // Split the FQDN into service name and type
    int index = fqName.indexOf('.');
    QByteArray serviceName = fqName.left(index);
    QByteArray serviceType = fqName.mid(index + 1);

    // Immediately return if a PTR record does not exist
    Record ptrRecord;
    if (!cache->lookupRecord(serviceType, PTR, ptrRecord)) {
        return false;
    }

    // If a SRV record is missing, query for it (by returning true)
    Record srvRecord;
    if (!cache->lookupRecord(fqName, SRV, srvRecord)) {
        return true;
    }

    Service service;
    service.setName(serviceName);
    service.setType(serviceType);
    service.setHostname(srvRecord.target());
    service.setPort(srvRecord.port());

    // If TXT records are available for the service, add their values
    QList<Record> txtRecords;
    if (cache->lookupRecords(fqName, TXT, txtRecords)) {
        QMap<QByteArray, QByteArray> attributes;
        foreach (Record record, txtRecords) {
            for (auto i = record.attributes().constBegin();
                    i != record.attributes().constEnd(); ++i) {
                attributes.insert(i.key(), i.value());
            }
        }
        service.setAttributes(attributes);
    }

    // If the service existed, this is an update; otherwise it is a new
    // addition; emit the appropriate signal
    if (!services.contains(fqName)) {
        emit q->serviceAdded(service);
    } else if(services.value(fqName) != service) {
        emit q->serviceUpdated(service);
    }

    services.insert(fqName, service);

    return false;
}

void BrowserPrivate::onMessageReceived(const Message &message)
{
    if (!message.isResponse()) {
        return;
    }

    // Use a set to track all services that are updated in the message to
    // prevent unnecessary queries for SRV and TXT records
    QSet<QByteArray> updateNames;
    foreach (Record record, message.records()) {
        cache->addRecord(record);
        bool any = type == MdnsBrowseType;
        switch (record.type()) {
        case PTR:
            if (any && record.name() == MdnsBrowseType) {
                ptrTargets.insert(record.target());
                serviceTimer.start();
            } else if (any || record.name() == type) {
                updateNames.insert(record.target());
            }
            break;
        case SRV:
        case TXT:
            if (any || record.name().endsWith("." + type)) {
                updateNames.insert(record.name());
            }
            break;
        }
    }

    // For each of the services marked to be updated, perform the update and
    // make a list of all missing SRV records
    QSet<QByteArray> queryNames;
    foreach (QByteArray name, updateNames) {
        if (updateService(name)) {
            queryNames.insert(name);
        }
    }

    // Build and send a query for all of the SRV and TXT records
    if (queryNames.count()) {
        Message queryMessage;
        foreach (QByteArray name, queryNames) {
            Query query;
            query.setName(name);
            query.setType(SRV);
            queryMessage.addQuery(query);
            query.setType(TXT);
            queryMessage.addQuery(query);
        }
        server->sendMessageToAll(queryMessage);
    }
}

void BrowserPrivate::onShouldQuery(const Record &record)
{
    // Assume that all messages in the cache are still in use (by the browser)
    // and attempt to renew them immediately

    Query query;
    query.setName(record.name());
    query.setType(record.type());
    Message message;
    message.addQuery(query);
    server->sendMessageToAll(message);
}

void BrowserPrivate::onRecordExpired(const Record &record)
{
    // If the PTR or SRV record has expired for a service, then it must be
    // removed - TXT records on the other hand, cause an update

    QByteArray serviceName;
    switch (record.type()) {
    case PTR:
        serviceName = record.target();
        break;
    case SRV:
        serviceName = record.name();
        break;
    case TXT:
        updateService(record.name());
        return;
    default:
        return;
    }
    Service service = services.value(serviceName);
    if (!service.name().isNull()) {
        emit q->serviceRemoved(service);
        services.remove(serviceName);
    }
}

void BrowserPrivate::onQueryTimeout()
{
    Query query;
    query.setName(type);
    query.setType(PTR);
    Message message;
    message.addQuery(query);

    // TODO: including too many records could cause problems

    // Include all currently valid PTR records
    QList<Record> records;
    if (cache->lookupRecords(QByteArray(), PTR, records)) {
        foreach (Record record, records) {
            message.addRecord(record);
        }
    }

    server->sendMessageToAll(message);
    queryTimer.start();
}

void BrowserPrivate::onServiceTimeout()
{
    if (ptrTargets.count()) {
        Message message;
        foreach (QByteArray target, ptrTargets) {

            // Add a query for PTR records
            Query query;
            query.setName(target);
            query.setType(PTR);
            message.addQuery(query);

            // Include PTR records for the target that are already known
            QList<Record> records;
            if (cache->lookupRecords(target, PTR, records)) {
                foreach (Record record, records) {
                    message.addRecord(record);
                }
            }
        }

        server->sendMessageToAll(message);
        ptrTargets.clear();
    }
}

Browser::Browser(AbstractServer *server, const QByteArray &type, Cache *cache, QObject *parent)
    : QObject(parent),
      d(new BrowserPrivate(this, server, type, cache))
{
}
<|endoftext|>


<commit_before>#define MS_CLASS "TEST::RTP"
#define MS_LOG_DEV

#include "fct.h"
#include "common.h"
#include "RTC/RtpPacket.h"
#include "Logger.h"
#include <fstream>

using namespace RTC;

FCTMF_SUITE_BGN(test_rtp)
{
	FCT_TEST_BGN(parse_packet)
	{
		// DOC: http://insanecoding.blogspot.com.es/2011/11/how-to-read-in-file-in-c.html

		uint8_t buffer[65536];
		std::ifstream in("worker/test/data/packet1.raw", std::ios::in | std::ios::binary);

		if (in)
		{
			in.seekg(0, std::ios::end);
			size_t file_size = in.tellg();
			in.seekg(0, std::ios::beg);

			in.read((char*)buffer, file_size);
			in.close();

			// TODO: Why -1??? It fails without it!
			RtpPacket* packet = RtpPacket::Parse(buffer, file_size - 1);

			if (!packet)
			{
				MS_ERROR("not a RTP packet");

				// TODO: fail the test here.
				return;
			}

			packet->Dump();
			// MS_ERROR("-- payload type: %" PRIu8, packet->GetPayloadType());

			// auto pt = packet->GetPayloadType();
			// uint8_t pt = 11;
			// fct_chk_eq_int(pt, 112);

			fct_chk(packet->HasMarker() == false);
			fct_chk(packet->HasExtensionHeader() == true);
			fct_chk_eq_int(packet->GetExtensionHeaderId(), 48862);
			fct_chk_eq_int(packet->GetExtensionHeaderLength(), 4);
			fct_chk_eq_int(packet->GetPayloadType(), 111);
			fct_chk_eq_int(packet->GetSequenceNumber(), 23617);
			fct_chk_eq_int(packet->GetTimestamp(), 1660241882);
			fct_chk_eq_int(packet->GetSsrc(), 2674985186);

			// delete packet;
		}
		else
		{
			// TODO: What here? how to fial the test?
			MS_ERROR("file not found");
		}
	}
	FCT_TEST_END()
}
FCTMF_SUITE_END();
<commit_msg>test-rtp.cpp: code cleanup.<commit_after>#define MS_CLASS "TEST::RTP"
#define MS_LOG_DEV

#include "fct.h"
#include "common.h"
#include "RTC/RtpPacket.h"
#include "Logger.h"
#include <fstream>

using namespace RTC;

FCTMF_SUITE_BGN(test_rtp)
{
	FCT_TEST_BGN(parse_rtp_packet)
	{
		// DOC: http://insanecoding.blogspot.com.es/2011/11/how-to-read-in-file-in-c.html

		uint8_t buffer[65536];
		std::ifstream in("worker/test/data/packet1.raw", std::ios::ate | std::ios::binary);

		if (in)
		{
			size_t file_size = (size_t)in.tellg() - 1;

			in.seekg(0, std::ios::beg);

			in.read((char*)buffer, file_size);
			in.close();

			RtpPacket* packet = RtpPacket::Parse(buffer, file_size);

			if (!packet)
			{
				MS_ERROR("not a RTP packet");

				// TODO: fail the test here.
				return;
			}

			// packet->Dump();

			fct_chk(packet->HasMarker() == false);
			fct_chk(packet->HasExtensionHeader() == true);
			fct_chk_eq_int(packet->GetExtensionHeaderId(), 48862);
			fct_chk_eq_int(packet->GetExtensionHeaderLength(), 4);
			fct_chk_eq_int(packet->GetPayloadType(), 111);
			fct_chk_eq_int(packet->GetSequenceNumber(), 23617);
			fct_chk_eq_int(packet->GetTimestamp(), 1660241882);
			fct_chk_eq_int(packet->GetSsrc(), 2674985186);

			delete packet;
		}
		else
		{
			// TODO: What here? how to fail the test?
			MS_ERROR("file not found");
		}
	}
	FCT_TEST_END()
}
FCTMF_SUITE_END();
<|endoftext|>


<commit_before>#include "game.hpp"
#include "component_drawable.hpp"
#include "component_position.hpp"

#include "strapon/resource_manager/resource_manager.hpp"

#include "entityx/entityx.h"
#include <glm/vec2.hpp>
#include <glm/gtx/polar_coordinates.hpp>
#include <SDL2/SDL.h>
#include <SDL2/SDL_image.h>

class DrawSystem : public entityx::System<DrawSystem> {
  public:
    DrawSystem(Game *game) : m_game(game) {
        int w, h;
        SDL_RenderGetLogicalSize(game->renderer(), &w, &h);
        m_camera = SDL_Rect{0, 0, w, h};
        m_drawtex =
            SDL_CreateTexture(game->renderer(), SDL_PIXELTYPE_UNKNOWN, SDL_TEXTUREACCESS_TARGET,
                              game->world_size().w, game->world_size().h);
    }

    ~DrawSystem() {
        SDL_DestroyTexture(m_drawtex);
    }

    glm::vec2 polar_to_euclid(glm::vec2 i) {
        glm::vec2 cart;
        cart.x = glm::cos(i[1]) * i[0];
        cart.y = glm::sin(i[1]) * i[0];
        return cart;
    }

    void update(entityx::EntityManager &es, entityx::EventManager &events,
                entityx::TimeDelta dt) override {

        // Change to render into rendertexture for now
        SDL_SetRenderTarget(m_game->renderer(), m_drawtex);
        SDL_SetRenderDrawColor(m_game->renderer(), 0, 100, 200, 255);
        SDL_RenderClear(m_game->renderer());

        entityx::ComponentHandle<Drawable> drawable;
        entityx::ComponentHandle<Position> position;

        /*auto player;
        bool found = false;
        for(entityx::Entity entity : es.entities_with_components(player)) {
            if(found) {
                // Shit is fucked up! We have 2 players
            }
            player = entity;
            found = true;
        }*/ // Becomes relevant when sascha finished the player crap

        for (entityx::Entity entity : es.entities_with_components(drawable, position)) {

            (void)entity; // why?

            auto coord_polar = entity.component<Position>();
            SDL_Rect dest;
            // now follow the player
            glm::vec2 coord_euclid = polar_to_euclid(coord_polar->position());
            // Converted position
            dest.x = coord_euclid[0];
            dest.y = coord_euclid[1];
            // Center on entity
            dest.x += drawable->width() / 2;
            dest.y += drawable->height() / 2;
            // Translate onto player. In fact will do this later. Too confusing w/o other entities
            dest.x += m_camera.w / 2;
            dest.y += m_camera.h / 2;

            dest.w = drawable->width();
            dest.h = drawable->height();

            SDL_RenderCopyEx(m_game->renderer(),
                             m_game->res_manager().texture(drawable->texture_key()), NULL, &dest, 0,
                             NULL, SDL_FLIP_NONE);
        }

        auto surf = TTF_RenderText_Blended(m_game->res_manager().font("font20"), "LOL", {200, 100, 100, 150});
        auto text = SDL_CreateTextureFromSurface(m_game->renderer(), surf);
        SDL_Rect dest = {0, 0, 50, 50};
        SDL_RenderCopy(m_game->renderer(), text, NULL, &dest);
        SDL_FreeSurface(surf);

        // Render to final window
        SDL_SetRenderTarget(m_game->renderer(), nullptr);
        SDL_RenderCopy(m_game->renderer(), m_drawtex, &m_camera, nullptr);
        SDL_RenderPresent(m_game->renderer());
    }

  private:
    Game *m_game;
    SDL_Rect m_camera;
    SDL_Texture *m_drawtex;
};
<commit_msg>Fixed: wrong centering<commit_after>#include "game.hpp"
#include "component_drawable.hpp"
#include "component_position.hpp"

#include "strapon/resource_manager/resource_manager.hpp"

#include "entityx/entityx.h"
#include <glm/vec2.hpp>
#include <glm/gtx/polar_coordinates.hpp>
#include <SDL2/SDL.h>
#include <SDL2/SDL_image.h>

class DrawSystem : public entityx::System<DrawSystem> {
  public:
    DrawSystem(Game *game) : m_game(game) {
        int w, h;
        SDL_RenderGetLogicalSize(game->renderer(), &w, &h);
        m_camera = SDL_Rect{0, 0, w, h};
        m_drawtex =
            SDL_CreateTexture(game->renderer(), SDL_PIXELTYPE_UNKNOWN, SDL_TEXTUREACCESS_TARGET,
                              game->world_size().w, game->world_size().h);
    }

    ~DrawSystem() {
        SDL_DestroyTexture(m_drawtex);
    }

    glm::vec2 polar_to_euclid(glm::vec2 i) {
        glm::vec2 cart;
        cart.x = glm::cos(i[1]) * i[0];
        cart.y = glm::sin(i[1]) * i[0];
        return cart;
    }

    void update(entityx::EntityManager &es, entityx::EventManager &events,
                entityx::TimeDelta dt) override {

        // Change to render into rendertexture for now
        SDL_SetRenderTarget(m_game->renderer(), m_drawtex);
        SDL_SetRenderDrawColor(m_game->renderer(), 0, 100, 200, 255);
        SDL_RenderClear(m_game->renderer());

        entityx::ComponentHandle<Drawable> drawable;
        entityx::ComponentHandle<Position> position;

        /*auto player;
        bool found = false;
        for(entityx::Entity entity : es.entities_with_components(player)) {
            if(found) {
                // Shit is fucked up! We have 2 players
            }
            player = entity;
            found = true;
        }*/ // Becomes relevant when sascha finished the player crap

        for (entityx::Entity entity : es.entities_with_components(drawable, position)) {

            (void)entity; // why?

            auto coord_polar = entity.component<Position>();
            SDL_Rect dest;
            // now follow the player
            glm::vec2 coord_euclid = polar_to_euclid(coord_polar->position());
            // Converted position
            dest.x = coord_euclid[0];
            dest.y = coord_euclid[1];
            // Center on entity
            dest.x -= drawable->width() / 2;
            dest.y -= drawable->height() / 2;
            // Translate onto player. In fact will do this later. Too confusing w/o other entities
            dest.x += m_camera.w / 2;
            dest.y += m_camera.h / 2;

            dest.w = drawable->width();
            dest.h = drawable->height();

            SDL_RenderCopyEx(m_game->renderer(),
                             m_game->res_manager().texture(drawable->texture_key()), NULL, &dest, 0,
                             NULL, SDL_FLIP_NONE);
        }

        auto surf = TTF_RenderText_Blended(m_game->res_manager().font("font20"), "LOL", {200, 100, 100, 150});
        auto text = SDL_CreateTextureFromSurface(m_game->renderer(), surf);
        SDL_Rect dest = {0, 0, 50, 50};
        SDL_RenderCopy(m_game->renderer(), text, NULL, &dest);
        SDL_FreeSurface(surf);

        // Render to final window
        SDL_SetRenderTarget(m_game->renderer(), nullptr);
        SDL_RenderCopy(m_game->renderer(), m_drawtex, &m_camera, nullptr);
        SDL_RenderPresent(m_game->renderer());
    }

  private:
    Game *m_game;
    SDL_Rect m_camera;
    SDL_Texture *m_drawtex;
};
<|endoftext|>


<commit_before>
<commit_msg>Delete 1.cpp<commit_after><|endoftext|>


<commit_before>#include "../tlang.h"
#include <taichi/util.h>
#include <taichi/visual/gui.h>
#include <taichi/system/profiler.h>
#include <taichi/visualization/particle_visualization.h>
#include <taichi/visual/texture.h>
#include "fem_interface.h"

TC_NAMESPACE_BEGIN

using namespace Tlang;

constexpr int dim = 3, n = 256;
constexpr int pre_and_post_smoothing = 3, bottom_smoothing = 10;
constexpr int mg_levels = 2;

auto mgpcg_poisson = []() {
  CoreState::set_trigger_gdb_when_crash(true);

  Program prog(Arch::x86_64);
  prog.config.print_ir = true;
  prog.config.lazy_compilation = false;

  auto r = Vector(DataType::f32, mg_levels);
  auto z = Vector(DataType::f32, mg_levels);
  Global(x, f32);
  Global(p, f32);
  Global(Ap, f32);
  Global(alpha, f32);
  Global(beta, f32);
  Global(sum, f32);
  Global(phase, i32);

  int block_size = 8;

  bool block_soa = true;

  layout([&]() {
    auto ijk = Indices(0, 1, 2);
    std::function<void(Expr & expr)> place_scalar;

    SNode *block;
    if (block_soa) {
      block = &root.dense(ijk, n / block_size).bitmasked();  //.pointer();
      place_scalar = [&](Expr &s) { block->dense(ijk, block_size).place(s); };
    } else {
      place_scalar = [&](Expr &mat) {
        root.dense(ijk, n / block_size).dense(ijk, block_size).place(mat);
      };
    }

    place_scalar(x);
    place_scalar(p);
    place_scalar(Ap);
    place_scalar(r(0));
    place_scalar(z(0));
    for (int i = 1; i < mg_levels; i++) {
      root.dense(ijk, n / block_size)
          .bitmasked()
          .dense(ijk, block_size)
          .place(r(i), z(i));
    }
    root.place(alpha, beta, sum, phase);
  });

  auto get_prolongate = [&](const Expr &z2,
                            const Expr &z) -> Program::Kernel & {
    return kernel([&] {
      Parallelize(8);
      Vectorize(1);
      For(z, [&](Expr i, Expr j, Expr k) {
        z[i, j, k] += z2[i / 2, j / 2, k / 2];
      });
    });
  };

  Kernel(compute_Ap).def([&] {
    BlockDim(1024);
    Parallelize(8);
    Vectorize(block_size);
    For(Ap, [&](Expr i, Expr j, Expr k) {
      Ap[i, j, k] = 6.0f * p[i, j, k] - p[i - 1, j, k] - p[i + 1, j, k] -
                    p[i, j + 1, k] - p[i, j - 1, k] - p[i, j, k + 1] -
                    p[i, j, k - 1];
    });
  });

  prog.config.print_ir = false;

  Kernel(reduce_r).def([&] {
    For(p, [&](Expr i, Expr j, Expr k) {
      Atomic(sum[Expr(0)]) += r(0)[i, j, k] * r(0)[i, j, k];
    });
  });

  Kernel(reduce_zTr).def([&] {
    For(p, [&](Expr i, Expr j, Expr k) {
      Atomic(sum[Expr(0)]) += z(0)[i, j, k] * r(0)[i, j, k];
    });
  });

  Kernel(reduce_pAp).def([&] {
    For(p, [&](Expr i, Expr j, Expr k) {
      auto tmp = Var(0.0f);
      tmp += p[i, j, k] * Ap[i, j, k];
      Atomic(sum[Expr(0)]) += tmp;
    });
  });

  Kernel(update_x).def([&] {
    For(x, [&](Expr i, Expr j, Expr k) {
      x[i, j, k] += alpha[Expr(0)] * p[i, j, k];
    });
  });

  Kernel(update_r).def([&] {
    For(p, [&](Expr i, Expr j, Expr k) {
      r(0)[i, j, k] -= alpha[Expr(0)] * Ap[i, j, k];
    });
  });

  Kernel(update_p).def([&] {
    For(p, [&](Expr i, Expr j, Expr k) {
      p[i, j, k] = z(0)[i, j, k] + beta[Expr(0)] * p[i, j, k];
    });
  });

  int begin = n / 4, end = n * 3 / 4;
  for (int i = begin; i < end; i++) {
    for (int j = begin; j < end; j++) {
      for (int k = begin; k < end; k++) {
        r(0).val<float32>(i, j, k) =
            (i == n / 2) && (j == n / 2) && (k == n / 2);
      }
    }
  }

  std::vector<std::function<void()>> smoothers(mg_levels),
      restrictors(mg_levels - 1), prolongators(mg_levels - 1),
      clearer_z(mg_levels), clearer_r(mg_levels);

  auto &prolongate = get_prolongate(z(1), z(0));

  for (int l = 0; l < mg_levels; l++) {
    if (l < mg_levels - 1) {
      restrictors[l] =
          kernel([&] {
            Parallelize(8);
            Vectorize(1);
            For(x, [&](Expr i, Expr j, Expr k) {
              auto res =
                  Var(r(l)[i, j, k] - (6.0f * z(l)[i, j, k] -
                                       z(l)[i - 1, j, k] - z(l)[i + 1, j, k] -
                                       z(l)[i, j + 1, k] - z(l)[i, j - 1, k] -
                                       z(l)[i, j, k + 1] - z(l)[i, j, k - 1]));
              r(l + 1)[i / 2, j / 2, k / 2] += res * 0.5f;
            });
          })
              .func();
    }
    smoothers[l] =
        kernel([&] {
          Parallelize(8);
          Vectorize(1);
          For(z(l), [&](Expr i, Expr j, Expr k) {
            If(((i + j + k) & 1) == phase[Expr(0)]).Then([&] {
              z(l)[i, j, k] =
                  (r(l)[i, j, k] + z(l)[i - 1, j, k] + z(l)[i + 1, j, k] +
                   z(l)[i, j + 1, k] + z(l)[i, j - 1, k] + z(l)[i, j, k + 1] +
                   z(l)[i, j, k - 1]) *
                  (1.0f / 6);
            });
          });
        })
            .func();
    clearer_r[l] =
        kernel([&] {
          For(r(l), [&](Expr i, Expr j, Expr k) { r(l)[i, j, k] = 0.0f; });
        })
            .func();
    clearer_z[l] =
        kernel([&] {
          For(z(l), [&](Expr i, Expr j, Expr k) { z(l)[i, j, k] = 0.0f; });
        })
            .func();
  }

  // z = M^-1 r
  auto apply_preconditioner = [&] {
    clearer_z[0]();
    for (int i = 0; i < pre_and_post_smoothing; i++) {
      phase.val<int32>() = 0;
      smoothers[0]();
      phase.val<int32>() = 1;
      smoothers[0]();
    }
    clearer_z[1]();
    clearer_r[1]();
    restrictors[0]();
    for (int i = 0; i < bottom_smoothing; i++) {
      phase.val<int32>() = 0;
      smoothers[1]();
      phase.val<int32>() = 1;
      smoothers[1]();
    }
    prolongate();
    for (int i = 0; i < pre_and_post_smoothing; i++) {
      phase.val<int32>() = 0;
      smoothers[0]();
      phase.val<int32>() = 1;
      smoothers[0]();
    }
  };

  // r = b - Ax = b    since x = 0
  // p = r = r + 0 p
  apply_preconditioner();
  update_p();
  sum.val<float32>() = 0;
  reduce_zTr();
  auto old_zTr = sum.val<float32>();

  // CG
  for (int i = 0; i < 40; i++) {
    TC_P(i);
    compute_Ap();
    sum.val<float32>() = 0;
    reduce_pAp();
    auto pAp = sum.val<float32>();
    // alpha = rTr / pTAp
    alpha.val<float32>() = old_zTr / pAp;
    TC_P(old_zTr);
    // TC_P(pAp);
    // TC_P(alpha.val<float32>());
    // x = x + alpha p
    update_x();
    // r = r - alpha Ap
    update_r();
    // return if |r| small
    // z = M^-1 r
    apply_preconditioner();
    sum.val<float32>() = 0;
    reduce_zTr();
    auto new_zTr = sum.val<float32>();
    TC_P(new_zTr);
    sum.val<float32>() = 0;
    reduce_r();
    auto rTr = sum.val<float32>();
    if (rTr < 1e-7f)
      break;
    // beta = new rTr / old rTr
    beta.val<float32>() = new_zTr / old_zTr;
    TC_P(beta.val<float32>());
    // p = z + beta p
    update_p();
    old_zTr = new_zTr;
  }
  get_current_program().profiler_print();

  compute_Ap();
  auto residual = 0.0f;
  for (int i = 0; i < n; i++) {
    for (int j = 0; j < n; j++) {
      for (int k = 0; k < n; k++) {
        for (int d = 0; d < dim; d++) {
          // residual += std::abs(R[i][j][k][d] - Ap(d).val<float32>(i, j, k));
        }
      }
    }
  }
  TC_P(residual);
  // TC_P(difference_max);

  int gui_res = 512;
  GUI gui("MGPCG Poisson", Vector2i(gui_res + 200, gui_res), false);
  int gt = 0;
  int k = 0;
  gui.slider("z", k, 0, n - 1).slider("Ground truth", gt, 0, 1);

  int scale = gui_res / n;
  auto &canvas = gui.get_canvas();
  for (int frame = 1;; frame++) {
    for (int i = 0; i < gui_res - scale; i++) {
      for (int j = 0; j < gui_res - scale; j++) {
        real dx = x.val<float32>(i / scale, j / scale, k);
        canvas.img[i][j] = Vector4(0.5f) + Vector4(dx, dx, dx, 0) * 15.0f;
      }
    }
    gui.update();
  }
};
TC_REGISTER_TASK(mgpcg_poisson);

TC_NAMESPACE_END<commit_msg>prolongator<commit_after>#include "../tlang.h"
#include <taichi/util.h>
#include <taichi/visual/gui.h>
#include <taichi/system/profiler.h>
#include <taichi/visualization/particle_visualization.h>
#include <taichi/visual/texture.h>
#include "fem_interface.h"

TC_NAMESPACE_BEGIN

using namespace Tlang;

constexpr int dim = 3, n = 256;
constexpr int pre_and_post_smoothing = 3, bottom_smoothing = 10;
constexpr int mg_levels = 2;

auto mgpcg_poisson = []() {
  CoreState::set_trigger_gdb_when_crash(true);

  Program prog(Arch::x86_64);
  prog.config.print_ir = true;
  prog.config.lazy_compilation = false;

  auto r = Vector(DataType::f32, mg_levels);
  auto z = Vector(DataType::f32, mg_levels);
  Global(x, f32);
  Global(p, f32);
  Global(Ap, f32);
  Global(alpha, f32);
  Global(beta, f32);
  Global(sum, f32);
  Global(phase, i32);

  int block_size = 8;

  bool block_soa = true;

  layout([&]() {
    auto ijk = Indices(0, 1, 2);
    std::function<void(Expr & expr)> place_scalar;

    SNode *block;
    if (block_soa) {
      block = &root.dense(ijk, n / block_size).bitmasked();  //.pointer();
      place_scalar = [&](Expr &s) { block->dense(ijk, block_size).place(s); };
    } else {
      place_scalar = [&](Expr &mat) {
        root.dense(ijk, n / block_size).dense(ijk, block_size).place(mat);
      };
    }

    place_scalar(x);
    place_scalar(p);
    place_scalar(Ap);
    place_scalar(r(0));
    place_scalar(z(0));
    for (int i = 1; i < mg_levels; i++) {
      root.dense(ijk, n / block_size)
          .bitmasked()
          .dense(ijk, block_size)
          .place(r(i), z(i));
    }
    root.place(alpha, beta, sum, phase);
  });

  Kernel(compute_Ap).def([&] {
    BlockDim(1024);
    Parallelize(8);
    Vectorize(block_size);
    For(Ap, [&](Expr i, Expr j, Expr k) {
      Ap[i, j, k] = 6.0f * p[i, j, k] - p[i - 1, j, k] - p[i + 1, j, k] -
                    p[i, j + 1, k] - p[i, j - 1, k] - p[i, j, k + 1] -
                    p[i, j, k - 1];
    });
  });

  prog.config.print_ir = false;

  Kernel(reduce_r).def([&] {
    For(p, [&](Expr i, Expr j, Expr k) {
      Atomic(sum[Expr(0)]) += r(0)[i, j, k] * r(0)[i, j, k];
    });
  });

  Kernel(reduce_zTr).def([&] {
    For(p, [&](Expr i, Expr j, Expr k) {
      Atomic(sum[Expr(0)]) += z(0)[i, j, k] * r(0)[i, j, k];
    });
  });

  Kernel(reduce_pAp).def([&] {
    For(p, [&](Expr i, Expr j, Expr k) {
      auto tmp = Var(0.0f);
      tmp += p[i, j, k] * Ap[i, j, k];
      Atomic(sum[Expr(0)]) += tmp;
    });
  });

  Kernel(update_x).def([&] {
    For(x, [&](Expr i, Expr j, Expr k) {
      x[i, j, k] += alpha[Expr(0)] * p[i, j, k];
    });
  });

  Kernel(update_r).def([&] {
    For(p, [&](Expr i, Expr j, Expr k) {
      r(0)[i, j, k] -= alpha[Expr(0)] * Ap[i, j, k];
    });
  });

  Kernel(update_p).def([&] {
    For(p, [&](Expr i, Expr j, Expr k) {
      p[i, j, k] = z(0)[i, j, k] + beta[Expr(0)] * p[i, j, k];
    });
  });

  int begin = n / 4, end = n * 3 / 4;
  for (int i = begin; i < end; i++) {
    for (int j = begin; j < end; j++) {
      for (int k = begin; k < end; k++) {
        r(0).val<float32>(i, j, k) =
            (i == n / 2) && (j == n / 2) && (k == n / 2);
      }
    }
  }

  std::vector<std::function<void()>> smoothers(mg_levels),
      restrictors(mg_levels - 1), prolongators(mg_levels - 1),
      clearer_z(mg_levels), clearer_r(mg_levels);

  for (int l = 0; l < mg_levels; l++) {
    if (l < mg_levels - 1) {
      restrictors[l] =
          kernel([&] {
            Parallelize(8);
            Vectorize(1);
            For(x, [&](Expr i, Expr j, Expr k) {
              auto res =
                  Var(r(l)[i, j, k] - (6.0f * z(l)[i, j, k] -
                                       z(l)[i - 1, j, k] - z(l)[i + 1, j, k] -
                                       z(l)[i, j + 1, k] - z(l)[i, j - 1, k] -
                                       z(l)[i, j, k + 1] - z(l)[i, j, k - 1]));
              r(l + 1)[i / 2, j / 2, k / 2] += res * 0.5f;
            });
          })
              .func();
      prolongators[l] = kernel([&] {
                          Parallelize(8);
                          Vectorize(1);
                          For(z(l), [&](Expr i, Expr j, Expr k) {
                            z(l)[i, j, k] += z(l + 1)[i / 2, j / 2, k / 2];
                          });
                        })
                            .func();
    }
    smoothers[l] =
        kernel([&] {
          Parallelize(8);
          Vectorize(1);
          For(z(l), [&](Expr i, Expr j, Expr k) {
            If(((i + j + k) & 1) == phase[Expr(0)]).Then([&] {
              z(l)[i, j, k] =
                  (r(l)[i, j, k] + z(l)[i - 1, j, k] + z(l)[i + 1, j, k] +
                   z(l)[i, j + 1, k] + z(l)[i, j - 1, k] + z(l)[i, j, k + 1] +
                   z(l)[i, j, k - 1]) *
                  (1.0f / 6);
            });
          });
        })
            .func();
    clearer_r[l] =
        kernel([&] {
          For(r(l), [&](Expr i, Expr j, Expr k) { r(l)[i, j, k] = 0.0f; });
        })
            .func();
    clearer_z[l] =
        kernel([&] {
          For(z(l), [&](Expr i, Expr j, Expr k) { z(l)[i, j, k] = 0.0f; });
        })
            .func();
  }

  // z = M^-1 r
  auto apply_preconditioner = [&] {
    clearer_z[0]();
    for (int i = 0; i < pre_and_post_smoothing; i++) {
      phase.val<int32>() = 0;
      smoothers[0]();
      phase.val<int32>() = 1;
      smoothers[0]();
    }
    clearer_z[1]();
    clearer_r[1]();
    restrictors[0]();
    for (int i = 0; i < bottom_smoothing; i++) {
      phase.val<int32>() = 0;
      smoothers[1]();
      phase.val<int32>() = 1;
      smoothers[1]();
    }
    prolongators[0]();
    for (int i = 0; i < pre_and_post_smoothing; i++) {
      phase.val<int32>() = 0;
      smoothers[0]();
      phase.val<int32>() = 1;
      smoothers[0]();
    }
  };

  // r = b - Ax = b    since x = 0
  // p = r = r + 0 p
  apply_preconditioner();
  update_p();
  sum.val<float32>() = 0;
  reduce_zTr();
  auto old_zTr = sum.val<float32>();

  // CG
  for (int i = 0; i < 40; i++) {
    TC_P(i);
    compute_Ap();
    sum.val<float32>() = 0;
    reduce_pAp();
    auto pAp = sum.val<float32>();
    // alpha = rTr / pTAp
    alpha.val<float32>() = old_zTr / pAp;
    TC_P(old_zTr);
    // TC_P(pAp);
    // TC_P(alpha.val<float32>());
    // x = x + alpha p
    update_x();
    // r = r - alpha Ap
    update_r();
    // return if |r| small
    // z = M^-1 r
    apply_preconditioner();
    sum.val<float32>() = 0;
    reduce_zTr();
    auto new_zTr = sum.val<float32>();
    TC_P(new_zTr);
    sum.val<float32>() = 0;
    reduce_r();
    auto rTr = sum.val<float32>();
    if (rTr < 1e-7f)
      break;
    // beta = new rTr / old rTr
    beta.val<float32>() = new_zTr / old_zTr;
    TC_P(beta.val<float32>());
    // p = z + beta p
    update_p();
    old_zTr = new_zTr;
  }
  get_current_program().profiler_print();

  compute_Ap();
  auto residual = 0.0f;
  for (int i = 0; i < n; i++) {
    for (int j = 0; j < n; j++) {
      for (int k = 0; k < n; k++) {
        for (int d = 0; d < dim; d++) {
          // residual += std::abs(R[i][j][k][d] - Ap(d).val<float32>(i, j, k));
        }
      }
    }
  }
  TC_P(residual);
  // TC_P(difference_max);

  int gui_res = 512;
  GUI gui("MGPCG Poisson", Vector2i(gui_res + 200, gui_res), false);
  int gt = 0;
  int k = 0;
  gui.slider("z", k, 0, n - 1).slider("Ground truth", gt, 0, 1);

  int scale = gui_res / n;
  auto &canvas = gui.get_canvas();
  for (int frame = 1;; frame++) {
    for (int i = 0; i < gui_res - scale; i++) {
      for (int j = 0; j < gui_res - scale; j++) {
        real dx = x.val<float32>(i / scale, j / scale, k);
        canvas.img[i][j] = Vector4(0.5f) + Vector4(dx, dx, dx, 0) * 15.0f;
      }
    }
    gui.update();
  }
};
TC_REGISTER_TASK(mgpcg_poisson);

TC_NAMESPACE_END<|endoftext|>


<commit_before>#include "twister_rss.h"
#include "init.h"
#include "bitcoinrpc.h"
#include "json/json_spirit.h"

#include <sstream>
#include <algorithm>
#include <vector>
#include <ctime>
#ifdef HAVE_BOOST_REGEX
    #include <boost/regex.hpp>
#endif
#include <boost/lexical_cast.hpp>

using namespace std;
using namespace json_spirit;

int generateRSS(string uri, string *output)
{
#ifndef HAVE_BOOST_REGEX
    return RSS_ERROR_BOOST_REGEX;
#else
    map<string, string> parameterMap = parseQuery(uri);
    int max = 20; //default value
    string account = parameterMap["account"];
    string strMax = parameterMap["max"];
    if(strMax!="")
    {
        try
        {
            max = boost::lexical_cast<int>(strMax);
        }
        catch(boost::bad_lexical_cast e)
        {
            return RSS_ERROR_NOT_A_NUMBER;
        }
    }

    const Array emptyArray;
    Array accountsArray = listwalletusers(emptyArray,false).get_array();

    // if no account was specified, choose the first one
    if(account=="")
    {
        if(accountsArray.size()>0)
        {
            account = accountsArray[0].get_str();
        }
        else return RSS_ERROR_NO_ACCOUNT;
    }

    // if an account name was specified, check that it exists
    else
    {
        bool accountExists = false;
        for(int i=0;i<accountsArray.size();i++)
        {
            if(accountsArray[i]==account)
                accountExists=true;
        }
        if(!accountExists) return RSS_ERROR_BAD_ACCOUNT;
    }

    // get an array of followed usernames and transform it to required format
    Array params1;
    params1.push_back(account);
    Array followingArray = getfollowing(params1,false).get_array();
    Array postSources;
    for(int i=0;i<followingArray.size();i++)
    {
        Object item;
        item.push_back(Pair("username",followingArray[i]));
        postSources.push_back(item);
    }

    Array params2;
    params2.push_back(max);
    params2.push_back(postSources);
    Array posts = getposts(params2,false).get_array();
    vector<Object> outputVector;

    if(GetBoolArg("-rss_dm",false))     //synchronizing direct messages is disabled by default
    {
        Array params3;
        params3.push_back(account);
        params3.push_back(max);
        params3.push_back(postSources);
        Object messages = getdirectmsgs(params3,false).get_obj();

        for(int j=0;j<messages.size();j++)
        {
            Array userArray = messages[j].value_.get_array();
            for(int i=0;i<userArray.size();i++)
            {
                try
                {
                    if(find_value(userArray[i].get_obj(),"fromMe").get_bool())      //only report received messages
                      continue;

                    string postTitle, postAuthor, postMsg;
                    postAuthor=messages[j].name_;
                    postTitle="Direct Message from "+postAuthor;
                    postMsg=find_value(userArray[i].get_obj(),"text").get_str();
                    time_t postTime(find_value(userArray[i].get_obj(),"time").get_int64());

                    Object item;
                    item.push_back(Pair("time",postTime));
                    item.push_back(Pair("title",postTitle));
                    item.push_back(Pair("author",postAuthor));
                    item.push_back(Pair("msg",postMsg));
                    outputVector.push_back(item);
                }
                catch(exception ex)
                {
                    fprintf(stderr, "Warning: RSS couldn't parse a direct message, skipping.\n");
                    continue;
                }
            }
        }
    }

    for(int i=0;i<posts.size();i++)
    {
        try
        {
            Object userpost = find_value(posts[i].get_obj(),"userpost").get_obj();
            string postTitle, postAuthor, postMsg;
            Value rt = find_value(userpost,"rt");

            if(rt.is_null())    // it's a normal post
            {
                postAuthor = find_value(userpost,"n").get_str();
                Value reply = find_value(userpost,"reply");
                if(!reply.is_null()&&find_value(reply.get_obj(),"n").get_str()==account)
                {
                    postTitle = "Reply from "+postAuthor;
                }
                else postTitle = postAuthor;
                postMsg = find_value(userpost,"msg").get_str();
            }
            else   // it's a retwist
            {
                postAuthor = find_value(rt.get_obj(),"n").get_str();
                postTitle = postAuthor + " - via " + find_value(userpost,"n").get_str();
                postMsg = find_value(rt.get_obj(),"msg").get_str();
            }

            time_t postTime(find_value(userpost,"time").get_int64());

            Object item;
            item.push_back(Pair("time",postTime));
            item.push_back(Pair("title",postTitle));
            item.push_back(Pair("author",postAuthor));
            item.push_back(Pair("msg",postMsg));
            outputVector.push_back(item);
        }
        catch(exception ex)
        {
            fprintf(stderr, "Warning: RSS couldn't parse a public post, skipping.\n");
            continue;
        }
    }

    sort(outputVector.begin(),outputVector.end(),sortByTime);

    ostringstream ret;

    ret << "<?xml version=\"1.0\" encoding=\"UTF-8\" ?>\n"
        << "<rss version=\"2.0\">\n"
        << "<channel>\n"
        << "  <title>Twister Postboard - " << account << "</title>\n"
        << "  <description>New posts from Twister</description>\n";

    int outputSize = (outputVector.size()>max)?max:outputVector.size();

    for(int i=0;i<outputSize;i++)
    {
        Object item = outputVector[i];
        time_t postTime(find_value(item,"time").get_int64());
        char timeString[100];
        strftime(timeString, sizeof(timeString), "%a, %d %b %Y %H:%M:%S %z", gmtime(&postTime));

        ret << "  <item>\n"
            << "    <title>" << find_value(item,"title").get_str() << "</title>\n"
            << "    <author>" << find_value(item,"author").get_str() << "</author>\n"
            << "    <description>" << find_value(item,"msg").get_str() << "</description>\n"
            << "    <pubDate>" << timeString << "</pubDate>\n"
            << "  </item>\n";
    }

    ret << "</channel>\n"
        << "</rss>\n";

    *output = ret.str();
    return RSS_OK;
#endif
}

#ifdef HAVE_BOOST_REGEX
map<string, string> parseQuery(const string& query)
{
    map<string, string> data;
    boost::regex pattern("([\\w+%]+)=([^&]*)");
    boost::sregex_iterator words_begin = boost::sregex_iterator(query.begin(), query.end(), pattern);
    boost::sregex_iterator words_end = boost::sregex_iterator();

    for (boost::sregex_iterator i = words_begin; i != words_end; i++)
    {
        string key = (*i)[1].str();
        string value = (*i)[2].str();
        data[key] = value;
    }

    return data;
}
#endif

bool sortByTime (Object i,Object j)
{
    return (find_value(i,"time").get_int64()>find_value(j,"time").get_int64());
}
<commit_msg>fix possible compile error with implicit time_t conversion<commit_after>#include "twister_rss.h"
#include "init.h"
#include "bitcoinrpc.h"
#include "json/json_spirit.h"

#include <sstream>
#include <algorithm>
#include <vector>
#include <ctime>
#ifdef HAVE_BOOST_REGEX
    #include <boost/regex.hpp>
#endif
#include <boost/lexical_cast.hpp>

using namespace std;
using namespace json_spirit;

int generateRSS(string uri, string *output)
{
#ifndef HAVE_BOOST_REGEX
    return RSS_ERROR_BOOST_REGEX;
#else
    map<string, string> parameterMap = parseQuery(uri);
    int max = 20; //default value
    string account = parameterMap["account"];
    string strMax = parameterMap["max"];
    if(strMax!="")
    {
        try
        {
            max = boost::lexical_cast<int>(strMax);
        }
        catch(boost::bad_lexical_cast e)
        {
            return RSS_ERROR_NOT_A_NUMBER;
        }
    }

    const Array emptyArray;
    Array accountsArray = listwalletusers(emptyArray,false).get_array();

    // if no account was specified, choose the first one
    if(account=="")
    {
        if(accountsArray.size()>0)
        {
            account = accountsArray[0].get_str();
        }
        else return RSS_ERROR_NO_ACCOUNT;
    }

    // if an account name was specified, check that it exists
    else
    {
        bool accountExists = false;
        for(int i=0;i<accountsArray.size();i++)
        {
            if(accountsArray[i]==account)
                accountExists=true;
        }
        if(!accountExists) return RSS_ERROR_BAD_ACCOUNT;
    }

    // get an array of followed usernames and transform it to required format
    Array params1;
    params1.push_back(account);
    Array followingArray = getfollowing(params1,false).get_array();
    Array postSources;
    for(int i=0;i<followingArray.size();i++)
    {
        Object item;
        item.push_back(Pair("username",followingArray[i]));
        postSources.push_back(item);
    }

    Array params2;
    params2.push_back(max);
    params2.push_back(postSources);
    Array posts = getposts(params2,false).get_array();
    vector<Object> outputVector;

    if(GetBoolArg("-rss_dm",false))     //synchronizing direct messages is disabled by default
    {
        Array params3;
        params3.push_back(account);
        params3.push_back(max);
        params3.push_back(postSources);
        Object messages = getdirectmsgs(params3,false).get_obj();

        for(int j=0;j<messages.size();j++)
        {
            Array userArray = messages[j].value_.get_array();
            for(int i=0;i<userArray.size();i++)
            {
                try
                {
                    if(find_value(userArray[i].get_obj(),"fromMe").get_bool())      //only report received messages
                      continue;

                    string postTitle, postAuthor, postMsg;
                    postAuthor=messages[j].name_;
                    postTitle="Direct Message from "+postAuthor;
                    postMsg=find_value(userArray[i].get_obj(),"text").get_str();
                    Value postTime = find_value(userArray[i].get_obj(),"time");

                    Object item;
                    item.push_back(Pair("time",postTime));
                    item.push_back(Pair("title",postTitle));
                    item.push_back(Pair("author",postAuthor));
                    item.push_back(Pair("msg",postMsg));
                    outputVector.push_back(item);
                }
                catch(exception ex)
                {
                    fprintf(stderr, "Warning: RSS couldn't parse a direct message, skipping.\n");
                    continue;
                }
            }
        }
    }

    for(int i=0;i<posts.size();i++)
    {
        try
        {
            Object userpost = find_value(posts[i].get_obj(),"userpost").get_obj();
            string postTitle, postAuthor, postMsg;
            Value rt = find_value(userpost,"rt");

            if(rt.is_null())    // it's a normal post
            {
                postAuthor = find_value(userpost,"n").get_str();
                Value reply = find_value(userpost,"reply");
                if(!reply.is_null()&&find_value(reply.get_obj(),"n").get_str()==account)
                {
                    postTitle = "Reply from "+postAuthor;
                }
                else postTitle = postAuthor;
                postMsg = find_value(userpost,"msg").get_str();
            }
            else   // it's a retwist
            {
                postAuthor = find_value(rt.get_obj(),"n").get_str();
                postTitle = postAuthor + " - via " + find_value(userpost,"n").get_str();
                postMsg = find_value(rt.get_obj(),"msg").get_str();
            }

            Value postTime = find_value(userpost,"time");

            Object item;
            item.push_back(Pair("time",postTime));
            item.push_back(Pair("title",postTitle));
            item.push_back(Pair("author",postAuthor));
            item.push_back(Pair("msg",postMsg));
            outputVector.push_back(item);
        }
        catch(exception ex)
        {
            fprintf(stderr, "Warning: RSS couldn't parse a public post, skipping.\n");
            continue;
        }
    }

    sort(outputVector.begin(),outputVector.end(),sortByTime);

    ostringstream ret;

    ret << "<?xml version=\"1.0\" encoding=\"UTF-8\" ?>\n"
        << "<rss version=\"2.0\">\n"
        << "<channel>\n"
        << "  <title>Twister Postboard - " << account << "</title>\n"
        << "  <description>New posts from Twister</description>\n";

    int outputSize = (outputVector.size()>max)?max:outputVector.size();

    for(int i=0;i<outputSize;i++)
    {
        Object item = outputVector[i];
        time_t postTime(find_value(item,"time").get_int64());
        char timeString[100];
        strftime(timeString, sizeof(timeString), "%a, %d %b %Y %H:%M:%S %z", gmtime(&postTime));

        ret << "  <item>\n"
            << "    <title>" << find_value(item,"title").get_str() << "</title>\n"
            << "    <author>" << find_value(item,"author").get_str() << "</author>\n"
            << "    <description>" << find_value(item,"msg").get_str() << "</description>\n"
            << "    <pubDate>" << timeString << "</pubDate>\n"
            << "  </item>\n";
    }

    ret << "</channel>\n"
        << "</rss>\n";

    *output = ret.str();
    return RSS_OK;
#endif
}

#ifdef HAVE_BOOST_REGEX
map<string, string> parseQuery(const string& query)
{
    map<string, string> data;
    boost::regex pattern("([\\w+%]+)=([^&]*)");
    boost::sregex_iterator words_begin = boost::sregex_iterator(query.begin(), query.end(), pattern);
    boost::sregex_iterator words_end = boost::sregex_iterator();

    for (boost::sregex_iterator i = words_begin; i != words_end; i++)
    {
        string key = (*i)[1].str();
        string value = (*i)[2].str();
        data[key] = value;
    }

    return data;
}
#endif

bool sortByTime (Object i,Object j)
{
    return (find_value(i,"time").get_int64()>find_value(j,"time").get_int64());
}
<|endoftext|>


<commit_before>#ifndef CONST_HPP_
#define CONST_HPP_

#include <iostream>
#include <string>

// All about "const" key word

// From Item 2 of book "Effective C++" by Scott Meyers:
// Prefer const values to #defines

// Global const value
const int RATE = 110;
const char* const AUTHOR_NAME = "Antony Cherepanov";
// it is better to use std::string instead of char*
const std::string ONE("one");

// How to make class-specific const value
class Container
{
public:
    // Member "value" will be created in each object of the Container class
    const int value = 1;
    // Member "rate" will be created only once and will be used by all objects
    // of the Container class
    static const int rate = 100;
};

// The enum hack
// "The enum hack is worth knowing about for several reasons. First, the
// enum hack behaves in some ways more like a #define than a const
// does, and sometimes that’s what you want. For example, it’s legal to
// take the address of a const, but it’s not legal to take the address of an
// enum, and it’s typically not legal to take the address of a #define,
// either.
//
// A second reason to know about the enum hack is purely pragmatic.
// Lots of code employs it, so you need to recognize it when you see it. In
// fact, the enum hack is a fundamental technique of template metaprogramming."
class Hacked
{
public:
    enum {SIZE = 16};
    int array[SIZE];
};

void ConstExamples();
void ConstReferences();
void ConstPointers();

#endif /* CONST_HPP_ */
<commit_msg>Add info about defines<commit_after>#ifndef CONST_HPP_
#define CONST_HPP_

#include <iostream>
#include <string>

// All about "const" key word

// From Item 2 of book "Effective C++" by Scott Meyers:
// Prefer const values to #defines

// We could use #define to create constant value
#define TEN 10
// Advantages:
// - we could create constant value
// - defines are compatible with C code

// Disadvantages
// - defines are just symbolic names. Compiler substitute define with its value
// in code. Compiler could remove name of the define from symbolic table and
// make your debug session harder. Also because of substitutions your code
// tend to increase in size - in every place where you have difene you will get
// new variable.
// - defines are public. If you put define in header and include that header
// in some other source file, you will be able to access your define in that
// source file. You could constrain visibility of define if you put it into
// .cpp file - it will be visible only in that cpp file.

// Global const value
const int RATE = 110;
const char* const AUTHOR_NAME = "Antony Cherepanov";
// it is better to use std::string instead of char*
const std::string ONE("one");

// How to make class-specific const value
class Container
{
public:
    // Member "value" will be created in each object of the Container class
    const int value = 1;
    // Member "rate" will be created only once and will be used by all objects
    // of the Container class
    static const int rate = 100;
};

// The enum hack
// "The enum hack is worth knowing about for several reasons. First, the
// enum hack behaves in some ways more like a #define than a const
// does, and sometimes that’s what you want. For example, it’s legal to
// take the address of a const, but it’s not legal to take the address of an
// enum, and it’s typically not legal to take the address of a #define,
// either.
//
// A second reason to know about the enum hack is purely pragmatic.
// Lots of code employs it, so you need to recognize it when you see it. In
// fact, the enum hack is a fundamental technique of template metaprogramming."
class Hacked
{
public:
    enum {SIZE = 16};
    int array[SIZE];
};

void ConstExamples();
void ConstReferences();
void ConstPointers();

#endif /* CONST_HPP_ */
<|endoftext|>


<commit_before>#include <AltSoftSerial.h>
void clearVars(void);

/**
 *  \brief Software serial port object
 *  \author volodink
 */
AltSoftSerial mySerial;

volatile uint8_t message[512]; /**<  Maximum message size */

/**
 * The first argument to calculate the checksum
 */
uint8_t CK_A = 0, CK_B = 0;
/**< The second argument to calculate the checksum */
/**
 * The first argument to comparison the checksum
 */
uint8_t CK_AU = 0, CK_BU = 0; /**< The second argument to comparison the checksum */
int gpsStep = 0; /**< If the data suits us, this parameter makes the step */
uint8_t UBX_id = 0; /**< Message ID */
uint8_t UBX_class = 0; /**< Message class */
uint8_t meslenL = 0; /**< variable, with which compare a message */
uint8_t meslenH = 0; /**< variable, which shifts to the left */
int16_t meslen = 0; /**< variable, which assigned a message to its further comparisons */
int count = 0; /**< Message size */
boolean gotUBX = false; /**< The parameter which checks getting data */
boolean POSLLH = 0; /**< Geodetic Position Solution; This message outputs
 * the Geodetic position in the currently selected Ellipsoid.
 */
boolean SVINFO = 0;/**< Space Vehicle Information */
boolean SOL = 0;/**< Navigation Solution Information; This message combines Position,
 * velocity and time solution in ECEF, including accuracy figures
 */
boolean VELNED = 0;/**< Velocity Solution in NED */
long int longitude; /**< Longitude */
float longitudef; /**< Output longitude */
long int latitude; /**< Latitude */
float latitudef; /**< Output latitude */
long int height; /**< Height */
float heightf; /**< Output height */
long int speed3; /**< Speed */
float speed3f; /**< Output speed */
long int hAcc; /**< Horizontal accuracy */
float hAccf; /**< Output horizontal accuracy */
long int vAcc; /**< Vertical accuracy */
float vAccf; /**< Output vertical accuracy */
long int pAcc; /**< 3D accuracy */
float pAccf; /**< Output 3D accuracy */
char bufer[20];

/**
 * GPSfix Type, range 0..5;
 * Is a measure of the communication
 * required for a GPS receiver to acquire
 * satellite signals and navigation data
 */
unsigned char FixType;
unsigned char NumSVs; /**< Number of SVs used in Nav Solution */

int ledPin = 13; /**< working pin */
int fixStep = 0; /**< If parameter FixType is 0x02 or 0x03, this parameter makes the step */
int nofixStep = 0; /**< If parameter FixType is not 0x02 or 0x03, this parameter makes the step */

/**
 * \brief uBlox checksum algorithm
 * \param ubx_data Contains the data over
 * which the checksum is to be calculated.
 */
void ubx_checksum(byte ubx_data) { //рассчет контрольной суммы
    CK_A += ubx_data;
    CK_B += CK_A;
}

/**
 * \brief Getting and comparing data
 */
void getUBX(void) { //Получаем пакет и делаем с ним штуки
    if (Serial.available() > 0) {
        uint8_t c = Serial.read();
        switch (gpsStep) {
        case 0:

            if (c == 0xB5) {
                gpsStep++;
                //                mySerial.print("Start 0xB5 ");
            }
            break;

        case 1:

            if (c == 0x62) {
                gpsStep++;
                //                          mySerial.print("Start 0x62 ");
            } else {
                gpsStep = 0;
                //                        mySerial.println(" Error not62 ");
            }
            break;

        case 2:

            UBX_class = c;
            ubx_checksum(c);
            gpsStep++;
            //                   mySerial.println(""); mySerial.print(" UBX_CLASS = "); mySerial.println(UBX_class);
            break;

        case 3:

            UBX_id = c;
            ubx_checksum(c);
            gpsStep++;
            //                     mySerial.println(""); mySerial.print(" UBX_id = "); mySerial.println(UBX_id);
            break;

        case 4:

            meslenL = c;
            ubx_checksum(c);
            gpsStep++;
            //                 mySerial.print(" ml ");
            break;

        case 5:

            meslenH = c;
            ubx_checksum(c);
            gpsStep++;
            meslen = 0xFF & meslenL;
            meslen |= meslenH << 8;
            count = 0;
            //               mySerial.print(" mh ");
            //                mySerial.print("mlen=");
            //                mySerial.print(meslen);
            break;

        case 6:

            message[count] = c;
            ubx_checksum(c);
            count++;
            if (count == meslen) {
                gpsStep++;
                count = 0;
            }
            break;

        case 7:

            CK_AU = c;
            gpsStep++;

            break;

        case 8:

            CK_BU = c;

            if (CK_A == CK_AU && CK_B == CK_BU) { //сравнение контрольной суммы
                gotUBX = true;
                gpsStep++;
            }

            break;

        }
    }
}

/**
 * \brief UBX-decoder
 */
void decodeUBX(void) {
    if (UBX_class == 0x01) {
        switch (UBX_id) {
        case 0x02:  //NAV-POSLLH
            longitude = 0xFF & message[7];       //долгота
            longitude = longitude << 8;
            longitude |= message[6];
            longitude = longitude << 8;
            longitude |= message[5];
            longitude = longitude << 8;
            longitude |= message[4];
            longitudef = longitude / 10000000.0;

            latitude = 0xFF & message[11];       //широта
            latitude = latitude << 8;
            latitude |= message[10];
            latitude = latitude << 8;
            latitude |= message[9];
            latitude = latitude << 8;
            latitude |= message[8];
            latitudef = latitude / 10000000.0;

            height = 0xFF & message[19];         //высота
            height = height << 8;
            height |= message[18];
            height = height << 8;
            height |= message[17];
            height = height << 8;
            height |= message[16];
            heightf = height / 1000.0;

            hAcc = 0xFF & message[23];          //гор. точность
            hAcc = hAcc << 8;
            hAcc |= message[22];
            hAcc = hAcc << 8;
            hAcc |= message[21];
            hAcc = hAcc << 8;
            hAcc |= message[20];
            hAccf = hAcc / 1000.0;

            vAcc = 0xFF & message[27];          //верт. точность
            vAcc = vAcc << 8;
            vAcc |= message[26];
            vAcc = vAcc << 8;
            vAcc |= message[25];
            vAcc = vAcc << 8;
            vAcc |= message[24];
            vAccf = vAcc / 1000.0;

            POSLLH = true;
            //            mySerial.println("POSLLH got.");
            break;
        case 0x12:  //NAV-VELNED
            speed3 = 0xFF & message[19];        //скорость
            speed3 = speed3 << 8;
            speed3 |= message[18];
            speed3 = speed3 << 8;
            speed3 |= message[17];
            speed3 = speed3 << 8;
            speed3 |= message[16];
            speed3f = speed3 / 100.0;

            VELNED = true;
            //         mySerial.println("VELNED got.");
            break;
        case 0x06:  // NAV-SOL
            NumSVs = message[47];          //кол-во спутников в решении
            FixType = message[10];        //тип фикса

            pAcc = 0xFF & message[27];        //3D точность
            pAcc = pAcc << 8;
            pAcc |= message[26];
            pAcc = pAcc << 8;
            pAcc |= message[25];
            pAcc = pAcc << 8;
            pAcc |= message[24];
            pAccf = pAcc / 100.0;

            if (FixType == 0x02 || FixType == 0x03) {
                fixStep++;
                nofixStep = 0;
            } else {
                nofixStep++;
                fixStep = 0;
            }

            if (fixStep == 12) digitalWrite(ledPin, HIGH);
            if (nofixStep == 12) digitalWrite(ledPin, LOW);

            //       mySerial.println(NumSVs);
            //       mySerial.println(FixType);

            SOL = true;

            //        mySerial.println("SOL got.");

            break;
            //      default:
//            mySerial.print("Unknown ubx id = ");
//           mySerial.println(UBX_id);

        }
    }
}

/**
* \brief Function output data
*/
void sendGPS(void) {
    mySerial.print("A");
    mySerial.print(longitudef, 6);

    mySerial.print("  B");
    mySerial.print(latitudef, 6);

    mySerial.print("  C");
    mySerial.print(heightf, 3);

    mySerial.print("  D");
    mySerial.print(speed3f, 3);

    mySerial.print("  E");
    mySerial.print(vAccf, 3);

    mySerial.print("  F");
    mySerial.print(hAccf, 3);

    mySerial.print("  G");
    mySerial.print(pAccf);

    mySerial.print("  H");
    mySerial.print(NumSVs);

    mySerial.print("  I");
    mySerial.println(FixType);

    longitude = 0;
    latitude = 0;
    height = 0;
    speed3 = 0;
    NumSVs = 0;
    FixType = 0;
}
/**
* \brief Data cleansing
*/
void clearVars(void) {
    gpsStep = 0;
    UBX_id = 0;
    UBX_class = 0;
    meslen = 0;
    count = 0;
    CK_AU = 0;
    CK_BU = 0;
    CK_A = 0;
    CK_B = 0;
    for(int i = 0; i < 60; i++) {
        message[i] = 0;
    }
}
/**
 *  \brief Starts only 1 time after reset
 *  \author volodink
 */
void setup() {
    Serial.begin(57600); /**< setup hardware serial for GPS side */
    mySerial.begin(9600);/**< setup hardware serial for GPS side */
    pinMode(13, OUTPUT);
    mySerial.println("UBX ready.");
    clearVars();
}
/**
 *  \brief Running in the infinite loop
 *  \author volodink
 */
void loop() {
    //mySerial.println("enter loop");

    getUBX();
    if (gotUBX == true) {
        //    mySerial.println("got ubx packet");
        decodeUBX();
        //    mySerial.println("ubx packet decoded");
        if (POSLLH == 1 && VELNED == 1 && SOL == 1) {

            //    mySerial.println("if posllh == 1");
            sendGPS();
            POSLLH = 0;
            VELNED = 0;
            SOL = 0;
        }
        clearVars();
        gotUBX = false;
    }
}
<commit_msg>Update ubx_decoder.cpp<commit_after>#include <AltSoftSerial.h>

/**
 *  \brief Software serial port object
 *  \author volodink
 */
AltSoftSerial mySerial;

volatile uint8_t message[512]; /**<  Maximum message size */

/**
 * The first argument to calculate the checksum
 */
uint8_t CK_A = 0, CK_B = 0;
/**< The second argument to calculate the checksum */
/**
 * The first argument to comparison the checksum
 */
uint8_t CK_AU = 0, CK_BU = 0; /**< The second argument to comparison the checksum */
int gpsStep = 0; /**< If the data suits us, this parameter makes the step */
uint8_t UBX_id = 0; /**< Message ID */
uint8_t UBX_class = 0; /**< Message class */
uint8_t meslenL = 0; /**< variable, with which compare a message */
uint8_t meslenH = 0; /**< variable, which shifts to the left */
int16_t meslen = 0; /**< variable, which assigned a message to its further comparisons */
int count = 0; /**< Message size */
boolean gotUBX = false; /**< The parameter which checks getting data */
boolean POSLLH = 0; /**< Geodetic Position Solution; This message outputs
 * the Geodetic position in the currently selected Ellipsoid.
 */
boolean SVINFO = 0;/**< Space Vehicle Information */
boolean SOL = 0;/**< Navigation Solution Information; This message combines Position,
 * velocity and time solution in ECEF, including accuracy figures
 */
boolean VELNED = 0;/**< Velocity Solution in NED */
long int longitude; /**< Longitude */
float longitudef; /**< Output longitude */
long int latitude; /**< Latitude */
float latitudef; /**< Output latitude */
long int height; /**< Height */
float heightf; /**< Output height */
long int speed3; /**< Speed */
float speed3f; /**< Output speed */
long int hAcc; /**< Horizontal accuracy */
float hAccf; /**< Output horizontal accuracy */
long int vAcc; /**< Vertical accuracy */
float vAccf; /**< Output vertical accuracy */
long int pAcc; /**< 3D accuracy */
float pAccf; /**< Output 3D accuracy */
char bufer[20];

/**
 * GPSfix Type, range 0..5;
 * Is a measure of the communication
 * required for a GPS receiver to acquire
 * satellite signals and navigation data
 */
unsigned char FixType;
unsigned char NumSVs; /**< Number of SVs used in Nav Solution */

int ledPin = 13; /**< working pin */
int fixStep = 0; /**< If parameter FixType is 0x02 or 0x03, this parameter makes the step */
int nofixStep = 0; /**< If parameter FixType is not 0x02 or 0x03, this parameter makes the step */

void clearVars(void);

/**
 * \brief uBlox checksum algorithm
 * \param ubx_data Contains the data over
 * which the checksum is to be calculated.
 */
void ubx_checksum(byte ubx_data) { //рассчет контрольной суммы
    CK_A += ubx_data;
    CK_B += CK_A;
}

/**
 * \brief Getting and comparing data
 */
void getUBX(void) { //Получаем пакет и делаем с ним штуки
    if (Serial.available() > 0) {
        uint8_t c = Serial.read();
        switch (gpsStep) {
        case 0:

            if (c == 0xB5) {
                gpsStep++;
                //                mySerial.print("Start 0xB5 ");
            }
            break;

        case 1:

            if (c == 0x62) {
                gpsStep++;
                //                          mySerial.print("Start 0x62 ");
            } else {
                gpsStep = 0;
                //                        mySerial.println(" Error not62 ");
            }
            break;

        case 2:

            UBX_class = c;
            ubx_checksum(c);
            gpsStep++;
            //                   mySerial.println(""); mySerial.print(" UBX_CLASS = "); mySerial.println(UBX_class);
            break;

        case 3:

            UBX_id = c;
            ubx_checksum(c);
            gpsStep++;
            //                     mySerial.println(""); mySerial.print(" UBX_id = "); mySerial.println(UBX_id);
            break;

        case 4:

            meslenL = c;
            ubx_checksum(c);
            gpsStep++;
            //                 mySerial.print(" ml ");
            break;

        case 5:

            meslenH = c;
            ubx_checksum(c);
            gpsStep++;
            meslen = 0xFF & meslenL;
            meslen |= meslenH << 8;
            count = 0;
            //               mySerial.print(" mh ");
            //                mySerial.print("mlen=");
            //                mySerial.print(meslen);
            break;

        case 6:

            message[count] = c;
            ubx_checksum(c);
            count++;
            if (count == meslen) {
                gpsStep++;
                count = 0;
            }
            break;

        case 7:

            CK_AU = c;
            gpsStep++;

            break;

        case 8:

            CK_BU = c;

            if (CK_A == CK_AU && CK_B == CK_BU) { //сравнение контрольной суммы
                gotUBX = true;
                gpsStep++;
            }

            break;

        }
    }
}

/**
 * \brief UBX-decoder
 */
void decodeUBX(void) {
    if (UBX_class == 0x01) {
        switch (UBX_id) {
        case 0x02:  //NAV-POSLLH
            longitude = 0xFF & message[7];       //долгота
            longitude = longitude << 8;
            longitude |= message[6];
            longitude = longitude << 8;
            longitude |= message[5];
            longitude = longitude << 8;
            longitude |= message[4];
            longitudef = longitude / 10000000.0;

            latitude = 0xFF & message[11];       //широта
            latitude = latitude << 8;
            latitude |= message[10];
            latitude = latitude << 8;
            latitude |= message[9];
            latitude = latitude << 8;
            latitude |= message[8];
            latitudef = latitude / 10000000.0;

            height = 0xFF & message[19];         //высота
            height = height << 8;
            height |= message[18];
            height = height << 8;
            height |= message[17];
            height = height << 8;
            height |= message[16];
            heightf = height / 1000.0;

            hAcc = 0xFF & message[23];          //гор. точность
            hAcc = hAcc << 8;
            hAcc |= message[22];
            hAcc = hAcc << 8;
            hAcc |= message[21];
            hAcc = hAcc << 8;
            hAcc |= message[20];
            hAccf = hAcc / 1000.0;

            vAcc = 0xFF & message[27];          //верт. точность
            vAcc = vAcc << 8;
            vAcc |= message[26];
            vAcc = vAcc << 8;
            vAcc |= message[25];
            vAcc = vAcc << 8;
            vAcc |= message[24];
            vAccf = vAcc / 1000.0;

            POSLLH = true;
            //            mySerial.println("POSLLH got.");
            break;
        case 0x12:  //NAV-VELNED
            speed3 = 0xFF & message[19];        //скорость
            speed3 = speed3 << 8;
            speed3 |= message[18];
            speed3 = speed3 << 8;
            speed3 |= message[17];
            speed3 = speed3 << 8;
            speed3 |= message[16];
            speed3f = speed3 / 100.0;

            VELNED = true;
            //         mySerial.println("VELNED got.");
            break;
        case 0x06:  // NAV-SOL
            NumSVs = message[47];          //кол-во спутников в решении
            FixType = message[10];        //тип фикса

            pAcc = 0xFF & message[27];        //3D точность
            pAcc = pAcc << 8;
            pAcc |= message[26];
            pAcc = pAcc << 8;
            pAcc |= message[25];
            pAcc = pAcc << 8;
            pAcc |= message[24];
            pAccf = pAcc / 100.0;

            if (FixType == 0x02 || FixType == 0x03) {
                fixStep++;
                nofixStep = 0;
            } else {
                nofixStep++;
                fixStep = 0;
            }

            if (fixStep == 12) digitalWrite(ledPin, HIGH);
            if (nofixStep == 12) digitalWrite(ledPin, LOW);

            //       mySerial.println(NumSVs);
            //       mySerial.println(FixType);

            SOL = true;

            //        mySerial.println("SOL got.");

            break;
            //      default:
//            mySerial.print("Unknown ubx id = ");
//           mySerial.println(UBX_id);

        }
    }
}

/**
* \brief Function output data
*/
void sendGPS(void) {
    mySerial.print("A");
    mySerial.print(longitudef, 6);

    mySerial.print("  B");
    mySerial.print(latitudef, 6);

    mySerial.print("  C");
    mySerial.print(heightf, 3);

    mySerial.print("  D");
    mySerial.print(speed3f, 3);

    mySerial.print("  E");
    mySerial.print(vAccf, 3);

    mySerial.print("  F");
    mySerial.print(hAccf, 3);

    mySerial.print("  G");
    mySerial.print(pAccf);

    mySerial.print("  H");
    mySerial.print(NumSVs);

    mySerial.print("  I");
    mySerial.println(FixType);

    longitude = 0;
    latitude = 0;
    height = 0;
    speed3 = 0;
    NumSVs = 0;
    FixType = 0;
}
/**
* \brief Data cleansing
*/
void clearVars(void) {
    gpsStep = 0;
    UBX_id = 0;
    UBX_class = 0;
    meslen = 0;
    count = 0;
    CK_AU = 0;
    CK_BU = 0;
    CK_A = 0;
    CK_B = 0;
    for(int i = 0; i < 60; i++) {
        message[i] = 0;
    }
}
/**
 *  \brief Starts only 1 time after reset
 *  \author volodink
 */
void setup() {
    Serial.begin(57600); /**< setup hardware serial for GPS side */
    mySerial.begin(9600);/**< setup hardware serial for GPS side */
    pinMode(13, OUTPUT);
    mySerial.println("UBX ready.");
    clearVars();
}
/**
 *  \brief Running in the infinite loop
 *  \author volodink
 */
void loop() {
    //mySerial.println("enter loop");

    getUBX();
    if (gotUBX == true) {
        //    mySerial.println("got ubx packet");
        decodeUBX();
        //    mySerial.println("ubx packet decoded");
        if (POSLLH == 1 && VELNED == 1 && SOL == 1) {

            //    mySerial.println("if posllh == 1");
            sendGPS();
            POSLLH = 0;
            VELNED = 0;
            SOL = 0;
        }
        clearVars();
        gotUBX = false;
    }
}
<|endoftext|>