<commit_before>//===----------------------------------------------------------------------===//
//
//                         Peloton
//
// tcp_connection.cpp
//
// Identification: src/backend/networking/tcp_connection.cpp
//
// Copyright (c) 2015-16, Carnegie Mellon University Database Group
//
//===----------------------------------------------------------------------===//

#include <iostream>
#include <mutex>

#include <pthread.h>

#include "backend/networking/tcp_connection.h"
#include "backend/networking/connection_manager.h"
#include "backend/networking/peloton_service.h"
#include "backend/networking/rpc_type.h"
#include "backend/common/macros.h"

namespace peloton {
namespace networking {

std::mutex send_mutex;
uint64_t server_response_send_number = 0;  // number of rpc
uint64_t server_response_send_bytes = 0;   // bytes

struct total_processed {
  size_t n;
};

/*
 * A connection includes a bufferevent which must be specified THREAD-SAFE
 */
Connection::Connection(int fd, struct event_base *base, void *arg,
                       NetworkAddress &addr)
: addr_(addr), close_(false), status_(INIT), base_(base) {
  // we must pass rpc_server when new a connection
  PL_ASSERT(arg != NULL);
  rpc_server_ = (RpcServer *)arg;

  // BEV_OPT_THREADSAFE must be specified
  bev_ = bufferevent_socket_new(base_, fd,
                                BEV_OPT_CLOSE_ON_FREE | BEV_OPT_THREADSAFE);

  // set read callback and event callback
  bufferevent_setcb(bev_, ReadCb, NULL, EventCb, this);

  bufferevent_enable(bev_, EV_READ | EV_WRITE);
}

Connection::~Connection() {
  // We must free event before free base
  this->Close();
}

/*
 * Set the connection status
 */
void Connection::SetStatus(ConnStatus status) { status_ = status; }

/*
 * Get the connection status
 */
Connection::ConnStatus Connection::GetStatus() { return status_; }

/*
 * Connect is only used by client to connect to server
 */
bool Connection::Connect(const NetworkAddress &addr) {
  struct sockaddr_in sin;
  addr.FillAddr(&sin);

  if (bufferevent_socket_connect(bev_, (struct sockaddr *)&sin, sizeof(sin)) <
      0) {
    /* Error starting connection */
    return false;
  }

  return true;
}

void Connection::Close() {
  if (close_ == false) {
    close_ = true;
    bufferevent_free(bev_);
  }
}

/*
 * @brief ProcessMessage is responsible for processing a message.
 *        Generally, a message is either a request or a response.
 *        So the type of a message is REQ or REP.
 *        The detail processing is through RPC call.
 *        Note: if a new RPC message is added we only need to implement the
 *          corresponding RPC method.
 */
void *Connection::ProcessMessage(void *connection) {
  PL_ASSERT(connection != NULL);

  Connection *conn = (Connection *)connection;

  while (conn->GetReadBufferLen()) {
    // Get the total readable data length
    uint32_t readable_len = conn->GetReadBufferLen();

    // If the total readable data is too less
    if (readable_len < HEADERLEN + OPCODELEN + TYPELEN) {
      LOG_TRACE("Readable data is too less, return");
      return NULL;
    }

    /*
     * Copy the header.
     * Note: should not remove the header from buffer, cuz we might return
     *       if there are no enough data as a whole message
     */
    uint32_t msg_len = 0;
    int nread = conn->CopyReadBuffer((char *)&msg_len, HEADERLEN);
    if (nread != HEADERLEN) {
      LOG_ERROR("nread does not match header length \n");
      return NULL;
    }

    /*
     * if readable data is less than a message, return to wait the next callback
     * Note: msg_len includes the length of type + opcode + message
     */
    if (readable_len < msg_len + HEADERLEN) {
      LOG_TRACE("Readable data is less than a message, return");
      return NULL;
    }

    /*
     * Get a message.
     * Note: we only get one message each time. so the buf is msg_len +
     * HEADERLEN
     */
    char buf[msg_len + HEADERLEN];

    // Get the data
    int ret = conn->GetReadData(buf, sizeof(buf));

    PL_ASSERT(ret == (int)sizeof(buf));
    // Get the message type
    uint16_t type = 0;
    PL_MEMCPY((char *)(&type), buf + HEADERLEN, sizeof(type));

    // Get the hashcode of the rpc method
    uint64_t opcode = 0;
    PL_MEMCPY((char *)(&opcode), buf + HEADERLEN + TYPELEN, sizeof(opcode));

    // Get the rpc method meta info: method descriptor
    RpcMethod *rpc_method = conn->GetRpcServer()->FindMethod(opcode);

    if (rpc_method == NULL) {
      LOG_TRACE("No method found");
      return NULL;
    }

    const google::protobuf::MethodDescriptor *method = rpc_method->method_;
    RpcController controller;

    switch (type) {
      case MSG_TYPE_REQ: {
        LOG_TRACE("Handle MSG_TYPE: Request");

        // Get request and response type and create them
        google::protobuf::Message *message = rpc_method->request_->New();
        google::protobuf::Message *response = rpc_method->response_->New();

        // Deserialize the receiving message
        message->ParseFromArray(buf + HEADERLEN + TYPELEN + OPCODELEN, msg_len - TYPELEN - OPCODELEN);

        // Invoke rpc call.
        rpc_method->service_->CallMethod(method, &controller, message, response,
                                         NULL);

        // Send back the response message. The message has been set up when
        // executing rpc method
        msg_len = response->ByteSize() + OPCODELEN + TYPELEN;

        char send_buf[sizeof(msg_len) + msg_len];
        PL_ASSERT(sizeof(msg_len) == HEADERLEN);

        // copy the header into the buf
        PL_MEMCPY(send_buf, &msg_len, sizeof(msg_len));

        // copy the type into the buf
        type = MSG_TYPE_REP;

        PL_ASSERT(sizeof(type) == TYPELEN);
        PL_MEMCPY(send_buf + HEADERLEN, &type, TYPELEN);

        // copy the opcode into the buf
        PL_ASSERT(sizeof(opcode) == OPCODELEN);
        PL_MEMCPY(send_buf + HEADERLEN + TYPELEN, &opcode, OPCODELEN);

        // call protobuf to serialize the request message into sending buf
        response->SerializeToArray(send_buf + HEADERLEN + TYPELEN + OPCODELEN,
                                   msg_len);

        // send data
        // Note: if we use raw socket send api, we should loop send
        PL_ASSERT(sizeof(send_buf) == HEADERLEN + msg_len);
        conn->AddToWriteBuffer(send_buf, sizeof(send_buf));

        delete message;
        delete response;

      } break;

      case MSG_TYPE_REP: {
        LOG_TRACE("Handle MSG_TYPE: Response");

        // Get response and response type and create them
        google::protobuf::Message *message = rpc_method->response_->New();

        // Deserialize the receiving message
        message->ParseFromArray(buf + HEADERLEN + TYPELEN + OPCODELEN, msg_len);

        // Invoke rpc call. request is null
        rpc_method->service_->CallMethod(method, &controller, NULL, message,
                                         NULL);

        delete message;

      } break;

      default:
        LOG_ERROR("Unrecognized message type %d", type);
        break;
    }

    // TODO: controller should be set within rpc method
    if (controller.Failed()) {
      std::string error = controller.ErrorText();
      LOG_TRACE("RpcServer with controller failed:%s ", error.c_str());
    }

/*    {
      std::lock_guard < std::mutex > lock(send_mutex);
      server_response_send_number++;
      server_response_send_bytes += (msg_len + HEADERLEN);

      struct timeval end;
      long useconds;
      gettimeofday(&end, NULL);
      useconds = end.tv_usec - ConnectionManager::GetInstance().start_time_;

      LOG_TRACE("server_response_send_number: %lu", server_response_send_number);
      LOG_TRACE("speed: %f-------------------------------------------------",
               (float)(server_response_send_number * 1000000)/useconds);

      LOG_TRACE("server_response_send_bytes: %lu", server_response_send_bytes);
      LOG_TRACE("speed: %f-------------------------------------------------",
               (float)(server_response_send_bytes * 1000000)/useconds);
    }*/

  }

  return NULL;
}

/*
 * ReadCb is invoked when there is new data coming.
 */
void Connection::ReadCb(UNUSED_ATTRIBUTE struct bufferevent *bev,
                        void *ctx) {
  // TODO: We might use bev in future
  PL_ASSERT(bev != NULL);

  Connection *conn = (Connection *)ctx;

  /* Change the status of the connection */
  // conn->SetStatus(RECVING);

  /*
   * Process the message will invoke rpc call.
   * Note: this might take a long time. So we put this in a thread
   */

  /* prepaere workers_thread to send and recv data */
  Connection::ProcessMessage(conn);
//  std::function<void()> worker_conn =
//      std::bind(&Connection::ProcessMessage, conn);
//
//  /*
//   * Add workers to thread pool to send and recv data
//   * Note: after AddTask, ReadCb will return and another
//   * request on this connection can be processed while
//   * the former is still being processed
//   */
//  ThreadPool::GetInstance().AddTask(worker_conn);
}

/*
 * If recv close event, we should delete the connection from conn_pool
 */
void Connection::EventCb(UNUSED_ATTRIBUTE struct bufferevent *bev,
                         short events, void *ctx) {
  Connection *conn = (Connection *)ctx;
  PL_ASSERT(conn != NULL && bev != NULL);

  if (events & BEV_EVENT_ERROR) {
    LOG_TRACE("Error from client bufferevent: %s",
              evutil_socket_error_to_string(EVUTIL_SOCKET_ERROR()));

    /* tell the client send error, and the client should send again or do some
     * other things*/
    if (conn->GetStatus() == SENDING) {
      LOG_TRACE("Send error");
      // TODO: let the client know
    }

    /* free the buffer event */
    conn->Close();

    /* Since the connection is closed, we should delete it from conn_pool */
    ConnectionManager::GetInstance().DeleteConn(conn);
  }

  /* The other side explicitly closes the connection */
  if (events & (BEV_EVENT_EOF | BEV_EVENT_ERROR)) {
    /* This means server explicitly closes the connection */
    LOG_TRACE("ClientEventCb: %s",
              evutil_socket_error_to_string(
                  EVUTIL_SOCKET_ERROR()));  // the error string is "SUCCESS"

    /* free the buffer event */
    conn->Close();

    /* Since the connection is closed, we should delete it from conn_pool */
    ConnectionManager::GetInstance().DeleteConn(conn);
  }
}

void Connection::BufferCb(UNUSED_ATTRIBUTE struct evbuffer *buffer,
                          const struct evbuffer_cb_info *info, void *arg) {
  struct total_processed *tp = (total_processed *)arg;
  size_t old_n = tp->n;
  int megabytes, i;
  tp->n += info->n_deleted;
  megabytes = ((tp->n) >> 20) - (old_n >> 20);
  for (i = 0; i < megabytes; ++i) putc('.', stdout);
}

RpcServer *Connection::GetRpcServer() { return rpc_server_; }

NetworkAddress &Connection::GetAddr() { return addr_; }
/*
 * Get the readable length of the read buf
 */
int Connection::GetReadBufferLen() {
  /*
   * Locking the bufferevent with this function will
   * lock its associated evbuffers as well
   * Note: it is automatically locked so we don't need
   *       to explicitly lock it
   *       bufferevent_lock(bev_);
   *       bufferevent_unlock(bev_);
   */
  struct evbuffer *input = bufferevent_get_input(bev_);
  return evbuffer_get_length(input);
}

/*
 * Get the len data from read buf and then save them in the give buffer
 * If the data in read buf are less than len, get all of the data
 * Return the length of moved data
 * Note: the len data are deleted from read buf after this operation
 */
int Connection::GetReadData(char *buffer, int len) {
  /*
   * Note: it is automatically locked so we don't need
   *       to explicitly lock it
   *       bufferevent_lock(bev_);
   *       bufferevent_unlock(bev_);
   */

  int remaining_len = len;
  do{
  struct evbuffer *input = bufferevent_get_input(bev_);
  remaining_len -= evbuffer_remove(input, buffer, len);

  }while(remaining_len > 0);
  return len;
}

/*
 * copy data (len) from read buf into the given buffer,
 * if the total data is less than len, then copy all of the data
 * return the length of the copied data
 * the data still exist in the read buf after this operation
 */
int Connection::CopyReadBuffer(char *buffer, int len) {
  /*
   * Note: it is automatically locked so we don't need
   *       to explicitly lock it
   *       bufferevent_lock(bev_);
   *       bufferevent_unlock(bev_);
   */
  struct evbuffer *input = bufferevent_get_input(bev_);
  return evbuffer_copyout(input, buffer, len);
}

/*
 * Get the lengh a write buf
 */
int Connection::GetWriteBufferLen() {
  /*
   * Note: it is automatically locked so we don't need
   *       to explicitly lock it
   *       bufferevent_lock(bev_);
   *       bufferevent_unlock(bev_);
   */
  struct evbuffer *output = bufferevent_get_output(bev_);
  return evbuffer_get_length(output);
}

/*
 * Add data to write buff
 */
bool Connection::AddToWriteBuffer(char *buffer, int len) {
  /*
   * Note: it is automatically locked so we don't need
   *       to explicitly lock it
   *       bufferevent_lock(bev_);
   *       bufferevent_unlock(bev_);
   */
  //    bufferevent_lock(bev_);
  //    struct evbuffer *output = bufferevent_get_output(bev_);
  //    int re = evbuffer_add(output, buffer, len);
  //    bufferevent_unlock(bev_);

  int re = bufferevent_write(bev_, buffer, len);

  if (re == 0) {
    return true;
  } else {
    return false;
  }
}

/*
 * put data in read buf into write buf
 */
void Connection::MoveBufferData() {
  /*
   * Note: it is automatically locked so we don't need
   *       to explicitly lock it
   *       bufferevent_lock(bev_);
   *       bufferevent_unlock(bev_);
   */
  struct evbuffer *input = bufferevent_get_input(bev_);
  struct evbuffer *output = bufferevent_get_output(bev_);

  /* Copy all the data from the input buffer to the output buffer. */
  evbuffer_add_buffer(output, input);
}

}  // namespace networking
}  // namespace peloton
<commit_msg>removed unused variable<commit_after>//===----------------------------------------------------------------------===//
//
//                         Peloton
//
// tcp_connection.cpp
//
// Identification: src/backend/networking/tcp_connection.cpp
//
// Copyright (c) 2015-16, Carnegie Mellon University Database Group
//
//===----------------------------------------------------------------------===//

#include <iostream>
#include <mutex>

#include <pthread.h>

#include "backend/networking/tcp_connection.h"
#include "backend/networking/connection_manager.h"
#include "backend/networking/peloton_service.h"
#include "backend/networking/rpc_type.h"
#include "backend/common/macros.h"

namespace peloton {
namespace networking {

std::mutex send_mutex;
uint64_t server_response_send_number = 0;  // number of rpc
uint64_t server_response_send_bytes = 0;   // bytes

struct total_processed {
  size_t n;
};

/*
 * A connection includes a bufferevent which must be specified THREAD-SAFE
 */
Connection::Connection(int fd, struct event_base *base, void *arg,
                       NetworkAddress &addr)
: addr_(addr), close_(false), status_(INIT), base_(base) {
  // we must pass rpc_server when new a connection
  PL_ASSERT(arg != NULL);
  rpc_server_ = (RpcServer *)arg;

  // BEV_OPT_THREADSAFE must be specified
  bev_ = bufferevent_socket_new(base_, fd,
                                BEV_OPT_CLOSE_ON_FREE | BEV_OPT_THREADSAFE);

  // set read callback and event callback
  bufferevent_setcb(bev_, ReadCb, NULL, EventCb, this);

  bufferevent_enable(bev_, EV_READ | EV_WRITE);
}

Connection::~Connection() {
  // We must free event before free base
  this->Close();
}

/*
 * Set the connection status
 */
void Connection::SetStatus(ConnStatus status) { status_ = status; }

/*
 * Get the connection status
 */
Connection::ConnStatus Connection::GetStatus() { return status_; }

/*
 * Connect is only used by client to connect to server
 */
bool Connection::Connect(const NetworkAddress &addr) {
  struct sockaddr_in sin;
  addr.FillAddr(&sin);

  if (bufferevent_socket_connect(bev_, (struct sockaddr *)&sin, sizeof(sin)) <
      0) {
    /* Error starting connection */
    return false;
  }

  return true;
}

void Connection::Close() {
  if (close_ == false) {
    close_ = true;
    bufferevent_free(bev_);
  }
}

/*
 * @brief ProcessMessage is responsible for processing a message.
 *        Generally, a message is either a request or a response.
 *        So the type of a message is REQ or REP.
 *        The detail processing is through RPC call.
 *        Note: if a new RPC message is added we only need to implement the
 *          corresponding RPC method.
 */
void *Connection::ProcessMessage(void *connection) {
  PL_ASSERT(connection != NULL);

  Connection *conn = (Connection *)connection;

  while (conn->GetReadBufferLen()) {
    // Get the total readable data length
    uint32_t readable_len = conn->GetReadBufferLen();

    // If the total readable data is too less
    if (readable_len < HEADERLEN + OPCODELEN + TYPELEN) {
      LOG_TRACE("Readable data is too less, return");
      return NULL;
    }

    /*
     * Copy the header.
     * Note: should not remove the header from buffer, cuz we might return
     *       if there are no enough data as a whole message
     */
    uint32_t msg_len = 0;
    int nread = conn->CopyReadBuffer((char *)&msg_len, HEADERLEN);
    if (nread != HEADERLEN) {
      LOG_ERROR("nread does not match header length \n");
      return NULL;
    }

    /*
     * if readable data is less than a message, return to wait the next callback
     * Note: msg_len includes the length of type + opcode + message
     */
    if (readable_len < msg_len + HEADERLEN) {
      LOG_TRACE("Readable data is less than a message, return");
      return NULL;
    }

    /*
     * Get a message.
     * Note: we only get one message each time. so the buf is msg_len +
     * HEADERLEN
     */
    char buf[msg_len + HEADERLEN];

    // Get the data
    conn->GetReadData(buf, sizeof(buf));
    // Get the message type
    uint16_t type = 0;
    PL_MEMCPY((char *)(&type), buf + HEADERLEN, sizeof(type));

    // Get the hashcode of the rpc method
    uint64_t opcode = 0;
    PL_MEMCPY((char *)(&opcode), buf + HEADERLEN + TYPELEN, sizeof(opcode));

    // Get the rpc method meta info: method descriptor
    RpcMethod *rpc_method = conn->GetRpcServer()->FindMethod(opcode);

    if (rpc_method == NULL) {
      LOG_TRACE("No method found");
      return NULL;
    }

    const google::protobuf::MethodDescriptor *method = rpc_method->method_;
    RpcController controller;

    switch (type) {
      case MSG_TYPE_REQ: {
        LOG_TRACE("Handle MSG_TYPE: Request");

        // Get request and response type and create them
        google::protobuf::Message *message = rpc_method->request_->New();
        google::protobuf::Message *response = rpc_method->response_->New();

        // Deserialize the receiving message
        message->ParseFromArray(buf + HEADERLEN + TYPELEN + OPCODELEN, msg_len - TYPELEN - OPCODELEN);

        // Invoke rpc call.
        rpc_method->service_->CallMethod(method, &controller, message, response,
                                         NULL);

        // Send back the response message. The message has been set up when
        // executing rpc method
        msg_len = response->ByteSize() + OPCODELEN + TYPELEN;

        char send_buf[sizeof(msg_len) + msg_len];
        PL_ASSERT(sizeof(msg_len) == HEADERLEN);

        // copy the header into the buf
        PL_MEMCPY(send_buf, &msg_len, sizeof(msg_len));

        // copy the type into the buf
        type = MSG_TYPE_REP;

        PL_ASSERT(sizeof(type) == TYPELEN);
        PL_MEMCPY(send_buf + HEADERLEN, &type, TYPELEN);

        // copy the opcode into the buf
        PL_ASSERT(sizeof(opcode) == OPCODELEN);
        PL_MEMCPY(send_buf + HEADERLEN + TYPELEN, &opcode, OPCODELEN);

        // call protobuf to serialize the request message into sending buf
        response->SerializeToArray(send_buf + HEADERLEN + TYPELEN + OPCODELEN,
                                   msg_len);

        // send data
        // Note: if we use raw socket send api, we should loop send
        PL_ASSERT(sizeof(send_buf) == HEADERLEN + msg_len);
        conn->AddToWriteBuffer(send_buf, sizeof(send_buf));

        delete message;
        delete response;

      } break;

      case MSG_TYPE_REP: {
        LOG_TRACE("Handle MSG_TYPE: Response");

        // Get response and response type and create them
        google::protobuf::Message *message = rpc_method->response_->New();

        // Deserialize the receiving message
        message->ParseFromArray(buf + HEADERLEN + TYPELEN + OPCODELEN, msg_len);

        // Invoke rpc call. request is null
        rpc_method->service_->CallMethod(method, &controller, NULL, message,
                                         NULL);

        delete message;

      } break;

      default:
        LOG_ERROR("Unrecognized message type %d", type);
        break;
    }

    // TODO: controller should be set within rpc method
    if (controller.Failed()) {
      std::string error = controller.ErrorText();
      LOG_TRACE("RpcServer with controller failed:%s ", error.c_str());
    }

/*    {
      std::lock_guard < std::mutex > lock(send_mutex);
      server_response_send_number++;
      server_response_send_bytes += (msg_len + HEADERLEN);

      struct timeval end;
      long useconds;
      gettimeofday(&end, NULL);
      useconds = end.tv_usec - ConnectionManager::GetInstance().start_time_;

      LOG_TRACE("server_response_send_number: %lu", server_response_send_number);
      LOG_TRACE("speed: %f-------------------------------------------------",
               (float)(server_response_send_number * 1000000)/useconds);

      LOG_TRACE("server_response_send_bytes: %lu", server_response_send_bytes);
      LOG_TRACE("speed: %f-------------------------------------------------",
               (float)(server_response_send_bytes * 1000000)/useconds);
    }*/

  }

  return NULL;
}

/*
 * ReadCb is invoked when there is new data coming.
 */
void Connection::ReadCb(UNUSED_ATTRIBUTE struct bufferevent *bev,
                        void *ctx) {
  // TODO: We might use bev in future
  PL_ASSERT(bev != NULL);

  Connection *conn = (Connection *)ctx;

  /* Change the status of the connection */
  // conn->SetStatus(RECVING);

  /*
   * Process the message will invoke rpc call.
   * Note: this might take a long time. So we put this in a thread
   */

  /* prepaere workers_thread to send and recv data */
  Connection::ProcessMessage(conn);
//  std::function<void()> worker_conn =
//      std::bind(&Connection::ProcessMessage, conn);
//
//  /*
//   * Add workers to thread pool to send and recv data
//   * Note: after AddTask, ReadCb will return and another
//   * request on this connection can be processed while
//   * the former is still being processed
//   */
//  ThreadPool::GetInstance().AddTask(worker_conn);
}

/*
 * If recv close event, we should delete the connection from conn_pool
 */
void Connection::EventCb(UNUSED_ATTRIBUTE struct bufferevent *bev,
                         short events, void *ctx) {
  Connection *conn = (Connection *)ctx;
  PL_ASSERT(conn != NULL && bev != NULL);

  if (events & BEV_EVENT_ERROR) {
    LOG_TRACE("Error from client bufferevent: %s",
              evutil_socket_error_to_string(EVUTIL_SOCKET_ERROR()));

    /* tell the client send error, and the client should send again or do some
     * other things*/
    if (conn->GetStatus() == SENDING) {
      LOG_TRACE("Send error");
      // TODO: let the client know
    }

    /* free the buffer event */
    conn->Close();

    /* Since the connection is closed, we should delete it from conn_pool */
    ConnectionManager::GetInstance().DeleteConn(conn);
  }

  /* The other side explicitly closes the connection */
  if (events & (BEV_EVENT_EOF | BEV_EVENT_ERROR)) {
    /* This means server explicitly closes the connection */
    LOG_TRACE("ClientEventCb: %s",
              evutil_socket_error_to_string(
                  EVUTIL_SOCKET_ERROR()));  // the error string is "SUCCESS"

    /* free the buffer event */
    conn->Close();

    /* Since the connection is closed, we should delete it from conn_pool */
    ConnectionManager::GetInstance().DeleteConn(conn);
  }
}

void Connection::BufferCb(UNUSED_ATTRIBUTE struct evbuffer *buffer,
                          const struct evbuffer_cb_info *info, void *arg) {
  struct total_processed *tp = (total_processed *)arg;
  size_t old_n = tp->n;
  int megabytes, i;
  tp->n += info->n_deleted;
  megabytes = ((tp->n) >> 20) - (old_n >> 20);
  for (i = 0; i < megabytes; ++i) putc('.', stdout);
}

RpcServer *Connection::GetRpcServer() { return rpc_server_; }

NetworkAddress &Connection::GetAddr() { return addr_; }
/*
 * Get the readable length of the read buf
 */
int Connection::GetReadBufferLen() {
  /*
   * Locking the bufferevent with this function will
   * lock its associated evbuffers as well
   * Note: it is automatically locked so we don't need
   *       to explicitly lock it
   *       bufferevent_lock(bev_);
   *       bufferevent_unlock(bev_);
   */
  struct evbuffer *input = bufferevent_get_input(bev_);
  return evbuffer_get_length(input);
}

/*
 * Get the len data from read buf and then save them in the give buffer
 * If the data in read buf are less than len, get all of the data
 * Return the length of moved data
 * Note: the len data are deleted from read buf after this operation
 */
int Connection::GetReadData(char *buffer, int len) {
  /*
   * Note: it is automatically locked so we don't need
   *       to explicitly lock it
   *       bufferevent_lock(bev_);
   *       bufferevent_unlock(bev_);
   */

  int remaining_len = len;
  do{
  struct evbuffer *input = bufferevent_get_input(bev_);
  remaining_len -= evbuffer_remove(input, buffer, len);

  }while(remaining_len > 0);
  return len;
}

/*
 * copy data (len) from read buf into the given buffer,
 * if the total data is less than len, then copy all of the data
 * return the length of the copied data
 * the data still exist in the read buf after this operation
 */
int Connection::CopyReadBuffer(char *buffer, int len) {
  /*
   * Note: it is automatically locked so we don't need
   *       to explicitly lock it
   *       bufferevent_lock(bev_);
   *       bufferevent_unlock(bev_);
   */
  struct evbuffer *input = bufferevent_get_input(bev_);
  return evbuffer_copyout(input, buffer, len);
}

/*
 * Get the lengh a write buf
 */
int Connection::GetWriteBufferLen() {
  /*
   * Note: it is automatically locked so we don't need
   *       to explicitly lock it
   *       bufferevent_lock(bev_);
   *       bufferevent_unlock(bev_);
   */
  struct evbuffer *output = bufferevent_get_output(bev_);
  return evbuffer_get_length(output);
}

/*
 * Add data to write buff
 */
bool Connection::AddToWriteBuffer(char *buffer, int len) {
  /*
   * Note: it is automatically locked so we don't need
   *       to explicitly lock it
   *       bufferevent_lock(bev_);
   *       bufferevent_unlock(bev_);
   */
  //    bufferevent_lock(bev_);
  //    struct evbuffer *output = bufferevent_get_output(bev_);
  //    int re = evbuffer_add(output, buffer, len);
  //    bufferevent_unlock(bev_);

  int re = bufferevent_write(bev_, buffer, len);

  if (re == 0) {
    return true;
  } else {
    return false;
  }
}

/*
 * put data in read buf into write buf
 */
void Connection::MoveBufferData() {
  /*
   * Note: it is automatically locked so we don't need
   *       to explicitly lock it
   *       bufferevent_lock(bev_);
   *       bufferevent_unlock(bev_);
   */
  struct evbuffer *input = bufferevent_get_input(bev_);
  struct evbuffer *output = bufferevent_get_output(bev_);

  /* Copy all the data from the input buffer to the output buffer. */
  evbuffer_add_buffer(output, input);
}

}  // namespace networking
}  // namespace peloton
<|endoftext|>


<commit_before>/*=====================================================================

QGroundControl Open Source Ground Control Station

(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>

This file is part of the QGROUNDCONTROL project

    QGROUNDCONTROL is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    QGROUNDCONTROL is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.

======================================================================*/

/**
 * @file
 *   @brief Implementation of class MAVLinkProtocol
 *   @author Lorenz Meier <mail@qgroundcontrol.org>
 */

#include <inttypes.h>
#include <iostream>

#include <QDebug>
#include <QTime>
#include <QApplication>

#include "MG.h"
#include "MAVLinkProtocol.h"
#include "UASInterface.h"
#include "UASManager.h"
#include "UASInterface.h"
#include "UAS.h"
#include "SlugsMAV.h"
#include "PxQuadMAV.h"
#include "ArduPilotMegaMAV.h"
#include "configuration.h"
#include "LinkManager.h"
#include <QGCMAVLink.h>
#include "QGC.h"

/**
 * The default constructor will create a new MAVLink object sending heartbeats at
 * the MAVLINK_HEARTBEAT_DEFAULT_RATE to all connected links.
 */
MAVLinkProtocol::MAVLinkProtocol() :
        heartbeatTimer(new QTimer(this)),
        heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE),
        m_heartbeatsEnabled(false),
        m_loggingEnabled(false),
        m_logfile(NULL)
{
    start(QThread::LowPriority);
    // Start heartbeat timer, emitting a heartbeat at the configured rate
    connect(heartbeatTimer, SIGNAL(timeout()), this, SLOT(sendHeartbeat()));
    heartbeatTimer->start(1000/heartbeatRate);
    totalReceiveCounter = 0;
    totalLossCounter = 0;
    currReceiveCounter = 0;
    currLossCounter = 0;
    for (int i = 0; i < 256; i++)
    {
        for (int j = 0; j < 256; j++)
        {
            lastIndex[i][j] = -1;
        }
    }
}

MAVLinkProtocol::~MAVLinkProtocol()
{
    if (m_logfile)
    {
        m_logfile->close();
        delete m_logfile;
    }
}

void MAVLinkProtocol::run()
{

}

QString MAVLinkProtocol::getLogfileName()
{
    return QCoreApplication::applicationDirPath()+"/mavlink.log";
}

/**
 * The bytes are copied by calling the LinkInterface::readBytes() method.
 * This method parses all incoming bytes and constructs a MAVLink packet.
 * It can handle multiple links in parallel, as each link has it's own buffer/
 * parsing state machine.
 * @param link The interface to read from
 * @see LinkInterface
 **/
void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
{
    receiveMutex.lock();
    mavlink_message_t message;
    mavlink_status_t status;
    for (int position = 0; position < b.size(); position++)
    {
        unsigned int decodeState = mavlink_parse_char(link->getId(), (uint8_t)(b.at(position)), &message, &status);

        if (decodeState == 1)
        {
            // Log data
            if (m_loggingEnabled)
            {
                uint8_t buf[MAVLINK_MAX_PACKET_LEN];
                quint64 time = MG::TIME::getGroundTimeNowUsecs();
                memcpy(buf, (void*)&time, sizeof(quint64));
                mavlink_msg_to_send_buffer(buf+sizeof(quint64), &message);
                m_logfile->write((const char*) buf);
                qDebug() << "WROTE LOGFILE";
            }

            // ORDER MATTERS HERE!
            // If the matching UAS object does not yet exist, it has to be created
            // before emitting the packetReceived signal
            UASInterface* uas = UASManager::instance()->getUASForId(message.sysid);

            // Check and (if necessary) create UAS object
            if (uas == NULL && message.msgid == MAVLINK_MSG_ID_HEARTBEAT)
            {
                // ORDER MATTERS HERE!
                // The UAS object has first to be created and connected,
                // only then the rest of the application can be made aware
                // of its existence, as it only then can send and receive
                // it's first messages.

                // FIXME Current debugging
                // check if the UAS has the same id like this system
                if (message.sysid == getSystemId())
                {
                    qDebug() << "WARNING\nWARNING\nWARNING\nWARNING\nWARNING\nWARNING\nWARNING\n\n RECEIVED MESSAGE FROM THIS SYSTEM WITH ID" << message.msgid << "FROM COMPONENT" << message.compid;
                }

                // Create a new UAS based on the heartbeat received
                // Todo dynamically load plugin at run-time for MAV
                // WIKISEARCH:AUTOPILOT_TYPE_INSTANTIATION

                // First create new UAS object
                // Decode heartbeat message
                mavlink_heartbeat_t heartbeat;
                mavlink_msg_heartbeat_decode(&message, &heartbeat);
                switch (heartbeat.autopilot)
                {
                case MAV_AUTOPILOT_GENERIC:
                    uas = new UAS(this, message.sysid);
                    // Connect this robot to the UAS object
                    connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), uas, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
                    break;
                case MAV_AUTOPILOT_PIXHAWK:
                    {
                    // Fixme differentiate between quadrotor and coaxial here
                    PxQuadMAV* mav = new PxQuadMAV(this, message.sysid);
                    // Connect this robot to the UAS object
                    // it is IMPORTANT here to use the right object type,
                    // else the slot of the parent object is called (and thus the special
                    // packets never reach their goal)
                    connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
                    uas = mav;
                }
                    break;
                case MAV_AUTOPILOT_SLUGS:
                    {
                    SlugsMAV* mav = new SlugsMAV(this, message.sysid);
                    // Connect this robot to the UAS object
                    // it is IMPORTANT here to use the right object type,
                    // else the slot of the parent object is called (and thus the special
                    // packets never reach their goal)
                    connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
                    uas = mav;
                }
                    break;
                case MAV_AUTOPILOT_ARDUPILOTMEGA:
                    {
                    ArduPilotMegaMAV* mav = new ArduPilotMegaMAV(this, message.sysid);
                    // Connect this robot to the UAS object
                    // it is IMPORTANT here to use the right object type,
                    // else the slot of the parent object is called (and thus the special
                    // packets never reach their goal)
                    connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
                    uas = mav;
                }
                    break;
                default:
                    uas = new UAS(this, message.sysid);
                    break;
                }

                // Make UAS aware that this link can be used to communicate with the actual robot
                uas->addLink(link);
                // Now add UAS to "official" list, which makes the whole application aware of it
                UASManager::instance()->addUAS(uas);
            }

            // Only count message if UAS exists for this message
            if (uas != NULL)
            {
                // Increase receive counter
                totalReceiveCounter++;
                currReceiveCounter++;
                qint64 lastLoss = totalLossCounter;
                // Update last packet index
                if (lastIndex[message.sysid][message.compid] == -1)
                {
                    lastIndex[message.sysid][message.compid] = message.seq;
                }
                else
                {
                    if (lastIndex[message.sysid][message.compid] == 255)
                    {
                        lastIndex[message.sysid][message.compid] = 0;
                    }
                    else
                    {
                        lastIndex[message.sysid][message.compid]++;
                    }

                    int safeguard = 0;
                    //qDebug() << "SYSID" << message.sysid << "COMPID" << message.compid << "MSGID" << message.msgid << "LASTINDEX" << lastIndex[message.sysid][message.compid] << "SEQ" << message.seq;
                    while(lastIndex[message.sysid][message.compid] != message.seq && safeguard < 255)
                    {
                        if (lastIndex[message.sysid][message.compid] == 255)
                        {
                            lastIndex[message.sysid][message.compid] = 0;
                        }
                        else
                        {
                            lastIndex[message.sysid][message.compid]++;
                        }
                        totalLossCounter++;
                        currLossCounter++;
                        safeguard++;
                    }
                }
                //            if (lastIndex.contains(message.sysid))
                //            {
                //                QMap<int, int>* lastCompIndex = lastIndex.value(message.sysid);
                //                if (lastCompIndex->contains(message.compid))
                //                while (lastCompIndex->value(message.compid, 0)+1 )
                //            }
                //if ()

                // If a new loss was detected or we just hit one 128th packet step
                if (lastLoss != totalLossCounter || (totalReceiveCounter == 128))
                {
                    // Calculate new loss ratio
                    // Receive loss
                    float receiveLoss = (double)currLossCounter/(double)(currReceiveCounter+currLossCounter);
                    receiveLoss *= 100.0f;
                    // qDebug() << "LOSSCHANGED" << receiveLoss;
                    currLossCounter = 0;
                    currReceiveCounter = 0;
                    emit receiveLossChanged(message.sysid, receiveLoss);
                }

                // The packet is emitted as a whole, as it is only 255 - 261 bytes short
                // kind of inefficient, but no issue for a groundstation pc.
                // It buys as reentrancy for the whole code over all threads
                emit messageReceived(link, message);
            }
        }
    }
    receiveMutex.unlock();
}

/**
 * @return The name of this protocol
 **/
QString MAVLinkProtocol::getName()
{
    return QString(tr("MAVLink protocol"));
}

/** @return System id of this application */
int MAVLinkProtocol::getSystemId()
{
    return MG::SYSTEM::ID;
}

/** @return Component id of this application */
int MAVLinkProtocol::getComponentId()
{
    return MG::SYSTEM::COMPID;
}

/**
 * @param message message to send
 */
void MAVLinkProtocol::sendMessage(mavlink_message_t message)
{
    // Get all links connected to this unit
    QList<LinkInterface*> links = LinkManager::instance()->getLinksForProtocol(this);

    // Emit message on all links that are currently connected
    QList<LinkInterface*>::iterator i;
    for (i = links.begin(); i != links.end(); ++i)
    {
        sendMessage(*i, message);
        //qDebug() << __FILE__ << __LINE__ << "SENT MESSAGE OVER" << ((LinkInterface*)*i)->getName() << "LIST SIZE:" << links.size();
    }
}

/**
 * @param link the link to send the message over
 * @param message message to send
 */
void MAVLinkProtocol::sendMessage(LinkInterface* link, mavlink_message_t message)
{
    // Create buffer
    uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
    // Rewriting header to ensure correct link ID is set
    if (link->getId() != 0) mavlink_finalize_message_chan(&message, this->getSystemId(), this->getComponentId(), link->getId(), message.len);
    // Write message into buffer, prepending start sign
    int len = mavlink_msg_to_send_buffer(buffer, &message);
    // If link is connected
    if (link->isConnected())
    {
        // Send the portion of the buffer now occupied by the message
        link->writeBytes((const char*)buffer, len);
    }
}

/**
 * The heartbeat is sent out of order and does not reset the
 * periodic heartbeat emission. It will be just sent in addition.
 * @return mavlink_message_t heartbeat message sent on serial link
 */
void MAVLinkProtocol::sendHeartbeat()
{
    if (m_heartbeatsEnabled)
    {
        mavlink_message_t beat;
        mavlink_msg_heartbeat_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID,&beat, OCU, MAV_AUTOPILOT_GENERIC);
        sendMessage(beat);
    }
}

/** @param enabled true to enable heartbeats emission at heartbeatRate, false to disable */
void MAVLinkProtocol::enableHeartbeats(bool enabled)
{
    m_heartbeatsEnabled = enabled;
    emit heartbeatChanged(enabled);
}

void MAVLinkProtocol::enableLogging(bool enabled)
{
    if (enabled && !m_loggingEnabled)
    {
       m_logfile = new QFile(getLogfileName());
       m_logfile->open(QIODevice::WriteOnly | QIODevice::Append);
    }
    else
    {
       m_logfile->close();
       delete m_logfile;
       m_logfile = NULL;
    }
    m_loggingEnabled = enabled;
}

bool MAVLinkProtocol::heartbeatsEnabled(void)
{
    return m_heartbeatsEnabled;
}

bool MAVLinkProtocol::loggingEnabled(void)
{
    return m_loggingEnabled;
}

/**
 * The default rate is 1 Hertz.
 *
 * @param rate heartbeat rate in hertz (times per second)
 */
void MAVLinkProtocol::setHeartbeatRate(int rate)
{
    heartbeatRate = rate;
    heartbeatTimer->setInterval(1000/heartbeatRate);
}

/** @return heartbeat rate in Hertz */
int MAVLinkProtocol::getHeartbeatRate()
{
    return heartbeatRate;
}
<commit_msg>Added updating of loss counter<commit_after>/*=====================================================================

QGroundControl Open Source Ground Control Station

(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>

This file is part of the QGROUNDCONTROL project

    QGROUNDCONTROL is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    QGROUNDCONTROL is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.

======================================================================*/

/**
 * @file
 *   @brief Implementation of class MAVLinkProtocol
 *   @author Lorenz Meier <mail@qgroundcontrol.org>
 */

#include <inttypes.h>
#include <iostream>

#include <QDebug>
#include <QTime>
#include <QApplication>

#include "MG.h"
#include "MAVLinkProtocol.h"
#include "UASInterface.h"
#include "UASManager.h"
#include "UASInterface.h"
#include "UAS.h"
#include "SlugsMAV.h"
#include "PxQuadMAV.h"
#include "ArduPilotMegaMAV.h"
#include "configuration.h"
#include "LinkManager.h"
#include <QGCMAVLink.h>
#include "QGC.h"

/**
 * The default constructor will create a new MAVLink object sending heartbeats at
 * the MAVLINK_HEARTBEAT_DEFAULT_RATE to all connected links.
 */
MAVLinkProtocol::MAVLinkProtocol() :
        heartbeatTimer(new QTimer(this)),
        heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE),
        m_heartbeatsEnabled(false),
        m_loggingEnabled(false),
        m_logfile(NULL)
{
    start(QThread::LowPriority);
    // Start heartbeat timer, emitting a heartbeat at the configured rate
    connect(heartbeatTimer, SIGNAL(timeout()), this, SLOT(sendHeartbeat()));
    heartbeatTimer->start(1000/heartbeatRate);
    totalReceiveCounter = 0;
    totalLossCounter = 0;
    currReceiveCounter = 0;
    currLossCounter = 0;
    for (int i = 0; i < 256; i++)
    {
        for (int j = 0; j < 256; j++)
        {
            lastIndex[i][j] = -1;
        }
    }
}

MAVLinkProtocol::~MAVLinkProtocol()
{
    if (m_logfile)
    {
        m_logfile->close();
        delete m_logfile;
    }
}

void MAVLinkProtocol::run()
{

}

QString MAVLinkProtocol::getLogfileName()
{
    return QCoreApplication::applicationDirPath()+"/mavlink.log";
}

/**
 * The bytes are copied by calling the LinkInterface::readBytes() method.
 * This method parses all incoming bytes and constructs a MAVLink packet.
 * It can handle multiple links in parallel, as each link has it's own buffer/
 * parsing state machine.
 * @param link The interface to read from
 * @see LinkInterface
 **/
void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
{
    receiveMutex.lock();
    mavlink_message_t message;
    mavlink_status_t status;
    for (int position = 0; position < b.size(); position++)
    {
        unsigned int decodeState = mavlink_parse_char(link->getId(), (uint8_t)(b.at(position)), &message, &status);

        if (decodeState == 1)
        {
            // Log data
            if (m_loggingEnabled)
            {
                uint8_t buf[MAVLINK_MAX_PACKET_LEN];
                quint64 time = MG::TIME::getGroundTimeNowUsecs();
                memcpy(buf, (void*)&time, sizeof(quint64));
                mavlink_msg_to_send_buffer(buf+sizeof(quint64), &message);
                m_logfile->write((const char*) buf);
                qDebug() << "WROTE LOGFILE";
            }

            // ORDER MATTERS HERE!
            // If the matching UAS object does not yet exist, it has to be created
            // before emitting the packetReceived signal
            UASInterface* uas = UASManager::instance()->getUASForId(message.sysid);

            // Check and (if necessary) create UAS object
            if (uas == NULL && message.msgid == MAVLINK_MSG_ID_HEARTBEAT)
            {
                // ORDER MATTERS HERE!
                // The UAS object has first to be created and connected,
                // only then the rest of the application can be made aware
                // of its existence, as it only then can send and receive
                // it's first messages.

                // FIXME Current debugging
                // check if the UAS has the same id like this system
                if (message.sysid == getSystemId())
                {
                    qDebug() << "WARNING\nWARNING\nWARNING\nWARNING\nWARNING\nWARNING\nWARNING\n\n RECEIVED MESSAGE FROM THIS SYSTEM WITH ID" << message.msgid << "FROM COMPONENT" << message.compid;
                }

                // Create a new UAS based on the heartbeat received
                // Todo dynamically load plugin at run-time for MAV
                // WIKISEARCH:AUTOPILOT_TYPE_INSTANTIATION

                // First create new UAS object
                // Decode heartbeat message
                mavlink_heartbeat_t heartbeat;
                mavlink_msg_heartbeat_decode(&message, &heartbeat);
                switch (heartbeat.autopilot)
                {
                case MAV_AUTOPILOT_GENERIC:
                    uas = new UAS(this, message.sysid);
                    // Connect this robot to the UAS object
                    connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), uas, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
                    break;
                case MAV_AUTOPILOT_PIXHAWK:
                    {
                    // Fixme differentiate between quadrotor and coaxial here
                    PxQuadMAV* mav = new PxQuadMAV(this, message.sysid);
                    // Connect this robot to the UAS object
                    // it is IMPORTANT here to use the right object type,
                    // else the slot of the parent object is called (and thus the special
                    // packets never reach their goal)
                    connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
                    uas = mav;
                }
                    break;
                case MAV_AUTOPILOT_SLUGS:
                    {
                    SlugsMAV* mav = new SlugsMAV(this, message.sysid);
                    // Connect this robot to the UAS object
                    // it is IMPORTANT here to use the right object type,
                    // else the slot of the parent object is called (and thus the special
                    // packets never reach their goal)
                    connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
                    uas = mav;
                }
                    break;
                case MAV_AUTOPILOT_ARDUPILOTMEGA:
                    {
                    ArduPilotMegaMAV* mav = new ArduPilotMegaMAV(this, message.sysid);
                    // Connect this robot to the UAS object
                    // it is IMPORTANT here to use the right object type,
                    // else the slot of the parent object is called (and thus the special
                    // packets never reach their goal)
                    connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
                    uas = mav;
                }
                    break;
                default:
                    uas = new UAS(this, message.sysid);
                    break;
                }

                // Make UAS aware that this link can be used to communicate with the actual robot
                uas->addLink(link);
                // Now add UAS to "official" list, which makes the whole application aware of it
                UASManager::instance()->addUAS(uas);
            }

            // Only count message if UAS exists for this message
            if (uas != NULL)
            {
                // Increase receive counter
                totalReceiveCounter++;
                currReceiveCounter++;
                qint64 lastLoss = totalLossCounter;
                // Update last packet index
                if (lastIndex[message.sysid][message.compid] == -1)
                {
                    lastIndex[message.sysid][message.compid] = message.seq;
                }
                else
                {
                    if (lastIndex[message.sysid][message.compid] == 255)
                    {
                        lastIndex[message.sysid][message.compid] = 0;
                    }
                    else
                    {
                        lastIndex[message.sysid][message.compid]++;
                    }

                    int safeguard = 0;
                    //qDebug() << "SYSID" << message.sysid << "COMPID" << message.compid << "MSGID" << message.msgid << "LASTINDEX" << lastIndex[message.sysid][message.compid] << "SEQ" << message.seq;
                    while(lastIndex[message.sysid][message.compid] != message.seq && safeguard < 255)
                    {
                        if (lastIndex[message.sysid][message.compid] == 255)
                        {
                            lastIndex[message.sysid][message.compid] = 0;
                        }
                        else
                        {
                            lastIndex[message.sysid][message.compid]++;
                        }
                        totalLossCounter++;
                        currLossCounter++;
                        safeguard++;
                    }
                }
                //            if (lastIndex.contains(message.sysid))
                //            {
                //                QMap<int, int>* lastCompIndex = lastIndex.value(message.sysid);
                //                if (lastCompIndex->contains(message.compid))
                //                while (lastCompIndex->value(message.compid, 0)+1 )
                //            }
                //if ()

                // If a new loss was detected or we just hit one 128th packet step
                if (lastLoss != totalLossCounter || (totalReceiveCounter % 64 == 0))
                {
                    // Calculate new loss ratio
                    // Receive loss
                    float receiveLoss = (double)currLossCounter/(double)(currReceiveCounter+currLossCounter);
                    receiveLoss *= 100.0f;
                    // qDebug() << "LOSSCHANGED" << receiveLoss;
                    currLossCounter = 0;
                    currReceiveCounter = 0;
                    emit receiveLossChanged(message.sysid, receiveLoss);
                }

                // The packet is emitted as a whole, as it is only 255 - 261 bytes short
                // kind of inefficient, but no issue for a groundstation pc.
                // It buys as reentrancy for the whole code over all threads
                emit messageReceived(link, message);
            }
        }
    }
    receiveMutex.unlock();
}

/**
 * @return The name of this protocol
 **/
QString MAVLinkProtocol::getName()
{
    return QString(tr("MAVLink protocol"));
}

/** @return System id of this application */
int MAVLinkProtocol::getSystemId()
{
    return MG::SYSTEM::ID;
}

/** @return Component id of this application */
int MAVLinkProtocol::getComponentId()
{
    return MG::SYSTEM::COMPID;
}

/**
 * @param message message to send
 */
void MAVLinkProtocol::sendMessage(mavlink_message_t message)
{
    // Get all links connected to this unit
    QList<LinkInterface*> links = LinkManager::instance()->getLinksForProtocol(this);

    // Emit message on all links that are currently connected
    QList<LinkInterface*>::iterator i;
    for (i = links.begin(); i != links.end(); ++i)
    {
        sendMessage(*i, message);
        //qDebug() << __FILE__ << __LINE__ << "SENT MESSAGE OVER" << ((LinkInterface*)*i)->getName() << "LIST SIZE:" << links.size();
    }
}

/**
 * @param link the link to send the message over
 * @param message message to send
 */
void MAVLinkProtocol::sendMessage(LinkInterface* link, mavlink_message_t message)
{
    // Create buffer
    uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
    // Rewriting header to ensure correct link ID is set
    if (link->getId() != 0) mavlink_finalize_message_chan(&message, this->getSystemId(), this->getComponentId(), link->getId(), message.len);
    // Write message into buffer, prepending start sign
    int len = mavlink_msg_to_send_buffer(buffer, &message);
    // If link is connected
    if (link->isConnected())
    {
        // Send the portion of the buffer now occupied by the message
        link->writeBytes((const char*)buffer, len);
    }
}

/**
 * The heartbeat is sent out of order and does not reset the
 * periodic heartbeat emission. It will be just sent in addition.
 * @return mavlink_message_t heartbeat message sent on serial link
 */
void MAVLinkProtocol::sendHeartbeat()
{
    if (m_heartbeatsEnabled)
    {
        mavlink_message_t beat;
        mavlink_msg_heartbeat_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID,&beat, OCU, MAV_AUTOPILOT_GENERIC);
        sendMessage(beat);
    }
}

/** @param enabled true to enable heartbeats emission at heartbeatRate, false to disable */
void MAVLinkProtocol::enableHeartbeats(bool enabled)
{
    m_heartbeatsEnabled = enabled;
    emit heartbeatChanged(enabled);
}

void MAVLinkProtocol::enableLogging(bool enabled)
{
    if (enabled && !m_loggingEnabled)
    {
       m_logfile = new QFile(getLogfileName());
       m_logfile->open(QIODevice::WriteOnly | QIODevice::Append);
    }
    else
    {
       m_logfile->close();
       delete m_logfile;
       m_logfile = NULL;
    }
    m_loggingEnabled = enabled;
}

bool MAVLinkProtocol::heartbeatsEnabled(void)
{
    return m_heartbeatsEnabled;
}

bool MAVLinkProtocol::loggingEnabled(void)
{
    return m_loggingEnabled;
}

/**
 * The default rate is 1 Hertz.
 *
 * @param rate heartbeat rate in hertz (times per second)
 */
void MAVLinkProtocol::setHeartbeatRate(int rate)
{
    heartbeatRate = rate;
    heartbeatTimer->setInterval(1000/heartbeatRate);
}

/** @return heartbeat rate in Hertz */
int MAVLinkProtocol::getHeartbeatRate()
{
    return heartbeatRate;
}
<|endoftext|>


<commit_before>#ifndef CMDSTAN_ARGUMENTS_ARG_METRIC_FILE_HPP
#define CMDSTAN_ARGUMENTS_ARG_METRIC_FILE_HPP

#include <cmdstan/arguments/singleton_argument.hpp>

namespace cmdstan {

  class arg_metric_file: public string_argument {
  public:
    arg_metric_file(): string_argument() {
      _name = "metric_file";
      _description = "Input file with precomputed Euclidean metric";
      _validity = "Path to existing file";
      _default = "\"\"";
      _default_value = "";
      _constrained = false;
      _good_value = "good";
      _value = _default_value;
    }
  };

}

#endif
<commit_msg>Changed good value for metric_file argument to ""<commit_after>#ifndef CMDSTAN_ARGUMENTS_ARG_METRIC_FILE_HPP
#define CMDSTAN_ARGUMENTS_ARG_METRIC_FILE_HPP

#include <cmdstan/arguments/singleton_argument.hpp>

namespace cmdstan {

  class arg_metric_file: public string_argument {
  public:
    arg_metric_file(): string_argument() {
      _name = "metric_file";
      _description = "Input file with precomputed Euclidean metric";
      _validity = "Path to existing file";
      _default = "\"\"";
      _default_value = "";
      _constrained = false;
      _good_value = "";
      _value = _default_value;
    }
  };

}

#endif
<|endoftext|>


<commit_before>/*  Copyright 2010 Anders Wallin (anders.e.e.wallin "at" gmail.com)
 *
 *  This file is part of OpenCAMlib.
 *
 *  OpenCAMlib is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  OpenCAMlib is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with OpenCAMlib.  If not, see <http://www.gnu.org/licenses/>.
*/
#include <iostream>
#include <stdio.h>
#include <sstream>
#include <math.h>
#include <boost/progress.hpp>

#include "cutter.h"
#include "point.h"
#include "triangle.h"
#include "stlsurf.h"
#include "cutter.h"
#include "numeric.h"

namespace ocl
{

//********   constructors ********************** */
ConeCutter::ConeCutter()
{
    setDiameter(1.0);
    angle = 45;
    height = (diameter/2)/tan(angle);
}

ConeCutter::ConeCutter(const double d, const double a)
{
    setDiameter(d);
    angle = a;
    height = (diameter/2)/tan(angle);
}

//********   drop-cutter methods ********************** */
int ConeCutter::vertexDrop(Point &cl, CCPoint &cc, const Triangle &t) const
{
    int result = 0;
    BOOST_FOREACH( const Point& p, t.p)
    {
        double q = cl.xyDistance(p); // distance in XY-plane from cl to p
        if (q<= diameter/2) { // p is inside the cutter
            // h1 = q / tan(alfa)
            // cutter_tip = p.z - h1
            assert( tan(angle) > 0.0 ); // guard against division by zero
            double h1 =  q/tan(angle);
            if (cl.liftZ(p.z - h1)) { // we need to lift the cutter
                cc = p;
                cc.type = VERTEX;
                result = 1;
            }
        } else {
            // point outside cutter, nothing to do.
        }
    }
    return result;
}

// we either hit the tip, when the slope of the plane is smaller than angle
// or when the slope is steep, the circular edge between the cone and the cylindrical shaft
int ConeCutter::facetDrop(Point &cl, CCPoint &cc, const Triangle &t) const
{
    int result = 0;
    // Drop cutter at (cl.x, cl.y) against facet of Triangle t

    Point normal; // facet surface normal

    if ( isZero_tol( t.n->z ) )  {// vertical surface
        return -1;  //can't drop against vertical surface
    } else if (t.n->z < 0) {  // normal is pointing down
        normal = -1* (*t.n); // flip normal
    } else {
        normal = *t.n;
    }

    assert( isPositive( normal.z ) );

    if ( (isZero_tol(normal.x)) && (isZero_tol(normal.y)) ) { // horizontal plane
        // so any vertex is at the correct height
        Point cc_tmp;
        cc_tmp.x = cl.x;
        cc_tmp.y = cl.y;
        cc_tmp.z = t.p[0].z;
        if (cc_tmp.isInside(t)) { // cc-point is on the axis of the cutter
            if ( cl.liftZ(cc_tmp.z) ) {
                cc = cc_tmp;
                cc.type = FACET_TIP;
                return 1;
            }
        } else { // not inside facet
                return 0;
        }
    } // end horizontal plane case.

    // define plane containing facet
    // a*x + b*y + c*z + d = 0, so
    // d = -a*x - b*y - c*z, where
    // (a,b,c) = surface normal
    double a = normal.x;
    double b = normal.y;
    double c = normal.z;
    //double d = - a * t.p[0].x - b * t.p[0].y - c * t.p[0].z;
    double d = - normal.dot(t.p[0]);

    normal.xyNormalize(); // make length of normal == 1.0

    // cylindrical contact point case
    // find the xy-coordinates of the cc-point
    Point cyl_cc_tmp = cl - (diameter/2)*normal;
    cyl_cc_tmp.z = (1.0/c)*(-d-a*cyl_cc_tmp.x-b*cyl_cc_tmp.y);
    double cyl_cl_z = cyl_cc_tmp.z - height; // tip positioned here

    Point tip_cc_tmp = Point(cl.x,cl.y,0);
    tip_cc_tmp.z = (1.0/c)*(-d-a*tip_cc_tmp.x-b*tip_cc_tmp.y);
    double tip_cl_z = tip_cc_tmp.z;

    if (tip_cc_tmp.isInside(t)) { // TIP case
        if ( cl.liftZ(tip_cl_z) ) {
            cc = tip_cc_tmp;
            cc.type = FACET_TIP;
            result = 1;
        }
    } if (cyl_cc_tmp.isInside(t))  { // CYLINDER case
        if ( cl.liftZ(cyl_cl_z) ) {
            cc = cyl_cc_tmp;
            cc.type = FACET_CYL;
            result = 1;
        }
    } else {
        return 0;
    }
    return result; // we never get here (?)
}

// FIXME FIXME (code is copy of ballcutter for now)
// cone sliced with vertical plane results in a hyperbola as the intersection curve
// find point where hyperbola and line slopes match? (or use radius-vector approach as in ball cutter?)
int ConeCutter::edgeDrop(Point &cl, CCPoint &cc, const Triangle &t) const
{
    // Drop cutter at (p.x, p.y) against edges of Triangle t
    // strategy:
    // 1) calculate distance to infinite line
    // 2) calculate intersection points w. cutter
    // 3) pick the higher intersection point and test if it is in the edge

    int result = 0;

    for (int n=0;n<3;n++) { // loop through all three edges

        // 1) distance from point to line in xy plane
        int start=n;      // index of the start-point of the edge
        int end=(n+1)%3;  // index of the end-point of the edge
        //std::cout << "testing edge " << start << " to " << end << "\n";
        Point p1 = t.p[start];
        Point p2 = t.p[end];

        // check that there is an edge in the xy-plane
        // can't drop against vertical edges!
        if ( !isZero_tol( p1.x - p2.x) || !isZero_tol( p1.y - p2.y) ) {

            //std::cout << "Points " << p1 << " to " << p2 << "\n";
            double d = cl.xyDistanceToLine(p1, p2);
            assert( d >= 0.0 );

            if (d<=(diameter/2)) { // potential hit

                // the plane of the line will slice the spherical cutter at
                // a distance d from the center of the cutter
                // here the radius of the circular section is
                double s = sqrt( square((diameter/2)) - square(d) );

                // the center-point of this circle, in the xy plane lies at
                Point sc = cl.xyClosestPoint( p1, p2 );

                Point v = p2 - p1;
                Point start2sc_dir = sc - p1;
                start2sc_dir.xyNormalize();
                start2sc_dir.z=0;
                double dz = p2.z - p1.z;
                double p2u = v.dot(start2sc_dir); // u-coord of p2 in plane coordinates.

                // in the vertical plane of the line:
                // (du,dz) points in the direction of the line
                // so (dz, -du) is a normal to the line
                Point tangent = Point(p2u,dz,0);
                tangent.xyNormalize();
                Point normal = Point (dz, -p2u, 0);
                normal.xyNormalize();
                if (normal.y < 0) { // flip normal so it points upward
                    normal = -1*normal;
                }
                assert( isPositive(normal.y) );

                Point start2sc = sc - p1;
                double sc_u = start2sc.dot( start2sc_dir  ); // horiz distance from startpoint to sc

                double cc_u = sc_u - s * normal.x; // horiz dist of cc-point in plane-cordinates

                Point cc_tmp = p1 + (cc_u/p2u)*v;

                double cl_z = cc_tmp.z + s*normal.y - (diameter/2);

                // test if cc-point is in edge
                if ( cc_tmp.isInsidePoints( p1, p2 ) ) {
                    if (cl.liftZ(cl_z)) {
                        cc = cc_tmp;
                        cc.type = EDGE;
                        result = 1;
                    }

                }

            }// end if(potential hit)
            else {
                // edge is too far away from cutter. nothing to do.
            }
        }// end if(vertical edge)

    } // end loop through all edges

    return result;
}

//******** string output ********************** */
std::string ConeCutter::str()
{
    std::ostringstream o;
    o << *this;
    return o.str();
}

std::ostream& operator<<(std::ostream &stream, ConeCutter c)
{
  stream << "ConeCutter (d=" << c.diameter << ", angle=" << c.angle << ", height=" << c.height << ")";
  return stream;
}

} // end namespace
// end file conecutter.cpp
<commit_msg>start on ConeCutter::edgeDrop()<commit_after>/*  Copyright 2010 Anders Wallin (anders.e.e.wallin "at" gmail.com)
 *
 *  This file is part of OpenCAMlib.
 *
 *  OpenCAMlib is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  OpenCAMlib is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with OpenCAMlib.  If not, see <http://www.gnu.org/licenses/>.
*/
#include <iostream>
#include <stdio.h>
#include <sstream>
#include <math.h>
#include <boost/progress.hpp>

#include "cutter.h"
#include "point.h"
#include "triangle.h"
#include "stlsurf.h"
#include "cutter.h"
#include "numeric.h"

namespace ocl
{

//********   constructors ********************** */
ConeCutter::ConeCutter()
{
    setDiameter(1.0);
    angle = 45;
    height = (diameter/2)/tan(angle);
}

ConeCutter::ConeCutter(const double d, const double a)
{
    setDiameter(d);
    angle = a;
    height = (diameter/2)/tan(angle);
}

//********   drop-cutter methods ********************** */
int ConeCutter::vertexDrop(Point &cl, CCPoint &cc, const Triangle &t) const
{
    int result = 0;
    BOOST_FOREACH( const Point& p, t.p)
    {
        double q = cl.xyDistance(p); // distance in XY-plane from cl to p
        if (q<= diameter/2) { // p is inside the cutter
            // h1 = q / tan(alfa)
            // cutter_tip = p.z - h1
            assert( tan(angle) > 0.0 ); // guard against division by zero
            double h1 =  q/tan(angle);
            if (cl.liftZ(p.z - h1)) { // we need to lift the cutter
                cc = p;
                cc.type = VERTEX;
                result = 1;
            }
        } else {
            // point outside cutter, nothing to do.
        }
    }
    return result;
}

// we either hit the tip, when the slope of the plane is smaller than angle
// or when the slope is steep, the circular edge between the cone and the cylindrical shaft
int ConeCutter::facetDrop(Point &cl, CCPoint &cc, const Triangle &t) const
{
    int result = 0;
    // Drop cutter at (cl.x, cl.y) against facet of Triangle t

    Point normal; // facet surface normal

    if ( isZero_tol( t.n->z ) )  {// vertical surface
        return -1;  //can't drop against vertical surface
    } else if (t.n->z < 0) {  // normal is pointing down
        normal = -1* (*t.n); // flip normal
    } else {
        normal = *t.n;
    }

    assert( isPositive( normal.z ) );

    if ( (isZero_tol(normal.x)) && (isZero_tol(normal.y)) ) { // horizontal plane
        // so any vertex is at the correct height
        Point cc_tmp;
        cc_tmp.x = cl.x;
        cc_tmp.y = cl.y;
        cc_tmp.z = t.p[0].z;
        if (cc_tmp.isInside(t)) { // cc-point is on the axis of the cutter
            if ( cl.liftZ(cc_tmp.z) ) {
                cc = cc_tmp;
                cc.type = FACET_TIP;
                return 1;
            }
        } else { // not inside facet
                return 0;
        }
    } // end horizontal plane case.

    // define plane containing facet
    // a*x + b*y + c*z + d = 0, so
    // d = -a*x - b*y - c*z, where
    // (a,b,c) = surface normal
    double a = normal.x;
    double b = normal.y;
    double c = normal.z;
    //double d = - a * t.p[0].x - b * t.p[0].y - c * t.p[0].z;
    double d = - normal.dot(t.p[0]);

    normal.xyNormalize(); // make length of normal == 1.0

    // cylindrical contact point case
    // find the xy-coordinates of the cc-point
    Point cyl_cc_tmp = cl - (diameter/2)*normal;
    cyl_cc_tmp.z = (1.0/c)*(-d-a*cyl_cc_tmp.x-b*cyl_cc_tmp.y);
    double cyl_cl_z = cyl_cc_tmp.z - height; // tip positioned here

    Point tip_cc_tmp = Point(cl.x,cl.y,0);
    tip_cc_tmp.z = (1.0/c)*(-d-a*tip_cc_tmp.x-b*tip_cc_tmp.y);
    double tip_cl_z = tip_cc_tmp.z;

    if (tip_cc_tmp.isInside(t)) { // TIP case
        if ( cl.liftZ(tip_cl_z) ) {
            cc = tip_cc_tmp;
            cc.type = FACET_TIP;
            result = 1;
        }
    } if (cyl_cc_tmp.isInside(t))  { // CYLINDER case
        if ( cl.liftZ(cyl_cl_z) ) {
            cc = cyl_cc_tmp;
            cc.type = FACET_CYL;
            result = 1;
        }
    } else {
        return 0;
    }
    return result; // we never get here (?)
}

// cone sliced with vertical plane results in a hyperbola as the intersection curve
// find point where hyperbola and line slopes match? (or use radius-vector approach as in ball cutter?)
int ConeCutter::edgeDrop(Point &cl, CCPoint &cc, const Triangle &t) const
{
    // Drop cutter at (p.x, p.y) against edges of Triangle t
    // strategy:
    // 1) calculate distance to infinite line
    // 2) calculate intersection points w. cutter

    int result = 0;

    for (int n=0;n<3;n++) { // loop through all three edges

        // 1) distance from point to line in xy plane
        int start=n;      // index of the start-point of the edge
        int end=(n+1)%3;  // index of the end-point of the edge
        Point p1 = t.p[start];
        Point p2 = t.p[end];

        // check that there is an edge in the xy-plane
        // can't drop against vertical edges!
        if ( !isZero_tol( p1.x - p2.x) || !isZero_tol( p1.y - p2.y) ) {

            //std::cout << "Points " << p1 << " to " << p2 << "\n";
            double d = cl.xyDistanceToLine(p1, p2);
            assert( d >= 0.0 );

            if (d<=(diameter/2)) { // potential hit

                // http://en.wikipedia.org/wiki/Hyperbola
                // hyperbola defined by
                // x^2 / a^2 - y^2 / b^2 = 1
                // or in parametric form
                // ( a*cosh(u), b*sinh(u) )
                // where
                // x = c * cos(theta) * cosh(u)
                // y = c * sin(theta) * sinh(u)
                // hyperbola has focus at c on the x-axis
                // theta= angle of asymptotes with line of foci (?)
                // this parabola lies on its side and opens towards the +x axis
                // at u=0 the x-coordinate is a, since
                // cosh(0) = 1.0

                double a=1.0;

                // the plane of the line slices the cone at
                // a distance d from the center of the cutter
                // here the width of the parabola should be
                double s = sqrt( square((diameter/2)) - square(d) );

                // the center-point of this circle, in the xy plane lies at
                Point sc = cl.xyClosestPoint( p1, p2 );

                Point v = p2 - p1;
                Point start2sc_dir = sc - p1;
                start2sc_dir.xyNormalize();
                start2sc_dir.z=0;
                double dz = p2.z - p1.z;
                double p2u = v.dot(start2sc_dir); // u-coord of p2 in plane coordinates.

                // in the vertical plane of the line:
                // (du,dz) points in the direction of the line
                // so (dz, -du) is a normal to the line
                Point tangent = Point(p2u,dz,0);
                tangent.xyNormalize();
                Point normal = Point (dz, -p2u, 0);
                normal.xyNormalize();
                if (normal.y < 0) { // flip normal so it points upward
                    normal = -1*normal;
                }
                assert( isPositive(normal.y) );

                Point start2sc = sc - p1;
                double sc_u = start2sc.dot( start2sc_dir  ); // horiz distance from startpoint to sc

                double cc_u = sc_u - s * normal.x; // horiz dist of cc-point in plane-cordinates

                Point cc_tmp = p1 + (cc_u/p2u)*v;

                double cl_z = cc_tmp.z + s*normal.y - (diameter/2);

                // test if cc-point is in edge
                if ( cc_tmp.isInsidePoints( p1, p2 ) ) {
                    if (cl.liftZ(cl_z)) {
                        cc = cc_tmp;
                        cc.type = EDGE;
                        result = 1;
                    }

                }

            }// end if(potential hit)
            else {
                // edge is too far away from cutter. nothing to do.
            }
        }// end if(vertical edge)

    } // end loop through all edges

    return result;
}

//******** string output ********************** */
std::string ConeCutter::str()
{
    std::ostringstream o;
    o << *this;
    return o.str();
}

std::ostream& operator<<(std::ostream &stream, ConeCutter c)
{
  stream << "ConeCutter (d=" << c.diameter << ", angle=" << c.angle << ", height=" << c.height << ")";
  return stream;
}

} // end namespace
// end file conecutter.cpp
<|endoftext|>


<commit_before>#ifndef CONS_UTIL_I_HH
#define CONS_UTIL_I_HH

#ifndef CONS_UTIL_HH
#error "Please include via parent file"
#endif

#include <utility>
#include <algorithm>
#include "util.hh"

inline
bool nullp(Lisp_ptr p){
  // When Lisp_ptr::get<Cons*>() became constexpr,
  // '<void*>' should be replaced with '<Cons*>'
  static_assert(Cons::NIL.get<void*>() == nullptr,
                "NIL's pointer part is not nullptr!");
  return (p.tag() == Ptr_tag::cons)
    && (p.get<void*>() == nullptr);
}

template<typename MainFun, typename LastFun>
auto do_list(Lisp_ptr lis, MainFun&& m_fun, LastFun&& l_fun)
  -> decltype(l_fun(lis)){
  Lisp_ptr p = lis;

  while(auto c = p.get<Cons*>()){
    auto next = c->cdr();
    if(!m_fun(c))
      break;

    p = next;
  }

  return l_fun(p);
}

template<typename MainFun, typename LastFun>
auto do_list_2(Lisp_ptr lis1, Lisp_ptr lis2, MainFun&& m_fun, LastFun&& l_fun)
  -> decltype(l_fun(lis1, lis2)){
  Lisp_ptr p1 = lis1;
  Lisp_ptr p2 = lis2;

  Cons *c1, *c2;

  while((c1 = p1.get<Cons*>()) && (c2 = p2.get<Cons*>())){
    auto next1 = c1->cdr();
    auto next2 = c2->cdr();
    if(!m_fun(c1, c2))
      break;

    p1 = next1;
    p2 = next2;
  }

  return l_fun(p1, p2);
}

// bind_cons_list first version. uses number of functions
template<int len, typename Fun1, typename... FunRest>
inline
int bind_cons_list_i(Lisp_ptr p, Fun1&& f, FunRest&&... fr){
  auto c = p.get<Cons*>();
  if(!c) return len;

  auto next = c->cdr();
  f(c);

  return bind_cons_list_i<len + 1>(next, fr...);
}

template<int len>
inline
int bind_cons_list_i(Lisp_ptr p){
  return (nullp(p)) ? len : len+1;
}

template<typename... Fun>
inline
int bind_cons_list(Lisp_ptr p, Fun&&... f){
  return bind_cons_list_i<0>(p, f...);
}

// bind_cons_list second version. uses std::function.
template<unsigned i>
struct destruct_cons_list{
  template<typename Fun, typename... Args>
  typename Fun::result_type
  operator()(Fun f, Lisp_ptr p, Args... args) const{
    static constexpr destruct_cons_list<i - 1> expander;
    Cons* c = p.get<Cons*>();
    return expander(f, c->cdr(), args..., c);
  }
};

template<>
template<typename Fun, typename... Args>
typename Fun::result_type
destruct_cons_list<0>::operator()(Fun f, Lisp_ptr, Args... args) const{
  return f(args...);
}

template<typename Ret, typename... Args>
Ret bind_cons_list(Lisp_ptr p, std::function<Ret (Args...)> fun){
  static constexpr destruct_cons_list<sizeof...(Args)> expander;
  return expander(fun, p);
}

// make_cons_list
template<typename Iter>
Lisp_ptr make_cons_list(Iter b, Iter e){
  if(b == e){
    return Cons::NIL;
  }

  auto i = b;
  GrowList gw;

  while(1){
    gw.push(Lisp_ptr{*i});

    ++i;
    if(i == e) break;
  }

  return gw.extract();
}

inline
Lisp_ptr make_cons_list(std::initializer_list<Lisp_ptr> lis){
  return make_cons_list(begin(lis), end(lis));
}

inline
Lisp_ptr push_cons_list(Lisp_ptr p, Lisp_ptr q){
  return Lisp_ptr(new Cons(p, q));
}

// GrowList class
inline
void GrowList::invalidate(){
  assert(head && next);
  head = {};
  next = nullptr;
}

inline
GrowList::GrowList()
  : head(Cons::NIL), next(&head)
{}

inline
Lisp_ptr GrowList::extract(){
  return extract_with_tail(Cons::NIL);
}

inline
Lisp_ptr GrowList::extract_with_tail(Lisp_ptr p){
  *next = p;
  auto ret = head;
  invalidate();
  return ret;
}

// ConsIter class

inline
ConsIter::ConsIter() : c_(nullptr){}

inline
ConsIter::ConsIter(const Cons* c) : c_(c){}

inline
ConsIter ConsIter::operator++(int){
  auto ret = *this;
  ++(*this);
  return ret;
}

inline
bool operator==(const ConsIter& i1, const ConsIter& i2){
  return i1.c_ == i2.c_;
}

inline
bool operator!=(const ConsIter& i1, const ConsIter& i2){
  return i1.c_ != i2.c_;
}

// cons_list_to_array
template<int size>
std::array<Lisp_ptr, size> cons_list_to_array(Lisp_ptr p){
  std::array<Lisp_ptr, size> ret;
  int i = 0;

  for(auto it = begin(p), e_it = end(p); it != e_it; ++it){
    if(i >= size)
      throw make_zs_error("passed list is longer than expected size (%d)\n", size);

    ret[i] = *it;
    ++i;
  }

  if(i != size)
    throw make_zs_error("passed list is shorter than expected size (%d)\n", size);

  return ret;
}

#endif //CONS_UTIL_I_HH
<commit_msg>removed std::function dependency from destructuring<commit_after>#ifndef CONS_UTIL_I_HH
#define CONS_UTIL_I_HH

#ifndef CONS_UTIL_HH
#error "Please include via parent file"
#endif

#include <utility>
#include <algorithm>
#include "util.hh"

inline
bool nullp(Lisp_ptr p){
  // When Lisp_ptr::get<Cons*>() became constexpr,
  // '<void*>' should be replaced with '<Cons*>'
  static_assert(Cons::NIL.get<void*>() == nullptr,
                "NIL's pointer part is not nullptr!");
  return (p.tag() == Ptr_tag::cons)
    && (p.get<void*>() == nullptr);
}

template<typename MainFun, typename LastFun>
auto do_list(Lisp_ptr lis, MainFun&& m_fun, LastFun&& l_fun)
  -> decltype(l_fun(lis)){
  Lisp_ptr p = lis;

  while(auto c = p.get<Cons*>()){
    auto next = c->cdr();
    if(!m_fun(c))
      break;

    p = next;
  }

  return l_fun(p);
}

template<typename MainFun, typename LastFun>
auto do_list_2(Lisp_ptr lis1, Lisp_ptr lis2, MainFun&& m_fun, LastFun&& l_fun)
  -> decltype(l_fun(lis1, lis2)){
  Lisp_ptr p1 = lis1;
  Lisp_ptr p2 = lis2;

  Cons *c1, *c2;

  while((c1 = p1.get<Cons*>()) && (c2 = p2.get<Cons*>())){
    auto next1 = c1->cdr();
    auto next2 = c2->cdr();
    if(!m_fun(c1, c2))
      break;

    p1 = next1;
    p2 = next2;
  }

  return l_fun(p1, p2);
}

// bind_cons_list first version. uses number of functions
template<int len, typename Fun1, typename... FunRest>
inline
int bind_cons_list_i(Lisp_ptr p, Fun1&& f, FunRest&&... fr){
  auto c = p.get<Cons*>();
  if(!c) return len;

  auto next = c->cdr();
  f(c);

  return bind_cons_list_i<len + 1>(next, fr...);
}

template<int len>
inline
int bind_cons_list_i(Lisp_ptr p){
  return (nullp(p)) ? len : len+1;
}

template<typename... Fun>
inline
int bind_cons_list(Lisp_ptr p, Fun&&... f){
  return bind_cons_list_i<0>(p, f...);
}

// bind_cons_list second version. uses std::function.
template<unsigned i>
struct destruct_cons_list{
  static constexpr destruct_cons_list<i - 1> expander
  = destruct_cons_list<i - 1>();

  template<typename Fun, typename... Args>
  auto operator()(Lisp_ptr p, Fun f, Args... args) const
    -> decltype(expander(p, f, args..., nullptr))
  {
    Cons* c = p.get<Cons*>();
    return expander(c->cdr(), f, args..., c);
  }
};

template<>
struct destruct_cons_list<0>{
  template<typename Fun, typename... Args>
  auto operator()(Lisp_ptr, Fun f, Args... args) const
    -> decltype(f(args...))
  {
    return f(args...);
  }
};

template<typename Ret, typename... Args>
Ret bind_cons_list(Lisp_ptr p, std::function<Ret (Args...)> fun){
  static constexpr destruct_cons_list<sizeof...(Args)> expander;
  return expander(p, fun);
}

// make_cons_list
template<typename Iter>
Lisp_ptr make_cons_list(Iter b, Iter e){
  if(b == e){
    return Cons::NIL;
  }

  auto i = b;
  GrowList gw;

  while(1){
    gw.push(Lisp_ptr{*i});

    ++i;
    if(i == e) break;
  }

  return gw.extract();
}

inline
Lisp_ptr make_cons_list(std::initializer_list<Lisp_ptr> lis){
  return make_cons_list(begin(lis), end(lis));
}

inline
Lisp_ptr push_cons_list(Lisp_ptr p, Lisp_ptr q){
  return Lisp_ptr(new Cons(p, q));
}

// GrowList class
inline
void GrowList::invalidate(){
  assert(head && next);
  head = {};
  next = nullptr;
}

inline
GrowList::GrowList()
  : head(Cons::NIL), next(&head)
{}

inline
Lisp_ptr GrowList::extract(){
  return extract_with_tail(Cons::NIL);
}

inline
Lisp_ptr GrowList::extract_with_tail(Lisp_ptr p){
  *next = p;
  auto ret = head;
  invalidate();
  return ret;
}

// ConsIter class

inline
ConsIter::ConsIter() : c_(nullptr){}

inline
ConsIter::ConsIter(const Cons* c) : c_(c){}

inline
ConsIter ConsIter::operator++(int){
  auto ret = *this;
  ++(*this);
  return ret;
}

inline
bool operator==(const ConsIter& i1, const ConsIter& i2){
  return i1.c_ == i2.c_;
}

inline
bool operator!=(const ConsIter& i1, const ConsIter& i2){
  return i1.c_ != i2.c_;
}

// cons_list_to_array
template<int size>
std::array<Lisp_ptr, size> cons_list_to_array(Lisp_ptr p){
  std::array<Lisp_ptr, size> ret;
  int i = 0;

  for(auto it = begin(p), e_it = end(p); it != e_it; ++it){
    if(i >= size)
      throw make_zs_error("passed list is longer than expected size (%d)\n", size);

    ret[i] = *it;
    ++i;
  }

  if(i != size)
    throw make_zs_error("passed list is shorter than expected size (%d)\n", size);

  return ret;
}

#endif //CONS_UTIL_I_HH
<|endoftext|>


<commit_before>#include "stats.h"

Stats::Stats() {
	this->mallocCount = 0;
	this->freeCount = 0;
	this->invalidReadCount = 0;
	this->invalidWriteCount = 0;
	this->fenceHitCount = 0;
	this->invalidFreeCount = 0;
	this->midFreeChunkCount = 0;
	this->freeNullCount = 0;
	this->invalidReturnCount = 0;
	this->fenceOverflowHitCount = 0;
	this->fenceUnderflowHitCount = 0;
}

void Stats::reset() {
	this->mallocCount = 0;
	this->freeCount = 0;
	this->invalidReadCount = 0;
	this->invalidWriteCount = 0;
	this->fenceHitCount = 0;
	this->invalidFreeCount = 0;
	this->midFreeChunkCount = 0;
	this->freeNullCount = 0;
	this->invalidReturnCount = 0;
	this->fenceOverflowHitCount = 0;
	this->fenceUnderflowHitCount = 0;
}

unsigned int Stats::getMallocCount() {
	return this->mallocCount;
}
unsigned int Stats::getFreeCount() {
	return this->freeCount;
}
unsigned int Stats::getInvalidReadCount() {
	return this->invalidReadCount;
}

unsigned int Stats::getInvalidWriteCount() {
	return this->invalidWriteCount;
}

unsigned int Stats::getFenceHitCount() {
	return this->fenceHitCount;
}

unsigned int Stats::getInvalidFreeCount() {
	return this->invalidFreeCount;
}

unsigned int Stats::getMidFreeChunkCount() {
	return this->midFreeChunkCount;
}

unsigned int Stats::getFreeNullCount() {
	return this->freeNullCount;
}

unsigned int Stats::getInvalidReturnCount() {
	return this->invalidReturnCount;
}

unsigned int Stats::getFenceOverflowHitCount() {
	return this->fenceOverflowHitCount;
}

unsigned int Stats::getFenceUnderflowHitCount() {
	return this->fenceUnderflowHitCount;
}

void Stats::setMallocCount(unsigned int count) {
	this->mallocCount = count;
}

void Stats::setFreeCount(unsigned int count) {
	this->freeCount = count;
}

void Stats::setInvalidReadCount(unsigned int count) {
	this->invalidReadCount = count;
}

void Stats::setInvalidWriteCount(unsigned int count) {
	this->invalidWriteCount = count;
}

void Stats::setFenceHitCount(unsigned int count) {
	this->fenceHitCount = count;
}

void Stats::setInvalidFreeCount(unsigned int count) {
	this->invalidFreeCount = count;
}

void Stats::setMidFreeChunkCount(unsigned int count) {
	this->midFreeChunkCount = count;
}

void Stats::setFreeNullCount(unsigned int count) {
	this->freeNullCount = count;
}

void Stats::setInvalidReturnCount(unsigned int count) {
	this->invalidReturnCount = count;
}

void Stats::setFenceOverflowHitCount(unsigned int count) {
	this->fenceOverflowHitCount = count;
}

void Stats::setFenceUnderflowHitCount(unsigned int count) {
	this->fenceUnderflowHitCount = count;
}

void Stats::incMallocCount() {
	this->mallocCount++;
}

void Stats::incFreeCount() {
	this->freeCount++;
}

void Stats::incInvalidReadCount() {
	this->invalidReadCount++;
}

void Stats::incInvalidWriteCount() {
	this->invalidWriteCount++;
}

void Stats::incFenceHitCount() {
	this->fenceHitCount++;
}

void Stats::incInvalidFreeCount() {
	this->invalidFreeCount++;
}

void Stats::incMidFreeChunkCount() {
	this->midFreeChunkCount++;
}

void Stats::incFreeNullCount() {
	this->freeNullCount++;
}

void Stats::incInvalidReturnCount() {
	this->invalidReturnCount++;
}

void Stats::incFenceOverflowHitCount() {
	this->fenceOverflowHitCount++;
}

void Stats::incFenceUnderflowHitCount() {
	this->fenceUnderflowHitCount++;
}

void Stats::displayResults(MemList memlist, FILE *fp) {
	if(fp == NULL) {
		fp = stdin;
	}
	// Print out the initial object
	fprintf(fp, "=== RESULTS ===\n");
	fprintf(fp, "%-20s %d\n", "allocations: ", this->mallocCount);
	fprintf(fp, "%-20s %d\n", "deallocations: ", this->freeCount);
	fprintf(fp, "%-20s %d\n", "invalid reads: ", this->invalidReadCount);
	fprintf(fp, "%-20s %d\n", "invalid writes: ", this->invalidWriteCount);
	fprintf(fp, "%-20s %d\n", "invalid frees: ", this->invalidFreeCount);
	fprintf(fp, "%-20s %d\n", "null free: ", this->freeNullCount);
	fprintf(fp, "%-20s %d\n", "midchunk free: ", this->midFreeChunkCount);
	fprintf(fp, "%-20s %d\n", "fence hit: ", this->fenceHitCount);
	fprintf(fp, "%-20s %d\n", "    underflow: ", this->fenceUnderflowHitCount);
	fprintf(fp, "%-20s %d\n", "    overflow: ", this->fenceOverflowHitCount);
	fprintf(fp, "%-20s %d\n", "invalid returns: ", this->invalidReturnCount);
	// Print out the contents of memlist if it was provided.
	fprintf(fp, "=== MEMLIST CONTENTS ===\n");
	char buffer[2048];
	int lostMemory = 0;
	for(int i = 0; i < memlist.size(); i++) {
		MemoryAlloc alloc = memlist.get(i);
		fprintf(fp, "%s\n", alloc.toString(buffer, 2048));
		lostMemory += alloc.getTotalSize();
	}
	fprintf(fp, "The memlist contains %d items.\nTotal loss of %d bytes\n", memlist.size(), lostMemory);
}
<commit_msg>Changed the format of the stats table that is printed out in the log.<commit_after>#include "stats.h"

Stats::Stats() {
	this->mallocCount = 0;
	this->freeCount = 0;
	this->invalidReadCount = 0;
	this->invalidWriteCount = 0;
	this->fenceHitCount = 0;
	this->invalidFreeCount = 0;
	this->midFreeChunkCount = 0;
	this->freeNullCount = 0;
	this->invalidReturnCount = 0;
	this->fenceOverflowHitCount = 0;
	this->fenceUnderflowHitCount = 0;
}

void Stats::reset() {
	this->mallocCount = 0;
	this->freeCount = 0;
	this->invalidReadCount = 0;
	this->invalidWriteCount = 0;
	this->fenceHitCount = 0;
	this->invalidFreeCount = 0;
	this->midFreeChunkCount = 0;
	this->freeNullCount = 0;
	this->invalidReturnCount = 0;
	this->fenceOverflowHitCount = 0;
	this->fenceUnderflowHitCount = 0;
}

unsigned int Stats::getMallocCount() {
	return this->mallocCount;
}
unsigned int Stats::getFreeCount() {
	return this->freeCount;
}
unsigned int Stats::getInvalidReadCount() {
	return this->invalidReadCount;
}

unsigned int Stats::getInvalidWriteCount() {
	return this->invalidWriteCount;
}

unsigned int Stats::getFenceHitCount() {
	return this->fenceHitCount;
}

unsigned int Stats::getInvalidFreeCount() {
	return this->invalidFreeCount;
}

unsigned int Stats::getMidFreeChunkCount() {
	return this->midFreeChunkCount;
}

unsigned int Stats::getFreeNullCount() {
	return this->freeNullCount;
}

unsigned int Stats::getInvalidReturnCount() {
	return this->invalidReturnCount;
}

unsigned int Stats::getFenceOverflowHitCount() {
	return this->fenceOverflowHitCount;
}

unsigned int Stats::getFenceUnderflowHitCount() {
	return this->fenceUnderflowHitCount;
}

void Stats::setMallocCount(unsigned int count) {
	this->mallocCount = count;
}

void Stats::setFreeCount(unsigned int count) {
	this->freeCount = count;
}

void Stats::setInvalidReadCount(unsigned int count) {
	this->invalidReadCount = count;
}

void Stats::setInvalidWriteCount(unsigned int count) {
	this->invalidWriteCount = count;
}

void Stats::setFenceHitCount(unsigned int count) {
	this->fenceHitCount = count;
}

void Stats::setInvalidFreeCount(unsigned int count) {
	this->invalidFreeCount = count;
}

void Stats::setMidFreeChunkCount(unsigned int count) {
	this->midFreeChunkCount = count;
}

void Stats::setFreeNullCount(unsigned int count) {
	this->freeNullCount = count;
}

void Stats::setInvalidReturnCount(unsigned int count) {
	this->invalidReturnCount = count;
}

void Stats::setFenceOverflowHitCount(unsigned int count) {
	this->fenceOverflowHitCount = count;
}

void Stats::setFenceUnderflowHitCount(unsigned int count) {
	this->fenceUnderflowHitCount = count;
}

void Stats::incMallocCount() {
	this->mallocCount++;
}

void Stats::incFreeCount() {
	this->freeCount++;
}

void Stats::incInvalidReadCount() {
	this->invalidReadCount++;
}

void Stats::incInvalidWriteCount() {
	this->invalidWriteCount++;
}

void Stats::incFenceHitCount() {
	this->fenceHitCount++;
}

void Stats::incInvalidFreeCount() {
	this->invalidFreeCount++;
}

void Stats::incMidFreeChunkCount() {
	this->midFreeChunkCount++;
}

void Stats::incFreeNullCount() {
	this->freeNullCount++;
}

void Stats::incInvalidReturnCount() {
	this->invalidReturnCount++;
}

void Stats::incFenceOverflowHitCount() {
	this->fenceOverflowHitCount++;
}

void Stats::incFenceUnderflowHitCount() {
	this->fenceUnderflowHitCount++;
}

void Stats::displayResults(MemList memlist, FILE *fp) {
	if(fp == NULL) {
		fp = stdin;
	}
	// Print out the initial object
	fprintf(fp, "==================================\n");
	fprintf(fp, "%-20s %s\n", "ACTIONS ", "COUNT");
	fprintf(fp, "----------------------------------\n");
	fprintf(fp, "%-20s %d\n", "allocations: ", this->mallocCount);
	fprintf(fp, "%-20s %d\n", "deallocations: ", this->freeCount);
	fprintf(fp, "----------------------------------\n");
	fprintf(fp, "%-20s %d\n", "invalid reads: ", this->invalidReadCount);
	fprintf(fp, "%-20s %d\n", "invalid writes: ", this->invalidWriteCount);
	fprintf(fp, "----------------------------------\n");
	fprintf(fp, "%-20s %d\n", "invalid frees: ", this->invalidFreeCount);
	fprintf(fp, "%-20s %d\n", "    null free: ", this->freeNullCount);
	fprintf(fp, "%-20s %d\n", "    midchunk free: ", this->midFreeChunkCount);
	fprintf(fp, "----------------------------------\n");
	fprintf(fp, "%-20s %d\n", "fence hit: ", this->fenceHitCount);
	fprintf(fp, "%-20s %d\n", "    underflow: ", this->fenceUnderflowHitCount);
	fprintf(fp, "%-20s %d\n", "    overflow: ", this->fenceOverflowHitCount);
	fprintf(fp, "----------------------------------\n");
	fprintf(fp, "%-20s %d\n", "invalid returns: ", this->invalidReturnCount);
	fprintf(fp, "==================================\n");
	// Print out the contents of memlist if it was provided.
	fprintf(fp, "%-20s\n", "MEMLIST CONTENTS");
	char buffer[2048];
	int lostMemory = 0;
	for(int i = 0; i < memlist.size(); i++) {
		MemoryAlloc alloc = memlist.get(i);
		fprintf(fp, "%s\n", alloc.toString(buffer, 2048));
		lostMemory += alloc.getTotalSize();
	}
	fprintf(fp, "----------------------------------\n");
	fprintf(fp, "%-20s %d\n", "list size: ", memlist.size());
	fprintf(fp, "%-20s %d %s\n", "total loss: ", lostMemory, "bytes");
	fprintf(fp, "==================================\n");
}
<|endoftext|>