<commit_before>/*
 *   C++ UDP socket server for live image upstreaming
 *   Modified from http://cs.ecs.baylor.edu/~donahoo/practical/CSockets/practical/UDPEchoServer.cpp
 *   Copyright (C) 2015
 *
 *   This program 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 2 of the License, or
 *   (at your option) any later version.
 *
 *   This program 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 this program; if not, write to the Free Software
 *   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#include "PracticalSocket.h" // For UDPSocket and SocketException
#include <iostream>          // For cout and cerr
#include <cstdlib>           // For atoi()

/* Added by CY */
#include <sys/poll.h>        // http://beej.us/guide/bgnet/output/html/multipage/pollman.html
#include <ncurses.h>         // sudo apt-get install libncurses-dev. For getch() getting pressed key.

#define BUF_LEN 65540 // Larger than maximum UDP packet size
#define TOTAL_PACK  30  // This value was dynamically configured in run-time with a range around 20 to 21 on my Surface. Set to 30 here to keep some room.

#include "opencv2/opencv.hpp"
using namespace cv;
#include "config.h"

timespec ts_diff(timespec start, timespec end);

int main(int argc, char * argv[]) {

    if (argc != 5) { // Test for correct number of parameters
        cerr << "Usage: " << argv[0] << " <IF#1 IP> <IF#2 IP> <Server IP> <Server Port> " << endl;
        exit(1);
    }

    string if1Address = argv[1];
    string if2Address = argv[2];
    string serverAddress = argv[3];
    unsigned short servPort1 = atoi(argv[4]); // First arg:  local port
    unsigned short servPort2 = servPort1;

    namedWindow("recv", CV_WINDOW_AUTOSIZE);
    try {
        UDPSocket sock1(if1Address, servPort1);
        UDPSocket sock2(if2Address, servPort2);

        //char buffer[BUF_LEN]; // Buffer for echo string

        int recvMsgSize; // Size of received message
        string sourceAddress; // Address of datagram source
        unsigned short sourcePort; // Port of datagram source

        struct timespec ts_last_if1;
        struct timespec ts_last_if2;
        clock_gettime(CLOCK_MONOTONIC, &ts_last_if1);
        clock_gettime(CLOCK_MONOTONIC, &ts_last_if2);

        struct timespec ts_last_HB;
        clock_gettime(CLOCK_MONOTONIC, &ts_last_HB);

        int timestamp_counter_if1 = 0;
        int timestamp_counter_if2 = 0;

        /* Variables for the primary interface */
        int sock1State = 0;
        int sock1TotoalPack = 0;
        int sock1PackCount = 0;
        char buffer1[BUF_LEN]; // Buffer for echo string
        char * longbuf1 = new char[PACK_SIZE * TOTAL_PACK];

        /* Variables for the secondary interface */
        int sock2State = 0;
        int sock2TotoalPack = 0;
        int sock2PackCount = 0;
        char buffer2[BUF_LEN]; // Buffer for echo string
        char * longbuf2 = new char[PACK_SIZE * TOTAL_PACK];

        /* Initialize getch() */
        initscr();
        clear();
        noecho();
        cbreak();
        nodelay(stdscr, TRUE);  // to make getch() non-blocking.
        int key = 0;
        int enable_if1_HB = 1;
        while (1) {

            key = getch();
            if (key != ERR) {
              if (enable_if1_HB == 1) {
                enable_if1_HB = 0;
              } else {
                enable_if1_HB = 1;
              }
              cout << "IF1 HB toggled.\r" << endl;
            }

            int rv;
            struct pollfd ufds[2];
            ufds[0].fd = sock1.getDescriptor();
            ufds[0].events = POLLIN; // check for just normal data

            ufds[1].fd = sock2.getDescriptor();
            ufds[1].events = POLLIN; // check for just normal data

            rv = poll(ufds, 2, 100);    // The third parameter is timeout in minisecond.

            if (rv == -1) {
                perror("poll"); // error occurred in poll()
            } else if (rv == 0) {
                //printf("Timeout occurred!  No data after 100 miniseconds.\n");
                continue;
            }

            /* sock1State and sock2State:
             * 0: Waiting for the initial packet (indicating the length of the frame packets) of a frame.
             * 1: The initial packet has been received, in the process of receiving frame packets.
             * 2: A complete frame has been received. This state is used for displaying the frame.
             */
            // check for events on s1:
            if (ufds[0].revents & POLLIN) {
              if (sock1State == 0) {
                recvMsgSize = sock1.recvFrom(buffer1, BUF_LEN, sourceAddress, sourcePort);
                if (recvMsgSize > sizeof(int)) {
                  ;
                } else {
                  sock1TotoalPack = ((int * ) buffer1)[0];
                  sock1State = 1;
                  sock1PackCount = 0;
                }

              } else if (sock1State == 1) {
                recvMsgSize = sock1.recvFrom(buffer1, BUF_LEN, sourceAddress, sourcePort);
                if (recvMsgSize != PACK_SIZE) {
                  cerr << "Received unexpected size pack:" << recvMsgSize << endl;
                  //continue;
                  sock1State = 0; // Something wrong happens, so reset the state.
                } else {
                  memcpy( & longbuf1[sock1PackCount * PACK_SIZE], buffer1, PACK_SIZE);
                  sock1PackCount++;
                  if (sock1PackCount == sock1TotoalPack) {
                    // One frame data is complete.
                    sock1State = 2;
                  }
                }
              }
            }

            // check for events on s2:
            if (ufds[1].revents & POLLIN) {
              if (sock2State == 0) {
                recvMsgSize = sock2.recvFrom(buffer2, BUF_LEN, sourceAddress, sourcePort);
                if (recvMsgSize > sizeof(int)) {
                  ;
                } else {
                  sock2TotoalPack = ((int * ) buffer2)[0];
                  sock2State = 1;
                  sock2PackCount = 0;
                }

              } else if (sock2State == 1) {
                recvMsgSize = sock2.recvFrom(buffer2, BUF_LEN, sourceAddress, sourcePort);
                if (recvMsgSize != PACK_SIZE) {
                  cerr << "Received unexpected size pack:" << recvMsgSize << endl;
                  //continue;
                  sock2State = 0; // Something wrong happens, so reset the state.
                } else {
                  memcpy( & longbuf2[sock2PackCount * PACK_SIZE], buffer2, PACK_SIZE);
                  sock2PackCount++;
                  if (sock2PackCount == sock2TotoalPack) {
                    // One frame data is complete.
                    sock2State = 2;
                  }
                }
              }
            }

            /* If any of the interface has received a complete frame, go and display it. */
            if ( (sock1State==2) || (sock2State==2) ) {

              struct timespec ts_next;
              clock_gettime(CLOCK_MONOTONIC, &ts_next);
              struct timespec ts_duration;
              double duration;

              int total_pack;
              Mat rawData;
              if (sock1State == 2) {
                /* sock1 has a frame. */
                rawData = Mat(1, PACK_SIZE * sock1TotoalPack, CV_8UC1, longbuf1);
                sock1State = 0;
                total_pack = sock1TotoalPack;
                if (sock2State == 2) {
                  /* Drop sock2's frame since we are using the frame from sock1. */
                  sock2State = 0;
                }
                //cout << "Fram displayed from Main interface.\r" << endl;
                ts_duration = ts_diff(ts_last_if1, ts_next);
                duration = (ts_duration.tv_nsec)/1000000000.0;

                ts_last_if1.tv_nsec = ts_next.tv_nsec;
                ts_last_if1.tv_sec = ts_next.tv_sec;
                cout << ts_next.tv_sec + (ts_next.tv_nsec/1000000000.0) << "," << "IF1," <<  (1 / duration) << "," << (PACK_SIZE * total_pack / duration / 1024 * 8) << "\r" << endl;
                cout << ts_next.tv_sec + (ts_next.tv_nsec/1000000000.0) << "," << "all," <<  (1 / duration) << "," << (PACK_SIZE * total_pack / duration / 1024 * 8) << "\r" << endl;

              } else if (sock2State == 2) {
                /* sock2 has a frame while sock1 doesn't have one. */
                rawData = Mat(1, PACK_SIZE * sock2TotoalPack, CV_8UC1, longbuf2);
                sock2State = 0;
                total_pack = sock2TotoalPack;
                //cout << "Fram displayed from Second interface.\r" << endl;
                ts_duration = ts_diff(ts_last_if2, ts_next);
                duration = (ts_duration.tv_nsec)/1000000000.0;

                ts_last_if2.tv_nsec = ts_next.tv_nsec;
                ts_last_if2.tv_sec = ts_next.tv_sec;
                cout << ts_next.tv_sec + (ts_next.tv_nsec/1000000000.0) << "," << "IF2," <<  (1 / duration) << "," << (PACK_SIZE * total_pack / duration / 1024 * 8) << "\r" << endl;
                cout << ts_next.tv_sec + (ts_next.tv_nsec/1000000000.0) << "," << "all," <<  (1 / duration) << "," << (PACK_SIZE * total_pack / duration / 1024 * 8) << "\r" << endl;
              }

              //Mat rawData = Mat(1, PACK_SIZE * total_pack, CV_8UC1, longbuf);
              Mat frame = imdecode(rawData, CV_LOAD_IMAGE_COLOR);
              if (frame.size().width == 0) {
                  cerr << "decode failure!" << endl;
                  continue;
              }
              imshow("recv", frame);
              //free(longbuf);

              waitKey(1);
              //cout << (ts_next.tv_nsec/1000000)+(timestamp_counter*1000) << "," << "all," <<  (1 / duration) << "," << (PACK_SIZE * total_pack / duration / 1024 * 8) << "\r" << endl;

            }

            struct timespec ts_this_HB;
            clock_gettime(CLOCK_MONOTONIC, &ts_this_HB);
            struct timespec ts_HB_diff = ts_diff(ts_last_HB, ts_this_HB);
            //if ( (ts_HB_diff.tv_sec>=(HEARTBEAT_PERIOD_MS/1000))
            //  && ((ts_HB_diff.tv_nsec/1000000)>=HEARTBEAT_PERIOD_MS)) {
            if ( (ts_HB_diff.tv_sec+(ts_HB_diff.tv_nsec/1000000000.0))>= (HEARTBEAT_PERIOD_MS/1000.0)) {
                char sendBuf1[1];
                sendBuf1[0] = 1;
                if (enable_if1_HB == 1) {
                  sock1.sendTo(sendBuf1, 1, serverAddress, 10005);
                }

                char sendBuf2[1];
                sendBuf2[0] = 2;
                sock2.sendTo(sendBuf2, 1, serverAddress, 10005);

                ts_last_HB.tv_nsec = ts_this_HB.tv_nsec;
                ts_last_HB.tv_sec = ts_this_HB.tv_sec;

                //cout << "@@@ Heartbeat sent out. \r" << endl;
            }

        }
    } catch (SocketException & e) {
        cerr << e.what() << endl;
        exit(1);
    }

    return 0;
}

timespec ts_diff(timespec start, timespec end)
{
	timespec temp;
	if ((end.tv_nsec-start.tv_nsec)<0) {
		temp.tv_sec = end.tv_sec-start.tv_sec-1;
		temp.tv_nsec = 1000000000+end.tv_nsec-start.tv_nsec;
	} else {
		temp.tv_sec = end.tv_sec-start.tv_sec;
		temp.tv_nsec = end.tv_nsec-start.tv_nsec;
	}
	return temp;
}<commit_msg>Make the polling timeout shorter.<commit_after>/*
 *   C++ UDP socket server for live image upstreaming
 *   Modified from http://cs.ecs.baylor.edu/~donahoo/practical/CSockets/practical/UDPEchoServer.cpp
 *   Copyright (C) 2015
 *
 *   This program 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 2 of the License, or
 *   (at your option) any later version.
 *
 *   This program 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 this program; if not, write to the Free Software
 *   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#include "PracticalSocket.h" // For UDPSocket and SocketException
#include <iostream>          // For cout and cerr
#include <cstdlib>           // For atoi()

/* Added by CY */
#include <sys/poll.h>        // http://beej.us/guide/bgnet/output/html/multipage/pollman.html
#include <ncurses.h>         // sudo apt-get install libncurses-dev. For getch() getting pressed key.

#define BUF_LEN 65540 // Larger than maximum UDP packet size
#define TOTAL_PACK  30  // This value was dynamically configured in run-time with a range around 20 to 21 on my Surface. Set to 30 here to keep some room.

#include "opencv2/opencv.hpp"
using namespace cv;
#include "config.h"

timespec ts_diff(timespec start, timespec end);

int main(int argc, char * argv[]) {

    if (argc != 5) { // Test for correct number of parameters
        cerr << "Usage: " << argv[0] << " <IF#1 IP> <IF#2 IP> <Server IP> <Server Port> " << endl;
        exit(1);
    }

    string if1Address = argv[1];
    string if2Address = argv[2];
    string serverAddress = argv[3];
    unsigned short servPort1 = atoi(argv[4]); // First arg:  local port
    unsigned short servPort2 = servPort1;

    namedWindow("recv", CV_WINDOW_AUTOSIZE);
    try {
        UDPSocket sock1(if1Address, servPort1);
        UDPSocket sock2(if2Address, servPort2);

        //char buffer[BUF_LEN]; // Buffer for echo string

        int recvMsgSize; // Size of received message
        string sourceAddress; // Address of datagram source
        unsigned short sourcePort; // Port of datagram source

        struct timespec ts_last_if1;
        struct timespec ts_last_if2;
        clock_gettime(CLOCK_MONOTONIC, &ts_last_if1);
        clock_gettime(CLOCK_MONOTONIC, &ts_last_if2);

        struct timespec ts_last_HB;
        clock_gettime(CLOCK_MONOTONIC, &ts_last_HB);

        int timestamp_counter_if1 = 0;
        int timestamp_counter_if2 = 0;

        /* Variables for the primary interface */
        int sock1State = 0;
        int sock1TotoalPack = 0;
        int sock1PackCount = 0;
        char buffer1[BUF_LEN]; // Buffer for echo string
        char * longbuf1 = new char[PACK_SIZE * TOTAL_PACK];

        /* Variables for the secondary interface */
        int sock2State = 0;
        int sock2TotoalPack = 0;
        int sock2PackCount = 0;
        char buffer2[BUF_LEN]; // Buffer for echo string
        char * longbuf2 = new char[PACK_SIZE * TOTAL_PACK];

        /* Initialize getch() */
        initscr();
        clear();
        noecho();
        cbreak();
        nodelay(stdscr, TRUE);  // to make getch() non-blocking.
        int key = 0;
        int enable_if1_HB = 1;
        while (1) {

            key = getch();
            if (key != ERR) {
              if (enable_if1_HB == 1) {
                enable_if1_HB = 0;
              } else {
                enable_if1_HB = 1;
              }
              cout << "IF1 HB toggled.\r" << endl;
            }

            int rv;
            struct pollfd ufds[2];
            ufds[0].fd = sock1.getDescriptor();
            ufds[0].events = POLLIN; // check for just normal data

            ufds[1].fd = sock2.getDescriptor();
            ufds[1].events = POLLIN; // check for just normal data

            rv = poll(ufds, 2, 1);    // The third parameter is timeout in minisecond.

            if (rv == -1) {
                perror("poll"); // error occurred in poll()
            } else if (rv == 0) {
                //printf("Timeout occurred!  No data after 100 miniseconds.\n");
                continue;
            }

            /* sock1State and sock2State:
             * 0: Waiting for the initial packet (indicating the length of the frame packets) of a frame.
             * 1: The initial packet has been received, in the process of receiving frame packets.
             * 2: A complete frame has been received. This state is used for displaying the frame.
             */
            // check for events on s1:
            if (ufds[0].revents & POLLIN) {
              if (sock1State == 0) {
                recvMsgSize = sock1.recvFrom(buffer1, BUF_LEN, sourceAddress, sourcePort);
                if (recvMsgSize > sizeof(int)) {
                  ;
                } else {
                  sock1TotoalPack = ((int * ) buffer1)[0];
                  sock1State = 1;
                  sock1PackCount = 0;
                }

              } else if (sock1State == 1) {
                recvMsgSize = sock1.recvFrom(buffer1, BUF_LEN, sourceAddress, sourcePort);
                if (recvMsgSize != PACK_SIZE) {
                  cerr << "Received unexpected size pack:" << recvMsgSize << endl;
                  //continue;
                  sock1State = 0; // Something wrong happens, so reset the state.
                } else {
                  memcpy( & longbuf1[sock1PackCount * PACK_SIZE], buffer1, PACK_SIZE);
                  sock1PackCount++;
                  if (sock1PackCount == sock1TotoalPack) {
                    // One frame data is complete.
                    sock1State = 2;
                  }
                }
              }
            }

            // check for events on s2:
            if (ufds[1].revents & POLLIN) {
              if (sock2State == 0) {
                recvMsgSize = sock2.recvFrom(buffer2, BUF_LEN, sourceAddress, sourcePort);
                if (recvMsgSize > sizeof(int)) {
                  ;
                } else {
                  sock2TotoalPack = ((int * ) buffer2)[0];
                  sock2State = 1;
                  sock2PackCount = 0;
                }

              } else if (sock2State == 1) {
                recvMsgSize = sock2.recvFrom(buffer2, BUF_LEN, sourceAddress, sourcePort);
                if (recvMsgSize != PACK_SIZE) {
                  cerr << "Received unexpected size pack:" << recvMsgSize << endl;
                  //continue;
                  sock2State = 0; // Something wrong happens, so reset the state.
                } else {
                  memcpy( & longbuf2[sock2PackCount * PACK_SIZE], buffer2, PACK_SIZE);
                  sock2PackCount++;
                  if (sock2PackCount == sock2TotoalPack) {
                    // One frame data is complete.
                    sock2State = 2;
                  }
                }
              }
            }

            /* If any of the interface has received a complete frame, go and display it. */
            if ( (sock1State==2) || (sock2State==2) ) {

              struct timespec ts_next;
              clock_gettime(CLOCK_MONOTONIC, &ts_next);
              struct timespec ts_duration;
              double duration;

              int total_pack;
              Mat rawData;
              if (sock1State == 2) {
                /* sock1 has a frame. */
                rawData = Mat(1, PACK_SIZE * sock1TotoalPack, CV_8UC1, longbuf1);
                sock1State = 0;
                total_pack = sock1TotoalPack;
                if (sock2State == 2) {
                  /* Drop sock2's frame since we are using the frame from sock1. */
                  sock2State = 0;
                }
                //cout << "Fram displayed from Main interface.\r" << endl;
                ts_duration = ts_diff(ts_last_if1, ts_next);
                duration = (ts_duration.tv_nsec)/1000000000.0;

                ts_last_if1.tv_nsec = ts_next.tv_nsec;
                ts_last_if1.tv_sec = ts_next.tv_sec;
                cout << ts_next.tv_sec + (ts_next.tv_nsec/1000000000.0) << "," << "IF1," <<  (1 / duration) << "," << (PACK_SIZE * total_pack / duration / 1024 * 8) << "\r" << endl;
                cout << ts_next.tv_sec + (ts_next.tv_nsec/1000000000.0) << "," << "all," <<  (1 / duration) << "," << (PACK_SIZE * total_pack / duration / 1024 * 8) << "\r" << endl;

              } else if (sock2State == 2) {
                /* sock2 has a frame while sock1 doesn't have one. */
                rawData = Mat(1, PACK_SIZE * sock2TotoalPack, CV_8UC1, longbuf2);
                sock2State = 0;
                total_pack = sock2TotoalPack;
                //cout << "Fram displayed from Second interface.\r" << endl;
                ts_duration = ts_diff(ts_last_if2, ts_next);
                duration = (ts_duration.tv_nsec)/1000000000.0;

                ts_last_if2.tv_nsec = ts_next.tv_nsec;
                ts_last_if2.tv_sec = ts_next.tv_sec;
                cout << ts_next.tv_sec + (ts_next.tv_nsec/1000000000.0) << "," << "IF2," <<  (1 / duration) << "," << (PACK_SIZE * total_pack / duration / 1024 * 8) << "\r" << endl;
                cout << ts_next.tv_sec + (ts_next.tv_nsec/1000000000.0) << "," << "all," <<  (1 / duration) << "," << (PACK_SIZE * total_pack / duration / 1024 * 8) << "\r" << endl;
              }

              //Mat rawData = Mat(1, PACK_SIZE * total_pack, CV_8UC1, longbuf);
              Mat frame = imdecode(rawData, CV_LOAD_IMAGE_COLOR);
              if (frame.size().width == 0) {
                  cerr << "decode failure!" << endl;
                  continue;
              }
              imshow("recv", frame);
              //free(longbuf);

              waitKey(1);
              //cout << (ts_next.tv_nsec/1000000)+(timestamp_counter*1000) << "," << "all," <<  (1 / duration) << "," << (PACK_SIZE * total_pack / duration / 1024 * 8) << "\r" << endl;

            }

            struct timespec ts_this_HB;
            clock_gettime(CLOCK_MONOTONIC, &ts_this_HB);
            struct timespec ts_HB_diff = ts_diff(ts_last_HB, ts_this_HB);
            //if ( (ts_HB_diff.tv_sec>=(HEARTBEAT_PERIOD_MS/1000))
            //  && ((ts_HB_diff.tv_nsec/1000000)>=HEARTBEAT_PERIOD_MS)) {
            if ( (ts_HB_diff.tv_sec+(ts_HB_diff.tv_nsec/1000000000.0))>= (HEARTBEAT_PERIOD_MS/1000.0)) {
                char sendBuf1[1];
                sendBuf1[0] = 1;
                if (enable_if1_HB == 1) {
                  sock1.sendTo(sendBuf1, 1, serverAddress, 10005);
                }

                char sendBuf2[1];
                sendBuf2[0] = 2;
                sock2.sendTo(sendBuf2, 1, serverAddress, 10005);

                ts_last_HB.tv_nsec = ts_this_HB.tv_nsec;
                ts_last_HB.tv_sec = ts_this_HB.tv_sec;

                //cout << "@@@ Heartbeat sent out. \r" << endl;
            }

        }
    } catch (SocketException & e) {
        cerr << e.what() << endl;
        exit(1);
    }

    return 0;
}

timespec ts_diff(timespec start, timespec end)
{
	timespec temp;
	if ((end.tv_nsec-start.tv_nsec)<0) {
		temp.tv_sec = end.tv_sec-start.tv_sec-1;
		temp.tv_nsec = 1000000000+end.tv_nsec-start.tv_nsec;
	} else {
		temp.tv_sec = end.tv_sec-start.tv_sec;
		temp.tv_nsec = end.tv_nsec-start.tv_nsec;
	}
	return temp;
}<|endoftext|>


<commit_before>/**
   This class is responsible to store global states for each requester ip. In particular
   it is supposed:
   - Memory allocation.
   - Take care of locking and unlocking.
   - Keeping track of memory limit and garbage collection.

   AUTHORS:
   - Vmon: Oct 2013: Initial version
*/

#include "ip_database.h"
#include "banjax.h"
#include "defer.h"

using namespace std;

/**
  check if  the ip is in the db, if not store it and updates its states
  related to that filter

  returns true on success and false on failure (locking not possible)
  eventually the continuation need to be retried
*/
bool
IPDatabase::set_ip_state(const std::string& ip, FilterIDType filter_id, FilterState state)
{
  //we need to lock at the begining because somebody can
  //delete an ip while we are searching
  if (TSMutexLockTry(db_mutex) != TS_SUCCESS) {
    TSDebug(BANJAX_PLUGIN_NAME, "Unable to get lock on the ip db");
    return false;
  }

  IPHashTable::iterator cur_ip_it = _ip_db.find(ip);
  if (cur_ip_it == _ip_db.end()) {
    _ip_db[ip] = IPState();
  }

  _ip_db[ip].state_array[filter_to_column[filter_id]] = state;

  TSMutexUnlock(db_mutex);

  return true;
}

/**
   Clean the ip state record mostly when it is reported to
   swabber.
*/
bool
IPDatabase::drop_ip(std::string& ip)
{
  if (TSMutexLockTry(db_mutex) != TS_SUCCESS) {
    TSDebug(BANJAX_PLUGIN_NAME, "Unable to get lock on the ip db");
    return false;
  }

  auto on_exit = defer([&] { TSMutexUnlock(db_mutex); });

  return _ip_db.erase(ip) != 0;
}

boost::optional<FilterState>
IPDatabase::get_ip_state(const std::string& ip, FilterIDType filter_id)
{
  if (TSMutexLockTry(db_mutex) != TS_SUCCESS) {
    TSDebug(BANJAX_PLUGIN_NAME, "Unable to get lock on the ip db");
    return boost::none;
  }

  auto on_exit = defer([&] { TSMutexUnlock(db_mutex); });

  IPHashTable::iterator cur_ip_it = _ip_db.find(ip);

  return cur_ip_it != _ip_db.end()
       ? cur_ip_it->second.state_array[filter_to_column[filter_id]]
       : FilterState{};
}
<commit_msg>Implify IPDatabase::set_ip_state<commit_after>/**
   This class is responsible to store global states for each requester ip. In particular
   it is supposed:
   - Memory allocation.
   - Take care of locking and unlocking.
   - Keeping track of memory limit and garbage collection.

   AUTHORS:
   - Vmon: Oct 2013: Initial version
*/

#include "ip_database.h"
#include "banjax.h"
#include "defer.h"

bool
IPDatabase::set_ip_state(const std::string& ip, FilterIDType filter_id, FilterState state)
{
  //we need to lock at the begining because somebody can
  //delete an ip while we are searching
  if (TSMutexLockTry(db_mutex) != TS_SUCCESS) {
    TSDebug(BANJAX_PLUGIN_NAME, "Unable to get lock on the ip db");
    return false;
  }

  auto on_exit = defer([&] { TSMutexUnlock(db_mutex); });

  IPHashTable::iterator i = _ip_db.find(ip);

  if (i == _ip_db.end()) {
    i = _ip_db.insert({ip, {}}).first;
  }

  i->second.state_array[filter_to_column[filter_id]] = state;

  return true;
}

bool
IPDatabase::drop_ip(std::string& ip)
{
  if (TSMutexLockTry(db_mutex) != TS_SUCCESS) {
    TSDebug(BANJAX_PLUGIN_NAME, "Unable to get lock on the ip db");
    return false;
  }

  auto on_exit = defer([&] { TSMutexUnlock(db_mutex); });

  return _ip_db.erase(ip) != 0;
}

boost::optional<FilterState>
IPDatabase::get_ip_state(const std::string& ip, FilterIDType filter_id)
{
  if (TSMutexLockTry(db_mutex) != TS_SUCCESS) {
    TSDebug(BANJAX_PLUGIN_NAME, "Unable to get lock on the ip db");
    return boost::none;
  }

  auto on_exit = defer([&] { TSMutexUnlock(db_mutex); });

  IPHashTable::iterator i = _ip_db.find(ip);

  return i != _ip_db.end()
       ? i->second.state_array[filter_to_column[filter_id]]
       : FilterState{};
}
<|endoftext|>


<commit_before>/*
 * istream implementation which reads a file from a NFS server.
 *
 * author: Max Kellermann <mk@cm4all.com>
 */

#include "istream_nfs.hxx"
#include "istream/istream.hxx"
#include "nfs_client.hxx"
#include "pool.hxx"
#include "util/Cast.hxx"
#include "util/ForeignFifoBuffer.hxx"

#include <assert.h>
#include <string.h>

static const size_t NFS_BUFFER_SIZE = 32768;

struct NfsIstream final : Istream, NfsClientReadFileHandler {
    NfsFileHandle *handle;

    /**
     * The offset of the next "pread" call on the NFS server.
     */
    uint64_t offset;

    /**
     * The number of bytes that are remaining on the NFS server, not
     * including the amount of data that is already pending.
     */
    uint64_t remaining;

    /**
     * The number of bytes currently scheduled by nfs_pread_async().
     */
    size_t pending_read = 0;

    /**
     * The number of bytes that shall be discarded from the
     * nfs_pread_async() result.  This is non-zero if istream_skip()
     * has been called while a read call was pending.
     */
    size_t discard_read = 0;

    ForeignFifoBuffer<uint8_t> buffer;

    explicit NfsIstream(struct pool &p, NfsFileHandle &_handle,
                        uint64_t start, uint64_t end)
        :Istream(p), handle(&_handle),
         offset(start), remaining(end - start),
         buffer(nullptr) {}

    using Istream::DestroyError;

    void ScheduleRead();

    /**
     * Check for end-of-file, and if there's more data to read, schedule
     * another read call.
     *
     * The input buffer must be empty.
     */
    void ScheduleReadOrEof();

    void Feed(const void *data, size_t length);

    void ReadFromBuffer();

    /* virtual methods from class Istream */

    off_t _GetAvailable(gcc_unused bool partial) override {
        return remaining + pending_read - discard_read +
            buffer.GetAvailable();
    }

    off_t _Skip(off_t length) override;

    void _Read() override {
        if (!buffer.IsEmpty())
            ReadFromBuffer();
        else
            ScheduleReadOrEof();
    }

    void Close() {
        nfs_client_close_file(handle);
        Istream::_Close();
    }

    /* virtual methods from class NfsClientReadFileHandler */
    void OnNfsRead(const void *data, size_t length) override;
    void OnNfsReadError(GError *error) override;
};

void
NfsIstream::ScheduleReadOrEof()
{
    assert(buffer.IsEmpty());

    if (pending_read > 0)
        return;

    if (remaining > 0) {
        /* read more */

        ScheduleRead();
    } else {
        /* end of file */

        nfs_client_close_file(handle);
        DestroyEof();
    }
}

inline void
NfsIstream::Feed(const void *data, size_t length)
{
    assert(length > 0);

    if (buffer.IsNull()) {
        const uint64_t total_size = remaining + length;
        const size_t buffer_size = total_size > NFS_BUFFER_SIZE
            ? NFS_BUFFER_SIZE
            : (size_t)total_size;
        buffer.SetBuffer(PoolAlloc<uint8_t>(GetPool(), buffer_size),
                         buffer_size);
    }

    auto w = buffer.Write();
    assert(w.size >= length);

    memcpy(w.data, data, length);
    buffer.Append(length);
}

void
NfsIstream::ReadFromBuffer()
{
    assert(buffer.IsDefined());

    size_t buffer_remaining = ConsumeFromBuffer(buffer);
    if (buffer_remaining == 0 && pending_read == 0)
        ScheduleReadOrEof();
}

/*
 * NfsClientReadFileHandler
 *
 */

void
NfsIstream::OnNfsRead(const void *data, size_t _length)
{
    assert(pending_read > 0);
    assert(discard_read <= pending_read);
    assert(_length <= pending_read);

    if (_length < pending_read) {
        nfs_client_close_file(handle);
        GError *error = g_error_new_literal(g_file_error_quark(), 0,
                                            "premature end of file");
        DestroyError(error);
        return;
    }

    const size_t discard = discard_read;
    const size_t length = pending_read - discard;
    pending_read = 0;
    discard_read = 0;

    if (length > 0)
        Feed((const char *)data + discard, length);
    ReadFromBuffer();
}

void
NfsIstream::OnNfsReadError(GError *error)
{
    assert(pending_read > 0);

    nfs_client_close_file(handle);
    DestroyError(error);
}

inline void
NfsIstream::ScheduleRead()
{
    assert(pending_read == 0);

    const size_t max = buffer.IsDefined()
        ? buffer.Write().size
        : NFS_BUFFER_SIZE;
    size_t nbytes = remaining > max
        ? max
        : (size_t)remaining;

    const uint64_t read_offset = offset;

    offset += nbytes;
    remaining -= nbytes;
    pending_read = nbytes;

    nfs_client_read_file(handle, read_offset, nbytes, *this);
}

/*
 * istream implementation
 *
 */

off_t
NfsIstream::_Skip(off_t _length)
{
    assert(discard_read <= pending_read);

    uint64_t length = _length;

    uint64_t result = 0;

    if (buffer.IsDefined()) {
        const uint64_t buffer_available = buffer.GetAvailable();
        const uint64_t consume = length < buffer_available
            ? length
            : buffer_available;
        buffer.Consume(consume);
        result += consume;
        length -= consume;
    }

    const uint64_t pending_available =
        pending_read - discard_read;
    uint64_t consume = length < pending_available
        ? length
        : pending_available;
    discard_read += consume;
    result += consume;
    length -= consume;

    if (length > remaining)
        length = remaining;

    remaining -= length;
    offset += length;
    result += length;

    Consumed(result);
    return result;
}

/*
 * constructor
 *
 */

Istream *
istream_nfs_new(struct pool &pool, NfsFileHandle &handle,
                uint64_t start, uint64_t end)
{
    assert(start <= end);

    return NewIstream<NfsIstream>(pool, handle, start, end);
}
<commit_msg>istream_nfs: convert struct to class<commit_after>/*
 * istream implementation which reads a file from a NFS server.
 *
 * author: Max Kellermann <mk@cm4all.com>
 */

#include "istream_nfs.hxx"
#include "istream/istream.hxx"
#include "nfs_client.hxx"
#include "pool.hxx"
#include "util/Cast.hxx"
#include "util/ForeignFifoBuffer.hxx"

#include <assert.h>
#include <string.h>

static const size_t NFS_BUFFER_SIZE = 32768;

class NfsIstream final : public Istream, NfsClientReadFileHandler {
    NfsFileHandle *handle;

    /**
     * The offset of the next "pread" call on the NFS server.
     */
    uint64_t offset;

    /**
     * The number of bytes that are remaining on the NFS server, not
     * including the amount of data that is already pending.
     */
    uint64_t remaining;

    /**
     * The number of bytes currently scheduled by nfs_pread_async().
     */
    size_t pending_read = 0;

    /**
     * The number of bytes that shall be discarded from the
     * nfs_pread_async() result.  This is non-zero if istream_skip()
     * has been called while a read call was pending.
     */
    size_t discard_read = 0;

    ForeignFifoBuffer<uint8_t> buffer;

public:
    explicit NfsIstream(struct pool &p, NfsFileHandle &_handle,
                        uint64_t start, uint64_t end)
        :Istream(p), handle(&_handle),
         offset(start), remaining(end - start),
         buffer(nullptr) {}

private:
    void ScheduleRead();

    /**
     * Check for end-of-file, and if there's more data to read, schedule
     * another read call.
     *
     * The input buffer must be empty.
     */
    void ScheduleReadOrEof();

    void Feed(const void *data, size_t length);

    void ReadFromBuffer();

    /* virtual methods from class Istream */

    off_t _GetAvailable(gcc_unused bool partial) override {
        return remaining + pending_read - discard_read +
            buffer.GetAvailable();
    }

    off_t _Skip(off_t length) override;

    void _Read() override {
        if (!buffer.IsEmpty())
            ReadFromBuffer();
        else
            ScheduleReadOrEof();
    }

    void Close() {
        nfs_client_close_file(handle);
        Istream::_Close();
    }

    /* virtual methods from class NfsClientReadFileHandler */
    void OnNfsRead(const void *data, size_t length) override;
    void OnNfsReadError(GError *error) override;
};

void
NfsIstream::ScheduleReadOrEof()
{
    assert(buffer.IsEmpty());

    if (pending_read > 0)
        return;

    if (remaining > 0) {
        /* read more */

        ScheduleRead();
    } else {
        /* end of file */

        nfs_client_close_file(handle);
        DestroyEof();
    }
}

inline void
NfsIstream::Feed(const void *data, size_t length)
{
    assert(length > 0);

    if (buffer.IsNull()) {
        const uint64_t total_size = remaining + length;
        const size_t buffer_size = total_size > NFS_BUFFER_SIZE
            ? NFS_BUFFER_SIZE
            : (size_t)total_size;
        buffer.SetBuffer(PoolAlloc<uint8_t>(GetPool(), buffer_size),
                         buffer_size);
    }

    auto w = buffer.Write();
    assert(w.size >= length);

    memcpy(w.data, data, length);
    buffer.Append(length);
}

void
NfsIstream::ReadFromBuffer()
{
    assert(buffer.IsDefined());

    size_t buffer_remaining = ConsumeFromBuffer(buffer);
    if (buffer_remaining == 0 && pending_read == 0)
        ScheduleReadOrEof();
}

/*
 * NfsClientReadFileHandler
 *
 */

void
NfsIstream::OnNfsRead(const void *data, size_t _length)
{
    assert(pending_read > 0);
    assert(discard_read <= pending_read);
    assert(_length <= pending_read);

    if (_length < pending_read) {
        nfs_client_close_file(handle);
        GError *error = g_error_new_literal(g_file_error_quark(), 0,
                                            "premature end of file");
        DestroyError(error);
        return;
    }

    const size_t discard = discard_read;
    const size_t length = pending_read - discard;
    pending_read = 0;
    discard_read = 0;

    if (length > 0)
        Feed((const char *)data + discard, length);
    ReadFromBuffer();
}

void
NfsIstream::OnNfsReadError(GError *error)
{
    assert(pending_read > 0);

    nfs_client_close_file(handle);
    DestroyError(error);
}

inline void
NfsIstream::ScheduleRead()
{
    assert(pending_read == 0);

    const size_t max = buffer.IsDefined()
        ? buffer.Write().size
        : NFS_BUFFER_SIZE;
    size_t nbytes = remaining > max
        ? max
        : (size_t)remaining;

    const uint64_t read_offset = offset;

    offset += nbytes;
    remaining -= nbytes;
    pending_read = nbytes;

    nfs_client_read_file(handle, read_offset, nbytes, *this);
}

/*
 * istream implementation
 *
 */

off_t
NfsIstream::_Skip(off_t _length)
{
    assert(discard_read <= pending_read);

    uint64_t length = _length;

    uint64_t result = 0;

    if (buffer.IsDefined()) {
        const uint64_t buffer_available = buffer.GetAvailable();
        const uint64_t consume = length < buffer_available
            ? length
            : buffer_available;
        buffer.Consume(consume);
        result += consume;
        length -= consume;
    }

    const uint64_t pending_available =
        pending_read - discard_read;
    uint64_t consume = length < pending_available
        ? length
        : pending_available;
    discard_read += consume;
    result += consume;
    length -= consume;

    if (length > remaining)
        length = remaining;

    remaining -= length;
    offset += length;
    result += length;

    Consumed(result);
    return result;
}

/*
 * constructor
 *
 */

Istream *
istream_nfs_new(struct pool &pool, NfsFileHandle &handle,
                uint64_t start, uint64_t end)
{
    assert(start <= end);

    return NewIstream<NfsIstream>(pool, handle, start, end);
}
<|endoftext|>


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

#ifndef JACUI_IMAGE_HPP
#define JACUI_IMAGE_HPP

#include "surface.hpp"

namespace jacui {
    /**
       \brief jacui image class
    */
    class image: public surface {
    public:
        /**
           \brief create an empty image
        */
        image();

        /**
           \brief create a copy of an existing image
        */
        image(const image& rhs);

        /**
           \brief create an image from a file
        */
        explicit image(const char* filename);

        /**
           \brief create an image from memory
        */
        image(const void* data, std::size_t size);

        /**
          \brief destroy an image
        */
        ~image();

        /**
          \brief load an image from a file
        */
        void load(const char* filename);

        /**
          \brief load an image from memory
        */
        void load(const void* data, std::size_t size);

        /**
           \brief swap two image instances

           \param rhs the image to be swapped
        */
        void swap(image& rhs);

        /**
           \brief copy an existing image

           \param rhs the image to be swapped
        */
        image& operator=(const image& rhs) {
            image tmp(rhs);
            swap(tmp);
            return *this;
        }

    public:
        /**
           \brief implementation detail
        */
        detail::surface_type* detail() const;

    private:
        struct impl;
        impl* pimpl_;
    };

    /**
       \brief swap two image instances

       \param lhs the first image to be swapped
       \param rhs the second image to be swapped
    */
    inline void swap(image& lhs, image& rhs) {
        lhs.swap(rhs);
    }
}

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

#ifndef JACUI_IMAGE_HPP
#define JACUI_IMAGE_HPP

#include "surface.hpp"

namespace jacui {
    /**
       \brief jacui image class

       An image represents a surface in memory that has been loaded
       from an external source, i.e. a file or a memory area.
    */
    class image: public surface {
    public:
        /**
           \brief create an empty image
        */
        image();

        /**
           \brief create a copy of an existing image
        */
        image(const image& rhs);

        /**
           \brief create an image from a file
        */
        explicit image(const char* filename);

        /**
           \brief create an image from memory
        */
        image(const void* data, std::size_t size);

        /**
          \brief destroy an image
        */
        ~image();

        /**
          \brief load an image from a file
        */
        void load(const char* filename);

        /**
          \brief load an image from memory
        */
        void load(const void* data, std::size_t size);

        /**
           \brief swap two image instances

           \param rhs the image to be swapped
        */
        void swap(image& rhs);

        /**
           \brief copy an existing image

           \param rhs the image to be swapped
        */
        image& operator=(const image& rhs) {
            image tmp(rhs);
            swap(tmp);
            return *this;
        }

    public:
        /**
           \brief implementation detail
        */
        detail::surface_type* detail() const;

    private:
        struct impl;
        impl* pimpl_;
    };

    /**
       \brief swap two image instances

       \param lhs the first image to be swapped
       \param rhs the second image to be swapped
    */
    inline void swap(image& lhs, image& rhs) {
        lhs.swap(rhs);
    }
}

#endif
<|endoftext|>


<commit_before>//===-- LibCxxOptional.cpp --------------------------------------*- C++ -*-===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is distributed under the University of Illinois Open Source
// License. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//

#include "LibCxx.h"
#include "lldb/DataFormatters/FormattersHelpers.h"

using namespace lldb;
using namespace lldb_private;

namespace {

class OptionalFrontEnd : public SyntheticChildrenFrontEnd {
public:
  OptionalFrontEnd(ValueObject &valobj) : SyntheticChildrenFrontEnd(valobj) {
    Update();
  }

  size_t GetIndexOfChildWithName(const ConstString &name) override {
    return formatters::ExtractIndexFromString(name.GetCString());
  }

  bool MightHaveChildren() override { return true; }
  bool Update() override;
  size_t CalculateNumChildren() override { return m_size; }
  ValueObjectSP GetChildAtIndex(size_t idx) override;

private:
  size_t m_size = 0;
  ValueObjectSP m_base_sp;
};
} // namespace

bool OptionalFrontEnd::Update() {
  ValueObjectSP engaged_sp(
      m_backend.GetChildMemberWithName(ConstString("__engaged_"), true));

  if (!engaged_sp)
    return false;

  // __engaged_ is a bool flag and is true if the optional contains a value.
  // Converting it to unsigned gives us a size of 1 if it contains a value
  // and 0 if not .
  m_size = engaged_sp->GetValueAsUnsigned(0);

  return false;
}

ValueObjectSP OptionalFrontEnd::GetChildAtIndex(size_t idx) {
  if (idx >= m_size)
    return ValueObjectSP();

  // __val_ contains the underlying value of an optional if it has one.
  // Currently because it is part of an anonymous union GetChildMemberWithName()
  // does not peer through and find it unless we are at the parent itself.
  // We can obtain the parent through __engaged_.
  ValueObjectSP val_sp(
      m_backend.GetChildMemberWithName(ConstString("__engaged_"), true)
          ->GetParent()
          ->GetChildAtIndex(0, true)
          ->GetChildMemberWithName(ConstString("__val_"), true));

  if (!val_sp)
    return ValueObjectSP();

  CompilerType holder_type = val_sp->GetCompilerType();

  if (!holder_type)
    return ValueObjectSP();

  return val_sp->Clone(ConstString(llvm::formatv("Value").str()));
}

SyntheticChildrenFrontEnd *
formatters::LibcxxOptionalFrontEndCreator(CXXSyntheticChildren *,
                                          lldb::ValueObjectSP valobj_sp) {
  if (valobj_sp)
    return new OptionalFrontEnd(*valobj_sp);
  return nullptr;
}
<commit_msg>Undoing first commit which added a space to a comment<commit_after>//===-- LibCxxOptional.cpp --------------------------------------*- C++ -*-===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is distributed under the University of Illinois Open Source
// License. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//

#include "LibCxx.h"
#include "lldb/DataFormatters/FormattersHelpers.h"

using namespace lldb;
using namespace lldb_private;

namespace {

class OptionalFrontEnd : public SyntheticChildrenFrontEnd {
public:
  OptionalFrontEnd(ValueObject &valobj) : SyntheticChildrenFrontEnd(valobj) {
    Update();
  }

  size_t GetIndexOfChildWithName(const ConstString &name) override {
    return formatters::ExtractIndexFromString(name.GetCString());
  }

  bool MightHaveChildren() override { return true; }
  bool Update() override;
  size_t CalculateNumChildren() override { return m_size; }
  ValueObjectSP GetChildAtIndex(size_t idx) override;

private:
  size_t m_size = 0;
  ValueObjectSP m_base_sp;
};
} // namespace

bool OptionalFrontEnd::Update() {
  ValueObjectSP engaged_sp(
      m_backend.GetChildMemberWithName(ConstString("__engaged_"), true));

  if (!engaged_sp)
    return false;

  // __engaged_ is a bool flag and is true if the optional contains a value.
  // Converting it to unsigned gives us a size of 1 if it contains a value
  // and 0 if not.
  m_size = engaged_sp->GetValueAsUnsigned(0);

  return false;
}

ValueObjectSP OptionalFrontEnd::GetChildAtIndex(size_t idx) {
  if (idx >= m_size)
    return ValueObjectSP();

  // __val_ contains the underlying value of an optional if it has one.
  // Currently because it is part of an anonymous union GetChildMemberWithName()
  // does not peer through and find it unless we are at the parent itself.
  // We can obtain the parent through __engaged_.
  ValueObjectSP val_sp(
      m_backend.GetChildMemberWithName(ConstString("__engaged_"), true)
          ->GetParent()
          ->GetChildAtIndex(0, true)
          ->GetChildMemberWithName(ConstString("__val_"), true));

  if (!val_sp)
    return ValueObjectSP();

  CompilerType holder_type = val_sp->GetCompilerType();

  if (!holder_type)
    return ValueObjectSP();

  return val_sp->Clone(ConstString(llvm::formatv("Value").str()));
}

SyntheticChildrenFrontEnd *
formatters::LibcxxOptionalFrontEndCreator(CXXSyntheticChildren *,
                                          lldb::ValueObjectSP valobj_sp) {
  if (valobj_sp)
    return new OptionalFrontEnd(*valobj_sp);
  return nullptr;
}
<|endoftext|>


<commit_before>#include "peepHoleWindowDemo.h"
#include "vrambatcher.h"
#include <cmath>
#include <nds/arm9/input.h>

PeepHoleWindowDemo::PeepHoleWindowDemo() : xPos(128), yPos(64), radius(MIN_HOLE_SIZE) {}
PeepHoleWindowDemo::~PeepHoleWindowDemo() {}

void PeepHoleWindowDemo::Load() {
	setupDefaultBG();
	WIN_IN = BIT(1);
	WIN_OUT = 0;

	REG_DISPCNT |= DISPLAY_WIN0_ON | DISPLAY_BG0_ACTIVE;
}
void PeepHoleWindowDemo::Unload() {
	REG_DISPCNT &= ~DISPLAY_WIN0_ON;
}

void PeepHoleWindowDemo::AcceptInput() {
	auto keys = keysCurrent();
	if(keys & KEY_X && this->radius > MIN_HOLE_SIZE) {
		radius -= 0.1;
	} else if(keys & KEY_Y && radius < MAX_HOLE_SIZE) {
		radius += 0.1;
	}

	if(keys & KEY_LEFT && xPos > 0) {
		xPos--;
	} else if(keys & KEY_RIGHT && xPos < SCREEN_WIDTH) {
		xPos++;
	}

	if(keys & KEY_UP && yPos > 0) {
		yPos--;
	} else if(keys& KEY_DOWN && yPos < SCREEN_HEIGHT) {
		yPos++;
	}
}

void PeepHoleWindowDemo::PrepareFrame(VramBatcher &batcher) {
	int height = radius * 2;

	WIN0_Y0 = yPos;
	WIN0_Y1 = yPos + height;

	int scanline = 0;
	//turn everything off until the first line of the hole
	if (yPos > 0) {
		batcher.AddPoke(scanline, 0, DISPLAY_WIN0_ON | DISPLAY_BG0_ACTIVE, &REG_DISPCNT);
	}

	//jump to the top of the hole
	scanline = yPos;
	//turn on display and windowing
	batcher.AddPoke(scanline, DISPLAY_WIN0_ON| DISPLAY_BG0_ACTIVE, DISPLAY_WIN0_ON| DISPLAY_BG0_ACTIVE, &REG_DISPCNT);

	//start generating the hole
	for (; scanline < yPos + height && scanline < SCREEN_HEIGHT; ++scanline) {
		float angle = std::asin((radius - (scanline - yPos)) / radius);
		int width=std::cos(angle)*radius*2;
		int left=xPos-width/2;
		int right=left+width;

		if(left < 0) left = 0;
		if(right > SCREEN_WIDTH) right = SCREEN_WIDTH;

		batcher.AddPoke(scanline, left, &WIN0_X0);
		batcher.AddPoke(scanline, right, &WIN0_X1);
	}

	//turn everything off again
	if (scanline < SCREEN_HEIGHT) {
		batcher.AddPoke(scanline, 0, DISPLAY_WIN0_ON | DISPLAY_BG0_ACTIVE, &REG_DISPCNT);
	}
}<commit_msg>Mostly functioning peephole demo<commit_after>#include "peepHoleWindowDemo.h"
#include "vrambatcher.h"
#include <cmath>
#include <nds/arm9/input.h>

PeepHoleWindowDemo::PeepHoleWindowDemo() : xPos(128), yPos(64-25), radius(50) {}
PeepHoleWindowDemo::~PeepHoleWindowDemo() {}

void PeepHoleWindowDemo::Load() {
	setupDefaultBG();
	WIN_IN = BIT(0);
	WIN_OUT = 0;

	REG_DISPCNT |= DISPLAY_WIN0_ON | DISPLAY_BG0_ACTIVE;
}
void PeepHoleWindowDemo::Unload() {
	REG_DISPCNT &= ~DISPLAY_WIN0_ON;
}

void PeepHoleWindowDemo::AcceptInput() {
	auto keys = keysCurrent();
	if(keys & KEY_X && this->radius > MIN_HOLE_SIZE) {
		radius -= 0.1;
	} else if(keys & KEY_Y && radius < MAX_HOLE_SIZE) {
		radius += 0.1;
	}

	if(keys & KEY_LEFT && xPos > 0) {
		xPos--;
	} else if(keys & KEY_RIGHT && xPos < SCREEN_WIDTH) {
		xPos++;
	}

	if(keys & KEY_UP && yPos > 0) {
		yPos--;
	} else if(keys& KEY_DOWN && yPos < SCREEN_HEIGHT) {
		yPos++;
	}
}

void PeepHoleWindowDemo::PrepareFrame(VramBatcher &batcher) {
	int height = radius * 2;

	WIN0_Y0 = yPos;
	WIN0_Y1 = yPos + height;

	//jump to the top of the hole
	 int scanline = yPos;

	//start generating the hole
	for (; scanline < yPos + height && scanline < SCREEN_HEIGHT; ++scanline) {
		float angle = std::asin((radius - (scanline - yPos)) / radius);
		int width=std::cos(angle)*radius*2;
		int left=xPos-width/2;
		int right=left+width;

		if(left < 0) left = 0;
		if(right > SCREEN_WIDTH) right = SCREEN_WIDTH;

		batcher.AddPoke(scanline, left, &WIN0_X0);
		batcher.AddPoke(scanline, right, &WIN0_X1);
	}
}<|endoftext|>


<commit_before>/*
 * Copyright (c) 2014 Zubax, zubax.com
 * Please refer to the file LICENSE for terms and conditions.
 * Author: Pavel Kirienko <pavel.kirienko@zubax.com>
 */

#pragma once

#include <ros/ros.h>
#include <ros/console.h>
#include <eigen3/Eigen/Eigen>
#include <cmath>
#include "mathematica.hpp"
#include "debug_publisher.hpp"
#include "linear_algebra.hpp"
#include "exception.hpp"
#include "state_vector_autogenerated.hpp"

namespace zubax_posekf
{
/**
 * Filter output - twist; compatible with geometry_msgs/TwistWithCovariance.
 * - Velocity of IMU in the world frame, transformed into the IMU frame
 * - Angular velocity of IMU in the IMU frame
 * - Covariance: x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis
 */
struct Twist
{
    Vector3 linear;
    Vector3 angular;
    Matrix<6, 6> linear_angular_cov;

    Twist()
    {
        linear.setZero();
        angular.setZero();
        linear_angular_cov.setZero();
    }
};

/**
 * Filter output - pose; compatible with geometry_msgs/PoseWithCovariance.
 * - Position of IMU in the world frame
 * - Orientation of IMU in the world frame
 * - Covariance: x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis
 */
struct Pose
{
    Vector3 position;
    Quaternion orientation;
    Matrix<6, 6> position_orientation_cov;

    Pose()
    {
        position.setZero();
        orientation.setIdentity();
        position_orientation_cov.setZero();
    }
};

/**
 * The business end.
 */
class Filter
{
    DebugPublisher debug_pub_;

    StateVector state_;
    Matrix<StateVector::Size, StateVector::Size> P_;
    Matrix<StateVector::Size, StateVector::Size> Q_;

    const Scalar MaxGyroDrift = 0.1;
    const Scalar MaxAccelDrift = 2.0;
    const Scalar MaxAngularVelocity = M_PI * 2;
    const Scalar MaxCovariance = 1e9;
    const Scalar MinVariance = 1e-9;

    Scalar state_timestamp_ = 0.0;
    bool initialized_ = false;

    static Vector3 constrainPerComponent(Vector3 vec, const Scalar limit, const char* name)
    {
        for (int i = 0; i < 3; i++)
        {
            if (!checkRangeAndConstrainSymmetric(vec[i], limit))
            {
                ROS_WARN_THROTTLE(1, "Too high - %s", name);
            }
        }
        return vec;
    }

    void normalizeAndCheck()
    {
        state_.normalize();

        state_.bw(constrainPerComponent(state_.bw(), MaxGyroDrift, "gyro drift"));

        state_.ba(constrainPerComponent(state_.ba(), MaxAccelDrift, "accel drift"));

        state_.w(constrainPerComponent(state_.w(), MaxAngularVelocity, "angular velocity"));

        // P validation
        if (!validateAndFixCovarianceMatrix(P_, MaxCovariance, MinVariance))
        {
            ROS_ERROR_STREAM_THROTTLE(1, "Matrix P has been fixed\n" << P_.format(Eigen::IOFormat(2)));
        }

        debug_pub_.publish("x", state_.x);
        debug_pub_.publish("P", P_);
        debug_pub_.publish("Q", Q_);

        debug_pub_.publish("P_diag", Vector<StateVector::Size>(P_.diagonal()));

        debug_pub_.publish("pvw", state_.pvw());
        debug_pub_.publish("qvw_rpy_deg", Vector3(quaternionToEuler(state_.qvw()) / mathematica::Degree));

        enforce("Non-finite states",
                std::isfinite(state_.x.sum()) &&
                std::isfinite(P_.sum()) &&
                std::isfinite(Q_.sum()));

        enforce("Invalid timestamp", std::isfinite(state_timestamp_) && (state_timestamp_ > 0.0));
    }

    template <int NumDims>
    void performMeasurementUpdate(const Vector<NumDims>& y,
                                  const Matrix<NumDims, NumDims>& R,
                                  const Matrix<NumDims, StateVector::Size>& H,
                                  const char* const source_name)
    {
        ROS_ASSERT(initialized_);

        /*
         * Ensure that R is symmetric, then validate
         */
        const Matrix<NumDims, NumDims> R_sym = 0.5 * (R + R.transpose());
        {
            bool R_sym_ok = true;
            for (int i = 0; i < NumDims; i++)
            {
                if (R_sym(i, i) <= 0)
                {
                    R_sym_ok = false;
                    break;
                }
            }

            if (!R_sym_ok || !std::isfinite(R_sym.sum()))
            {
                std::ostringstream os;
                os << "Measurement R_sym [" << source_name << "]:\n" << R_sym;
                throw Exception(os.str());
            }
        }

        /*
         * Compute S inverse with validation
         */
        Matrix<NumDims, NumDims> S_inv;
        {
            const Matrix<NumDims, NumDims> S = H * P_ * H.transpose() + R_sym;
            bool s_is_invertible = false;
            S.computeInverseWithCheck(S_inv, s_is_invertible);
            if (!s_is_invertible)
            {
                std::ostringstream os;
                os << "S is not invertible (numerical failure?) [" << source_name << "]:\n" << S
                    << "\nR_sym:\n" << R_sym;
                throw Exception(os.str());
            }
        }

        /*
         * Kalman update equations
         * Joseph form is used instead of the simplified form to improve numerical stability
         */
        const auto K = static_cast<Matrix<StateVector::Size, NumDims> >(P_ * H.transpose() * S_inv);

        state_.x = state_.x + K * y;

        const Matrix<StateVector::Size, StateVector::Size> IKH = decltype(P_)::Identity() - K * H;
        P_ = IKH * P_ * IKH.transpose() + K * R_sym * K.transpose();

        normalizeAndCheck();

//        std::cout << "P:\n" << P_.format(Eigen::IOFormat(2)) << "\n" << std::endl;
//        std::cout << "x:\n" << state_.x.transpose().format(Eigen::IOFormat(2)) << "\n" << std::endl;
    }

public:
    Filter()
    {
        // Initial state - zero everything, null rotations
        state_.qwi(Quaternion(1, 0, 0, 0));
        state_.qvw(Quaternion(1, 0, 0, 0));

        // Initial P
        P_.setZero();
        P_ = StateVector::Pinitdiag().asDiagonal();

        // Initial Q
        // TODO: runtime Q estimation
        Q_ = StateVector::Qmindiag().asDiagonal();
    }

    bool isInitialized() const { return initialized_; }

    void initialize(Scalar timestamp, const Quaternion& orientation)
    {
        ROS_ASSERT(!initialized_);
        initialized_ = true;

        state_timestamp_ = timestamp;

        state_.qwi(orientation);

        normalizeAndCheck();

        ROS_INFO_STREAM("Initial P:\n" << P_.format(Eigen::IOFormat(2)));
        ROS_INFO_STREAM("Initial Q:\n" << Q_.format(Eigen::IOFormat(2)));
        ROS_INFO_STREAM("Initial x:\n" << state_.x.transpose().format(Eigen::IOFormat(2)));
    }

    void performTimeUpdate(Scalar timestamp)
    {
        ROS_ASSERT(initialized_);
        ROS_ASSERT(timestamp > 0);

        /*
         * Compute and check dt
         */
        const Scalar dt = timestamp - state_timestamp_;
        debug_pub_.publish("dt", dt);
        if (dt <= 0)
        {
            ROS_WARN_THROTTLE(1, "Time update: Nonpositive dt [%f]", dt);
            return;
        }
        state_timestamp_ = timestamp;

        /*
         * Update the state vector and process covariance matrix
         */
        const auto F = state_.F(dt);

        state_.x = state_.f(dt);

        P_ = F * P_ * F.transpose() + Q_ * dt;

        normalizeAndCheck();

        // TODO: Commit point - add new X to the state history
    }

    void performAccelUpdate(const Vector3& accel, const Matrix3& cov)
    {
        const Vector3 y = accel - state_.hacc();
        performMeasurementUpdate(y, cov, state_.Hacc(), "acc");
    }

    void performGyroUpdate(const Vector3& angvel, const Matrix3& cov)
    {
        const Vector3 y = angvel - state_.hgyro();
        performMeasurementUpdate(y, cov, state_.Hgyro(), "gyro");
    }

    void performGNSSPosUpdate(const Vector3& pos, const Matrix3& cov)
    {
        const Vector3 y = pos - state_.hgnsspos();
        performMeasurementUpdate(y, cov, state_.Hgnsspos(), "gnsspos");
    }

    void performGNSSVelUpdate(const Vector3& vel, const Matrix3& cov)
    {
        const Vector3 y = vel - state_.hgnssvel();
        performMeasurementUpdate(y, cov, state_.Hgnssvel(), "gnssvel");
    }

    void performVisPosUpdate(const Vector3& pos, const Matrix3& cov)
    {
        const Vector3 y = pos - state_.hvispos();
        performMeasurementUpdate(y, cov, state_.Hvispos(), "vispos");
    }

    void performVisVelUpdate(const Vector3& vel, const Matrix3& cov)
    {
        const Vector3 y = vel - state_.hvisvel();
        performMeasurementUpdate(y, cov, state_.Hvisvel(), "visvel");
    }

    void performVisAttUpdate(const Quaternion& z, const Matrix3& cov)
    {
        const Vector3 y = quaternionToEuler(z) - state_.hvisatt();
        performMeasurementUpdate(y, cov, state_.Hvisatt(), "visatt");
    }

    void overrideVisOffsetUncertainty(Scalar scalar = 999)
    {
        // TODO: special Q if the visual odometer is lost
        P_.block<3, 3>(StateVector::Idx::pvwx, StateVector::Idx::pvwx) = Matrix3::Identity() * scalar;
        P_.block<4, 4>(StateVector::Idx::qvww, StateVector::Idx::qvww) = Matrix4::Identity() * scalar;
    }

    Scalar getTimestamp() const { return state_timestamp_; }

    /**
     * Pose, compatible with geometry_msgs/PoseWithCovariance
     */
    Pose getOutputPose() const
    {
        Pose out;

        out.position = state_.pwi();
        out.orientation = state_.qwi();

        const Matrix<3, 4> G = quaternionToEulerJacobian(out.orientation);
        const Matrix4 C = P_.block<4, 4>(StateVector::Idx::qwiw, StateVector::Idx::qwiw);

        out.position_orientation_cov.block<3, 3>(0, 0) = P_.block<3, 3>(StateVector::Idx::pwix, StateVector::Idx::pwix);
        out.position_orientation_cov.block<3, 3>(3, 3) = G * C * G.transpose();

        return out;
    }

    /**
     * Twist, compatible with geometry_msgs/TwistWithCovariance
     */
    Twist getOutputTwistInIMUFrame() const
    {
        Twist out;
        out.linear = rotateVectorByQuaternion(state_.vwi(), state_.qwi());  // World --> IMU
        out.angular = state_.w();

        const Matrix3 G = rotateVectorByQuaternionJacobian(state_.qwi());
        const Matrix3 C = P_.block<3, 3>(StateVector::Idx::vwix, StateVector::Idx::vwix);

        out.linear_angular_cov.block<3, 3>(0, 0) = G * C * G.transpose();
        out.linear_angular_cov.block<3, 3>(3, 3) = P_.block<3, 3>(StateVector::Idx::wx,   StateVector::Idx::wx);

        return out;
    }

    /**
     * Gravity compensated acceleration in IMU frame with covariance
     */
    std::pair<Vector3, Matrix3> getOutputAcceleration() const
    {
        return { state_.a(), P_.block<3, 3>(StateVector::Idx::ax, StateVector::Idx::ax) };
    }
};

}
<commit_msg>Fixed Q computation<commit_after>/*
 * Copyright (c) 2014 Zubax, zubax.com
 * Please refer to the file LICENSE for terms and conditions.
 * Author: Pavel Kirienko <pavel.kirienko@zubax.com>
 */

#pragma once

#include <ros/ros.h>
#include <ros/console.h>
#include <eigen3/Eigen/Eigen>
#include <cmath>
#include "mathematica.hpp"
#include "debug_publisher.hpp"
#include "linear_algebra.hpp"
#include "exception.hpp"
#include "state_vector_autogenerated.hpp"

namespace zubax_posekf
{
/**
 * Filter output - twist; compatible with geometry_msgs/TwistWithCovariance.
 * - Velocity of IMU in the world frame, transformed into the IMU frame
 * - Angular velocity of IMU in the IMU frame
 * - Covariance: x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis
 */
struct Twist
{
    Vector3 linear;
    Vector3 angular;
    Matrix<6, 6> linear_angular_cov;

    Twist()
    {
        linear.setZero();
        angular.setZero();
        linear_angular_cov.setZero();
    }
};

/**
 * Filter output - pose; compatible with geometry_msgs/PoseWithCovariance.
 * - Position of IMU in the world frame
 * - Orientation of IMU in the world frame
 * - Covariance: x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis
 */
struct Pose
{
    Vector3 position;
    Quaternion orientation;
    Matrix<6, 6> position_orientation_cov;

    Pose()
    {
        position.setZero();
        orientation.setIdentity();
        position_orientation_cov.setZero();
    }
};

/**
 * The business end.
 */
class Filter
{
    DebugPublisher debug_pub_;

    StateVector state_;
    Matrix<StateVector::Size, StateVector::Size> P_;
    Matrix<StateVector::Size, StateVector::Size> Q_;

    const Scalar MaxGyroDrift = 0.1;
    const Scalar MaxAccelDrift = 2.0;
    const Scalar MaxAngularVelocity = M_PI * 2;
    const Scalar MaxCovariance = 1e9;
    const Scalar MinVariance = 1e-9;

    Scalar state_timestamp_ = 0.0;
    bool initialized_ = false;

    static Vector3 constrainPerComponent(Vector3 vec, const Scalar limit, const char* name)
    {
        for (int i = 0; i < 3; i++)
        {
            if (!checkRangeAndConstrainSymmetric(vec[i], limit))
            {
                ROS_WARN_THROTTLE(1, "Too high - %s", name);
            }
        }
        return vec;
    }

    void normalizeAndCheck()
    {
        state_.normalize();

        state_.bw(constrainPerComponent(state_.bw(), MaxGyroDrift, "gyro drift"));

        state_.ba(constrainPerComponent(state_.ba(), MaxAccelDrift, "accel drift"));

        state_.w(constrainPerComponent(state_.w(), MaxAngularVelocity, "angular velocity"));

        // P validation
        if (!validateAndFixCovarianceMatrix(P_, MaxCovariance, MinVariance))
        {
            ROS_ERROR_STREAM_THROTTLE(1, "Matrix P has been fixed\n" << P_.format(Eigen::IOFormat(2)));
        }

        debug_pub_.publish("x", state_.x);
        debug_pub_.publish("P_diag", Vector<StateVector::Size>(P_.diagonal()));
        debug_pub_.publish("Q_diag", Vector<StateVector::Size>(Q_.diagonal()));

        debug_pub_.publish("pvw", state_.pvw());
        debug_pub_.publish("qvw_rpy_deg", Vector3(quaternionToEuler(state_.qvw()) / mathematica::Degree));

        enforce("Non-finite states",
                std::isfinite(state_.x.sum()) &&
                std::isfinite(P_.sum()) &&
                std::isfinite(Q_.sum()));

        enforce("Invalid timestamp", std::isfinite(state_timestamp_) && (state_timestamp_ > 0.0));
    }

    template <int NumDims>
    void performMeasurementUpdate(const Vector<NumDims>& y,
                                  const Matrix<NumDims, NumDims>& R,
                                  const Matrix<NumDims, StateVector::Size>& H,
                                  const char* const source_name)
    {
        ROS_ASSERT(initialized_);

        /*
         * Ensure that R is symmetric, then validate
         */
        const Matrix<NumDims, NumDims> R_sym = 0.5 * (R + R.transpose());
        {
            bool R_sym_ok = true;
            for (int i = 0; i < NumDims; i++)
            {
                if (R_sym(i, i) <= 0)
                {
                    R_sym_ok = false;
                    break;
                }
            }

            if (!R_sym_ok || !std::isfinite(R_sym.sum()))
            {
                std::ostringstream os;
                os << "Measurement R_sym [" << source_name << "]:\n" << R_sym;
                throw Exception(os.str());
            }
        }

        /*
         * Compute S inverse with validation
         */
        Matrix<NumDims, NumDims> S_inv;
        {
            const Matrix<NumDims, NumDims> S = H * P_ * H.transpose() + R_sym;
            bool s_is_invertible = false;
            S.computeInverseWithCheck(S_inv, s_is_invertible);
            if (!s_is_invertible)
            {
                std::ostringstream os;
                os << "S is not invertible (numerical failure?) [" << source_name << "]:\n" << S
                    << "\nR_sym:\n" << R_sym;
                throw Exception(os.str());
            }
        }

        /*
         * Kalman update equations
         * Joseph form is used instead of the simplified form to improve numerical stability
         */
        const auto K = static_cast<Matrix<StateVector::Size, NumDims> >(P_ * H.transpose() * S_inv);

        state_.x = state_.x + K * y;

        const Matrix<StateVector::Size, StateVector::Size> IKH = decltype(P_)::Identity() - K * H;
        P_ = IKH * P_ * IKH.transpose() + K * R_sym * K.transpose();

        normalizeAndCheck();

//        std::cout << "P:\n" << P_.format(Eigen::IOFormat(2)) << "\n" << std::endl;
//        std::cout << "x:\n" << state_.x.transpose().format(Eigen::IOFormat(2)) << "\n" << std::endl;
    }

public:
    Filter()
    {
        // Initial state - zero everything, null rotations
        state_.qwi(Quaternion(1, 0, 0, 0));
        state_.qvw(Quaternion(1, 0, 0, 0));

        // Initial P
        P_.setZero();
        P_ = StateVector::Pinitdiag().asDiagonal();

        // Initial Q
        // TODO: runtime Q estimation
        Q_ = StateVector::Qmindiag().asDiagonal();
    }

    bool isInitialized() const { return initialized_; }

    void initialize(Scalar timestamp, const Quaternion& orientation)
    {
        ROS_ASSERT(!initialized_);
        initialized_ = true;

        state_timestamp_ = timestamp;

        state_.qwi(orientation);

        normalizeAndCheck();

        ROS_INFO_STREAM("Initial P:\n" << P_.format(Eigen::IOFormat(2)));
        ROS_INFO_STREAM("Initial Q:\n" << Q_.format(Eigen::IOFormat(2)));
        ROS_INFO_STREAM("Initial x:\n" << state_.x.transpose().format(Eigen::IOFormat(2)));
    }

    void performTimeUpdate(Scalar timestamp)
    {
        ROS_ASSERT(initialized_);
        ROS_ASSERT(timestamp > 0);

        /*
         * Compute and check dt
         */
        const Scalar dt = timestamp - state_timestamp_;
        debug_pub_.publish("dt", dt);
        if (dt <= 0)
        {
            ROS_WARN_THROTTLE(1, "Time update: Nonpositive dt [%f]", dt);
            return;
        }
        state_timestamp_ = timestamp;

        /*
         * Update the state vector and process covariance matrix
         */
        const auto F = state_.F(dt);

        state_.x = state_.f(dt);

        P_ = F * P_ * F.transpose() + Q_ * dt * dt;

        normalizeAndCheck();

        // TODO: Commit point - add new X to the state history
    }

    void performAccelUpdate(const Vector3& accel, const Matrix3& cov)
    {
        const Vector3 y = accel - state_.hacc();
        performMeasurementUpdate(y, cov, state_.Hacc(), "acc");
    }

    void performGyroUpdate(const Vector3& angvel, const Matrix3& cov)
    {
        const Vector3 y = angvel - state_.hgyro();
        performMeasurementUpdate(y, cov, state_.Hgyro(), "gyro");
    }

    void performGNSSPosUpdate(const Vector3& pos, const Matrix3& cov)
    {
        const Vector3 y = pos - state_.hgnsspos();
        performMeasurementUpdate(y, cov, state_.Hgnsspos(), "gnsspos");
    }

    void performGNSSVelUpdate(const Vector3& vel, const Matrix3& cov)
    {
        const Vector3 y = vel - state_.hgnssvel();
        performMeasurementUpdate(y, cov, state_.Hgnssvel(), "gnssvel");
    }

    void performVisPosUpdate(const Vector3& pos, const Matrix3& cov)
    {
        const Vector3 y = pos - state_.hvispos();
        performMeasurementUpdate(y, cov, state_.Hvispos(), "vispos");
    }

    void performVisVelUpdate(const Vector3& vel, const Matrix3& cov)
    {
        const Vector3 y = vel - state_.hvisvel();
        performMeasurementUpdate(y, cov, state_.Hvisvel(), "visvel");
    }

    void performVisAttUpdate(const Quaternion& z, const Matrix3& cov)
    {
        const Vector3 y = quaternionToEuler(z) - state_.hvisatt();
        performMeasurementUpdate(y, cov, state_.Hvisatt(), "visatt");
    }

    void overrideVisOffsetUncertainty(Scalar scalar = 999)
    {
        // TODO: special Q if the visual odometer is lost
        P_.block<3, 3>(StateVector::Idx::pvwx, StateVector::Idx::pvwx) = Matrix3::Identity() * scalar;
        P_.block<4, 4>(StateVector::Idx::qvww, StateVector::Idx::qvww) = Matrix4::Identity() * scalar;
    }

    Scalar getTimestamp() const { return state_timestamp_; }

    /**
     * Pose, compatible with geometry_msgs/PoseWithCovariance
     */
    Pose getOutputPose() const
    {
        Pose out;

        out.position = state_.pwi();
        out.orientation = state_.qwi();

        const Matrix<3, 4> G = quaternionToEulerJacobian(out.orientation);
        const Matrix4 C = P_.block<4, 4>(StateVector::Idx::qwiw, StateVector::Idx::qwiw);

        out.position_orientation_cov.block<3, 3>(0, 0) = P_.block<3, 3>(StateVector::Idx::pwix, StateVector::Idx::pwix);
        out.position_orientation_cov.block<3, 3>(3, 3) = G * C * G.transpose();

        return out;
    }

    /**
     * Twist, compatible with geometry_msgs/TwistWithCovariance
     */
    Twist getOutputTwistInIMUFrame() const
    {
        Twist out;
        out.linear = rotateVectorByQuaternion(state_.vwi(), state_.qwi());  // World --> IMU
        out.angular = state_.w();

        const Matrix3 G = rotateVectorByQuaternionJacobian(state_.qwi());
        const Matrix3 C = P_.block<3, 3>(StateVector::Idx::vwix, StateVector::Idx::vwix);

        out.linear_angular_cov.block<3, 3>(0, 0) = G * C * G.transpose();
        out.linear_angular_cov.block<3, 3>(3, 3) = P_.block<3, 3>(StateVector::Idx::wx,   StateVector::Idx::wx);

        return out;
    }

    /**
     * Gravity compensated acceleration in IMU frame with covariance
     */
    std::pair<Vector3, Matrix3> getOutputAcceleration() const
    {
        return { state_.a(), P_.block<3, 3>(StateVector::Idx::ax, StateVector::Idx::ax) };
    }
};

}
<|endoftext|>


<commit_before>/*
* mod_dup - duplicates apache requests
*
* Copyright (C) 2013 Orange
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
*     http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/

#include "mod_dup.hh"

#include <http_config.h>

namespace DupModule {

const unsigned int max_bytes = 8192;

static bool
extractBrigadeContent(apr_bucket_brigade *bb, request_rec *pRequest, std::string &content) {
    if (ap_get_brigade(pRequest->input_filters,
                       bb, AP_MODE_READBYTES, APR_BLOCK_READ, max_bytes) == APR_SUCCESS) {
        // Read brigade content
        for (apr_bucket *b = APR_BRIGADE_FIRST(bb);
             b != APR_BRIGADE_SENTINEL(bb);
             b = APR_BUCKET_NEXT(b) ) {
            // Metadata end of stream
            if (APR_BUCKET_IS_EOS(b)) {
                return true;
            }
            const char *data = 0;
            apr_size_t len = 0;
            apr_status_t rv = apr_bucket_read(b, &data, &len, APR_BLOCK_READ);
            if (rv != APR_SUCCESS) {
                Log::error(42, "Bucket read failed, skipping the rest of the body");
                return true;
            }
            if (len) {
                content.append(data, len);
            }
        }
    }
    else {
        Log::error(42, "Get brigade failed, skipping the rest of the body");
        return true;
    }
    return false;
}

int
earlyHook(request_rec *pRequest) {
    struct DupConf *tConf = reinterpret_cast<DupConf *>(ap_get_module_config(pRequest->per_dir_config, &dup_module));
    assert(tConf);
    if (!tConf->dirName) {
        Log::warn(1,"declined earlyHook in directory %s %p", pRequest->uri, pRequest);
        return DECLINED;
    }
    Log::warn(1,"in earlyHook in directory %s %p", pRequest->uri, pRequest);

    RequestInfo *info = new RequestInfo(tConf->getNextReqId());
    // Backup in request context
    ap_set_module_config(pRequest->request_config, &dup_module, (void *)info);

    if (!pRequest->connection->pool) {
        Log::error(42, "No connection pool associated to the request");
        return DECLINED;
    }
    if (!pRequest->connection->bucket_alloc) {
        pRequest->connection->bucket_alloc = apr_bucket_alloc_create(pRequest->connection->pool);
        if (!pRequest->connection->bucket_alloc) {
            Log::error(42, "Request bucket allocation failed");
            return DECLINED;
        }
    }

    apr_bucket_brigade *bb = apr_brigade_create(pRequest->connection->pool, pRequest->connection->bucket_alloc);
    if (!bb) {
        Log::error(42, "Bucket brigade allocation failed");
        return DECLINED;
    }
    while (!extractBrigadeContent(bb, pRequest, info->mBody)){
        apr_brigade_cleanup(bb);
    }
//    Log::warn(1,"Extracted body: %s", info->mBody.c_str());
    apr_brigade_cleanup(bb);
    // Body read :)

    // Copy Request ID in both headers
    std::string reqId = boost::lexical_cast<std::string>(info->mId);
    apr_table_set(pRequest->headers_in, c_UNIQUE_ID, reqId.c_str());
    apr_table_set(pRequest->headers_out, c_UNIQUE_ID, reqId.c_str());

    // Synchronous context enrichment
    info->mConfPath = tConf->dirName;
    info->mArgs = pRequest->args ? pRequest->args : "";
    gProcessor->enrichContext(pRequest, *info);

    // If the request has a previous member
    // It means it has been rewritten
    if (pRequest->prev) {
        RequestInfo *prev = reinterpret_cast<RequestInfo *>(ap_get_module_config(pRequest->prev->request_config,
                                                                                &dup_module));
        if (prev) {
            info->mBody = prev->mBody;
        }
    }
    return DECLINED;
}

/**
 * Reinject the body we saved in earlyHook into a brigade
 */
apr_status_t
inputFilterBody2Brigade(ap_filter_t *pF, apr_bucket_brigade *pB, ap_input_mode_t pMode, apr_read_type_e pBlock, apr_off_t pReadbytes)
{

    request_rec *pRequest = pF->r;
    Log::warn(1,"InputFilterB2B %p", pF);
    if (!pRequest || !pRequest->per_dir_config) {
        apr_bucket *e = apr_bucket_eos_create(pF->c->bucket_alloc);
        assert(e);
        APR_BRIGADE_INSERT_TAIL(pB, e);
    }
    // Retrieve request info from context
    RequestInfo *info = reinterpret_cast<RequestInfo *>(ap_get_module_config(pRequest->request_config, &dup_module));
    if (!info) {
        Log::warn(1,"no info");
        apr_bucket *e = apr_bucket_eos_create(pF->c->bucket_alloc);
        assert(e);
        APR_BRIGADE_INSERT_TAIL(pB, e);
        // TODO HANDLE ERROR
        return APR_SUCCESS;
    }
    if (!pF->ctx) {
        if (!info->mBody.empty()) {
            Log::warn(1,"Reinjecting a body in a brigade: %s",  info->mBody.c_str());
            if (apr_brigade_write(pB, NULL, NULL, info->mBody.c_str(), info->mBody.size()) != APR_SUCCESS ) {
                Log::warn(1,"Failed to write a body in a brigade: %s",  info->mBody.c_str());
            }
        }
        pF->ctx = (void *) -1;
        // CLEANUP?
        return APR_SUCCESS;
    }
    Log::warn(1,"=== SENDING EOS");
    apr_bucket *e = apr_bucket_eos_create(pF->c->bucket_alloc);
    assert(e);
    APR_BRIGADE_INSERT_TAIL(pB, e);
    return APR_SUCCESS;
}

/*
 * Callback to iterate over the headers tables
 * Pushes a copy of key => value in a list
 */
static int iterateOverHeadersCallBack(void *d, const char *key, const char *value) {
    RequestInfo::tHeaders *headers = reinterpret_cast<RequestInfo::tHeaders *>(d);
    headers->push_back(std::pair<std::string, std::string>(key, value));
    return 1;
}

static void
prepareRequestInfo(DupConf *tConf, request_rec *pRequest, RequestInfo &r, bool withAnswer) {
    // Basic
    r.mPoison = false;
    r.mConfPath = tConf->dirName;
    r.mPath = pRequest->uri;
    r.mArgs = pRequest->args ? pRequest->args : "";

    // Copy headers in
    apr_table_do(&iterateOverHeadersCallBack, &r.mHeadersIn, pRequest->headers_in, NULL);
    if (withAnswer) {
        // Copy headers out
        apr_table_do(&iterateOverHeadersCallBack, &r.mHeadersOut, pRequest->headers_out, NULL);
    }
}

static void
printRequest(request_rec *pRequest, RequestInfo *pBH, DupConf *tConf) {
    const char *reqId = apr_table_get(pRequest->headers_in, c_UNIQUE_ID);
    Log::debug("### Pushing a request with ID: %s, body size:%ld", reqId, pBH->mBody.size());
    Log::debug("### Uri:%s, dir name:%s", pRequest->uri, tConf->dirName);
    Log::debug("### Request args: %s", pRequest->args);
}

/*
 * Request context used during brigade run
 */
class RequestContext {
public:
    apr_bucket_brigade  *mTmpBB;
    RequestInfo         *mReq; /** Req is pushed to requestprocessor for duplication and will be deleted later*/

    RequestContext(ap_filter_t *pFilter) {
        mTmpBB = apr_brigade_create(pFilter->r->pool, pFilter->c->bucket_alloc);
        mReq = reinterpret_cast<RequestInfo *>(ap_get_module_config(pFilter->r->request_config,
                                                                    &dup_module));
        assert(mReq);

        // A previous request exists. It never be pushed, therefore we clean the requestinfo struct
        if (pFilter->r->prev) {
            RequestInfo *info = reinterpret_cast<RequestInfo *>(ap_get_module_config(pFilter->r->prev->request_config,
                                                                    &dup_module));
            if (info) {
                delete info;
                ap_set_module_config(pFilter->r->prev->request_config, &dup_module, NULL);
            }
        }
    }

    RequestContext()
        : mTmpBB(NULL)
        , mReq(NULL) {
    }

    ~RequestContext() {
        if (mTmpBB) {
            apr_brigade_cleanup(mTmpBB);
        }
    }
};

/**
 * Here we can get the response body and finally duplicate or not
 */
apr_status_t
outputFilterHandler(ap_filter_t *pFilter, apr_bucket_brigade *pBrigade) {
    request_rec *pRequest = pFilter->r;
    Log::warn(1,"OutputFilterHandler in directory %s %p", pRequest->uri, pRequest);
    if (!pRequest || !pRequest->per_dir_config)
        return ap_pass_brigade(pFilter->next, pBrigade);

    struct DupConf *tConf = reinterpret_cast<DupConf *>(ap_get_module_config(pRequest->per_dir_config, &dup_module));
    if ( (! tConf) || (! tConf->dirName) ) {

         return ap_pass_brigade(pFilter->next, pBrigade);
    }
    // Log::warn(1,"doing outputFilterHandler in directory %s %p", pRequest->uri, pRequest);

    if ( tConf->getHighestDuplicationType() == DuplicationType::NONE ) {
        // Log::warn(1,"exiting outputFilterHandler because of dup type NONE in directory %s %p", pRequest->uri, pRequest);
        return ap_pass_brigade(pFilter->next, pBrigade);
    }

    // Request answer analyse
    RequestContext *ctx = static_cast<RequestContext *>(pFilter->ctx);
    if (ctx == NULL) {
        // Context init
        ctx = new RequestContext(pFilter);
        if (ctx->mReq->mAnswer.size()) {
            // Happens after a rewrite
            pFilter->ctx = (void *) -1;
            return ap_pass_brigade(pFilter->next, pBrigade);
        }
        pFilter->ctx = ctx;
    } else if (ctx == (void *) -1) {
        return ap_pass_brigade(pFilter->next, pBrigade);
    }

    // We need to get the highest one as we haven't matched which rule it is yet
    if (tConf->getHighestDuplicationType() != DuplicationType::REQUEST_WITH_ANSWER) {
        // Asynchronous push of request WITHOUT the answer
        RequestInfo *rH = ctx->mReq;
        prepareRequestInfo(tConf, pRequest, *rH, false);
        printRequest(pRequest, rH, tConf);
        gThreadPool->push(rH);
        delete ctx;
        pFilter->ctx = (void *) -1;
        return ap_pass_brigade(pFilter->next, pBrigade);
    }
    // Asynchronous push of request WITH the answer
    apr_bucket *currentBucket;
    while ((currentBucket = APR_BRIGADE_FIRST(pBrigade)) != APR_BRIGADE_SENTINEL(pBrigade)) {
        const char *data;
        apr_size_t len;
        apr_status_t rv;
        rv = apr_bucket_read(currentBucket, &data, &len, APR_BLOCK_READ);

        if ((rv == APR_SUCCESS) && (data != NULL)) {
            ctx->mReq->mAnswer.append(data, len);
        }
        /* Remove bucket e from bb. */
        APR_BUCKET_REMOVE(currentBucket);
        /* Insert it into  temporary brigade. */
        APR_BRIGADE_INSERT_HEAD(ctx->mTmpBB, currentBucket);
        /* Pass brigade downstream. */
        rv = ap_pass_brigade(pFilter->next, ctx->mTmpBB);
        if (rv != APR_SUCCESS) {
            // Something went wrong, no duplication performed
            delete ctx;
            pFilter->ctx = (void *) -1;
            return rv;
        }
        if (APR_BUCKET_IS_EOS(currentBucket)) {
            // Pushing the answer to the processor
            prepareRequestInfo(tConf, pRequest, *(ctx->mReq), true);
            printRequest(pRequest, ctx->mReq, tConf);
            gThreadPool->push(ctx->mReq);
            delete ctx;
            pFilter->ctx = (void *) -1;
        }
        else {
            apr_brigade_cleanup(ctx->mTmpBB);
        }
    }
    return OK;
}

};
<commit_msg>Cleaned filters<commit_after>/*
* mod_dup - duplicates apache requests
*
* Copyright (C) 2013 Orange
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
*     http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/

#include "mod_dup.hh"

#include <http_config.h>

namespace DupModule {

const unsigned int max_bytes = 8192;

static bool
extractBrigadeContent(apr_bucket_brigade *bb, request_rec *pRequest, std::string &content) {
    if (ap_get_brigade(pRequest->input_filters,
                       bb, AP_MODE_READBYTES, APR_BLOCK_READ, max_bytes) == APR_SUCCESS) {
        // Read brigade content
        for (apr_bucket *b = APR_BRIGADE_FIRST(bb);
             b != APR_BRIGADE_SENTINEL(bb);
             b = APR_BUCKET_NEXT(b) ) {
            // Metadata end of stream
            if (APR_BUCKET_IS_EOS(b)) {
                return true;
            }
            const char *data = 0;
            apr_size_t len = 0;
            apr_status_t rv = apr_bucket_read(b, &data, &len, APR_BLOCK_READ);
            if (rv != APR_SUCCESS) {
                Log::error(42, "Bucket read failed, skipping the rest of the body");
                return true;
            }
            if (len) {
                content.append(data, len);
            }
        }
    }
    else {
        Log::error(42, "Get brigade failed, skipping the rest of the body");
        return true;
    }
    return false;
}

int
earlyHook(request_rec *pRequest) {
    struct DupConf *tConf = reinterpret_cast<DupConf *>(ap_get_module_config(pRequest->per_dir_config, &dup_module));
    assert(tConf);
    if (!tConf->dirName) {
        return DECLINED;
    }
    RequestInfo *info = new RequestInfo(tConf->getNextReqId());
    // Backup in request context
    ap_set_module_config(pRequest->request_config, &dup_module, (void *)info);

    if (!pRequest->connection->pool) {
        Log::error(42, "No connection pool associated to the request");
        return DECLINED;
    }
    if (!pRequest->connection->bucket_alloc) {
        pRequest->connection->bucket_alloc = apr_bucket_alloc_create(pRequest->connection->pool);
        if (!pRequest->connection->bucket_alloc) {
            Log::error(42, "Request bucket allocation failed");
            return DECLINED;
        }
    }

    apr_bucket_brigade *bb = apr_brigade_create(pRequest->connection->pool, pRequest->connection->bucket_alloc);
    if (!bb) {
        Log::error(42, "Bucket brigade allocation failed");
        return DECLINED;
    }
    while (!extractBrigadeContent(bb, pRequest, info->mBody)){
        apr_brigade_cleanup(bb);
    }
    apr_brigade_cleanup(bb);
    // Body read :)

    // Copy Request ID in both headers
    std::string reqId = boost::lexical_cast<std::string>(info->mId);
    apr_table_set(pRequest->headers_in, c_UNIQUE_ID, reqId.c_str());
    apr_table_set(pRequest->headers_out, c_UNIQUE_ID, reqId.c_str());

    // Synchronous context enrichment
    info->mConfPath = tConf->dirName;
    info->mArgs = pRequest->args ? pRequest->args : "";
    gProcessor->enrichContext(pRequest, *info);

    // If the request has a previous member
    // It means it has been rewritten
    if (pRequest->prev) {
        RequestInfo *prev = reinterpret_cast<RequestInfo *>(ap_get_module_config(pRequest->prev->request_config,
                                                                                &dup_module));
        if (prev) {
            info->mBody = prev->mBody;
        }
    }
    return DECLINED;
}

/**
 * Reinject the body we saved in earlyHook into a brigade
 */
apr_status_t
inputFilterBody2Brigade(ap_filter_t *pF, apr_bucket_brigade *pB, ap_input_mode_t pMode, apr_read_type_e pBlock, apr_off_t pReadbytes)
{

    request_rec *pRequest = pF->r;
    if (!pRequest || !pRequest->per_dir_config) {
        apr_bucket *e = apr_bucket_eos_create(pF->c->bucket_alloc);
        assert(e);
        APR_BRIGADE_INSERT_TAIL(pB, e);
        return APR_SUCCESS;
    }
    // Retrieve request info from context
    RequestInfo *info = reinterpret_cast<RequestInfo *>(ap_get_module_config(pRequest->request_config, &dup_module));
    if (!info) {
        // Should not happen
        apr_bucket *e = apr_bucket_eos_create(pF->c->bucket_alloc);
        assert(e);
        APR_BRIGADE_INSERT_TAIL(pB, e);
        return APR_SUCCESS;
    }
    if (!pF->ctx) {
        if (!info->mBody.empty()) {
            apr_status_t st;
            if ((st = apr_brigade_write(pB, NULL, NULL, info->mBody.c_str(), info->mBody.size())) != APR_SUCCESS ) {
                Log::warn(1, "Failed to write request body in a brigade: %s",  info->mBody.c_str());
                return st;
            }
        }
        // Marks the body as sent
        pF->ctx = (void *) -1;
    }
    // Marks that we do not have any more data to read
    apr_bucket *e = apr_bucket_eos_create(pF->c->bucket_alloc);
    assert(e);
    APR_BRIGADE_INSERT_TAIL(pB, e);
    return APR_SUCCESS;
}

/*
 * Callback to iterate over the headers tables
 * Pushes a copy of key => value in a list
 */
static int iterateOverHeadersCallBack(void *d, const char *key, const char *value) {
    RequestInfo::tHeaders *headers = reinterpret_cast<RequestInfo::tHeaders *>(d);
    headers->push_back(std::pair<std::string, std::string>(key, value));
    return 1;
}

static void
prepareRequestInfo(DupConf *tConf, request_rec *pRequest, RequestInfo &r, bool withAnswer) {
    // Basic
    r.mPoison = false;
    r.mConfPath = tConf->dirName;
    r.mPath = pRequest->uri;
    r.mArgs = pRequest->args ? pRequest->args : "";

    // Copy headers in
    apr_table_do(&iterateOverHeadersCallBack, &r.mHeadersIn, pRequest->headers_in, NULL);
    if (withAnswer) {
        // Copy headers out
        apr_table_do(&iterateOverHeadersCallBack, &r.mHeadersOut, pRequest->headers_out, NULL);
    }
}

static void
printRequest(request_rec *pRequest, RequestInfo *pBH, DupConf *tConf) {
    const char *reqId = apr_table_get(pRequest->headers_in, c_UNIQUE_ID);
    Log::debug("### Pushing a request with ID: %s, body size:%ld", reqId, pBH->mBody.size());
    Log::debug("### Uri:%s, dir name:%s", pRequest->uri, tConf->dirName);
    Log::debug("### Request args: %s", pRequest->args);
}

/*
 * Request context used during brigade run
 */
class RequestContext {
public:
    apr_bucket_brigade  *mTmpBB;
    RequestInfo         *mReq; /** Req is pushed to requestprocessor for duplication and will be deleted later*/

    RequestContext(ap_filter_t *pFilter) {
        mTmpBB = apr_brigade_create(pFilter->r->pool, pFilter->c->bucket_alloc);
        mReq = reinterpret_cast<RequestInfo *>(ap_get_module_config(pFilter->r->request_config,
                                                                    &dup_module));
    }

    RequestContext()
        : mTmpBB(NULL)
        , mReq(NULL) {
    }

    ~RequestContext() {
        if (mTmpBB) {
            apr_brigade_cleanup(mTmpBB);
        }
    }
};

/**
 * Here we can get the response body and finally duplicate or not
 */
apr_status_t
outputFilterHandler(ap_filter_t *pFilter, apr_bucket_brigade *pBrigade) {
    request_rec *pRequest = pFilter->r;
    // Reject requests that do not meet our requirements
    if (!pRequest || !pRequest->per_dir_config)
        return ap_pass_brigade(pFilter->next, pBrigade);

    struct DupConf *tConf = reinterpret_cast<DupConf *>(ap_get_module_config(pRequest->per_dir_config, &dup_module));
    if ((!tConf) || (!tConf->dirName) || (tConf->getHighestDuplicationType() == DuplicationType::NONE)) {
         return ap_pass_brigade(pFilter->next, pBrigade);
    }

    // Request answer analyse
    RequestContext *ctx = static_cast<RequestContext *>(pFilter->ctx);
    if (ctx == NULL) {
        // First pass in the output filter => context init
        ctx = new RequestContext(pFilter);
        pFilter->ctx = ctx;
    } else if (ctx == (void *) -1) {
        // Output filter already did his job, we return
        return ap_pass_brigade(pFilter->next, pBrigade);
    }

    // We need to get the highest one as we haven't matched which rule it is yet
    if (tConf->getHighestDuplicationType() != DuplicationType::REQUEST_WITH_ANSWER) {
        // Asynchronous push of request WITHOUT the answer
        RequestInfo *rH = ctx->mReq;
        prepareRequestInfo(tConf, pRequest, *rH, false);
        printRequest(pRequest, rH, tConf);
        gThreadPool->push(rH);
        delete ctx;
        pFilter->ctx = (void *) -1;
        return ap_pass_brigade(pFilter->next, pBrigade);
    }
    // Asynchronous push of request WITH the answer
    apr_bucket *currentBucket;
    while ((currentBucket = APR_BRIGADE_FIRST(pBrigade)) != APR_BRIGADE_SENTINEL(pBrigade)) {
        const char *data;
        apr_size_t len;
        apr_status_t rv;
        rv = apr_bucket_read(currentBucket, &data, &len, APR_BLOCK_READ);

        if ((rv == APR_SUCCESS) && (data != NULL)) {
            ctx->mReq->mAnswer.append(data, len);
        }
        /* Remove bucket e from bb. */
        APR_BUCKET_REMOVE(currentBucket);
        /* Insert it into  temporary brigade. */
        APR_BRIGADE_INSERT_HEAD(ctx->mTmpBB, currentBucket);
        /* Pass brigade downstream. */
        rv = ap_pass_brigade(pFilter->next, ctx->mTmpBB);
        if (rv != APR_SUCCESS) {
            // Something went wrong, no duplication performed
            delete ctx;
            pFilter->ctx = (void *) -1;
            return rv;
        }
        if (APR_BUCKET_IS_EOS(currentBucket)) {
            // Pushing the answer to the processor
            prepareRequestInfo(tConf, pRequest, *(ctx->mReq), true);
            printRequest(pRequest, ctx->mReq, tConf);
            gThreadPool->push(ctx->mReq);
            delete ctx;
            pFilter->ctx = (void *) -1;
        }
        else {
            apr_brigade_cleanup(ctx->mTmpBB);
        }
    }
    return OK;
}

};
<|endoftext|>