<commit_before>#include <pybind11/pybind11.h>
#include <pybind11/stl.h>

#include "Models/StateSpace/Multivariate/MultivariateStateSpaceRegressionModel.hpp"
#include "Models/StateSpace/Multivariate/PosteriorSamplers/MultivariateStateSpaceModelSampler.hpp"

#include "cpputil/math_utils.hpp"

#include "cpputil/Ptr.hpp"

namespace py = pybind11;
PYBIND11_DECLARE_HOLDER_TYPE(T, BOOM::Ptr<T>, true);

namespace BayesBoom {
  using namespace BOOM;
  void MultivariateStateSpaceModel_def(py::module &boom) {

    py::class_<MultivariateStateSpaceModelBase,
               Model,
               BOOM::Ptr<MultivariateStateSpaceModelBase>>(
                   boom,
                   "MultivariateStateSpaceModelBase",
                   py::multiple_inheritance())
        .def_property_readonly(
            "nseries",
            [](const MultivariateStateSpaceModelBase &model) {
              return model.nseries();
            },
            "The number of time series described by the model.")
        .def_property_readonly(
            "time_dimension",
            [](const MultivariateStateSpaceModelBase &model) {
              return model.time_dimension();
            },
            "The number of time points in the model training data.")
        .def_property_readonly(
            "state_dimension",
            [](const MultivariateStateSpaceModelBase &model) {
              return model.state_dimension();
            },
            "The dimension of the state vector shared across all time series.")
        .def_property_readonly(
            "number_of_state_models",
            [](const MultivariateStateSpaceModelBase &model) {
              return model.number_of_state_models();
            },
            "The number state models defining the shared state vector.")
        .def_property_readonly(
            "log_likelihood",
            [](MultivariateStateSpaceModelBase &model) {
              return model.log_likelihood();
            },
            "The log likelihood under the current set of model parameters.")
        .def("state_contributions",
             [](const MultivariateStateSpaceModelBase &model, int which_state_model) {
               return model.state_contributions(which_state_model);
             },
             py::arg("which_state_model"),
             "Args:\n"
             "  which_state_model: The state model whose contribution is desired.\n"
             "\n"
             "Returns:\n"
             "  A Matrix.  Element (t, d) is the contrubtion of the specified "
             "state model to series d at time t.")
        ;

    py::class_<ConditionallyIndependentMultivariateStateSpaceModelBase,
               MultivariateStateSpaceModelBase,
               BOOM::Ptr<ConditionallyIndependentMultivariateStateSpaceModelBase>>(
                   boom,
                   "ConditionallyIndependentMultivariateStateSpaceModelBase",
                   py::multiple_inheritance())
        ;

    py::class_<MultivariateTimeSeriesRegressionData,
               RegressionData,
               BOOM::Ptr<MultivariateTimeSeriesRegressionData>>(
                   boom,
                   "MultivariateTimeSeriesRegressionData")
        .def(py::init(
            [](double y, const Vector &x, int series, int timestamp) {
              return new MultivariateTimeSeriesRegressionData(
                  y, x, series, timestamp);
            }),
             py::arg("y"),
             py::arg("x"),
             py::arg("series"),
             py::arg("timestamp"),
             "Args:\n"
             "  y: The response variable.\n"
             "  x: A vector of predictors.\n"
             "  series: The identifier of the time series (0.. number of series - 1) to\n"
             "    which this observation belongs.\n"
             "  timestamp: The time-index of the time series (0.. sample_size - 1)\n"
             "    containing this observation.\n")
        ;

    py::class_<MultivariateStateSpaceRegressionModel,
               ConditionallyIndependentMultivariateStateSpaceModelBase,
               PriorPolicy,
               BOOM::Ptr<MultivariateStateSpaceRegressionModel>>(
                   boom,
                   "MultivariateStateSpaceRegressionModel",
                   py::multiple_inheritance())
        .def(py::init(
            [](int xdim, int nseries) {
              return new BOOM::MultivariateStateSpaceRegressionModel(
                  xdim, nseries);
            }),
             py::arg("xdim"),
             py::arg("nseries"),
             "Args:\n"
             "  xdim:  The dimension of the predictor variables.\n"
             "  nseries: The number of time series being modeled.\n")
        .def_property_readonly(
            "xdim",
            [](const MultivariateStateSpaceRegressionModel &model) {
              return model.xdim();
            },
            "Dimension of the vector of predictor variables.")
        .def("add_data",
             [](MultivariateStateSpaceRegressionModel &model,
                const Ptr<MultivariateTimeSeriesRegressionData> &data_point) {
               model.add_data(data_point);
             },
             py::arg("data_point"),
             "Args:\n"
             "  data_point: A MultivariateTimeSeriesRegressionData object\n"
             "    containing information for a single data point.\n")
        .def("add_state",
             [](MultivariateStateSpaceRegressionModel &model,
                SharedStateModel &state_model) {
               model.add_state(Ptr<SharedStateModel>(&state_model));
             },
             "Args:\n"
             "  state_model:  A SharedStateModel object defining an element of"
             " state.\n")
        .def("set_method",
             [](MultivariateStateSpaceRegressionModel &model,
                PosteriorSampler *sampler) {
               model.set_method(Ptr<PosteriorSampler>(sampler));
             })
        .def("set_regression_coefficients",
             [](MultivariateStateSpaceRegressionModel &model,
                const Matrix &coefficients) {
               if (coefficients.nrow() != model.nseries()) {
                 std::ostringstream err;
                 err << "The model describes " << model.nseries()
                     << " series but the input matrix has "
                     << coefficients.nrow() << " rows.";
                 report_error(err.str());
               }
               if (coefficients.ncol() != model.xdim()) {
                 std::ostringstream err;
                 err << "The model has predictor dimension "
                     << model.xdim() << " but the input matrix has "
                     << coefficients.ncol() << "columns.";
                 report_error(err.str());
               }
               for (int i = 0; i < coefficients.nrow(); ++i) {
                 model.observation_model()->model(i)->set_Beta(
                     coefficients.row(i));
               }
             },
             "Args:\n\n"
             "  coefficients:  A boom.Matrix with model.nseries rows and "
             "model.xdim columns.  Each row contains the regression "
             "coefficients for a specific series.\n")
        .def("set_regression_coefficients",
             [](MultivariateStateSpaceRegressionModel &model,
                const Vector &coefficients,
                int which_model) {
               model.observation_model()->model(which_model)->set_Beta(coefficients);
             },
             "Args:\n\n"
             "  coefficients:  The boom.Vector of regression coefficients for "
             "the regression model describing a single series.\n"
             "  which_model: The (integer) index of the model to update.\n")
        .def("set_residual_sd",
             [](MultivariateStateSpaceRegressionModel &model,
                const Vector &residual_sd) {
               if (residual_sd.size() != model.nseries()) {
                 std::ostringstream err;
                 err << "The model describes " << model.nseries()
                     << " series but the input vector has "
                     << residual_sd.size() << " entries.";
                 report_error(err.str());
               }
               for (int i = 0; i < model.nseries(); ++i) {
                 model.observation_model()->model(i)->set_sigsq(
                     square(residual_sd[i]));
               }
             },
             "Args:\n\n"
             "  residual_sd: A boom.Vector containing the residual standard "
             "deviation for each series.\n")
        .def("set_residual_sd",
             [](MultivariateStateSpaceRegressionModel &model,
                double residual_sd,
                int which_model) {
               model.observation_model()->model(which_model)->set_sigsq(
                   square(residual_sd));
             },
             "Args:\n\n"
             "  residual_sd:  The scalar valued residual standard deviation "
             "for a single model.\n"
             "  which_model: The (integer) index of the model to update.\n")
        .def("mle",
             [](MultivariateStateSpaceRegressionModel &model,
                double epsilon,
                int max_tries) {
               return model.mle(epsilon, max_tries);
             },
             py::arg("epsilon"),
             py::arg("max_tries") = 500,
             "Set model parameters to their maximum likelihood estimates.\n"
             "\n"
             "Args:\n"
             "  epsilon: A small positive number.  Absolute changes to log likelihood\n"
             "    less than this value indicate that the algorithm has converged.\n"
             "  max_tries:  Stop trying to optimzize after this many iterations.\n"
             "\n"
             "Returns:\n"
             "  The log likelihood value at the maximum.\n"
             "\n"
             "Effects:\n"
             "  Model parameters are set to the maximum likelihood estimates.\n")
        ;

    py::class_<MultivariateStateSpaceModelSampler,
               PosteriorSampler,
               Ptr<MultivariateStateSpaceModelSampler>>(
                   boom,
                   "MultivariateStateSpaceModelSampler")
        .def(py::init(
            [](MultivariateStateSpaceModelBase *model,
               RNG &seeding_rng = BOOM::GlobalRng::rng) {
              return new MultivariateStateSpaceModelSampler(model, seeding_rng);
            }))
        ;
  }
}  // namespace BayesBoom
<commit_msg>Expose shared_state to the python interface.<commit_after>#include <pybind11/pybind11.h>
#include <pybind11/stl.h>

#include "Models/StateSpace/Multivariate/MultivariateStateSpaceRegressionModel.hpp"
#include "Models/StateSpace/Multivariate/PosteriorSamplers/MultivariateStateSpaceModelSampler.hpp"

#include "cpputil/math_utils.hpp"

#include "cpputil/Ptr.hpp"

namespace py = pybind11;
PYBIND11_DECLARE_HOLDER_TYPE(T, BOOM::Ptr<T>, true);

namespace BayesBoom {
  using namespace BOOM;
  void MultivariateStateSpaceModel_def(py::module &boom) {

    py::class_<MultivariateStateSpaceModelBase,
               Model,
               BOOM::Ptr<MultivariateStateSpaceModelBase>>(
                   boom,
                   "MultivariateStateSpaceModelBase",
                   py::multiple_inheritance())
        .def_property_readonly(
            "nseries",
            [](const MultivariateStateSpaceModelBase &model) {
              return model.nseries();
            },
            "The number of time series described by the model.")
        .def_property_readonly(
            "time_dimension",
            [](const MultivariateStateSpaceModelBase &model) {
              return model.time_dimension();
            },
            "The number of time points in the model training data.")
        .def_property_readonly(
            "state_dimension",
            [](const MultivariateStateSpaceModelBase &model) {
              return model.state_dimension();
            },
            "The dimension of the state vector shared across all time series.")
        .def_property_readonly(
            "number_of_state_models",
            [](const MultivariateStateSpaceModelBase &model) {
              return model.number_of_state_models();
            },
            "The number state models defining the shared state vector.")
        .def_property_readonly(
            "log_likelihood",
            [](MultivariateStateSpaceModelBase &model) {
              return model.log_likelihood();
            },
            "The log likelihood under the current set of model parameters.")
        .def("state_contributions",
             [](const MultivariateStateSpaceModelBase &model, int which_state_model) {
               return model.state_contributions(which_state_model);
             },
             py::arg("which_state_model"),
             "Args:\n"
             "  which_state_model: The state model whose contribution is desired.\n"
             "\n"
             "Returns:\n"
             "  A Matrix.  Element (t, d) is the contrubtion of the specified "
             "state model to series d at time t.")
        .def_property_readonly(
            "shared_state",
            [](const MultivariateStateSpaceModelBase &model) {return model.shared_state();},
            "The full state matrix for the model")
        ;

    py::class_<ConditionallyIndependentMultivariateStateSpaceModelBase,
               MultivariateStateSpaceModelBase,
               BOOM::Ptr<ConditionallyIndependentMultivariateStateSpaceModelBase>>(
                   boom,
                   "ConditionallyIndependentMultivariateStateSpaceModelBase",
                   py::multiple_inheritance())
        ;

    py::class_<MultivariateTimeSeriesRegressionData,
               RegressionData,
               BOOM::Ptr<MultivariateTimeSeriesRegressionData>>(
                   boom,
                   "MultivariateTimeSeriesRegressionData")
        .def(py::init(
            [](double y, const Vector &x, int series, int timestamp) {
              return new MultivariateTimeSeriesRegressionData(
                  y, x, series, timestamp);
            }),
             py::arg("y"),
             py::arg("x"),
             py::arg("series"),
             py::arg("timestamp"),
             "Args:\n"
             "  y: The response variable.\n"
             "  x: A vector of predictors.\n"
             "  series: The identifier of the time series (0.. number of series - 1) to\n"
             "    which this observation belongs.\n"
             "  timestamp: The time-index of the time series (0.. sample_size - 1)\n"
             "    containing this observation.\n")
        ;

    py::class_<MultivariateStateSpaceRegressionModel,
               ConditionallyIndependentMultivariateStateSpaceModelBase,
               PriorPolicy,
               BOOM::Ptr<MultivariateStateSpaceRegressionModel>>(
                   boom,
                   "MultivariateStateSpaceRegressionModel",
                   py::multiple_inheritance())
        .def(py::init(
            [](int xdim, int nseries) {
              return new BOOM::MultivariateStateSpaceRegressionModel(
                  xdim, nseries);
            }),
             py::arg("xdim"),
             py::arg("nseries"),
             "Args:\n"
             "  xdim:  The dimension of the predictor variables.\n"
             "  nseries: The number of time series being modeled.\n")
        .def_property_readonly(
            "xdim",
            [](const MultivariateStateSpaceRegressionModel &model) {
              return model.xdim();
            },
            "Dimension of the vector of predictor variables.")
        .def("add_data",
             [](MultivariateStateSpaceRegressionModel &model,
                const Ptr<MultivariateTimeSeriesRegressionData> &data_point) {
               model.add_data(data_point);
             },
             py::arg("data_point"),
             "Add a single data point to the model.\n\n"
             "Args:\n"
             "  data_point: A MultivariateTimeSeriesRegressionData object\n"
             "    containing information for a single data point.\n")
        .def("add_data",
             [](MultivariateStateSpaceRegressionModel &model,
                const std::vector<int> &time_index,
                const std::vector<int> &series_index,
                const Vector &response,
                const Matrix &predictors) {
               size_t nobs = time_index.size();
               if (series_index.size() != nobs) {
                 report_error("The series_index and time_index must have "
                              "the same number of elements.");
               }
               if (response.size() != nobs) {
                 report_error("The response must have the same number of "
                              "elements as the time_index.");
               }
               if (predictors.nrow() != nobs) {
                 report_error("The matrix of predictors must have the same "
                              "number of rows as the time_index.");
               }
               for (size_t i = 0; i < nobs; ++i) {
                 NEW(MultivariateTimeSeriesRegressionData, data_point)(
                     response[i],
                     predictors.row(i),
                     series_index[i],
                     time_index[i]);
                 model.add_data(data_point);
               }
             },
             py::arg("time_index"),
             py::arg("series_index"),
             py::arg("response"),
             py::arg("predictors"),
             "Add a full data set to the model.\n\n"
             "Args:\n"
             "  time_index:  A list of integers indicating the time stamp "
             "(0, 1, 2...) associated with the observation.\n"
             "  series_index:  A list of integers indicating which series the "
             "observation describes.\n"
             "  response:  A boom.Vector giving the values of each series at "
             "the specified times.\n"
             "  predictors:  A boom.Matrix giving the row of predictor "
             "variables to use for each observation.\n\n"
             "Effect:\n"
             "  The model object is populated with the supplied data.\n")
        .def("add_state",
             [](MultivariateStateSpaceRegressionModel &model,
                SharedStateModel &state_model) {
               model.add_state(Ptr<SharedStateModel>(&state_model));
             },
             "Args:\n"
             "  state_model:  A SharedStateModel object defining an element of"
             " state.\n")
        .def("set_method",
             [](MultivariateStateSpaceRegressionModel &model,
                PosteriorSampler *sampler) {
               model.set_method(Ptr<PosteriorSampler>(sampler));
             })
        .def("set_regression_coefficients",
             [](MultivariateStateSpaceRegressionModel &model,
                const Matrix &coefficients) {
               if (coefficients.nrow() != model.nseries()) {
                 std::ostringstream err;
                 err << "The model describes " << model.nseries()
                     << " series but the input matrix has "
                     << coefficients.nrow() << " rows.";
                 report_error(err.str());
               }
               if (coefficients.ncol() != model.xdim()) {
                 std::ostringstream err;
                 err << "The model has predictor dimension "
                     << model.xdim() << " but the input matrix has "
                     << coefficients.ncol() << "columns.";
                 report_error(err.str());
               }
               for (int i = 0; i < coefficients.nrow(); ++i) {
                 model.observation_model()->model(i)->set_Beta(
                     coefficients.row(i));
               }
             },
             "Args:\n\n"
             "  coefficients:  A boom.Matrix with model.nseries rows and "
             "model.xdim columns.  Each row contains the regression "
             "coefficients for a specific series.\n")
        .def("set_regression_coefficients",
             [](MultivariateStateSpaceRegressionModel &model,
                const Vector &coefficients,
                int which_model) {
               model.observation_model()->model(which_model)->set_Beta(coefficients);
             },
             "Args:\n\n"
             "  coefficients:  The boom.Vector of regression coefficients for "
             "the regression model describing a single series.\n"
             "  which_model: The (integer) index of the model to update.\n")
        .def("set_residual_sd",
             [](MultivariateStateSpaceRegressionModel &model,
                const Vector &residual_sd) {
               if (residual_sd.size() != model.nseries()) {
                 std::ostringstream err;
                 err << "The model describes " << model.nseries()
                     << " series but the input vector has "
                     << residual_sd.size() << " entries.";
                 report_error(err.str());
               }
               for (int i = 0; i < model.nseries(); ++i) {
                 model.observation_model()->model(i)->set_sigsq(
                     square(residual_sd[i]));
               }
             },
             "Args:\n\n"
             "  residual_sd: A boom.Vector containing the residual standard "
             "deviation for each series.\n")
        .def("set_residual_sd",
             [](MultivariateStateSpaceRegressionModel &model,
                double residual_sd,
                int which_model) {
               model.observation_model()->model(which_model)->set_sigsq(
                   square(residual_sd));
             },
             "Args:\n\n"
             "  residual_sd:  The scalar valued residual standard deviation "
             "for a single model.\n"
             "  which_model: The (integer) index of the model to update.\n")
        .def("mle",
             [](MultivariateStateSpaceRegressionModel &model,
                double epsilon,
                int max_tries) {
               return model.mle(epsilon, max_tries);
             },
             py::arg("epsilon"),
             py::arg("max_tries") = 500,
             "Set model parameters to their maximum likelihood estimates.\n"
             "\n"
             "Args:\n"
             "  epsilon: A small positive number.  Absolute changes to log likelihood\n"
             "    less than this value indicate that the algorithm has converged.\n"
             "  max_tries:  Stop trying to optimzize after this many iterations.\n"
             "\n"
             "Returns:\n"
             "  The log likelihood value at the maximum.\n"
             "\n"
             "Effects:\n"
             "  Model parameters are set to the maximum likelihood estimates.\n")
        ;

    py::class_<MultivariateStateSpaceModelSampler,
               PosteriorSampler,
               Ptr<MultivariateStateSpaceModelSampler>>(
                   boom,
                   "MultivariateStateSpaceModelSampler")
        .def(py::init(
            [](MultivariateStateSpaceModelBase *model,
               RNG &seeding_rng = BOOM::GlobalRng::rng) {
              return new MultivariateStateSpaceModelSampler(model, seeding_rng);
            }))
        ;
  }
}  // namespace BayesBoom
<|endoftext|>


<commit_before>#include "GameNetworkWorker.h"
#include "easylogging++.h"
#include "src/common/net/EventId.h"
#include "src/common/net/Deserializer.h"
#include <cassert>

namespace net {

GameNetworkWorker::GameNetworkWorker(quint16 port, std::shared_ptr<GameTimer> game_timer, std::weak_ptr<World> world, std::vector<Client> clients)
  : port_(port),
    game_timer_(game_timer),
    last_packet_id_(1),
    world_ptr_(world),
    send_entities_timer_(),
    detect_disconnect_timer_() {
  QObject::connect(&socket_, SIGNAL(readyRead()), this, SLOT(ReadPendingDatagrams()));
  QObject::connect(&socket_, SIGNAL(error(QAbstractSocket::SocketError)), this, SLOT(SocketError(QAbstractSocket::SocketError)));
  socket_.bind( port_);
  for(Client client : clients) {
    clients_.insert(std::pair<int, Client>(client.GetId(), client));
    last_event_ids_[client.GetId()] = 0; // not event has been ACKed yet for this client (the first event ID will be 1)
    last_received_pings_[client.GetId()] = game_timer_->GetTimestamp();
  }
  QObject::connect(&send_entities_timer_, &QTimer::timeout, this, &GameNetworkWorker::BroadcastWorld);
  send_entities_timer_.start(100);
  QObject::connect(&detect_disconnect_timer_, &QTimer::timeout, this, &GameNetworkWorker::DetectConnectionLost);
  detect_disconnect_timer_.start(10000);
  LOG(DEBUG) << "In game network worker initialized and ready on port " << port_;
}

void GameNetworkWorker::ReadPendingDatagrams() {
  VLOG(9) << "Reading pending datagrams...";
  while(socket_.hasPendingDatagrams()) {
    QByteArray datagram;
    datagram.resize(socket_.pendingDatagramSize());
    QHostAddress sender;
    quint16 sender_port;
    socket_.readDatagram(datagram.data(), datagram.size(), &sender, &sender_port);
    ProcessDatagram(datagram);
  }
}

void GameNetworkWorker::SocketError(QAbstractSocket::SocketError error) {
  LOG(ERROR) << "Error on socket: " << socket_.errorString().toStdWString();
  (void) error;
}

void GameNetworkWorker::ProcessDatagram(const QByteArray& datagram) {
  QDataStream stream(datagram);
  if(!CheckProtocolAndVersion(stream)) {
    LOG(WARNING) << "Protocol or version ID mismatch, dropping datagram.";
    return;
  }
  quint8 client_id = GetClientId(stream);
  if(!CheckStreamStatus(stream)) {
    LOG(WARNING) << "Could not deserialize the client id.";
    return;
  }
  auto it = clients_.find(client_id);
  if(it == clients_.end()) {
    LOG(WARNING) << "Got a datagram with client id " << (int) client_id << " that does not match a registered client, dropping it.";
    return;
  }
  Client client = it->second;
  if(!client.IsConnected()) {
    LOG(WARNING) << "Received a datagram from a previously disconnected client. This server does not support client reconnection ; dropping the datagram";
  }
  VLOG(5) << "Datagram comes from client " << client.GetId();
  quint32 packet_id = GetPacketId(stream);
  if(!CheckStreamStatus(stream)) {
    LOG(WARNING) << "Could not deserialize the packet id.";
    return;
  }
  VLOG(5) << "Received datagram packet id: " << packet_id;
  quint8 packet_type = GetPacketType(stream);
  if(!CheckStreamStatus(stream)) {
    LOG(WARNING) << "Could not deserialize the packet type.";
    return;
  }
  switch(packet_type) {
    case kPingPacketId:
      VLOG(5) << "The datagram is a ping packet.";
      ProcessPingPacket(stream, client, packet_id);
      break;
    case kEventPacketId:
      VLOG(5) << "The datagram is an event packet.";
      ProcessEventPacket(stream, client, packet_id);
      break;
    default:
      LOG(WARNING) << "Received unknown packet type " << packet_type << ", dropping datagram.";
      break;
  }
}

bool GameNetworkWorker::CheckProtocolAndVersion(QDataStream &stream) {
  assert(stream.status() == QDataStream::Ok);
  quint8 protocol_id;
  quint8 server_version;
  stream >> protocol_id;
  stream >> server_version;

  if(!CheckStreamStatus(stream)) {
    LOG(WARNING) << "Could not deserialize the protocol and version.";
    return false;
  }

  quint8 accepted_version = GameNetworkWorker::kServerVersion;
  quint8 accepted_protocol_id = GameNetworkWorker::kProtocolId;
  if(protocol_id != accepted_protocol_id) {
    LOG(WARNING) << "Received unexpected protocol ID: got " << protocol_id << ", expected " << accepted_protocol_id;
    return false;
  }
  if(server_version != accepted_version) {
    LOG(WARNING) << "Received unsupported server version: got " << server_version << ", but only version " << accepted_version << " is supported by this client";
    return false;
  }
  return true;
}

quint8 GameNetworkWorker::GetPacketType(QDataStream& stream) {
  assert(stream.status() == QDataStream::Ok);
  quint8 packet_id;
  stream >> packet_id;
  return packet_id;
}

quint32 GameNetworkWorker::GetPacketId(QDataStream& stream) {
  assert(stream.status() == QDataStream::Ok);
  quint32 packet_id;
  stream >> packet_id;
  return packet_id;
}

quint8 GameNetworkWorker::GetClientId(QDataStream& stream) {
  assert(stream.status() == QDataStream::Ok);
  quint8 client_id;
  stream >> client_id;
  return client_id;
}

void GameNetworkWorker::ProcessPingPacket(QDataStream& stream, const Client& client, quint32 packet_id) {
  (void) stream; // unused for now, the ping content does not matter
  last_received_pings_[client.GetId()] = game_timer_->GetTimestamp();
  SendPongPacket(client, packet_id);
}

void GameNetworkWorker::ProcessEventPacket(QDataStream& stream, const Client& client, quint32 packet_id) {
  assert(stream.status() == QDataStream::Ok);
  (void) packet_id;
  quint8 events_size;
  stream >> events_size;

  if(!CheckStreamStatus(stream)) {
    LOG(WARNING) << "Could not deserialize the number of events.";
    return;
  }

  for(int i = 0; i < events_size; i++) {
    std::unique_ptr<InGameEvent> event = Deserializer::DeserializeInGameEvent(stream);
    if(stream.status() != QDataStream::Ok) {
      LOG(WARNING) << "Stream is in an invalid state after having deserialized the event. Continuing anyway.";
    }
    if(event.get() == nullptr) {
      LOG(WARNING) << "Could not deserialize the received event. Dropping remaining data.";
      return;
    }
    VLOG(9) << "Deserialized event has ID " << event->GetId();
    if(event->GetId() <= last_event_ids_[client.GetId()]) {
      VLOG(8) << "The client has sent an event that has already been processed (according to its ID).";
      continue;
    }
    auto client_event = std::unique_ptr<BaseClientEvent>(new BaseClientEvent(std::move(event), client, game_timer_->GetTimestamp()));
    HandlePendingEvent(std::move(client_event));
  }
  EmitReadyEvents(client);
}

// TODO: defends against a client attacking the server by sending bad events id
void GameNetworkWorker::HandlePendingEvent(std::unique_ptr<BaseClientEvent> event) {
  Client client = event->GetClient();
  auto pair = std::pair<const quint32, std::unique_ptr<BaseClientEvent>>(event->GetEvent()->GetId(), std::move(event));
  event_cache_[client.GetId()].insert(std::move(pair));
}

void GameNetworkWorker::EmitReadyEvents(Client client) {
  quint8 client_id = client.GetId();
  // look if the next event from this client (ordered by id) has been received
  auto it = event_cache_[client_id].find(last_event_ids_[client_id] + 1);
  bool send_ack = false;
  while(it != event_cache_[client_id].end()) {
    // if yes, send it to the application
    EmitterVisitor visitor(this, it->second.get());
    it->second->GetEvent()->Accept(visitor);
    event_cache_[client_id].erase(it);
    // this event will be ACKed
    last_event_ids_[client_id]++;
    send_ack = true;
    // update iterator
    it = event_cache_[client_id].find(last_event_ids_[client_id] + 1);
  }
  if(send_ack) {
    SendAckPacket(client, last_event_ids_[client_id]);
  }
}

void GameNetworkWorker::SendAckPacket(const Client& client, quint32 event_id) {
  QByteArray buffer;
  QDataStream stream(&buffer, QIODevice::OpenModeFlag::WriteOnly);
  PrepareHeader(stream, kEventAckPacketId);
  stream << event_id;
  assert(stream.status() == QDataStream::Ok);
  socket_.writeDatagram(buffer, client.GetAddress(), client.GetPort());
  VLOG(9) << "Sent ACK packet for ids <= " << (int) event_id << " to client ID " << (int) client.GetId();
}

void GameNetworkWorker::SendPongPacket(const Client& client, quint32 packet_id) {
  QByteArray buffer;
  QDataStream stream(&buffer, QIODevice::OpenModeFlag::WriteOnly);
  PrepareHeader(stream, kPingPacketId);
  stream << packet_id;
  quint32 timestamp = game_timer_->GetTimestamp();
  stream << timestamp;
  assert(stream.status() == QDataStream::Ok);
  socket_.writeDatagram(buffer, client.GetAddress(), client.GetPort());
  VLOG(9) << "Answered pong with timestamp " << timestamp << " to the ping with id " << packet_id;
  VLOG(9) << "Client was at address " << client.GetAddress().toString().toStdWString() << ":" << client.GetPort();
}

quint32 GameNetworkWorker::PrepareHeader(QDataStream& stream, quint8 packet_type) {
  assert(stream.status() == QDataStream::Ok);
  quint32 packet_id = GetNextPacketId();
  stream << kProtocolId;
  stream << kServerVersion;
  stream << packet_id;
  stream << packet_type;
  return packet_id;
}

quint32 GameNetworkWorker::GetNextPacketId() {
  last_packet_id_ += 1;
  return last_packet_id_;
}

void GameNetworkWorker::EmitEvent(ClientEvent<BombEvent> event) {
  VLOG(EVENT_READY_LOG_LEVEL) << "Network worker: bomb event ready / client id: " << (int) event.GetClientId() << " / id: " << event.GetEventData().GetId();
  emit BombEventReceived(event);
}

void GameNetworkWorker::EmitEvent(ClientEvent<MoveEvent> event) {
  VLOG(EVENT_READY_LOG_LEVEL) << "Network worker: move event ready / client id: " << (int) event.GetClientId() << " / id: " << event.GetEventData().GetId();
  emit MoveEventReceived(event);
}

void GameNetworkWorker::EmitEvent(ClientEvent<PlayerLeftEvent> event) {
  VLOG(EVENT_READY_LOG_LEVEL) << "Network worker: player left event ready / client id: " << (int) event.GetClientId() << " / id: " << event.GetEventData().GetId();
  emit PlayerLeftEventReceived(event);
}

void GameNetworkWorker::BroadcastWorld() {
  quint32 timestamp = game_timer_->GetTimestamp();
  VLOG(9) << "Broadcasting the world state to clients, timestamp is " << timestamp;
  auto world = world_ptr_.lock();
  if(world.get() == nullptr) {
    LOG(WARNING) << "The World has gone out of scope, can not send it to clients!";
    return;
  }
  std::vector<Entity*> to_send;
  // do not put to many entites in the same packet to prevent the packet from
  // being cut by the network layer
  for(int i = 0; i < world->GetWidth(); i++)  {
    for(int j = 0; j < world->GetHeight(); j++) {
      QPoint point(i, j);// TODO check indices
      for(auto it = world->IteratorAtBegin(point); it != world->IteratorAtEnd(point); ++it) {
        to_send.push_back(it->get());
      }
    }
  }
  for(auto it = world->CharacterIteratorBegin(); it != world->CharacterIteratorEnd(); ++it) {
    to_send.push_back(it->get());
  }
  auto it = to_send.begin();
  while(it != to_send.end()) {
    QByteArray buffer;
    QDataStream stream(&buffer, QIODevice::OpenModeFlag::WriteOnly);
    quint32 packet_id = PrepareHeader(stream, kEntitiesPacketId);
    stream << timestamp;
    std::vector<Entity*> packet;
    for(int i = 0; i < 10 && it != to_send.end(); ++i) {
      packet.push_back(*it);
      ++it;
    }
    stream << (quint8) packet.size();
    for(Entity* entity: packet) {
      stream << *entity;
    }
    for(auto p: clients_) {
      socket_.writeDatagram(buffer, buffer.size(), p.second.GetAddress(), p.second.GetPort());
    }
    VLOG(9) << "Sent the entities packet with ID: " << packet_id << " to all the clients";
  }
}

void GameNetworkWorker::DetectConnectionLost() {
  for(auto it = last_received_pings_.begin(); it != last_received_pings_.end(); ++it) {
    if(game_timer_->GetTimestamp() > it->second + kDisconnectTimeout) {
      auto itt = clients_.find(it->first);
      assert(itt != clients_.end());
      Client client = itt->second;
      client.SetConnected(false);
      LOG(INFO) << "The client " << (int) client.GetId() << " did not send a ping for at least " << (int) kDisconnectTimeout << " seconds ; marking it as disconnected.";
      emit ConnectionLost(client);
      last_received_pings_.erase(it);
    }
  }
}

bool GameNetworkWorker::CheckStreamStatus(const QDataStream& stream) const {
  if(stream.status() != QDataStream::Ok) {
    LOG(WARNING) << "Deserialization stream status is not OK (code is: " << (int) stream.status() << "), the current message will be lost.";
    return false;
  }
  return true;
}

}
<commit_msg>Do not hardcode the frequency of disconnection checks.<commit_after>#include "GameNetworkWorker.h"
#include "easylogging++.h"
#include "src/common/net/EventId.h"
#include "src/common/net/Deserializer.h"
#include <cassert>

namespace net {

GameNetworkWorker::GameNetworkWorker(quint16 port, std::shared_ptr<GameTimer> game_timer, std::weak_ptr<World> world, std::vector<Client> clients)
  : port_(port),
    game_timer_(game_timer),
    last_packet_id_(1),
    world_ptr_(world),
    send_entities_timer_(),
    detect_disconnect_timer_() {
  QObject::connect(&socket_, SIGNAL(readyRead()), this, SLOT(ReadPendingDatagrams()));
  QObject::connect(&socket_, SIGNAL(error(QAbstractSocket::SocketError)), this, SLOT(SocketError(QAbstractSocket::SocketError)));
  socket_.bind( port_);
  for(Client client : clients) {
    clients_.insert(std::pair<int, Client>(client.GetId(), client));
    last_event_ids_[client.GetId()] = 0; // not event has been ACKed yet for this client (the first event ID will be 1)
    last_received_pings_[client.GetId()] = game_timer_->GetTimestamp();
  }
  QObject::connect(&send_entities_timer_, &QTimer::timeout, this, &GameNetworkWorker::BroadcastWorld);
  send_entities_timer_.start(100);
  QObject::connect(&detect_disconnect_timer_, &QTimer::timeout, this, &GameNetworkWorker::DetectConnectionLost);
  detect_disconnect_timer_.start(kDisconnectTimeout / 3);
  LOG(DEBUG) << "In game network worker initialized and ready on port " << port_;
}

void GameNetworkWorker::ReadPendingDatagrams() {
  VLOG(9) << "Reading pending datagrams...";
  while(socket_.hasPendingDatagrams()) {
    QByteArray datagram;
    datagram.resize(socket_.pendingDatagramSize());
    QHostAddress sender;
    quint16 sender_port;
    socket_.readDatagram(datagram.data(), datagram.size(), &sender, &sender_port);
    ProcessDatagram(datagram);
  }
}

void GameNetworkWorker::SocketError(QAbstractSocket::SocketError error) {
  LOG(ERROR) << "Error on socket: " << socket_.errorString().toStdWString();
  (void) error;
}

void GameNetworkWorker::ProcessDatagram(const QByteArray& datagram) {
  QDataStream stream(datagram);
  if(!CheckProtocolAndVersion(stream)) {
    LOG(WARNING) << "Protocol or version ID mismatch, dropping datagram.";
    return;
  }
  quint8 client_id = GetClientId(stream);
  if(!CheckStreamStatus(stream)) {
    LOG(WARNING) << "Could not deserialize the client id.";
    return;
  }
  auto it = clients_.find(client_id);
  if(it == clients_.end()) {
    LOG(WARNING) << "Got a datagram with client id " << (int) client_id << " that does not match a registered client, dropping it.";
    return;
  }
  Client client = it->second;
  if(!client.IsConnected()) {
    LOG(WARNING) << "Received a datagram from a previously disconnected client. This server does not support client reconnection ; dropping the datagram";
  }
  VLOG(5) << "Datagram comes from client " << client.GetId();
  quint32 packet_id = GetPacketId(stream);
  if(!CheckStreamStatus(stream)) {
    LOG(WARNING) << "Could not deserialize the packet id.";
    return;
  }
  VLOG(5) << "Received datagram packet id: " << packet_id;
  quint8 packet_type = GetPacketType(stream);
  if(!CheckStreamStatus(stream)) {
    LOG(WARNING) << "Could not deserialize the packet type.";
    return;
  }
  switch(packet_type) {
    case kPingPacketId:
      VLOG(5) << "The datagram is a ping packet.";
      ProcessPingPacket(stream, client, packet_id);
      break;
    case kEventPacketId:
      VLOG(5) << "The datagram is an event packet.";
      ProcessEventPacket(stream, client, packet_id);
      break;
    default:
      LOG(WARNING) << "Received unknown packet type " << packet_type << ", dropping datagram.";
      break;
  }
}

bool GameNetworkWorker::CheckProtocolAndVersion(QDataStream &stream) {
  assert(stream.status() == QDataStream::Ok);
  quint8 protocol_id;
  quint8 server_version;
  stream >> protocol_id;
  stream >> server_version;

  if(!CheckStreamStatus(stream)) {
    LOG(WARNING) << "Could not deserialize the protocol and version.";
    return false;
  }

  quint8 accepted_version = GameNetworkWorker::kServerVersion;
  quint8 accepted_protocol_id = GameNetworkWorker::kProtocolId;
  if(protocol_id != accepted_protocol_id) {
    LOG(WARNING) << "Received unexpected protocol ID: got " << protocol_id << ", expected " << accepted_protocol_id;
    return false;
  }
  if(server_version != accepted_version) {
    LOG(WARNING) << "Received unsupported server version: got " << server_version << ", but only version " << accepted_version << " is supported by this client";
    return false;
  }
  return true;
}

quint8 GameNetworkWorker::GetPacketType(QDataStream& stream) {
  assert(stream.status() == QDataStream::Ok);
  quint8 packet_id;
  stream >> packet_id;
  return packet_id;
}

quint32 GameNetworkWorker::GetPacketId(QDataStream& stream) {
  assert(stream.status() == QDataStream::Ok);
  quint32 packet_id;
  stream >> packet_id;
  return packet_id;
}

quint8 GameNetworkWorker::GetClientId(QDataStream& stream) {
  assert(stream.status() == QDataStream::Ok);
  quint8 client_id;
  stream >> client_id;
  return client_id;
}

void GameNetworkWorker::ProcessPingPacket(QDataStream& stream, const Client& client, quint32 packet_id) {
  (void) stream; // unused for now, the ping content does not matter
  last_received_pings_[client.GetId()] = game_timer_->GetTimestamp();
  SendPongPacket(client, packet_id);
}

void GameNetworkWorker::ProcessEventPacket(QDataStream& stream, const Client& client, quint32 packet_id) {
  assert(stream.status() == QDataStream::Ok);
  (void) packet_id;
  quint8 events_size;
  stream >> events_size;

  if(!CheckStreamStatus(stream)) {
    LOG(WARNING) << "Could not deserialize the number of events.";
    return;
  }

  for(int i = 0; i < events_size; i++) {
    std::unique_ptr<InGameEvent> event = Deserializer::DeserializeInGameEvent(stream);
    if(stream.status() != QDataStream::Ok) {
      LOG(WARNING) << "Stream is in an invalid state after having deserialized the event. Continuing anyway.";
    }
    if(event.get() == nullptr) {
      LOG(WARNING) << "Could not deserialize the received event. Dropping remaining data.";
      return;
    }
    VLOG(9) << "Deserialized event has ID " << event->GetId();
    if(event->GetId() <= last_event_ids_[client.GetId()]) {
      VLOG(8) << "The client has sent an event that has already been processed (according to its ID).";
      continue;
    }
    auto client_event = std::unique_ptr<BaseClientEvent>(new BaseClientEvent(std::move(event), client, game_timer_->GetTimestamp()));
    HandlePendingEvent(std::move(client_event));
  }
  EmitReadyEvents(client);
}

// TODO: defends against a client attacking the server by sending bad events id
void GameNetworkWorker::HandlePendingEvent(std::unique_ptr<BaseClientEvent> event) {
  Client client = event->GetClient();
  auto pair = std::pair<const quint32, std::unique_ptr<BaseClientEvent>>(event->GetEvent()->GetId(), std::move(event));
  event_cache_[client.GetId()].insert(std::move(pair));
}

void GameNetworkWorker::EmitReadyEvents(Client client) {
  quint8 client_id = client.GetId();
  // look if the next event from this client (ordered by id) has been received
  auto it = event_cache_[client_id].find(last_event_ids_[client_id] + 1);
  bool send_ack = false;
  while(it != event_cache_[client_id].end()) {
    // if yes, send it to the application
    EmitterVisitor visitor(this, it->second.get());
    it->second->GetEvent()->Accept(visitor);
    event_cache_[client_id].erase(it);
    // this event will be ACKed
    last_event_ids_[client_id]++;
    send_ack = true;
    // update iterator
    it = event_cache_[client_id].find(last_event_ids_[client_id] + 1);
  }
  if(send_ack) {
    SendAckPacket(client, last_event_ids_[client_id]);
  }
}

void GameNetworkWorker::SendAckPacket(const Client& client, quint32 event_id) {
  QByteArray buffer;
  QDataStream stream(&buffer, QIODevice::OpenModeFlag::WriteOnly);
  PrepareHeader(stream, kEventAckPacketId);
  stream << event_id;
  assert(stream.status() == QDataStream::Ok);
  socket_.writeDatagram(buffer, client.GetAddress(), client.GetPort());
  VLOG(9) << "Sent ACK packet for ids <= " << (int) event_id << " to client ID " << (int) client.GetId();
}

void GameNetworkWorker::SendPongPacket(const Client& client, quint32 packet_id) {
  QByteArray buffer;
  QDataStream stream(&buffer, QIODevice::OpenModeFlag::WriteOnly);
  PrepareHeader(stream, kPingPacketId);
  stream << packet_id;
  quint32 timestamp = game_timer_->GetTimestamp();
  stream << timestamp;
  assert(stream.status() == QDataStream::Ok);
  socket_.writeDatagram(buffer, client.GetAddress(), client.GetPort());
  VLOG(9) << "Answered pong with timestamp " << timestamp << " to the ping with id " << packet_id;
  VLOG(9) << "Client was at address " << client.GetAddress().toString().toStdWString() << ":" << client.GetPort();
}

quint32 GameNetworkWorker::PrepareHeader(QDataStream& stream, quint8 packet_type) {
  assert(stream.status() == QDataStream::Ok);
  quint32 packet_id = GetNextPacketId();
  stream << kProtocolId;
  stream << kServerVersion;
  stream << packet_id;
  stream << packet_type;
  return packet_id;
}

quint32 GameNetworkWorker::GetNextPacketId() {
  last_packet_id_ += 1;
  return last_packet_id_;
}

void GameNetworkWorker::EmitEvent(ClientEvent<BombEvent> event) {
  VLOG(EVENT_READY_LOG_LEVEL) << "Network worker: bomb event ready / client id: " << (int) event.GetClientId() << " / id: " << event.GetEventData().GetId();
  emit BombEventReceived(event);
}

void GameNetworkWorker::EmitEvent(ClientEvent<MoveEvent> event) {
  VLOG(EVENT_READY_LOG_LEVEL) << "Network worker: move event ready / client id: " << (int) event.GetClientId() << " / id: " << event.GetEventData().GetId();
  emit MoveEventReceived(event);
}

void GameNetworkWorker::EmitEvent(ClientEvent<PlayerLeftEvent> event) {
  VLOG(EVENT_READY_LOG_LEVEL) << "Network worker: player left event ready / client id: " << (int) event.GetClientId() << " / id: " << event.GetEventData().GetId();
  emit PlayerLeftEventReceived(event);
}

void GameNetworkWorker::BroadcastWorld() {
  quint32 timestamp = game_timer_->GetTimestamp();
  VLOG(9) << "Broadcasting the world state to clients, timestamp is " << timestamp;
  auto world = world_ptr_.lock();
  if(world.get() == nullptr) {
    LOG(WARNING) << "The World has gone out of scope, can not send it to clients!";
    return;
  }
  std::vector<Entity*> to_send;
  // do not put to many entites in the same packet to prevent the packet from
  // being cut by the network layer
  for(int i = 0; i < world->GetWidth(); i++)  {
    for(int j = 0; j < world->GetHeight(); j++) {
      QPoint point(i, j);// TODO check indices
      for(auto it = world->IteratorAtBegin(point); it != world->IteratorAtEnd(point); ++it) {
        to_send.push_back(it->get());
      }
    }
  }
  for(auto it = world->CharacterIteratorBegin(); it != world->CharacterIteratorEnd(); ++it) {
    to_send.push_back(it->get());
  }
  auto it = to_send.begin();
  while(it != to_send.end()) {
    QByteArray buffer;
    QDataStream stream(&buffer, QIODevice::OpenModeFlag::WriteOnly);
    quint32 packet_id = PrepareHeader(stream, kEntitiesPacketId);
    stream << timestamp;
    std::vector<Entity*> packet;
    for(int i = 0; i < 10 && it != to_send.end(); ++i) {
      packet.push_back(*it);
      ++it;
    }
    stream << (quint8) packet.size();
    for(Entity* entity: packet) {
      stream << *entity;
    }
    for(auto p: clients_) {
      socket_.writeDatagram(buffer, buffer.size(), p.second.GetAddress(), p.second.GetPort());
    }
    VLOG(9) << "Sent the entities packet with ID: " << packet_id << " to all the clients";
  }
}

void GameNetworkWorker::DetectConnectionLost() {
  for(auto it = last_received_pings_.begin(); it != last_received_pings_.end(); ++it) {
    if(game_timer_->GetTimestamp() > it->second + kDisconnectTimeout) {
      auto itt = clients_.find(it->first);
      assert(itt != clients_.end());
      Client client = itt->second;
      client.SetConnected(false);
      LOG(INFO) << "The client " << (int) client.GetId() << " did not send a ping for at least " << (int) kDisconnectTimeout << " seconds ; marking it as disconnected.";
      emit ConnectionLost(client);
      last_received_pings_.erase(it);
    }
  }
}

bool GameNetworkWorker::CheckStreamStatus(const QDataStream& stream) const {
  if(stream.status() != QDataStream::Ok) {
    LOG(WARNING) << "Deserialization stream status is not OK (code is: " << (int) stream.status() << "), the current message will be lost.";
    return false;
  }
  return true;
}

}
<|endoftext|>


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

/**
 * @file FlightTaskManualPosition.cpp
 */

#include "FlightTaskManualPosition.hpp"
#include <mathlib/mathlib.h>
#include <float.h>

using namespace matrix;

FlightTaskManualPosition::FlightTaskManualPosition(control::SuperBlock *parent, const char *name) :
	FlightTaskManualAltitude(parent, name),
	_vel_xy_manual_max(parent, "MPC_VEL_MANUAL", false),
	_acc_xy_max(parent, "MPC_ACC_HOR_MAX", false),
	_vel_hold_thr_xy(parent,  "MPC_HOLD_MAX_XY", false)
{}

bool FlightTaskManualPosition::activate()
{
	_pos_sp_xy = matrix::Vector2f(NAN, NAN);
	_vel_sp_xy = matrix::Vector2f(0.0f, 0.0f);
	return FlightTaskManualAltitude::activate();
}

void FlightTaskManualPosition::_scaleSticks()
{
	/* Use same scaling as for FlightTaskManualAltitude to
	 * get yaw and z */
	FlightTaskManualAltitude::_scaleSticks();

	/* Constrain length of stick inputs to 1 for xy*/
	matrix::Vector2f stick_xy(_sticks_expo(0), _sticks_expo(1));

	float mag = math::constrain(stick_xy.length(), 0.0f, 1.0f);

	if (mag > FLT_EPSILON) {
		stick_xy = stick_xy.normalized() * mag;
	}

	/* Scale to velocity.*/
	_vel_sp_xy = stick_xy * _vel_xy_manual_max.get();

	/* Rotate setpoint into local frame. */
	matrix::Vector3f vel_sp{_vel_sp_xy(0), _vel_sp_xy(1), 0.0f};
	vel_sp = (matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, _yaw)) * vel_sp);
	_vel_sp_xy = matrix::Vector2f(vel_sp(0), vel_sp(1));
}

void FlightTaskManualPosition::_updateXYlock()
{
	/* If position lock is not active, position setpoint is set to NAN.*/
	const float vel_xy_norm = Vector2f(&_velocity(0)).length();
	const bool apply_brake = _vel_sp_xy.length() < FLT_EPSILON;
	const bool stopped = (_vel_hold_thr_xy.get() < FLT_EPSILON || vel_xy_norm < _vel_hold_thr_xy.get());

	if (apply_brake && stopped && !PX4_ISFINITE(_pos_sp_xy(0))) {
		_pos_sp_xy = matrix::Vector2f(&_position(0));

	} else if (!apply_brake) {
		_pos_sp_xy = _pos_sp_xy * NAN;
	}
}

void FlightTaskManualPosition::_updateSetpoints()
{
	FlightTaskManualAltitude::_updateSetpoints(); // get yaw and z setpoints
	_updateXYlock(); // check for position lock
}

bool FlightTaskManualPosition::update()
{
	_scaleSticks();
	_updateSetpoints();

	_setPositionSetpoint(Vector3f(_pos_sp_xy(0), _pos_sp_xy(1), _pos_sp_z));
	_setVelocitySetpoint(Vector3f(_vel_sp_xy(0), _vel_sp_xy(1), _vel_sp_z));
	_setYawSetpoint(_yaw_sp);
	_setYawspeedSetpoint(_yaw_rate_sp);
	_setThrustSetpoint(Vector3f(NAN, NAN, NAN));

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

/**
 * @file FlightTaskManualPosition.cpp
 */

#include "FlightTaskManualPosition.hpp"
#include <mathlib/mathlib.h>
#include <float.h>

using namespace matrix;

FlightTaskManualPosition::FlightTaskManualPosition(control::SuperBlock *parent, const char *name) :
	FlightTaskManualAltitude(parent, name),
	_vel_xy_manual_max(parent, "MPC_VEL_MANUAL", false),
	_acc_xy_max(parent, "MPC_ACC_HOR_MAX", false),
	_vel_hold_thr_xy(parent,  "MPC_HOLD_MAX_XY", false)
{}

bool FlightTaskManualPosition::activate()
{
	_pos_sp_xy = matrix::Vector2f(NAN, NAN);
	_vel_sp_xy = matrix::Vector2f(0.0f, 0.0f);
	return FlightTaskManualAltitude::activate();
}

void FlightTaskManualPosition::_scaleSticks()
{
	/* Use same scaling as for FlightTaskManualAltitude to
	 * get yaw and z */
	FlightTaskManualAltitude::_scaleSticks();

	/* Constrain length of stick inputs to 1 for xy*/
	matrix::Vector2f stick_xy(_sticks_expo(0), _sticks_expo(1));

	float mag = math::constrain(stick_xy.length(), 0.0f, 1.0f);

	if (mag > FLT_EPSILON) {
		stick_xy = stick_xy.normalized() * mag;
	}

	/* Scale to velocity.*/
	_vel_sp_xy = stick_xy * _vel_xy_manual_max.get();

	/* Rotate setpoint into local frame. */
	matrix::Quatf q_yaw = matrix::AxisAnglef(matrix::Vector3f(0.0f, 0.0f, 1.0f), _yaw);
	matrix::Vector3f vel_world = q_yaw.conjugate(matrix::Vector3f(_vel_sp_xy(0), _vel_sp_xy(1), 0.0f));
	_vel_sp_xy = matrix::Vector2f(vel_world(0), vel_world(1));
}

void FlightTaskManualPosition::_updateXYlock()
{
	/* If position lock is not active, position setpoint is set to NAN.*/
	const float vel_xy_norm = Vector2f(&_velocity(0)).length();
	const bool apply_brake = _vel_sp_xy.length() < FLT_EPSILON;
	const bool stopped = (_vel_hold_thr_xy.get() < FLT_EPSILON || vel_xy_norm < _vel_hold_thr_xy.get());

	if (apply_brake && stopped && !PX4_ISFINITE(_pos_sp_xy(0))) {
		_pos_sp_xy = matrix::Vector2f(&_position(0));

	} else if (!apply_brake) {
		/* Don't use position setpoint */
		_pos_sp_xy = _pos_sp_xy * NAN;
	}
}

void FlightTaskManualPosition::_updateSetpoints()
{
	FlightTaskManualAltitude::_updateSetpoints(); // get yaw and z setpoints
	_updateXYlock(); // check for position lock
}

bool FlightTaskManualPosition::update()
{
	_scaleSticks();
	_updateSetpoints();

	_setPositionSetpoint(Vector3f(_pos_sp_xy(0), _pos_sp_xy(1), _pos_sp_z));
	_setVelocitySetpoint(Vector3f(_vel_sp_xy(0), _vel_sp_xy(1), _vel_sp_z));
	_setYawSetpoint(_yaw_sp);
	_setYawspeedSetpoint(_yaw_rate_sp);
	_setThrustSetpoint(Vector3f(NAN, NAN, NAN));

	return true;
}
<|endoftext|>


<commit_before>/*
 * Copyright(c) Sophist Solutions, Inc. 1990-2017.  All rights reserved
 */
#include "../../StroikaPreComp.h"

#include "../../Characters/CString/Utilities.h"
#include "../../Characters/String.h"
#include "../../Characters/StringBuilder.h"
#include "../../Characters/String_Constant.h"
#include "../../DataExchange/Variant/CharacterDelimitedLines/Reader.h"
#include "../../Execution/Finally.h"
#include "../../Memory/SmallStackBuffer.h"

#include "FileInputStream.h"

#include "MountedFilesystem.h"

using namespace Stroika::Foundation;
using namespace Stroika::Foundation::Characters;
using namespace Stroika::Foundation::Containers;
using namespace Stroika::Foundation::IO;
using namespace Stroika::Foundation::IO::FileSystem;

// Comment this in to turn on aggressive noisy DbgTrace in this module
//#define   USE_NOISY_TRACE_IN_THIS_MODULE_       1

#if qPlatform_Linux
namespace {
    Collection<MountedFilesystemType> ReadMountInfo_FromProcFSMounts_ ()
    {
        /*
         *  I haven't found this clearly documented yet, but it appears that a filesystem can be over-mounted.
         *  See https://www.kernel.org/doc/Documentation/filesystems/ramfs-rootfs-initramfs.txt
         *
         *  So the last one with a given mount point in the file wins.
         */
        Collection<MountedFilesystemType>                      results;
        DataExchange::Variant::CharacterDelimitedLines::Reader reader{{' ', '\t'}};
        // Note - /procfs files always unseekable
        static const String_Constant kProcMountsFileName_{L"/proc/mounts"};
        for (Sequence<String> line : reader.ReadMatrix (FileInputStream::mk (kProcMountsFileName_, FileInputStream::eNotSeekable))) {
#if USE_NOISY_TRACE_IN_THIS_MODULE_
            DbgTrace (L"in IO::FileSystem::{}::ReadMountInfo_FromProcFSMounts_ linesize=%d, line[0]=%s", line.size (), line.empty () ? L"" : line[0].c_str ());
#endif
            //
            // https://www.centos.org/docs/5/html/5.2/Deployment_Guide/s2-proc-mounts.html
            //
            //  1 - device name
            //  2 - mounted on
            //  3 - fstype
            //
            if (line.size () >= 3) {
                String devName = line[0];
                // procfs/mounts often contains symbolic links to device files
                // e.g. /dev/disk/by-uuid/e1d70192-1bb0-461d-b89f-b054e45bfa00
                if (devName.StartsWith (L"/")) {
                    IgnoreExceptionsExceptThreadAbortForCall (devName = IO::FileSystem::FileSystem::Default ().CanonicalizeName (devName));
                }
                String mountedAt = line[1];
                String fstype    = line[2];
                result.Add (MountedFilesystemType{mountedAt, devName, fstype});
            }
        }
        return results;
    }
}
#elif qPlatform_Windows
namespace {
    Collection<MountedFilesystemType> GetMountedFilesystems_Windows_ ()
    {
        Collection<MountedFilesystemType> results{};
        TCHAR                             volumeNameBuf[1024];

        HANDLE hVol    = INVALID_HANDLE_VALUE;
        auto&& cleanup = Execution::Finally ([&]() noexcept { if (hVol != INVALID_HANDLE_VALUE) { ::CloseHandle (hVol); } });

        for (HANDLE hVol = ::FindFirstVolume (volumeNameBuf, static_cast<DWORD> (NEltsOf (volumeNameBuf))); hVol != INVALID_HANDLE_VALUE;) {
            DWORD lpMaximumComponentLength;
            DWORD dwSysFlags;
            TCHAR fileSysNameBuf[1024];
            if (::GetVolumeInformation (volumeNameBuf, nullptr, static_cast<DWORD> (NEltsOf (volumeNameBuf)), nullptr, &lpMaximumComponentLength, &dwSysFlags, fileSysNameBuf, static_cast<DWORD> (NEltsOf (fileSysNameBuf)))) {
                MountedFilesystemType v;
                v.fFileSystemType = String::FromSDKString (fileSysNameBuf);
                v.fVolumeID       = String::FromSDKString (volumeNameBuf);

                ///
                TCHAR volPathsBuf[10 * 1024];
                DWORD retLen = 0;
                DWORD x      = ::GetVolumePathNamesForVolumeName (volumeNameBuf, volPathsBuf, static_cast<DWORD> (NEltsOf (volPathsBuf)), &retLen);
                if (x == 0) {
                    DbgTrace (SDKSTR ("Ignoring error getting paths (volume='%s')"), volumeNameBuf);
                }
                else if (volPathsBuf[0] == 0) {
                    // Ignore - unmounted!
                    DbgTrace (SDKSTR ("Ignoring unmounted filesystem (volume='%s')"), volumeNameBuf);
                }
                else {
                    for (const TCHAR* NameIdx = volPathsBuf; NameIdx[0] != L'\0'; NameIdx += Characters::CString::Length (NameIdx) + 1) {
                        v.fMountedOn = String::FromSDKString (NameIdx);
                        results.Add (v);
                    }
                }
            }

            // find next
            if (not::FindNextVolume (hVol, volumeNameBuf, static_cast<DWORD> (NEltsOf (volumeNameBuf)))) {
                ::FindVolumeClose (hVol);
                hVol = INVALID_HANDLE_VALUE;
            }
        }
        return results;
    }
}
#endif

/*
 ********************************************************************************
 ******************** IO::FileSystem::MountedFilesystemType *********************
 ********************************************************************************
 */
String MountedFilesystemType::ToString () const
{
    StringBuilder sb;
    sb += L"{";
    sb += L"Mounted-On: '" + fMountedOn + L"', ";
    sb += L"Device-Path: '" + fDevicePath + L"', ";
    if (fFileSystemType) {
        sb += L"FileSystem-Type: '" + *fFileSystemType + L"', ";
    }
    if (fVolumeID) {
        sb += L"Volume-ID: '" + *fVolumeID + L"', ";
    }
    sb += L"}";
    return sb.str ();
}

/*
 ********************************************************************************
 ******************** IO::FileSystem::GetMountedFilesystems *********************
 ********************************************************************************
 */
Containers::Collection<MountedFilesystemType> FileSystem::GetMountedFilesystems ()
{
#if qPlatform_Linux
    return ReadMountInfo_FromProcFSMounts_ ();
#elif qPlatform_Windows
    return GetMountedFilesystems_Windows_ ();
#else
    AssertNotImplemented ();
    return {};
#endif
}
<commit_msg>improved IO::FileSystem::GetMountedFilesystems<commit_after>/*
 * Copyright(c) Sophist Solutions, Inc. 1990-2017.  All rights reserved
 */
#include "../../StroikaPreComp.h"

#include "../../Characters/CString/Utilities.h"
#include "../../Characters/String.h"
#include "../../Characters/StringBuilder.h"
#include "../../Characters/String_Constant.h"
#include "../../DataExchange/Variant/CharacterDelimitedLines/Reader.h"
#include "../../Execution/Finally.h"
#include "../../Memory/SmallStackBuffer.h"

#include "FileInputStream.h"
#include "FileSystem.h"

#include "MountedFilesystem.h"

using namespace Stroika::Foundation;
using namespace Stroika::Foundation::Characters;
using namespace Stroika::Foundation::Containers;
using namespace Stroika::Foundation::IO;
using namespace Stroika::Foundation::IO::FileSystem;

// Comment this in to turn on aggressive noisy DbgTrace in this module
//#define   USE_NOISY_TRACE_IN_THIS_MODULE_       1

namespace {
    /*
     *  Something like this is used on many unix systems.
     */
    Collection<MountedFilesystemType> ReadMountInfo_MTabLikeFile_ (const String& filename)
    {
        /*
         *  I haven't found this clearly documented yet, but it appears that a filesystem can be over-mounted.
         *  See https://www.kernel.org/doc/Documentation/filesystems/ramfs-rootfs-initramfs.txt
         *
         *  So the last one with a given mount point in the file wins.
         */
        Collection<MountedFilesystemType>                      results;
        DataExchange::Variant::CharacterDelimitedLines::Reader reader{{' ', '\t'}};
        // Note - /procfs files always unseekable
        for (Sequence<String> line : reader.ReadMatrix (FileInputStream::mk (filename, FileInputStream::eNotSeekable))) {
#if USE_NOISY_TRACE_IN_THIS_MODULE_
            DbgTrace (L"in IO::FileSystem::{}::ReadMountInfo_MTabLikeFile_ linesize=%d, line[0]=%s", line.size (), line.empty () ? L"" : line[0].c_str ());
#endif
            //
            // https://www.centos.org/docs/5/html/5.2/Deployment_Guide/s2-proc-mounts.html
            //
            //  1 - device name
            //  2 - mounted on
            //  3 - fstype
            //
            if (line.size () >= 3) {
                String devName = line[0];
                // procfs/mounts often contains symbolic links to device files
                // e.g. /dev/disk/by-uuid/e1d70192-1bb0-461d-b89f-b054e45bfa00
                if (devName.StartsWith (L"/")) {
                    IgnoreExceptionsExceptThreadAbortForCall (devName = IO::FileSystem::FileSystem::Default ().CanonicalizeName (devName));
                }
                String mountedAt = line[1];
                String fstype    = line[2];
                results.Add (MountedFilesystemType{mountedAt, devName, fstype});
            }
        }
        return results;
    }
}
#if qPlatform_Linux
namespace {
    Collection<MountedFilesystemType> ReadMountInfo_FromProcFSMounts_ ()
    {
        static const String_Constant kProcMountsFileName_{L"/proc/mounts"};
        return ReadMountInfo_MTabLikeFile_ (ReadMountInfo_FromProcFSMounts_);
    }
}
#endif
#if qPlatform_Linux or qPlatform_MacOS
namespace {
    Collection<MountedFilesystemType> ReadMountInfo_ETC_MTAB_ ()
    {
        static const String_Constant kProcMountsFileName_{L"/etc/mtab"};
        return ReadMountInfo_MTabLikeFile_ (kProcMountsFileName_);
    }
}
#endif
#if qPlatform_Windows
namespace {
    Collection<MountedFilesystemType> GetMountedFilesystems_Windows_ ()
    {
        Collection<MountedFilesystemType> results{};
        TCHAR                             volumeNameBuf[1024];

        HANDLE hVol    = INVALID_HANDLE_VALUE;
        auto&& cleanup = Execution::Finally ([&]() noexcept { if (hVol != INVALID_HANDLE_VALUE) { ::CloseHandle (hVol); } });

        for (HANDLE hVol = ::FindFirstVolume (volumeNameBuf, static_cast<DWORD> (NEltsOf (volumeNameBuf))); hVol != INVALID_HANDLE_VALUE;) {
            DWORD lpMaximumComponentLength;
            DWORD dwSysFlags;
            TCHAR fileSysNameBuf[1024];
            if (::GetVolumeInformation (volumeNameBuf, nullptr, static_cast<DWORD> (NEltsOf (volumeNameBuf)), nullptr, &lpMaximumComponentLength, &dwSysFlags, fileSysNameBuf, static_cast<DWORD> (NEltsOf (fileSysNameBuf)))) {
                MountedFilesystemType v;
                v.fFileSystemType = String::FromSDKString (fileSysNameBuf);
                v.fVolumeID       = String::FromSDKString (volumeNameBuf);

                ///
                TCHAR volPathsBuf[10 * 1024];
                DWORD retLen = 0;
                DWORD x      = ::GetVolumePathNamesForVolumeName (volumeNameBuf, volPathsBuf, static_cast<DWORD> (NEltsOf (volPathsBuf)), &retLen);
                if (x == 0) {
                    DbgTrace (SDKSTR ("Ignoring error getting paths (volume='%s')"), volumeNameBuf);
                }
                else if (volPathsBuf[0] == 0) {
                    // Ignore - unmounted!
                    DbgTrace (SDKSTR ("Ignoring unmounted filesystem (volume='%s')"), volumeNameBuf);
                }
                else {
                    for (const TCHAR* NameIdx = volPathsBuf; NameIdx[0] != L'\0'; NameIdx += Characters::CString::Length (NameIdx) + 1) {
                        v.fMountedOn = String::FromSDKString (NameIdx);
                        results.Add (v);
                    }
                }
            }

            // find next
            if (not::FindNextVolume (hVol, volumeNameBuf, static_cast<DWORD> (NEltsOf (volumeNameBuf)))) {
                ::FindVolumeClose (hVol);
                hVol = INVALID_HANDLE_VALUE;
            }
        }
        return results;
    }
}
#endif

/*
 ********************************************************************************
 ******************** IO::FileSystem::MountedFilesystemType *********************
 ********************************************************************************
 */
String MountedFilesystemType::ToString () const
{
    StringBuilder sb;
    sb += L"{";
    sb += L"Mounted-On: '" + fMountedOn + L"', ";
    sb += L"Device-Path: '" + fDevicePath + L"', ";
    if (fFileSystemType) {
        sb += L"FileSystem-Type: '" + *fFileSystemType + L"', ";
    }
    if (fVolumeID) {
        sb += L"Volume-ID: '" + *fVolumeID + L"', ";
    }
    sb += L"}";
    return sb.str ();
}

/*
 ********************************************************************************
 ******************** IO::FileSystem::GetMountedFilesystems *********************
 ********************************************************************************
 */
Containers::Collection<MountedFilesystemType> IO::FileSystem::GetMountedFilesystems ()
{
#if qPlatform_Linux
    return ReadMountInfo_FromProcFSMounts_ ();
#elif qPlatform_MacOS
    return ReadMountInfo_ETC_MTAB_ ();
#elif qPlatform_Windows
    return GetMountedFilesystems_Windows_ ();
#else
    AssertNotImplemented ();
    return {};
#endif
}
<|endoftext|>


<commit_before>/*
 * Copyright(c) Sophist Solutions, Inc. 1990-2017.  All rights reserved
 */
#include "../../StroikaPreComp.h"

#if qPlatform_Linux
#include <poll.h>
#elif qPlatform_Windows
#include <Windows.h>
#include <winioctl.h>
#endif

#include "../../Characters/CString/Utilities.h"
#include "../../Characters/Format.h"
#include "../../Characters/String.h"
#include "../../Characters/StringBuilder.h"
#include "../../Characters/String_Constant.h"
#include "../../Characters/ToString.h"
#include "../../DataExchange/Variant/CharacterDelimitedLines/Reader.h"
#include "../../Execution/Finally.h"
#include "../../Execution/Synchronized.h"
#include "../../Memory/SmallStackBuffer.h"

#include "FileInputStream.h"
#include "FileSystem.h"

#include "MountedFilesystem.h"

using namespace Stroika::Foundation;
using namespace Stroika::Foundation::Characters;
using namespace Stroika::Foundation::Containers;
using namespace Stroika::Foundation::IO;
using namespace Stroika::Foundation::IO::FileSystem;

// Comment this in to turn on aggressive noisy DbgTrace in this module
//#define   USE_NOISY_TRACE_IN_THIS_MODULE_       1

//#define qUseWATCHER_ 1
#ifndef qUseWATCHER_
#define qUseWATCHER_ qPlatform_POSIX
#endif

#if qUseWATCHER_
namespace {
    // experimental basis for file watcher
    struct Watcher_ {
        int mfd;
        Watcher_ (const String& fn)
            : mfd (::open (fn.AsNarrowSDKString ().c_str (), O_RDONLY, 0))
        {
        }
        ~Watcher_ ()
        {
            ::close (mfd);
        }

        bool IsNewAvail () const
        {
            struct pollfd pfd;
            int           rv;
            int           changes = 0;
            pfd.fd                = mfd;
            pfd.events            = POLLERR | POLLPRI;
            pfd.revents           = 0;
            if ((rv = poll (&pfd, 1, 5)) >= 0) {
                if (pfd.revents & POLLERR) {
                    return true;
                }
            }
        }
    };
}
#endif

namespace {
    /*
     *  Something like this is used on many unix systems.
     */
    Collection<MountedFilesystemType> ReadMountInfo_MTabLikeFile_ (const Streams::InputStream<Memory::Byte>& readStream)
    {
        /*
         *  I haven't found this clearly documented yet, but it appears that a filesystem can be over-mounted.
         *  See https://www.kernel.org/doc/Documentation/filesystems/ramfs-rootfs-initramfs.txt
         *
         *  So the last one with a given mount point in the file wins.
         */
        Collection<MountedFilesystemType>                      results;
        DataExchange::Variant::CharacterDelimitedLines::Reader reader{{' ', '\t'}};
        for (Sequence<String> line : reader.ReadMatrix (readStream)) {
#if USE_NOISY_TRACE_IN_THIS_MODULE_
            DbgTrace (L"in IO::FileSystem::{}::ReadMountInfo_MTabLikeFile_ linesize=%d, line[0]=%s", line.size (), line.empty () ? L"" : line[0].c_str ());
#endif
            //
            // https://www.centos.org/docs/5/html/5.2/Deployment_Guide/s2-proc-mounts.html
            //
            //  1 - device name
            //  2 - mounted on
            //  3 - fstype
            //
            if (line.size () >= 3) {
                String devName = line[0];
                // procfs/mounts often contains symbolic links to device files
                // e.g. /dev/disk/by-uuid/e1d70192-1bb0-461d-b89f-b054e45bfa00
                if (devName.StartsWith (L"/")) {
                    IgnoreExceptionsExceptThreadAbortForCall (devName = IO::FileSystem::FileSystem::Default ().CanonicalizeName (devName));
                }
                String mountedAt = line[1];
                String fstype    = line[2];
                results.Add (MountedFilesystemType{mountedAt, Set<String>{devName}, fstype});
            }
        }
        return results;
    }
}
#if qPlatform_Linux
namespace {
    Collection<MountedFilesystemType> ReadMountInfo_FromProcFSMounts_ ()
    {
        // Note - /procfs files always unseekable
        static const String_Constant kUseFile2List_{L"/proc/mounts"};
#if qUseWATCHER_
        static const Watcher_                                             sWatcher_{kUseFile2List_};
        static Execution::Syncrhonized<Collection<MountedFilesystemType>> sLastResult_;
        static bool                                                       sFirstTime_{true};
        if (sFirstTime_ or sWatcher_.IsNewAvail ()) {
            sLastResult_ = ReadMountInfo_MTabLikeFile_ (FileInputStream::mk (kUseFile2List_, FileInputStream::eNotSeekable));
        }
        return sLastResult_;
#else
        return ReadMountInfo_MTabLikeFile_ (FileInputStream::mk (kUseFile2List_, FileInputStream::eNotSeekable));
#endif
    }
}
#endif
#if qPlatform_Linux
namespace {
    Collection<MountedFilesystemType> ReadMountInfo_ETC_MTAB_ ()
    {
        // Note - /procfs files always unseekable and this is sklink to /procfs
        static const String_Constant kUseFile2List_{L"/etc/mtab"};
        return ReadMountInfo_MTabLikeFile_ (FileInputStream::mk (kUseFile2List_, FileInputStream::eNotSeekable));
    }
}
#endif
#if qPlatform_Windows
namespace {
    using DynamicDiskIDType = String;
    static String GetPhysNameForDriveNumber_ (unsigned int i)
    {
        // This format is NOT super well documented, and was mostly derived from reading the remarks section
        // of https://msdn.microsoft.com/en-us/library/windows/desktop/aa363216%28v=vs.85%29.aspx?f=255&MSPPError=-2147217396
        // (DeviceIoControl function)
        return Characters::Format (L"\\\\.\\PhysicalDrive%d", i);
    }
    Optional<Set<DynamicDiskIDType>> GetDisksForVolume_ (String volumeName)
    {
        wchar_t volPathsBuf[10 * 1024] = {0};
        DWORD   retLen                 = 0;
        DWORD   x                      = ::GetVolumePathNamesForVolumeNameW (volumeName.c_str (), volPathsBuf, static_cast<DWORD> (NEltsOf (volPathsBuf)), &retLen);
        if (x == 0 or retLen <= 1) {
            return Set<String> ();
        }
        Assert (1 <= Characters::CString::Length (volPathsBuf) and Characters::CString::Length (volPathsBuf) < NEltsOf (volPathsBuf));
        volumeName = L"\\\\.\\" + String::FromSDKString (volPathsBuf).CircularSubString (0, -1);

        // @todo - rewrite this - must somehow otherwise callocate this to be large enuf (dynamic alloc) - if we want more disk exents, but not sure when that happens...
        VOLUME_DISK_EXTENTS volumeDiskExtents;
        {
            /*
             *  For reasons I don't understand (maybe a hit at http://superuser.com/questions/733687/give-regular-user-permission-to-access-physical-drive-on-windows)
             *  this only works with admin privilges
             */
            HANDLE hHandle = ::CreateFileW (volumeName.c_str (), GENERIC_READ, FILE_SHARE_READ | FILE_SHARE_WRITE, NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, NULL);
            if (hHandle == INVALID_HANDLE_VALUE) {
                return {};
            }
            DWORD dwBytesReturned = 0;
            BOOL  bResult         = ::DeviceIoControl (hHandle, IOCTL_VOLUME_GET_VOLUME_DISK_EXTENTS, NULL, 0, &volumeDiskExtents, sizeof (volumeDiskExtents), &dwBytesReturned, NULL);
            ::CloseHandle (hHandle);
            if (not bResult) {
                return {};
            }
        }
        Set<DynamicDiskIDType> result;
        for (DWORD n = 0; n < volumeDiskExtents.NumberOfDiskExtents; ++n) {
            PDISK_EXTENT pDiskExtent = &volumeDiskExtents.Extents[n];
            result.Add (GetPhysNameForDriveNumber_ (pDiskExtent->DiskNumber));
        }
        return result;
    }

    Collection<MountedFilesystemType> GetMountedFilesystems_Windows_ ()
    {
        Collection<MountedFilesystemType> results{};
        TCHAR                             volumeNameBuf[1024];

        HANDLE hVol    = INVALID_HANDLE_VALUE;
        auto&& cleanup = Execution::Finally ([&]() noexcept { if (hVol != INVALID_HANDLE_VALUE) { ::CloseHandle (hVol); } });

        for (HANDLE hVol = ::FindFirstVolume (volumeNameBuf, static_cast<DWORD> (NEltsOf (volumeNameBuf))); hVol != INVALID_HANDLE_VALUE;) {
            DWORD lpMaximumComponentLength;
            DWORD dwSysFlags;
            TCHAR fileSysNameBuf[1024];
            if (::GetVolumeInformation (volumeNameBuf, nullptr, static_cast<DWORD> (NEltsOf (volumeNameBuf)), nullptr, &lpMaximumComponentLength, &dwSysFlags, fileSysNameBuf, static_cast<DWORD> (NEltsOf (fileSysNameBuf)))) {
                MountedFilesystemType v;
                v.fFileSystemType = String::FromSDKString (fileSysNameBuf);
                v.fVolumeID       = String::FromSDKString (volumeNameBuf);
                v.fDevicePaths    = GetDisksForVolume_ (volumeNameBuf);

                ///
                TCHAR volPathsBuf[10 * 1024];
                DWORD retLen = 0;
                DWORD x      = ::GetVolumePathNamesForVolumeName (volumeNameBuf, volPathsBuf, static_cast<DWORD> (NEltsOf (volPathsBuf)), &retLen);
                if (x == 0) {
                    DbgTrace (SDKSTR ("Ignoring error getting paths (volume='%s')"), volumeNameBuf);
                }
                else if (volPathsBuf[0] == 0) {
                    // Ignore - unmounted!
                    DbgTrace (SDKSTR ("Ignoring unmounted filesystem (volume='%s')"), volumeNameBuf);
                }
                else {
                    for (const TCHAR* NameIdx = volPathsBuf; NameIdx[0] != L'\0'; NameIdx += Characters::CString::Length (NameIdx) + 1) {
                        v.fMountedOn = String::FromSDKString (NameIdx);
                        results.Add (v);
                    }
                }
            }

            // find next
            if (not::FindNextVolume (hVol, volumeNameBuf, static_cast<DWORD> (NEltsOf (volumeNameBuf)))) {
                ::FindVolumeClose (hVol);
                hVol = INVALID_HANDLE_VALUE;
            }
        }
        return results;
    }
}
#endif

/*
 ********************************************************************************
 ******************** IO::FileSystem::MountedFilesystemType *********************
 ********************************************************************************
 */
String MountedFilesystemType::ToString () const
{
    StringBuilder sb;
    sb += L"{";
    sb += L"Mounted-On: '" + fMountedOn + L"', ";
    if (fDevicePaths) {
        sb += L"Device-Paths: " + Characters::ToString (*fDevicePaths) + L", ";
    }
    if (fFileSystemType) {
        sb += L"FileSystem-Type: '" + *fFileSystemType + L"', ";
    }
    if (fVolumeID) {
        sb += L"Volume-ID: '" + *fVolumeID + L"', ";
    }
    sb += L"}";
    return sb.str ();
}

/*
 ********************************************************************************
 ******************** IO::FileSystem::GetMountedFilesystems *********************
 ********************************************************************************
 */
Containers::Collection<MountedFilesystemType> IO::FileSystem::GetMountedFilesystems ()
{
#if qPlatform_Linux
    return ReadMountInfo_FromProcFSMounts_ ();
#elif qPlatform_Windows
    return GetMountedFilesystems_Windows_ ();
#else
    AssertNotImplemented ();
    return {};
#endif
}
<commit_msg>enable/tweak/fix new faster watcher code for mounted filesystems for Linux<commit_after>/*
 * Copyright(c) Sophist Solutions, Inc. 1990-2017.  All rights reserved
 */
#include "../../StroikaPreComp.h"

#if qPlatform_Linux
#include <fcntl.h>
#include <poll.h>
#include <unistd.h>
#elif qPlatform_Windows
#include <Windows.h>
#include <winioctl.h>
#endif

#include "../../Characters/CString/Utilities.h"
#include "../../Characters/Format.h"
#include "../../Characters/String.h"
#include "../../Characters/StringBuilder.h"
#include "../../Characters/String_Constant.h"
#include "../../Characters/ToString.h"
#include "../../DataExchange/Variant/CharacterDelimitedLines/Reader.h"
#include "../../Execution/Finally.h"
#include "../../Execution/Synchronized.h"
#include "../../Memory/SmallStackBuffer.h"

#include "FileInputStream.h"
#include "FileSystem.h"

#include "MountedFilesystem.h"

using namespace Stroika::Foundation;
using namespace Stroika::Foundation::Characters;
using namespace Stroika::Foundation::Containers;
using namespace Stroika::Foundation::IO;
using namespace Stroika::Foundation::IO::FileSystem;

// Comment this in to turn on aggressive noisy DbgTrace in this module
//#define   USE_NOISY_TRACE_IN_THIS_MODULE_       1

#ifndef qUseWATCHER_
#define qUseWATCHER_ qPlatform_POSIX
#endif

#if qUseWATCHER_
namespace {
    // experimental basis for file watcher
    struct Watcher_ {
        int mfd;
        Watcher_ (const String& fn)
            : mfd (::open (fn.AsNarrowSDKString ().c_str (), O_RDONLY, 0))
        {
        }
        ~Watcher_ ()
        {
            ::close (mfd);
        }

        bool IsNewAvail () const
        {
            // according to http://stackoverflow.com/questions/5070801/monitoring-mount-point-changes-via-proc-mounts
            // have to use poll with POLLPRI | POLLERR flags
            struct pollfd pfd;
            int           rv;
            int           changes = 0;
            pfd.fd                = mfd;
            pfd.events            = POLLERR | POLLPRI;
            pfd.revents           = 0;
            if ((rv = poll (&pfd, 1, 0)) >= 0) {
                if (pfd.revents & POLLERR) {
                    return true;
                }
            }
            return false;
        }
    };
}
#endif

namespace {
    /*
     *  Something like this is used on many unix systems.
     */
    Collection<MountedFilesystemType> ReadMountInfo_MTabLikeFile_ (const Streams::InputStream<Memory::Byte>& readStream)
    {
        /*
         *  I haven't found this clearly documented yet, but it appears that a filesystem can be over-mounted.
         *  See https://www.kernel.org/doc/Documentation/filesystems/ramfs-rootfs-initramfs.txt
         *
         *  So the last one with a given mount point in the file wins.
         */
        Collection<MountedFilesystemType>                      results;
        DataExchange::Variant::CharacterDelimitedLines::Reader reader{{' ', '\t'}};
        for (Sequence<String> line : reader.ReadMatrix (readStream)) {
#if USE_NOISY_TRACE_IN_THIS_MODULE_
            DbgTrace (L"in IO::FileSystem::{}::ReadMountInfo_MTabLikeFile_ linesize=%d, line[0]=%s", line.size (), line.empty () ? L"" : line[0].c_str ());
#endif
            //
            // https://www.centos.org/docs/5/html/5.2/Deployment_Guide/s2-proc-mounts.html
            //
            //  1 - device name
            //  2 - mounted on
            //  3 - fstype
            //
            if (line.size () >= 3) {
                String devName = line[0];
                // procfs/mounts often contains symbolic links to device files
                // e.g. /dev/disk/by-uuid/e1d70192-1bb0-461d-b89f-b054e45bfa00
                if (devName.StartsWith (L"/")) {
                    IgnoreExceptionsExceptThreadAbortForCall (devName = IO::FileSystem::FileSystem::Default ().CanonicalizeName (devName));
                }
                String mountedAt = line[1];
                String fstype    = line[2];
                results.Add (MountedFilesystemType{mountedAt, Set<String>{devName}, fstype});
            }
        }
        return results;
    }
}
#if qPlatform_Linux
namespace {
    Collection<MountedFilesystemType> ReadMountInfo_FromProcFSMounts_ ()
    {
        // Note - /procfs files always unseekable
        static const String_Constant kUseFile2List_{L"/proc/mounts"};
#if qUseWATCHER_
        static const Watcher_                                             sWatcher_{kUseFile2List_};
        static Execution::Synchronized<Collection<MountedFilesystemType>> sLastResult_;
        static bool                                                       sFirstTime_{true};
        if (sFirstTime_ or sWatcher_.IsNewAvail ()) {
            sLastResult_ = ReadMountInfo_MTabLikeFile_ (FileInputStream::mk (kUseFile2List_, FileInputStream::eNotSeekable));
            sFirstTime_  = false;
        }
        return sLastResult_;
#else
        return ReadMountInfo_MTabLikeFile_ (FileInputStream::mk (kUseFile2List_, FileInputStream::eNotSeekable));
#endif
    }
}
#endif
#if qPlatform_Linux
namespace {
    Collection<MountedFilesystemType> ReadMountInfo_ETC_MTAB_ ()
    {
        // Note - /procfs files always unseekable and this is sklink to /procfs
        static const String_Constant kUseFile2List_{L"/etc/mtab"};
        return ReadMountInfo_MTabLikeFile_ (FileInputStream::mk (kUseFile2List_, FileInputStream::eNotSeekable));
    }
}
#endif
#if qPlatform_Windows
namespace {
    using DynamicDiskIDType = String;
    static String GetPhysNameForDriveNumber_ (unsigned int i)
    {
        // This format is NOT super well documented, and was mostly derived from reading the remarks section
        // of https://msdn.microsoft.com/en-us/library/windows/desktop/aa363216%28v=vs.85%29.aspx?f=255&MSPPError=-2147217396
        // (DeviceIoControl function)
        return Characters::Format (L"\\\\.\\PhysicalDrive%d", i);
    }
    Optional<Set<DynamicDiskIDType>> GetDisksForVolume_ (String volumeName)
    {
        wchar_t volPathsBuf[10 * 1024] = {0};
        DWORD   retLen                 = 0;
        DWORD   x                      = ::GetVolumePathNamesForVolumeNameW (volumeName.c_str (), volPathsBuf, static_cast<DWORD> (NEltsOf (volPathsBuf)), &retLen);
        if (x == 0 or retLen <= 1) {
            return Set<String> ();
        }
        Assert (1 <= Characters::CString::Length (volPathsBuf) and Characters::CString::Length (volPathsBuf) < NEltsOf (volPathsBuf));
        volumeName = L"\\\\.\\" + String::FromSDKString (volPathsBuf).CircularSubString (0, -1);

        // @todo - rewrite this - must somehow otherwise callocate this to be large enuf (dynamic alloc) - if we want more disk exents, but not sure when that happens...
        VOLUME_DISK_EXTENTS volumeDiskExtents;
        {
            /*
             *  For reasons I don't understand (maybe a hit at http://superuser.com/questions/733687/give-regular-user-permission-to-access-physical-drive-on-windows)
             *  this only works with admin privilges
             */
            HANDLE hHandle = ::CreateFileW (volumeName.c_str (), GENERIC_READ, FILE_SHARE_READ | FILE_SHARE_WRITE, NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, NULL);
            if (hHandle == INVALID_HANDLE_VALUE) {
                return {};
            }
            DWORD dwBytesReturned = 0;
            BOOL  bResult         = ::DeviceIoControl (hHandle, IOCTL_VOLUME_GET_VOLUME_DISK_EXTENTS, NULL, 0, &volumeDiskExtents, sizeof (volumeDiskExtents), &dwBytesReturned, NULL);
            ::CloseHandle (hHandle);
            if (not bResult) {
                return {};
            }
        }
        Set<DynamicDiskIDType> result;
        for (DWORD n = 0; n < volumeDiskExtents.NumberOfDiskExtents; ++n) {
            PDISK_EXTENT pDiskExtent = &volumeDiskExtents.Extents[n];
            result.Add (GetPhysNameForDriveNumber_ (pDiskExtent->DiskNumber));
        }
        return result;
    }

    Collection<MountedFilesystemType> GetMountedFilesystems_Windows_ ()
    {
        Collection<MountedFilesystemType> results{};
        TCHAR                             volumeNameBuf[1024];

        HANDLE hVol    = INVALID_HANDLE_VALUE;
        auto&& cleanup = Execution::Finally ([&]() noexcept { if (hVol != INVALID_HANDLE_VALUE) { ::CloseHandle (hVol); } });

        for (HANDLE hVol = ::FindFirstVolume (volumeNameBuf, static_cast<DWORD> (NEltsOf (volumeNameBuf))); hVol != INVALID_HANDLE_VALUE;) {
            DWORD lpMaximumComponentLength;
            DWORD dwSysFlags;
            TCHAR fileSysNameBuf[1024];
            if (::GetVolumeInformation (volumeNameBuf, nullptr, static_cast<DWORD> (NEltsOf (volumeNameBuf)), nullptr, &lpMaximumComponentLength, &dwSysFlags, fileSysNameBuf, static_cast<DWORD> (NEltsOf (fileSysNameBuf)))) {
                MountedFilesystemType v;
                v.fFileSystemType = String::FromSDKString (fileSysNameBuf);
                v.fVolumeID       = String::FromSDKString (volumeNameBuf);
                v.fDevicePaths    = GetDisksForVolume_ (volumeNameBuf);

                ///
                TCHAR volPathsBuf[10 * 1024];
                DWORD retLen = 0;
                DWORD x      = ::GetVolumePathNamesForVolumeName (volumeNameBuf, volPathsBuf, static_cast<DWORD> (NEltsOf (volPathsBuf)), &retLen);
                if (x == 0) {
                    DbgTrace (SDKSTR ("Ignoring error getting paths (volume='%s')"), volumeNameBuf);
                }
                else if (volPathsBuf[0] == 0) {
                    // Ignore - unmounted!
                    DbgTrace (SDKSTR ("Ignoring unmounted filesystem (volume='%s')"), volumeNameBuf);
                }
                else {
                    for (const TCHAR* NameIdx = volPathsBuf; NameIdx[0] != L'\0'; NameIdx += Characters::CString::Length (NameIdx) + 1) {
                        v.fMountedOn = String::FromSDKString (NameIdx);
                        results.Add (v);
                    }
                }
            }

            // find next
            if (not::FindNextVolume (hVol, volumeNameBuf, static_cast<DWORD> (NEltsOf (volumeNameBuf)))) {
                ::FindVolumeClose (hVol);
                hVol = INVALID_HANDLE_VALUE;
            }
        }
        return results;
    }
}
#endif

/*
 ********************************************************************************
 ******************** IO::FileSystem::MountedFilesystemType *********************
 ********************************************************************************
 */
String MountedFilesystemType::ToString () const
{
    StringBuilder sb;
    sb += L"{";
    sb += L"Mounted-On: '" + fMountedOn + L"', ";
    if (fDevicePaths) {
        sb += L"Device-Paths: " + Characters::ToString (*fDevicePaths) + L", ";
    }
    if (fFileSystemType) {
        sb += L"FileSystem-Type: '" + *fFileSystemType + L"', ";
    }
    if (fVolumeID) {
        sb += L"Volume-ID: '" + *fVolumeID + L"', ";
    }
    sb += L"}";
    return sb.str ();
}

/*
 ********************************************************************************
 ******************** IO::FileSystem::GetMountedFilesystems *********************
 ********************************************************************************
 */
Containers::Collection<MountedFilesystemType> IO::FileSystem::GetMountedFilesystems ()
{
#if qPlatform_Linux
    return ReadMountInfo_FromProcFSMounts_ ();
#elif qPlatform_Windows
    return GetMountedFilesystems_Windows_ ();
#else
    AssertNotImplemented ();
    return {};
#endif
}
<|endoftext|>