#ifndef CITY_MAP_HPP
#define CITY_MAP_HPP

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string>
#include <vector>
#include <boost/filesystem.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/iostreams/device/mapped_file.hpp>

class CityMap
{
public:
  struct Location {
    Location() {}
    Location(double la, double lo): lat(la), lon(lo) {}
    double lat, lon;

    double & operator[](int i) { return ((double*)this)[i]; }
    const double & operator[](int i) const { return ((double*)this)[i]; }
  };
  typedef std::pair<int,int> IntPair;
  typedef std::vector<int> IntVec;
  typedef std::vector<float> FloatVec;
  typedef boost::unordered_set<int> IntSet;
  typedef boost::unordered_map<int,int> IntMap;
  typedef boost::unordered_map<int,IntMap> Int2Map;
  typedef IntVec Path;
  typedef IntPair Street;
  typedef boost::unordered_map<CityMap::Street, int> StreetMap;
  typedef boost::associative_property_map<IntMap> IntMapProperty;

public:
  CityMap(): GRID_DEGREE(0.001) {}

  CityMap(const char * filename, const char *pathFilename=0): GRID_DEGREE(0.001)
  {
    this->loadFromFile(filename);
    if (pathFilename)
      this->loadPaths(pathFilename);
  }

  inline void setIntersection(int idx, const Location &L)
  {
    this->intersections[idx] = L;
    if (L.lat<this->bounds[0].lat) this->bounds[0].lat = L.lat;
    if (L.lat>this->bounds[1].lat) this->bounds[1].lat = L.lat;
    if (L.lon<this->bounds[0].lon) this->bounds[0].lon = L.lon;
    if (L.lon>this->bounds[1].lon) this->bounds[1].lon = L.lon;
  }

  inline void setStreet(int idx, const Street &S, float weight)
  {
    this->streets[idx] = S;
    this->streetId[S] = idx;
    this->streetProps[idx] = EdgeProperty(weight, idx);
  }

  inline void initCity(int nV, int nE)
  {
    this->intersections.resize(nV);
    this->streets.resize(nE);
    this->streetId.rehash(nE/ceil(this->streetId.max_load_factor()));
    // this->streetId.reserve(nE);
    this->streetProps.resize(nE);
    this->bounds[0] = Location(1e36, 1e36);
    this->bounds[1] = Location(-1e36, -1e36);
  }

  void loadFromText(const char *filename)
  {
    FILE *fi = fopen(filename, "r");
    int nV, nE;
    bool hasWeight;
    fscanf(fi, "%d %d", &nV, &nE);
    hasWeight = nE<0;
    if (nE<0)
      nE = -nE;
    this->initCity(nV, nE);
    for (int i=0; i<nV; i++) {
      Location L;
      fscanf(fi, "%lf %lf", &L.lat, &L.lon);
      this->setIntersection(i, L);
    }
    char line[100];
    fgets(line, sizeof(line), fi);
    for (int i=0; i<nE; i++) {
      int idx[2];
      float weight;
      fgets(line, sizeof(line), fi);
      if (sscanf(line, "%d %d %f", idx, idx+1, &weight)<3)
        weight = CityMap::distance(this->intersections[idx[0]], this->intersections[idx[1]]);
      this->setStreet(i, Street(idx[0], idx[1]), weight);
    }
    fclose(fi);
  }

  void loadFromBinary(const char *filename)
  {
    FILE *fi = fopen(filename, "rb");
    int nV, nE;
    bool hasWeight;
    fread(&nV, sizeof(nV), 1, fi);
    fread(&nE, sizeof(nE), 1, fi);
    hasWeight = nE<0;
    if (nE<0)
      nE = -nE;
    this->initCity(nV, nE);
    for (int i=0; i<nV; i++) {
      Location L;
      fread(&L, sizeof(double), 2, fi);
      this->setIntersection(i, L);
    }
    for (int i=0; i<nE; i++) {
      int idx[2];
      float weight;
      fread(idx, sizeof(int), 2, fi);
      if (hasWeight)
        fread(&weight, sizeof(float), 1, fi);
      else
        weight = CityMap::distance(this->intersections[idx[0]], this->intersections[idx[1]]);
      this->setStreet(i, Street(idx[0], idx[1]), weight);
    }
    fclose(fi);
  }

#pragma pack(push,1)
  struct OSRMNode
  {
    int lat;
    int lon;
    int id;
    uint8_t bollard;
    uint8_t traffic;
    uint8_t unused[2];
  };

  struct OSRMEdge
  {
    uint32_t src;
    uint32_t dst;
    int distance;
    uint16_t oneway;
    int weight;
    uint16_t edgeType;
    int nameIndex;
    uint8_t roundabout;
    uint8_t ignoreInGrid;
    uint8_t accessRestricted;
  };
#pragma pack(pop)

  void loadFromOSRM(const char *filename)
  {
    FILE *fi = fopen(filename, "rb");
    int nV, nE, cnt;
    std::vector<OSRMNode> nodes;
    std::vector<OSRMEdge> edges;

    boost::unordered_map<int,int> nodeMap;
    fread(&nV, sizeof(int), 1, fi);
    nodes.resize(nV);
    fread(&nodes[0], sizeof(OSRMNode), nV, fi);
    for (int i=0; i<nV; i++)
      nodeMap[nodes[i].id] = i;

    fread(&nE, sizeof(int), 1, fi);
    edges.resize(nE);
    nE = fread(&edges[0], sizeof(OSRMEdge), nE, fi);
    fclose(fi);
    cnt = 0;
    for (int i=0; i<nE; i++)
      cnt += !edges[i].oneway;
    this->initCity(nV, cnt+nE);
    for (int i=0; i<nV; i++)
      this->setIntersection(i, Location(nodes[i].lat*1e-5, nodes[i].lon*1e-5));
    for (int i=0, cnt=0; i<nE; i++) {
      this->setStreet(cnt++, Street(nodeMap[edges[i].src], nodeMap[edges[i].dst]), edges[i].weight/10.0);
      if (!edges[i].oneway)
        this->setStreet(cnt++, Street(nodeMap[edges[i].dst], nodeMap[edges[i].src]), edges[i].weight/10.0);
    }
  }

  void loadFromFile(const char *filename)
  {
    std::string ext = boost::filesystem::extension(filename);
    if (ext==".txt")
      this->loadFromText(filename);
    else if (ext==".osrm")
      this->loadFromOSRM(filename);
    else
      this->loadFromBinary(filename);

    this->city = Graph(this->streets.begin(), this->streets.end(), this->streetProps.begin(), this->intersections.size());

    this->center.lat = (this->bounds[0].lat+this->bounds[1].lat)*0.5;
    this->center.lon = (this->bounds[0].lon+this->bounds[1].lon)*0.5;
    double dLat = CityMap::distance(Location(this->center.lat-0.5, this->center.lon),
                                    Location(this->center.lat+0.5, this->center.lon));
    double dLon = CityMap::distance(Location(this->center.lat, this->center.lon-0.5),
                                    Location(this->center.lat, this->center.lon+0.5));
    this->ratioLatLon = dLat/dLon;

    this->buildIndex();
    this->pathIndex.assign(this->numIntersections(), -1);
  }

  void loadPaths(const char *pathFilename)
  {
    this->pathIndex.assign(this->numIntersections(), -1);
    try {
      this->pathFile.open(pathFilename);
    } catch (boost::exception &) {
      return;
    }
    const int *data = reinterpret_cast<const int*>(this->pathFile.data());
    int srcCount = data[0];
    const int *srcIndex = data+1;
    if (srcCount>0)
    this->pathData = data + 1 + srcCount*2;
    this->paths.clear();
    for (int i=0; i<srcCount; i++) {
      if (srcIndex[i*2]<static_cast<int>(this->numIntersections()))
        this->pathIndex[srcIndex[i*2]] = srcIndex[i*2+1];
    }
  }

  inline bool buildPathTree(int src)
  {
    int index = this->pathIndex[src];
    if (index==-1)
      return false;
    Int2Map::iterator i2mi = this->paths.find(src);
    if (i2mi!=this->paths.end())
      return true;

    const int *data = this->pathData + index+2;
    int size = this->pathData[index+1];
    IntMap &prev = this->paths[src];
    prev.rehash(ceil(size/prev.max_load_factor())/prev.max_load_factor());
    // prev.reserve(size/prev.max_load_factor());
    for (int i=0; i<size; i++)
      prev[data[i*2+1]] = data[i*2];
    return true;
  }

  inline void getBounds(double rect[4]) const
  {
    rect[0] = this->bounds[0].lat;
    rect[1] = this->bounds[0].lon;
    rect[2] = this->bounds[1].lat;
    rect[3] = this->bounds[1].lon;
  }

  inline size_t numIntersections() const
  {
    return this->intersections.size();
  }

  inline const Location & getIntersection(int idx) const
  {
    return this->intersections[idx];
  }

  inline size_t numStreets() const
  {
    return this->streets.size();
  }

  inline const Street & getStreet(int idx) const
  {
    return this->streets[idx];
  }

  inline float getStreetWeight(const Street &S) const
  {
    StreetMap::const_iterator it = this->streetId.find(S);
    if (it!=this->streetId.end())
      return this->streetProps[(*it).second].weight;
    return 0;
  }

  int mapToIntersection(const Location &loc) const
  {
    int X = this->gridX(loc.lat);
    int Y = this->gridY(loc.lon);
    if (X<0 || X>=this->gridSize[0] || Y<0 || Y>=this->gridSize[1])
      return -1;
    int minInter = -1;
    double len = 1e30;
    for (int dy=-1; dy<=1; dy++) {
      for (int dx=-1; dx<=1; dx++) {
        if (X+dx>=0 && X+dx<this->gridSize[0] &&
            Y+dy>=0 && Y+dy<this->gridSize[1]) {
          int gid = (Y+dy)*this->gridSize[0]+(X+dx);
          for (int i=0; i<this->gridCount[gid]; i++) {
            int inter = this->gridIntersections[this->gridIndex[gid]+i];
            double dis = CityMap::pseudo_distance(loc, this->intersections[inter], this->ratioLatLon);
            if (minInter<0 || dis<len) {
              minInter = inter;
              len = dis;
            }
          }
        }
      }
    }
    if (minInter==-1) {
      int idx = this->nearestIntersection(loc);
      len = CityMap::distance(loc, this->intersections[idx]);
      if (len<0.35)
        minInter = idx;
    }
    return minInter;
  }

  int nearestIntersection(const Location &p) const
  {
    int minInter = -1;
    double minDis = 1e36;
    this->searchKdNearestIntersection(p, 0, 0, minInter, minDis);
    return minInter;
  }

  inline int shortestPath(int src, int dst, int *prev, float *dist) const
  {
    CityMap::dijkstra_exit_on_dst visitor(dst);
    try {
      boost::dijkstra_shortest_paths(this->city, src,
                                     boost::predecessor_map(prev)
                                     .distance_map(dist)
                                     .weight_map(boost::get(&EdgeProperty::weight, this->city))
                                     .visitor(visitor)
                                     );
    } catch (const CityMap::dijkstra_exit_exception) {
      return 1;
    }
    return 0;
  }

  inline int shortestPaths(int src, IntSet &dst, int *prev, float *dist) const
  {
    CityMap::dijkstra_exit_on_dst_set visitor(&dst);
    int dstCount = dst.size();
    try {
      boost::dijkstra_shortest_paths(this->city, src,
                                     boost::predecessor_map(prev)
                                     .distance_map(dist)
                                     .weight_map(boost::get(&EdgeProperty::weight, this->city))
                                     .visitor(visitor)
                                     );
    } catch (const CityMap::dijkstra_exit_exception) {
    }
    return dstCount-dst.size();
  }

  inline bool queryShortestPath(int src, int dst, Path &path)
  {
    if (!this->buildPathTree(src)) return false;
    path.clear();
    const IntMap &prev = this->paths[src];
    while (dst!=src) {
      IntMap::const_iterator it = prev.find(dst);
      if (it==prev.end()) return false;
      path.push_back(dst);
      dst = (*it).second;
    }
    path.push_back(dst);
    return true;
  }

  inline bool computeShortestPath(int src, int dst, Path &path)
  {
    IntVec prev(this->numIntersections());
    FloatVec dist(this->numIntersections());
    int numFound = this->shortestPath(src, dst, &prev[0], &dist[0]);
    if (numFound) {
      path.clear();
      while (prev[dst]!=dst) {
        path.push_back(dst);
        dst = prev[dst];
      }
      return true;
    }
    return false;
  }

  inline bool findShortestPath(int src, int dst, Path &path)
  {
    return (this->queryShortestPath(src, dst, path) || this->computeShortestPath(src, dst, path));
  }

  inline static double pseudo_distance(const Location &src, const Location &dst, double ratio=1.0)
  {
    const double d0 = src.lat-dst.lat;
    const double d1 = (src.lon-dst.lon)*ratio;
    return d0*d0+d1*d1;
  }

  inline static double distance(const Location &src, const Location &dst)
  {
    const double lat1 = src.lat*M_PI/180.0;
    const double lat2 = dst.lat*M_PI/180.0;
    const double dLat = sin((lat2-lat1)/2);
    const double dLon = sin((dst.lon-src.lon)*M_PI/180.0/2);
    const double a = dLat*dLat + dLon*dLon*cos(lat1)*cos(lat2);
    return 2*3961*atan2(sqrt(a), sqrt(1-a));
  }

protected:
  struct EdgeProperty {
    EdgeProperty() {}
    EdgeProperty(float l, int idx): weight(l), index(idx) {}
    float weight;
    int   index;
  };
  typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property, EdgeProperty> Graph;
  class dijkstra_exit_exception {};
  class dijkstra_exit_on_dst : public boost::dijkstra_visitor<>
  {
  public:
    dijkstra_exit_on_dst(int d): dst(d) {}
    template<class VType, class GType>
    void finish_vertex(VType v, GType &/*g*/)
    {
      if (static_cast<int>(v)==dst) throw dijkstra_exit_exception();
    }
  private:
    int dst;
  };
  class dijkstra_exit_on_dst_set : public boost::dijkstra_visitor<>
  {
  public:
    dijkstra_exit_on_dst_set(IntSet *d): dst(d) {}
    template<class VType, class GType>
    void finish_vertex(VType v, GType &/*g*/)
    {
      IntSet::iterator it = this->dst->find(v);
      if (it!=this->dst->end()) {
        this->dst->erase(it);
        if (this->dst->size()==0)
          throw dijkstra_exit_exception();
      }
    }
  private:
    IntSet *dst;
  };

  void buildIndex()
  {
    this->gridSize[0] = (int)(ceil((this->bounds[1].lat-this->bounds[0].lat)/GRID_DEGREE));
    this->gridSize[1] = (int)(ceil((this->bounds[1].lon-this->bounds[0].lon)/GRID_DEGREE));
    int N = this->gridSize[0]*this->gridSize[1];
    this->gridIndex.resize(N, 0);
    this->gridCount.resize(N, 0);
    this->gridIntersections.resize(this->intersections.size());

    for (unsigned i=0; i<this->intersections.size(); i++) {
      int gid = this->gridLocation(this->intersections[i]);
      this->gridIndex[gid]++;
    }

    int last = 0;
    for (int i=0; i<N; i++) {
      int tmp = this->gridIndex[i];
      this->gridIndex[i] = last;
      last += tmp;
    }

    for (unsigned i=0; i<this->intersections.size(); i++) {
      int gid = this->gridLocation(this->intersections[i]);
      int index = this->gridCount[gid]++;
      this->gridIntersections[this->gridIndex[gid]+index] = i;
    }

    this->createKdTree();
  }

  inline int gridX(double lat) const
  {
    return (int)((lat-this->bounds[0].lat)/(this->bounds[1].lat-this->bounds[0].lat)*(this->gridSize[0]-1)+0.49);
  }

  inline int gridY(double lon) const
  {
    return (int)((lon-this->bounds[0].lon)/(this->bounds[1].lon-this->bounds[0].lon)*(this->gridSize[1]-1)+0.49);
  }

  inline int gridLocation(const Location &loc) const
  {
    return this->gridY(loc.lon)*this->gridSize[0]+this->gridX(loc.lat);
  }

  typedef struct
  {
    double median;
    int   index;
  } KdNode;

  void buildKdTree(double *tmp, int *indices, int n, int depth, int thisNode, int &freeNode) {
    KdNode *node = &this->kdNodes[thisNode];
    if (n<2) {
      node->index = -indices[0];
      return;
    }
    int keyIndex = depth%2;
    int medianIndex = n/2-1;
    for (int i=0; i<n; i++)
      tmp[i] = this->intersections[indices[i]][keyIndex];
    std::nth_element(tmp, tmp+medianIndex, tmp+n);
    node->median = tmp[medianIndex];
    int l = 0;
    int r = n-1;
    while (l<r) {
      while (l<n && this->intersections[indices[l]][keyIndex]<node->median) l++;
      while (r>l && this->intersections[indices[r]][keyIndex]>=node->median) r--;
      if (l<r)
        std::swap(indices[l], indices[r]);
    }
    medianIndex = r;

    int child_index = freeNode;
    node->index = child_index;
    freeNode += 2;
    this->kdNodes.resize(freeNode);
    buildKdTree(tmp, indices, medianIndex+1, depth+1, child_index, freeNode);
    if (medianIndex<n-1)
      buildKdTree(tmp, indices + medianIndex+1, n-medianIndex-1, depth+1, child_index+1, freeNode);
    else
      this->kdNodes[child_index+1].index = INT_MIN;
  }

  void createKdTree() {
    std::vector<double> tmp(this->numIntersections());
    std::vector<int> indices(this->numIntersections());
    for (unsigned i=0; i<indices.size(); i++)
      indices[i] = i;

    int freeNode = 1;
    this->kdNodes.resize(freeNode);
    buildKdTree(&tmp[0], &indices[0], this->numIntersections(), 0, 0, freeNode);
  }

  void searchKdNearestIntersection(const Location &p, int root, int depth, int &minInter, double &minDis) const
  {
    const KdNode *node = &this->kdNodes[root];
    if (node->index<=0) {
      if (node->index==INT_MIN) return;
      int inter = -node->index;
      double dis = CityMap::pseudo_distance(p, this->intersections[inter], this->ratioLatLon);
      if (dis<minDis) {
        minDis = dis;
        minInter = inter;
      }
      return;
    }
    double v = ((const double*)(&p))[depth%2];
    if (v-this->GRID_DEGREE<=node->median)
      searchKdNearestIntersection(p, node->index, depth+1, minInter, minDis);
    if (v+this->GRID_DEGREE>node->median) {
      searchKdNearestIntersection(p, node->index+1, depth+1, minInter, minDis);
    }
  }

private:
  const double              GRID_DEGREE;
  double                    ratioLatLon;
  Location                  bounds[2];
  Location                  center;
  int                       gridSize[2];
  std::vector<int>          gridIndex;
  std::vector<int>          gridCount;
  std::vector<int>          gridIntersections;

  std::vector<Location>     intersections;
  std::vector<Street>       streets;
  std::vector<EdgeProperty> streetProps;
  Graph                     city;
  StreetMap                 streetId;
  std::vector<KdNode>       kdNodes;

  boost::iostreams::mapped_file_source pathFile;
  IntVec                    pathIndex;
  const int *               pathData;
  Int2Map                   paths;
};

#endif


/*
  Copyright (c) 2012, Matthew H. Reilly (kb1vc)
  All rights reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions are
  met:

  Redistributions of source code must retain the above copyright
  notice, this list of conditions and the following disclaimer.
  Redistributions in binary form must reproduce the above copyright
  notice, this list of conditions and the following disclaimer in
  the documentation and/or other materials provided with the
  distribution.

  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
  HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include "OSFilter.hxx"

#include <iostream>
#include <string.h>
#include <fftw3.h>
#include <math.h>
#include <boost/format.hpp>

static unsigned int ipow(unsigned int x, unsigned int y)
{
  unsigned int ret;
  ret = 1;
  unsigned int i;

  for(i = 0; i < y; i++) {
    ret *= x;
  }

  return ret;
}

SoDa::OSFilter::OSFilter(float * filter_impulse_response,
			 unsigned int filter_length,
			 float filter_gain,
			 unsigned int inout_buffer_length,
			 OSFilter * cascade,
			 unsigned int suggested_transform_length)
{
  // these are the salient dimensions for this Overlap/Save
  // widget (for terminology, see Lyons pages 719ff
  Q = filter_length; // to start with.
  M = inout_buffer_length;

  if((cascade != NULL) && (cascade->M != M)) cascade = NULL;

  if(M < 4 * Q) {
    std::cerr << "Warning -- OSFilter asked to implement a long filter against a short buffer." << std::endl;
  }

  // now find N.
  if(suggested_transform_length > (M + Q - 1)) {
    N = suggested_transform_length;
  }
  else {
    N = guessN();
  }

  // now that we have N, we can back-calculate Q.
  Q = N - M;

  tail_index = M - (Q - 1);

  // now build the transform image for the filter.
  std::complex<float> * filter_in = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * N);
  filter_fft = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * N);

  // create a temporary plan
  fftwf_plan tplan = fftwf_plan_dft_1d(N, (fftwf_complex*) filter_in, (fftwf_complex*) filter_fft,
				       FFTW_FORWARD, FFTW_ESTIMATE);

  // now build the filter
  // fill with zeros
  unsigned int i;
  for(i = 0; i < N; i++) filter_in[i] = std::complex<float>(0.0,0.0);
  // fill in the impulse response
  float gain_corr = 1.0 / ((float) N) * filter_gain;
  for(i = 0; i < filter_length; i++) filter_in[i] = std::complex<float>(filter_impulse_response[i] * gain_corr, 0.0);

  // transform the filter.
  fftwf_execute(tplan);

  // and forget the plan.
  fftwf_destroy_plan(tplan);

  // filter lengths must be equal too
  if((cascade != NULL) && (cascade->N != N)) cascade = NULL;

  // if there is a cascaded filter, multiply our filter coeffs by the cascaded filter coeffs
  if(cascade != NULL) {
    for(i = 0; i < N; i++) {
      std::complex<double> a = filter_fft[i];
      std::complex<double> b = cascade->filter_fft[i];
      std::complex<double> ff = a * b * ((double) N);
      filter_fft[i] = std::complex<float>(ff.real(), ff.imag());
    }
  }

  // setup the fft buffers
  setupFFT();
}

SoDa::OSFilter::OSFilter(float low_cutoff,
			 float low_pass_edge,
			 float high_pass_edge,
			 float high_cutoff,

			 unsigned int filter_length,
			 float filter_gain,
			 float sample_rate,

			 unsigned int inout_buffer_length,
			 unsigned int suggested_transform_length)
{
  // remember our edges
  low_edge = (double) low_pass_edge;
  high_edge = (double) high_pass_edge;

  // first find our buffer sizes.
  Q = filter_length;
  M = inout_buffer_length;
  if(M < 4 * Q) {
    std::cerr << "Warning -- OSFilter asked to implement a long filter against a short buffer." << std::endl;
  }

  // now find N.
  if(suggested_transform_length > (M + Q - 1)) {
    N = suggested_transform_length;
  }
  else {
    N = guessN();
  }

  // now back calculate the actual filter length
  Q = N - M;

  tail_index = M - (Q - 1);

  // OK.  now we build the filter.
  // First, build an allpass filter with the appropriate delay
  std::complex<float> * filter_in = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * N);
  filter_fft = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * N);

  unsigned int i, j;
  for(i = 0; i < N; i++) filter_in[i] = std::complex<float>(0.0,0.0);
  filter_in[Q/2] = std::complex<float>(1.0, 0.0);
  // we'll use the image of this filter for the phase part of our filter.

  fftwf_plan fplan = fftwf_plan_dft_1d(N, (fftwf_complex *) filter_in, (fftwf_complex *) filter_fft,
				       FFTW_FORWARD, FFTW_ESTIMATE);
  fftwf_plan ifplan = fftwf_plan_dft_1d(N, (fftwf_complex *) filter_fft, (fftwf_complex *) filter_in,
				       FFTW_BACKWARD, FFTW_ESTIMATE);

  // create an image that we can fill in.
  fftwf_execute(fplan);

  float freq_step = sample_rate / ((float) N);
  float fr;
  // now setup the filter image;
  for(fr = 0.0, i = 0, j = (N - 1); i < N/2; i++, j--, fr += freq_step) {
    if(fr < low_cutoff) {
      filter_fft[i] = std::complex<float>(0.0,0.0);
      filter_fft[j] = std::complex<float>(0.0,0.0);
    }
    else if(fr < low_pass_edge) {
      float mult = (fr - low_cutoff) / (low_pass_edge - low_cutoff);
      filter_fft[i] = filter_fft[i] * std::complex<float>(mult, 0.0);
      filter_fft[j] = filter_fft[j] * std::complex<float>(mult, 0.0);
    }
    else if(fr < high_pass_edge) {
      // keep the all pass value
    }
    else if(fr < high_cutoff) {
      float mult = 1.0 - ((fr - high_pass_edge) / (high_cutoff - high_pass_edge));
      filter_fft[i] = filter_fft[i] * std::complex<float>(mult, 0.0);
      filter_fft[j] = filter_fft[j] * std::complex<float>(mult, 0.0);
    }
    else {
      filter_fft[i] = std::complex<float>(0.0,0.0);
      filter_fft[j] = std::complex<float>(0.0,0.0);
    }
  }

  // inverse transform the filter image
  fftwf_execute(ifplan);

  // now we've got a FIR filter, but it needs to be windowed before we can trust it.
  for(i = 0; i < Q/2; i++) {
    int ii = (Q/2) - i;
    float ang = 2.0 * M_PI * ((float) ii) / ((float) Q);
    std::complex<float> wf(0.5 + 0.5 * cos(ang), 0.0);
    filter_in[i] = filter_in[i] * wf;
    filter_in[Q - i] = filter_in[Q - i] * wf;
  }
  for(i = Q; i < N; i++) {
    filter_in[i] = std::complex<float>(0.0, 0.0);
  }

  // now create the filter image
  fftwf_execute(fplan);

  // now we need to normalize the envelope so that we get the specified gain.
  float maxval = 0.0;
  for(i = 0; i < N; i++) {
    float v = abs(filter_fft[i]);
    if(v > maxval) maxval = v;
  }

  std::complex<float> normalize(filter_gain / (((float) N) * maxval), 0.0);

  for(i = 0; i < N; i++) {
    filter_fft[i] = filter_fft[i] * normalize;
  }

  // and destroy the plans
  fftwf_destroy_plan(fplan);
  fftwf_destroy_plan(ifplan);

  // and free the impulse response
  fftwf_free(filter_in);

  // and setup the FFT buffers
  setupFFT();
}

int SoDa::OSFilter::guessN()
{
    // give preferences to convenient powers of two.
    // but let's search for the nearest solution that is
    // a multiple of 2^a * 3^b * 5^c where b and c are in the range 0..3
    // and a is in the range 1..16
  unsigned int N_guess, N_best, E_best;
  N_best = 2;
  E_best = 0x80000000;
  int i;
  for(i = 1; i <= 0xff; i++) {
    unsigned int a = i & 0xf;
    unsigned int b = (i >> 4) & 0x3;
    unsigned int c = (i >> 6) & 0x3;
    N_guess = ipow(2, a) * ipow(3, b) * ipow(5, c);
    if(N_guess >= (M + Q - 1)) {
      unsigned slop = N_guess - (M + Q - 1);
      if(slop < E_best) {
	N_best = N_guess;
	E_best = slop;
      }
    }
  }

  return N_best;
}

void SoDa::OSFilter::setupFFT()
{
  unsigned int i;
  // now allocate all the storage vectors
  fft_input = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * N);
  fft_output = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * N);
  ifft_output = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * N);

  // and create the plans
  forward_plan = fftwf_plan_dft_1d(N,
				   (fftwf_complex *) fft_input,
				   (fftwf_complex *) fft_output,
				   FFTW_FORWARD, FFTW_ESTIMATE);
  backward_plan = fftwf_plan_dft_1d(N,
				    (fftwf_complex *) fft_output,
				    (fftwf_complex *) ifft_output,
				    FFTW_BACKWARD, FFTW_ESTIMATE);

  // zero out the start of the fft_input buffer for the first iteration.
  for(i = 0; i < Q-1; i++) fft_input[i] = std::complex<float>(0.0,0.0);

}

unsigned int SoDa::OSFilter::apply(float * inbuf, float * outbuf, float outgain, int instride, int outstride)
{
  unsigned int i, j;
  // copy the input buffer.
  for(i = 0, j = Q-1; i < (M * instride); i += instride, j++) {
    fft_input[j] = std::complex<float>(inbuf[i], 0.0);
  }

  // now do the forward FFT on the input
  fftwf_execute(forward_plan);

  // save the last bits of the input buffer to the Q-1 side of the FFT input vector
  // Do this now incase in buf and outbuf are the same buffers.  (we're going to
  // over-write most of outbuf with the memcpy at the bottom.... )
  for(i = (tail_index * instride), j = 0; j < Q-1; i += instride, j++) {
    fft_input[j] = std::complex<float>(inbuf[i], 0.0);
  }

  // apply the filter.
  for(i = 0; i < N; i++) {
    fft_output[i] = (fft_output[i] * filter_fft[i]) * outgain;
  }

  // now do the backward FFT on the result
  fftwf_execute(backward_plan);

  // and copy the result to the output buffer, but discard the
  // first Q-1 chunks
  for(i = 0, j = Q-1; i < (M * outstride); i += outstride, j++) {
    outbuf[i] = ifft_output[j].real();
  }

  return M;
}

unsigned int SoDa::OSFilter::apply(std::complex<float> * inbuf, std::complex<float> * outbuf, float outgain)
{
  // This is the overlap-save FFT filter technique described in Lyons pages 719ff.
  // first we need to copy the input buffer to the M side of the FFT input vector.
  memcpy(&(fft_input[Q-1]), inbuf, sizeof(std::complex<float>) * M);

  // now do the forward FFT on the input
  fftwf_execute(forward_plan);

  // save the last bits of the input buffer to the Q-1 side of the FFT input vector
  // Do this now incase inbuf and outbuf are the same buffers.  (we're going to
  // over-write most of outbuf with the memcpy at the bottom.... )
  memcpy(fft_input, &(inbuf[tail_index]), sizeof(std::complex<float>) * (Q-1));

  // apply the filter.
  unsigned int i;
  for(i = 0; i < N; i++) {
    fft_output[i] = (fft_output[i] * filter_fft[i]) * outgain;
  }

  // now do the backward FFT on the result
  fftwf_execute(backward_plan);

  // and copy the result to the output buffer, but discard the
  // first Q-1 chunks
  memcpy(outbuf, &(ifft_output[Q-1]), sizeof(std::complex<float>) * M);

  return M;
}

void SoDa::OSFilter::dump(std::ostream & os)
{
  unsigned int i;
  os << "# idx  real   imag   abs   arg" << std::endl;
  for(i = 0; i < N; i++) {
    float mag = abs(filter_fft[i]);
    float phase = arg(filter_fft[i]);
    os << i << " " << filter_fft[i].real() << " " << filter_fft[i].imag() << " " << mag << " " << phase << std::endl;
  }
}


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

//-----------------------------------------------------------------------------
/*!\file    ForwardDynamicsSolver.cpp
 *
 * \author  Stefan Scherzinger <scherzin@fzi.de>
 * \date    2020/03/24
 *
 */
//-----------------------------------------------------------------------------

// this package
#include <cartesian_controller_base/ForwardDynamicsSolver.h>

// other
#include <map>
#include <sstream>
#include <boost/algorithm/clamp.hpp>
#include <eigen_conversions/eigen_kdl.h>

// KDL
#include <kdl/jntarrayvel.hpp>
#include <kdl/framevel.hpp>

// Pluginlib
#include <pluginlib/class_list_macros.h>

/**
 * \class cartesian_controller_base::ForwardDynamicsSolver
 *
 * Users may explicitly specify it with \a "forward_dynamics" as \a ik_solver
 * in their controllers.yaml configuration file for each controller:
 *
 * \code{.yaml}
 * <name_of_your_controller>:
 *     type: "<type_of_your_controller>"
 *     ik_solver: "forward_dynamics"
 *     ...
 *
 *     solver:
 *         ...
 *         forward_dynamics:
 *             link_mass: 0.5
 * \endcode
 *
 */
PLUGINLIB_EXPORT_CLASS(cartesian_controller_base::ForwardDynamicsSolver, cartesian_controller_base::IKSolver)

namespace cartesian_controller_base{

  ForwardDynamicsSolver::ForwardDynamicsSolver()
    : m_min(0.01)
  {
  }

  ForwardDynamicsSolver::~ForwardDynamicsSolver(){}

  trajectory_msgs::JointTrajectoryPoint ForwardDynamicsSolver::getJointControlCmds(
        ros::Duration period,
        const ctrl::Vector6D& net_force)
  {

    // Compute joint space inertia matrix
    m_jnt_space_inertia_solver->JntToMass(m_current_positions,m_jnt_space_inertia);

    // Compute joint jacobian
    m_jnt_jacobian_solver->JntToJac(m_current_positions,m_jnt_jacobian);

    // Compute joint accelerations according to: \f$ \ddot{q} = H^{-1} ( J^T f) \f$
    m_current_accelerations.data = m_jnt_space_inertia.data.inverse() * m_jnt_jacobian.data.transpose() * net_force;

    // Integrate once, starting with zero motion
    m_current_velocities.data = 0.5 * m_current_accelerations.data * period.toSec();

    // Integrate twice, starting with zero motion
    m_current_positions.data = m_last_positions.data + 0.5 * m_current_velocities.data * period.toSec();

    // Make sure positions stay in allowed margins
    applyJointLimits();

    // Apply results
    trajectory_msgs::JointTrajectoryPoint control_cmd;
    for (int i = 0; i < m_number_joints; ++i)
    {
      control_cmd.positions.push_back(m_current_positions(i));
      control_cmd.velocities.push_back(m_current_velocities(i));

      // Accelerations should be left empty. Those values will be interpreted
      // by most hardware joint drivers as max. tolerated values. As a
      // consequence, the robot will move very slowly.
    }
    control_cmd.time_from_start = period; // valid for this duration

    return control_cmd;
  }

  bool ForwardDynamicsSolver::init(ros::NodeHandle& nh,
                                   const KDL::Chain& chain,
                                   const KDL::JntArray& upper_pos_limits,
                                   const KDL::JntArray& lower_pos_limits)
  {
    IKSolver::init(nh, chain, upper_pos_limits, lower_pos_limits);

    if (!buildGenericModel())
    {
      ROS_ERROR("ForwardDynamicsSolver: Something went wrong in setting up the internal model.");
      return false;
    }

    // Forward dynamics
    m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain));
    m_jnt_space_inertia_solver.reset(new KDL::ChainDynParam(m_chain,KDL::Vector::Zero()));
    m_jnt_jacobian.resize(m_number_joints);
    m_jnt_space_inertia.resize(m_number_joints);

    // Connect dynamic reconfigure and overwrite the default values with values
    // on the parameter server. This is done automatically if parameters with
    // the according names exist.
    m_callback_type = boost::bind(
        &ForwardDynamicsSolver::dynamicReconfigureCallback, this, _1, _2);

    m_dyn_conf_server.reset(
        new dynamic_reconfigure::Server<IKConfig>(
          ros::NodeHandle(nh.getNamespace() + "/solver/forward_dynamics")));

    m_dyn_conf_server->setCallback(m_callback_type);

    ROS_INFO("Forward dynamics solver initialized");
    ROS_INFO("Forward dynamics solver has control over %i joints", m_number_joints);

    return true;
  }

  bool ForwardDynamicsSolver::buildGenericModel()
  {
    // Set all masses and inertias to minimal (yet stable) values.
    double ip_min = 0.000001;
    for (size_t i = 0; i < m_chain.segments.size(); ++i)
    {
      // Fixed joint segment
      if (m_chain.segments[i].getJoint().getType() == KDL::Joint::None)
      {
        m_chain.segments[i].setInertia(
            KDL::RigidBodyInertia::Zero());
      }
      else  // relatively moving segment
      {
        m_chain.segments[i].setInertia(
            KDL::RigidBodyInertia(
              m_min,                // mass
              KDL::Vector::Zero(),  // center of gravity
              KDL::RotationalInertia(
                ip_min,             // ixx
                ip_min,             // iyy
                ip_min              // izz
                // ixy, ixy, iyz default to 0.0
                )));
      }
    }

    // Only give the last segment a generic mass and inertia.
    // See https://arxiv.org/pdf/1908.06252.pdf for a motivation for this setting.
    double m = 1;
    double ip = 1;
    m_chain.segments[m_chain.segments.size()-1].setInertia(
        KDL::RigidBodyInertia(
          m,
          KDL::Vector::Zero(),
          KDL::RotationalInertia(ip, ip, ip)));

    return true;
  }

  void ForwardDynamicsSolver::dynamicReconfigureCallback(IKConfig& config, uint32_t level)
  {
    m_min = config.link_mass;
  }

} // namespace


/*
 * ext.cc
 *
 *  Copyright (C) 2015 Diamond Light Source
 *
 *  Author: James Parkhurst
 *
 *  This code is distributed under the BSD license, a copy of which is
 *  included in the root directory of this package.
 */
#include <boost/python.hpp>
#include <boost/python/def.hpp>
#include <boost/make_shared.hpp>
#include <scitbx/glmtbx/glm.h>
#include <scitbx/glmtbx/robust_glm.h>

namespace scitbx { namespace glmtbx { namespace boost_python {

  using namespace boost::python;

  template <typename Family>
  class_< glm<Family> > wrap_glm(const char *name) {

    typedef glm<Family> glm_type;

    class_<glm_type> wrapper(name, no_init);

    wrapper
      .def("parameters",
          &glm_type::parameters)
      .def("niter",
          &glm_type::niter)
      .def("error",
          &glm_type::error)
      .def("converged",
          &glm_type::converged)
      .def("mu",
          &glm_type::mu, (
            arg("X")))
      ;

    return wrapper;
  }

  template <typename Family>
  class_< robust_glm<Family> > wrap_robust_glm(const char *name) {

    typedef robust_glm<Family> glm_type;

    class_<glm_type> wrapper(name, no_init);

    wrapper
      .def("parameters",
          &glm_type::parameters)
      .def("niter",
          &glm_type::niter)
      .def("error",
          &glm_type::error)
      .def("converged",
          &glm_type::converged)
      .def("mu",
          &glm_type::mu, (
            arg("X")))
      ;

    return wrapper;
  }

  object glm_selector(
      const af::const_ref< double, af::c_grid<2> > &X,
      const af::const_ref< double > &Y,
      const af::const_ref< double > &B,
      const af::const_ref< double > &P,
      double tolerance,
      std::size_t max_iter,
      const std::string &family) {
    object result;
    if (family == "poisson") {
      result = object(new glm<poisson>(
            X,
            Y,
            B,
            P,
            tolerance,
            max_iter));
    } else {
      SCITBX_ERROR("Unknown distribution");
    }
    return result;
  }

  object robust_glm_selector(
      const af::const_ref< double, af::c_grid<2> > &X,
      const af::const_ref< double > &Y,
      const af::const_ref< double > &B,
      double c,
      double tolerance,
      std::size_t max_iter,
      const std::string &family) {
    object result;
    if (family == "poisson") {
      result = object(new robust_glm<poisson>(
            X,
            Y,
            B,
            c,
            tolerance,
            max_iter));
    } else {
      SCITBX_ERROR("Unknown distribution");
    }
    return result;
  }

  BOOST_PYTHON_MODULE(scitbx_glmtbx_ext)
  {

    wrap_glm<poisson>("glm_poisson");
    wrap_robust_glm<poisson>("robust_glm_poisson");

    def("glm",
        &glm_selector, (
          arg("X"),
          arg("Y"),
          arg("B"),
          arg("P"),
          arg("tolerance")=1e-3,
          arg("max_iter")=10,
          arg("family")="poisson"));

    def("robust_glm",
        &robust_glm_selector, (
          arg("X"),
          arg("Y"),
          arg("B"),
          arg("c")=1.345,
          arg("tolerance")=1e-3,
          arg("max_iter")=10,
          arg("family")="poisson"));
  }

}}} // namespace = dials::model::boost_python


#include <memory>
#include <unordered_map>
#include <vector>

#include <Eigen/Core>
#include <aslam/cameras/camera-pinhole.h>
#include <aslam/cameras/distortion-fisheye.h>
#include <aslam/common/hash-id.h>
#include <aslam/frames/visual-frame.h>
#include <ceres-error-terms/parameterization/pose-param-jpl.h>
#include <ceres-error-terms/parameterization/quaternion-param-jpl.h>
#include <ceres-error-terms/visual-error-term.h>
#include <maplab-common/pose_types.h>
#include <maplab-common/quaternion-math.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>
#include <vi-map/landmark.h>
#include <vi-map/mission.h>
#include <vi-map/pose-graph.h>
#include <vi-map/vertex.h>
#include <vi-map/viwls-edge.h>

namespace map_optimization_legacy {

class ViwlsGraph : public testing::Test {
 protected:
  typedef aslam::FisheyeDistortion DistortionType;
  typedef aslam::PinholeCamera CameraType;

  virtual void SetUp() {
    distortion_param_ = 1.0;
    fu_ = 10;
    fv_ = 10;
    res_u_ = 640;
    res_v_ = 480;
    cu_ = res_u_ / 2.0;
    cv_ = res_v_ / 2.0;
    dummy_7d_0_.setZero();
    dummy_7d_1_.setZero();
    base_frame_.setZero();
    G_q_B_ << 1, 0, 0, 0;
    G_p_B_.setZero();
  }

  void solve();
  void copyDataFromPosegraph();
  void copyDataToPosegraph();

  void constructCamera() {
    Eigen::VectorXd distortion_parameters(1);
    distortion_parameters << distortion_param_;
    aslam::Distortion::UniquePtr distortion(
        new DistortionType(distortion_parameters));

    Eigen::VectorXd intrinsics(4);
    intrinsics << fu_, fv_, cu_, cv_;

    aslam::Camera::Ptr camera = std::shared_ptr<CameraType>(
        new CameraType(intrinsics, res_u_, res_v_, distortion));

    aslam::CameraId camera_id;
    common::generateId(&camera_id);
    camera->setId(camera_id);

    std::vector<aslam::Camera::Ptr> camera_vector;
    camera_vector.push_back(camera);
    aslam::TransformationVector T_C_B_vector;
    // We use identity transformation to T_C_B from default constructor.
    T_C_B_vector.resize(kNumCameras);
    aslam::NCameraId n_camera_id;
    common::generateId(&n_camera_id);
    cameras_.reset(
        new aslam::NCamera(
            n_camera_id, T_C_B_vector, camera_vector, "Test camera rig"));
  }

  vi_map::PoseGraph posegraph_;
  std::vector<pose_graph::VertexId> vertex_ids_;
  std::unordered_map<vi_map::LandmarkId, vi_map::Landmark::Ptr> landmarks_;
  std::vector<vi_map::LandmarkId> landmark_ids_;
  std::unordered_map<vi_map::MissionId, std::shared_ptr<vi_map::VIMission> >
      missions_;

  aslam::NCamera::Ptr cameras_;

  double distortion_param_;

  double fu_, fv_;
  double cu_, cv_;
  double res_u_, res_v_;

  Eigen::Vector4d G_q_B_;
  Eigen::Vector3d G_p_B_;

  // Containers for 7-element pose objects.
  Eigen::Matrix<double, 7, 1> base_frame_;
  typedef std::unordered_map<pose_graph::VertexId, int> VertexIdRotationMap;
  VertexIdRotationMap vertex_id_to_pose_idx_;
  Eigen::Matrix<double, 7, Eigen::Dynamic> keyframe_poses_;

  // Ordering is [orientation position] -> [xyzw xyz].
  Eigen::Matrix<double, 7, 1> dummy_7d_0_;
  Eigen::Matrix<double, 7, 1> dummy_7d_1_;

  static constexpr int kNumCameras = 1;
  static constexpr int kVisualFrameIndex = 0;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

void ViwlsGraph::copyDataFromPosegraph() {
  pose_graph::VertexIdList all_vertex_ids;
  posegraph_.getAllVertexIds(&all_vertex_ids);

  keyframe_poses_.resize(Eigen::NoChange, all_vertex_ids.size());

  unsigned int col_idx = 0;
  for (pose_graph::VertexId vertex_id : all_vertex_ids) {
    vi_map::Vertex* ba_vertex = dynamic_cast<vi_map::Vertex*>(
        posegraph_.getVertexPtrMutable(vertex_id));
    CHECK(ba_vertex) << "Couldn't cast to BA edge type.";

    vertex_id_to_pose_idx_.emplace(vertex_id, col_idx);
    keyframe_poses_.col(col_idx) << ba_vertex->get_q_M_I().coeffs(),
        ba_vertex->get_p_M_I();

    ++col_idx;
  }

  base_frame_ << G_q_B_, G_p_B_;
}

void ViwlsGraph::copyDataToPosegraph() {
  for (const VertexIdRotationMap::value_type& vertex_id_to_idx :
       vertex_id_to_pose_idx_) {
    vi_map::Vertex* ba_vertex = dynamic_cast<vi_map::Vertex*>(
        posegraph_.getVertexPtrMutable(vertex_id_to_idx.first));
    CHECK(ba_vertex) << "Couldn't cast to BA edge type.";

    Eigen::Map<Eigen::Vector3d> position(ba_vertex->get_p_M_I_Mutable());
    Eigen::Map<Eigen::Vector4d> rotation(ba_vertex->get_q_M_I_Mutable());

    position = keyframe_poses_.col(vertex_id_to_idx.second).tail(3);
    rotation = keyframe_poses_.col(vertex_id_to_idx.second).head(4);
  }

  G_q_B_ = base_frame_.head(4);
  G_p_B_ = base_frame_.tail(3);
}

void ViwlsGraph::solve() {
  ceres::Problem problem;
  ceres::LocalParameterization* quaternion_parameterization =
      new ceres_error_terms::JplQuaternionParameterization;
  ceres::LocalParameterization* pose_parameterization =
      new ceres_error_terms::JplPoseParameterization;

  pose_graph::VertexIdList all_vertex_ids;
  posegraph_.getAllVertexIds(&all_vertex_ids);
  CHECK(!all_vertex_ids.empty()) << "No vertices on the posegraph";

  copyDataFromPosegraph();

  for (const pose_graph::VertexId& vertex_id : all_vertex_ids) {
    vi_map::Vertex* ba_vertex = dynamic_cast<vi_map::Vertex*>(
        posegraph_.getVertexPtrMutable(vertex_id));
    CHECK(ba_vertex) << "Couldn't cast to BA edge type.";

    const Eigen::Matrix2Xd& image_points_distorted =
        ba_vertex->getVisualFrame(kVisualFrameIndex).getKeypointMeasurements();
    const Eigen::VectorXd& image_points_uncertainties =
        ba_vertex->getVisualFrame(kVisualFrameIndex)
            .getKeypointMeasurementUncertainties();

    const std::shared_ptr<CameraType> camera_ptr =
        std::dynamic_pointer_cast<CameraType>(
            ba_vertex->getCamera(kVisualFrameIndex));
    CHECK(camera_ptr != nullptr);

    // Retrieve keyframe pose idx.
    VertexIdRotationMap::const_iterator it;
    it = vertex_id_to_pose_idx_.find(vertex_id);
    CHECK(it != vertex_id_to_pose_idx_.end());
    int pose_idx = it->second;

    for (int i = 0; i < image_points_distorted.cols(); ++i) {
      ceres::CostFunction* visual_term_cost =
          new ceres_error_terms::VisualReprojectionError<CameraType,
                                                         DistortionType>(
              image_points_distorted.col(i), image_points_uncertainties(i),
              ceres_error_terms::visual::VisualErrorType::kLocalMission,
              camera_ptr.get());

      problem.AddResidualBlock(
          visual_term_cost, NULL,
          landmarks_
              .find(ba_vertex->getObservedLandmarkId(kVisualFrameIndex, i))
              ->second->get_p_B_Mutable(),
          base_frame_.data(), dummy_7d_0_.data(), dummy_7d_1_.data(),
          keyframe_poses_.col(pose_idx).data(),
          cameras_->get_T_C_B_Mutable(kVisualFrameIndex)
              .getRotation()
              .toImplementation()
              .coeffs()
              .data(),
          cameras_->get_T_C_B_Mutable(kVisualFrameIndex).getPosition().data(),
          camera_ptr->getParametersMutable(),
          camera_ptr->getDistortionMutable()->getParametersMutable());

      problem.SetParameterBlockConstant(
          landmarks_
              .find(ba_vertex->getObservedLandmarkId(kVisualFrameIndex, i))
              ->second->get_p_B_Mutable());
    }

    // We fix the dummy parameter blocks because
    // they have no meaning.
    problem.SetParameterBlockConstant(dummy_7d_0_.data());
    problem.SetParameterBlockConstant(dummy_7d_1_.data());

    problem.SetParameterBlockConstant(
        cameras_->get_T_C_B_Mutable(kVisualFrameIndex)
            .getRotation()
            .toImplementation()
            .coeffs()
            .data());
    problem.SetParameterBlockConstant(
        cameras_->get_T_C_B_Mutable(kVisualFrameIndex).getPosition().data());
    problem.SetParameterBlockConstant(camera_ptr->getParametersMutable());
    problem.SetParameterBlockConstant(
        camera_ptr->getDistortionMutable()->getParametersMutable());

    problem.SetParameterization(
        keyframe_poses_.col(pose_idx).data(), pose_parameterization);
    problem.SetParameterization(
        cameras_->get_T_C_B_Mutable(kVisualFrameIndex)
            .getRotation()
            .toImplementation()
            .coeffs()
            .data(),
        quaternion_parameterization);
  }

  problem.SetParameterBlockConstant(base_frame_.data());
  problem.SetParameterization(base_frame_.data(), pose_parameterization);

  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_SCHUR;
  options.minimizer_progress_to_stdout = false;
  options.max_num_iterations = 200;
  options.gradient_tolerance = 1e-20;
  options.function_tolerance = 1e-20;
  options.parameter_tolerance = 1e-20;
  ceres::Solver::Summary summary;

  ceres::Solve(options, &problem, &summary);

  LOG(INFO) << summary.BriefReport();

  copyDataToPosegraph();
}

TEST_F(ViwlsGraph, VisualBundleAdjTestCamPositionOptimization) {
  constructCamera();

  vi_map::MissionId mission_id;
  mission_id.fromHexString("aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa0");
  std::shared_ptr<vi_map::VIMission> mission_ptr(new vi_map::VIMission);
  mission_ptr->setId(mission_id);
  missions_.insert(std::make_pair(mission_id, mission_ptr));

  vi_map::Landmark::Ptr landmark_ptr(new vi_map::Landmark);
  vi_map::LandmarkId landmark_id;
  landmark_id.fromHexString("aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa1");
  landmark_ptr->setId(landmark_id);
  landmark_ptr->set_p_B(pose::Position3D(0, 0, 1));
  landmark_ptr->set_p_B_Covariance(Eigen::Matrix<double, 3, 3>::Identity());
  landmarks_.insert(std::make_pair(landmark_ptr->id(), landmark_ptr));
  landmark_ids_.push_back(landmark_ptr->id());

  Eigen::Matrix<double, 6, 1> imu_bias;
  imu_bias << 1, 2, 3, 4, 5, 6;

  Eigen::Matrix<double, 2, 1> points;
  points << cu_, cv_;

  Eigen::VectorXd uncertainties;
  uncertainties.resize(1);
  uncertainties << 0.5;

  aslam::VisualFrame::DescriptorsT descriptors;
  descriptors.resize(48, 1);
  descriptors.setRandom();

  pose_graph::VertexId vertex0;
  vertex0.fromHexString("aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa2");
  vertex_ids_.push_back(vertex0);

  aslam::FrameId frame_id;
  frame_id.fromHexString("aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa3");
  int64_t frame_timestamp = 0;

  // Base position and rotation.
  G_q_B_ << 0, 0, 0, 1;
  G_p_B_ << 0, 0, 0;

  posegraph_.addVIVertex(
      vertex0, imu_bias, points, uncertainties, descriptors, landmark_ids_,
      mission_ptr->id(), frame_id, frame_timestamp, cameras_);
  EXPECT_TRUE(posegraph_.vertexExists(vertex0));
  Eigen::Vector3d vertex_position(0.2, -0.3, 0.2);
  Eigen::Quaterniond vertex_orientation(1, 0, 0, 0);

  vi_map::Vertex* ba_vertex =
      dynamic_cast<vi_map::Vertex*>(posegraph_.getVertexPtrMutable(vertex0));
  CHECK_NOTNULL(ba_vertex);
  ba_vertex->set_q_M_I(vertex_orientation);
  ba_vertex->set_p_M_I(vertex_position);
  solve();

  const Eigen::Quaterniond& G_q_I = ba_vertex->get_q_M_I();
  const Eigen::Vector3d& G_p_I = ba_vertex->get_p_M_I();
  Eigen::Matrix3d I_R_G = G_q_I.toRotationMatrix().transpose();
  const Eigen::Vector3d C_p_fi = I_R_G * (landmark_ptr->get_p_B() - G_p_I);

  // We don't really care about exact keyframe pose, but we do care
  // about landmark location in the camera coordinate frame: it needs to be
  // located on the optical axis.
  EXPECT_NEAR_EIGEN(C_p_fi.normalized(), Eigen::Vector3d(0, 0, 1), 1e-9);
}

}  // namespace map_optimization_legacy

MAPLAB_UNITTEST_ENTRYPOINT


/**************************************************************************\
|
|    Copyright (C) 2009 Marc Stevens
|
|    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 3 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, see <http://www.gnu.org/licenses/>.
|
\**************************************************************************/

#include <stdexcept>
#include <iostream>
#include <fstream>
#include <vector>
#include <string>

#include <boost/lexical_cast.hpp>
#include <boost/program_options.hpp>

#include "main.hpp"

#include <hashclash/sdr.hpp>
#include <hashclash/timer.hpp>

using namespace hashclash;
using namespace std;
namespace po = boost::program_options;
namespace fs = boost::filesystem;

std::string workdir;
parameters_type mainparameters;

int main(int argc, char** argv)
{
	hashclash::timer runtime(true);

	try {
		parameters_type parameters;
		vector< vector<unsigned> > msgmask(16);
		unsigned msgoffset = 0;
		bool msglcext = false;
		bool docheckokpaths = false;

		// Define program options
		po::options_description
			cmds("Allowed commands"),
			desc("Allowed options"),
			msg("Define message differences (as +bitnr and -bitnr, bitnr=1-32)"),
			all("Allowed options");

		cmds.add_options()
			("help,h",				"Show options\n")
			;
		desc.add_options()
			("workdir,w"
				, po::value<string>(&workdir)->default_value("./data")
				, "Set working directory.")
			("rnd234merel"
				, po::value< vector<string> >(&parameters.rnd234_m_bitrelationfiles)
				, "File with round 2,3,4 msg.exp. bitrelations.\n(Can be specified multiple times)")
			("rnd234path"
				, po::value<string>(&parameters.rnd234_pathfile)
				, "File with upper path.")
			("rnd1paths"
				, po::value<string>(&parameters.rnd1_pathsfile)
				, "File with round 1 paths.")
			("tend_rnd1_me"
				, po::value<unsigned>(&parameters.tend_rnd1_me)->default_value(22)
				, "Apply round 1 path msg.exp. values for t=[0,tend).\n")

			("checkokpaths"
				, po::bool_switch(&docheckokpaths)
				, "Analyse ok paths from collfind.")

			("analyzetunnels,a"
				, po::bool_switch(&parameters.usetunnelbitconditions)
				, "Analyze tunnels instead of creating program.")
			("tunnel,t"
				, po::value<string>(&parameters.tunnelfile)
				, "Analyze tunnel.")
			("deepanalysis"
				, po::bool_switch(&parameters.tunneldeepanalysis)
				, "Analyze tunnel strengthening.")
			("tunnelmecarry"
				, po::value<unsigned>(&parameters.mecarry)->default_value(0)
				, "Tunnel analysis message carries.")
			("tunnelshowbitchangestat"
				, po::bool_switch(&parameters.changebitstat)
				, "Show bit change statistics.")
			("mintunnelpov"
				, po::value<int>(&parameters.mintunnelpov)->default_value(-1)
				, "Only  show bitchangestats when POV>=bound.")
			("tunnelfilterbitcondition"
				, po::bool_switch(&parameters.tunnelfilterbitcondition)
				, "Only analyze tunnels with compatible b.c.")
			("threebittunnels"
				, po::bool_switch(&parameters.threebittunnels)
				, "Beside 1,2 bit tunnels do 3 bit tunnels.")
			("simpletunnels"
				, po::bool_switch(&parameters.simpletunnels)
				, "Only consider simple ^+01 tunnels.")
			;

		msg.add_options()
			("diffm0", po::value< vector<unsigned> >(&msgmask[0]), "mask m0")
			("diffm1", po::value< vector<unsigned> >(&msgmask[1]), "mask m1")
			("diffm2", po::value< vector<unsigned> >(&msgmask[2]), "mask m2")
			("diffm3", po::value< vector<unsigned> >(&msgmask[3]), "mask m3")
			("diffm4", po::value< vector<unsigned> >(&msgmask[4]), "mask m4")
			("diffm5", po::value< vector<unsigned> >(&msgmask[5]), "mask m5")
			("diffm6", po::value< vector<unsigned> >(&msgmask[6]), "mask m6")
			("diffm7", po::value< vector<unsigned> >(&msgmask[7]), "mask m7")
			("diffm8", po::value< vector<unsigned> >(&msgmask[8]), "mask m8")
			("diffm9", po::value< vector<unsigned> >(&msgmask[9]), "mask m9")
			("diffm10", po::value< vector<unsigned> >(&msgmask[10]), "mask m10")
			("diffm11", po::value< vector<unsigned> >(&msgmask[11]), "mask m11")
			("diffm12", po::value< vector<unsigned> >(&msgmask[12]), "mask m12")
			("diffm13", po::value< vector<unsigned> >(&msgmask[13]), "mask m13")
			("diffm14", po::value< vector<unsigned> >(&msgmask[14]), "mask m14")
			("diffm15", po::value< vector<unsigned> >(&msgmask[15]), "mask m15")
			("diffoffset", po::value< unsigned >(&msgoffset), "mask offset")
			("difflcext", po::bool_switch(&msglcext), "extend to local coll.")
			;
		all.add(cmds).add(desc).add(msg);

		// Parse program options
		po::positional_options_description p;
		po::variables_map vm;
		po::store(po::command_line_parser(argc, argv)
			.options(all).positional(p).run(), vm);
		{
			std::ifstream ifs("sha1diffpathcollfind.cfg");
			if (ifs) po::store(po::parse_config_file(ifs, all), vm);
		}
		po::notify(vm);

		// Process program options
		if (vm.count("help")) {
			cout << cmds << desc << endl;
			return 0;
		}
		parameters.usetunnelbitconditions = !parameters.usetunnelbitconditions;

		uint32 me[16];
		for (unsigned k = 0; k < 16; ++k) {
			me[k] = 0;
			for (unsigned j = 0; j < msgmask[k].size(); ++j)
				me[k] |= 1<<msgmask[k][j];
		}
		sha1_me_generalised(parameters.m_mask, me, msgoffset);
		if (msglcext) {
			uint32 met[16];
			uint32 mex[80];
			for (unsigned i = 0; i < 16; ++i)
				met[i] = rotate_left(me[i],5);
			sha1_me_generalised(mex, met, msgoffset+1);
			for (unsigned i = 0; i < 80; ++i)
				parameters.m_mask[i] ^= mex[i];

			sha1_me_generalised(mex, me, msgoffset+2);
			for (unsigned i = 0; i < 80; ++i)
				parameters.m_mask[i] ^= mex[i];

			for (unsigned i = 0; i < 16; ++i)
				met[i] = rotate_left(me[i],30);
			for (unsigned j = 3; j < 6; ++j) {
				sha1_me_generalised(mex, met, msgoffset+j);
				for (unsigned i = 0; i < 80; ++i)
					parameters.m_mask[i] ^= mex[i];
			}
		}

		mainparameters = parameters;
		if (docheckokpaths)
			checkokpaths();
		return collisionfinding(parameters);
		// Start job with given parameters
	} catch (exception& e) {
		cout << "Runtime: " << runtime.time() << endl;
		cerr << "Caught exception!!:" << endl << e.what() << endl;
		throw;
	} catch (...) {
		cout << "Runtime: " << runtime.time() << endl;
		cerr << "Unknown exception caught!!" << endl;
		throw;
	}
	cout << "Runtime: " << runtime.time() << endl;
	return 0;
}

unsigned load_block(istream& i, uint32 block[])
{
	for (unsigned k = 0; k < 16; ++k)
		block[k] = 0;

	unsigned len = 0;
	char uc;
	for (unsigned k = 0; k < 16; ++k)
		for (unsigned c = 0; c < 4; ++c)
		{
			i.get(uc);
			if (i) {
				++len;
				block[k] += uint32((unsigned char)(uc)) << ((3-c)*8);
			} else {
				i.putback(uc);
				i.setstate(ios::failbit);
				return len;
			}
		}
	return len;
}

void save_block(ostream& o, uint32 block[])
{
	for (unsigned k = 0; k < 16; ++k)
		for (unsigned c = 0; c < 4; ++c)
			o << (unsigned char)((block[k]>>((3-c)*8))&0xFF);
}


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020 Alexander Sokolov <asokolov@nil.foundation>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_DETAIL_IMPLODER_HPP
#define CRYPTO3_DETAIL_IMPLODER_HPP

#include <boost/crypto3/detail/stream_endian.hpp>
#include <boost/crypto3/detail/unbounded_shift.hpp>
#include <boost/crypto3/detail/reverser.hpp>

#include <boost/static_assert.hpp>

namespace boost {
    namespace crypto3 {
        namespace detail {

            // By definition, for all imploders, InputValueBits < OutputValueBits,
            // so we're taking many smaller values and combining them into one value

            /*!
             * @defgroup imploder Imploder functions
             */

            /*!
             * @brief imploder_shift trait is used to determine whether the input elements are packed into
             * an output element in reverse order. Since the input and output types are integral now, this
             * trait contains the shift indicating the position of input element in the output element when
             * k input bits have already been processed.
             *
             * @ingroup imploder
             *
             * @tparam OutputEndianness
             * @tparam UnitBits
             * @tparam InputBits
             * @tparam OutputBits
             * @tparam k
             * @tparam IsLittleUnit
             */
            template<typename OutputEndianness, int UnitBits, int InputBits, int OutputBits, int k,
                     bool IsLittleUnit = is_little_unit<OutputEndianness, UnitBits>::value>
            struct imploder_shift;

            template<typename OutputEndianness, int UnitBits, int InputBits, int OutputBits, int k>
            struct imploder_shift<OutputEndianness, UnitBits, InputBits, OutputBits, k, false> {
                constexpr static int const value = OutputBits - (InputBits + k);
            };

            template<typename OutputEndianness, int UnitBits, int InputBits, int OutputBits, int k>
            struct imploder_shift<OutputEndianness, UnitBits, InputBits, OutputBits, k, true> {
                constexpr static int const value = k;
            };

            /*!
             * @brief imploder_step packs an input value represented in InputEndianness endianness
             * into an output value represented in OutputEndianness endianness when k input bits
             * have already been processed. It uses unit_reverser and bit_reverser to deal with the
             * order of units and bits in the input value, respectively. Shift constant is determined
             * by the imploder_shift trait.
             *
             * @ingroup imploder
             *
             * @tparam InputEndianness
             * @tparam OutputEndianness
             * @tparam UnitBits
             * @tparam InputBits
             * @tparam OutputBits
             * @tparam k
             */
            template<typename InputEndianness, typename OutputEndianness, int UnitBits, int InputBits, int OutputBits,
                     int k>
            struct imploder_step {
                constexpr static int const shift =
                    imploder_shift<OutputEndianness, UnitBits, InputBits, OutputBits, k>::value;

                template<typename InputValue, typename OutputValue>
                inline static void step(InputValue &in, OutputValue &out) {
                    InputValue tmp = in;
                    unit_reverser<InputEndianness, OutputEndianness, UnitBits>::reverse(tmp);
                    bit_reverser<InputEndianness, OutputEndianness, UnitBits>::reverse(tmp);
                    out |= unbounded_shl<shift>(low_bits<InputBits>(OutputValue(tmp)));
                }
            };

            /*!
             * @brief imploder processes a sequence of input values represented in InputEndianness endianness
             * into an output value represented in OutputEndianness endianness. The function implode is
             * invoked recursively, and the parameter k is used to track the number of already processed
             * input values packed into the output value. The recursion ends when all elements the output
             * value can hold have already been processed, i.e. when k == OutputBits.
             *
             * @ingroup imploder
             *
             * @tparam InputEndianness
             * @tparam OutputEndianness
             * @tparam InputBits
             * @tparam OutputBits
             * @tparam k
             */
            template<typename InputEndianness, typename OutputEndianness, int InputBits, int OutputBits, int k = 0>
            struct imploder;

            template<template<int> class InputEndian, template<int> class OutputEndian, int UnitBits, int InputBits,
                     int OutputBits, int k>
            struct imploder<InputEndian<UnitBits>, OutputEndian<UnitBits>, InputBits, OutputBits, k> {

                // To keep the implementation managable, input and output sizes must
                // be multiples or factors of the unit size.
                // If one of these is firing, you may want a bit-only stream_endian
                // rather than one that mentions bytes or octets.
                BOOST_STATIC_ASSERT(!(InputBits % UnitBits && UnitBits % InputBits));
                BOOST_STATIC_ASSERT(!(OutputBits % UnitBits && UnitBits % OutputBits));

                typedef InputEndian<UnitBits> InputEndianness;
                typedef OutputEndian<UnitBits> OutputEndianness;
                typedef imploder_step<InputEndianness, OutputEndianness, UnitBits, InputBits, OutputBits, k> step_type;
                typedef imploder<InputEndianness, OutputEndianness, InputBits, OutputBits, k + InputBits> next_type;

                template<typename InIter, typename OutputValue>
                inline static void implode(InIter &in, OutputValue &x) {
                    step_type::step(*in++, x);
                    next_type::implode(in, x);
                }
            };

            template<template<int> class InputEndian, template<int> class OutputEndian, int UnitBits, int InputBits,
                     int OutputBits>
            struct imploder<InputEndian<UnitBits>, OutputEndian<UnitBits>, InputBits, OutputBits, OutputBits> {
                template<typename InIter, typename OutputValue>
                inline static void implode(InIter &, OutputValue &) {
                }
            };

        }    // namespace detail
    }        // namespace crypto3
}    // namespace boost

#endif    // CRYPTO3_DETAIL_IMPLODER_HPP


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2018-2019 Oracle and/or its affiliates.
// Contributed and/or modified by Vissarion Fisikopoulos, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_SEGMENT_BOX_HPP
#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_SEGMENT_BOX_HPP

#include <boost/geometry/srs/spheroid.hpp>

#include <boost/geometry/algorithms/detail/distance/segment_to_box.hpp>

#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/strategies/geographic/parameters.hpp>
#include <boost/geometry/strategies/geographic/azimuth.hpp>
#include <boost/geometry/strategies/geographic/distance_cross_track.hpp>
#include <boost/geometry/strategies/normalize.hpp>
#include <boost/geometry/strategies/spherical/disjoint_box_box.hpp>
#include <boost/geometry/strategies/spherical/distance_segment_box.hpp>
#include <boost/geometry/strategies/spherical/point_in_point.hpp>

#include <boost/geometry/util/promote_floating_point.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace distance
{

template
<
    typename FormulaPolicy = strategy::andoyer,
    typename Spheroid = srs::spheroid<double>,
    typename CalculationType = void
>
struct geographic_segment_box
{
    template <typename PointOfSegment, typename PointOfBox>
    struct return_type
        : promote_floating_point
          <
              typename select_calculation_type
                  <
                      PointOfSegment,
                      PointOfBox,
                      CalculationType
                  >::type
          >
    {};

    // point-point strategy getters
    struct distance_pp_strategy
    {
        typedef geographic<FormulaPolicy, Spheroid, CalculationType> type;
    };

    inline typename distance_pp_strategy::type get_distance_pp_strategy() const
    {
        typedef typename distance_pp_strategy::type distance_type;
        return distance_type(m_spheroid);
    }
    // point-segment strategy getters
    struct distance_ps_strategy
    {
        typedef geographic_cross_track
                <
                    FormulaPolicy,
                    Spheroid,
                    CalculationType
                > type;
    };

    inline typename distance_ps_strategy::type get_distance_ps_strategy() const
    {
        typedef typename distance_ps_strategy::type distance_type;
        return distance_type(m_spheroid);
    }

    typedef within::spherical_point_point equals_point_point_strategy_type;

    static inline equals_point_point_strategy_type get_equals_point_point_strategy()
    {
        return equals_point_point_strategy_type();
    }

    //constructor

    explicit geographic_segment_box(Spheroid const& spheroid = Spheroid())
             : m_spheroid(spheroid)
    {}

    // methods

    template <typename LessEqual, typename ReturnType,
              typename SegmentPoint, typename BoxPoint>
    inline ReturnType segment_below_of_box(SegmentPoint const& p0,
                                   SegmentPoint const& p1,
                                   BoxPoint const& top_left,
                                   BoxPoint const& top_right,
                                   BoxPoint const& bottom_left,
                                   BoxPoint const& bottom_right) const
    {
        typedef typename azimuth::geographic
        <
                FormulaPolicy,
                Spheroid,
                CalculationType
        > azimuth_strategy_type;
        azimuth_strategy_type az_strategy(m_spheroid);

        typedef typename envelope::geographic_segment
        <
                FormulaPolicy,
                Spheroid,
                CalculationType
        > envelope_segment_strategy_type;
        envelope_segment_strategy_type es_strategy(m_spheroid);

        return generic_segment_box::segment_below_of_box
               <
                    LessEqual,
                    ReturnType
               >(p0,p1,top_left,top_right,bottom_left,bottom_right,
                 geographic_segment_box<FormulaPolicy, Spheroid, CalculationType>(),
                 az_strategy, es_strategy,
                 normalize::spherical_point(),
                 covered_by::spherical_point_box(),
                 disjoint::spherical_box_box());
    }

    template <typename SPoint, typename BPoint>
    static void mirror(SPoint& p0,
                       SPoint& p1,
                       BPoint& bottom_left,
                       BPoint& bottom_right,
                       BPoint& top_left,
                       BPoint& top_right)
    {

       generic_segment_box::mirror(p0, p1,
                                   bottom_left, bottom_right,
                                   top_left, top_right);
    }

private :
    Spheroid m_spheroid;
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

//tags

template <typename FormulaPolicy>
struct tag<geographic_segment_box<FormulaPolicy> >
{
    typedef strategy_tag_distance_segment_box type;
};

template
<
        typename FormulaPolicy,
        typename Spheroid
>
struct tag<geographic_segment_box<FormulaPolicy, Spheroid> >
{
    typedef strategy_tag_distance_segment_box type;
};

template
<
        typename FormulaPolicy,
        typename Spheroid,
        typename CalculationType
>
struct tag<geographic_segment_box<FormulaPolicy, Spheroid, CalculationType> >
{
    typedef strategy_tag_distance_segment_box type;
};

// return types

template <typename FormulaPolicy, typename PS, typename PB>
struct return_type<geographic_segment_box<FormulaPolicy>, PS, PB>
    : geographic_segment_box<FormulaPolicy>::template return_type<PS, PB>
{};

template
<
        typename FormulaPolicy,
        typename Spheroid,
        typename PS,
        typename PB
>
struct return_type<geographic_segment_box<FormulaPolicy, Spheroid>, PS, PB>
    : geographic_segment_box<FormulaPolicy, Spheroid>::template return_type<PS, PB>
{};

template
<
        typename FormulaPolicy,
        typename Spheroid,
        typename CalculationType,
        typename PS,
        typename PB
>
struct return_type<geographic_segment_box<FormulaPolicy, Spheroid, CalculationType>, PS, PB>
    : geographic_segment_box<FormulaPolicy, Spheroid, CalculationType>::template return_type<PS, PB>
{};

//comparable types

template
<
        typename FormulaPolicy,
        typename Spheroid,
        typename CalculationType
>
struct comparable_type<geographic_segment_box<FormulaPolicy, Spheroid, CalculationType> >
{
    typedef geographic_segment_box
        <
            FormulaPolicy, Spheroid, CalculationType
        >  type;
};

template
<
        typename FormulaPolicy,
        typename Spheroid,
        typename CalculationType
>
struct get_comparable<geographic_segment_box<FormulaPolicy, Spheroid, CalculationType> >
{
    typedef typename comparable_type
        <
            geographic_segment_box<FormulaPolicy, Spheroid, CalculationType>
        >::type comparable_type;
public :
    static inline comparable_type
    apply(geographic_segment_box<FormulaPolicy, Spheroid, CalculationType> const& )
    {
        return comparable_type();
    }
};

// result from distance

template
<
    typename FormulaPolicy,
    typename PS,
    typename PB
>
struct result_from_distance<geographic_segment_box<FormulaPolicy>, PS, PB>
{
private :
    typedef typename geographic_segment_box
        <
            FormulaPolicy
        >::template return_type<PS, PB>::type return_type;
public :
    template <typename T>
    static inline return_type
    apply(geographic_segment_box<FormulaPolicy> const& , T const& distance)
    {
        return distance;
    }
};

template
<
    typename FormulaPolicy,
    typename Spheroid,
    typename CalculationType,
    typename PS,
    typename PB
>
struct result_from_distance<geographic_segment_box<FormulaPolicy, Spheroid, CalculationType>, PS, PB>
{
private :
    typedef typename geographic_segment_box
        <
            FormulaPolicy, Spheroid, CalculationType
        >::template return_type<PS, PB>::type return_type;
public :
    template <typename T>
    static inline return_type
    apply(geographic_segment_box<FormulaPolicy, Spheroid, CalculationType> const& , T const& distance)
    {
        return distance;
    }
};

// default strategies

template <typename Segment, typename Box>
struct default_strategy
    <
        segment_tag, box_tag, Segment, Box,
        geographic_tag, geographic_tag
    >
{
    typedef geographic_segment_box<> type;
};

template <typename Box, typename Segment>
struct default_strategy
    <
        box_tag, segment_tag, Box, Segment,
        geographic_tag, geographic_tag
    >
{
    typedef typename default_strategy
        <
            segment_tag, box_tag, Segment, Box,
            geographic_tag, geographic_tag
        >::type type;
};

}
#endif

}} // namespace strategy::distance

}} // namespace boost::geometry
#endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_SEGMENT_BOX_HPP


/*
 * kml_writer.hpp
  * Copyright (C) 2011, 2012 Danny Havenith
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 *  Created on: Sep 28, 2011
 *      Author: danny
 */

#ifndef KML_WRITER_HPP_
#define KML_WRITER_HPP_
#include "messages.hpp"
#include "bytes_to_numbers.hpp"

#include <boost/date_time/posix_time/posix_time.hpp>

#include <ostream>
#include <string>
#include <iterator>
#include <boost/cstdint.hpp>
#include <utility>
#include <vector>

/**
 * This class looks for specific events and logs the gps position and other data on such events
 */
struct data_at_event : public rtlogs::messages_definition
{
    data_at_event(
            std::string sourceName,
            std::ostream &out,
            bool emit_header = true)
       :sourceName(std::move( sourceName)),
        out(out)
    {
        if (emit_header)
        {
            out << prolog();
        }
        out.precision(8);
    }

    /**
     * ignore all messages that are not of any of the handled types
     */
    void handle(...){};

    template< typename iterator>
    void handle( gps_position, iterator begin, iterator end)
    {
        typedef boost::int32_t long_type;
        using bytes_to_numbers::get_big_endian;

        ++begin;
        const auto longitude = get_big_endian<long_type>(begin);
        std::advance( begin, 4);
        const auto latitude = get_big_endian< long_type>( begin);

        lastposition = {longitude/10000000.0, latitude/10000000.0};
    }

    /// store the last gps time
    template< typename iterator>
    void handle( gps_time_storage, iterator begin, iterator end)
    {
      using namespace boost::posix_time;
      using namespace boost::gregorian;
      using namespace bytes_to_numbers;

      ++begin; // skip header

      // extract gps timestamp (4-byte, big-endian, unsigned integer)
      const auto gps_timestamp = get_big_endian<uint32_t>( begin);
      // gps_timestamp now contains the number of milliseconds since 00:00:00 on Sunday.

      static const date start( 2012, Jan, 1); //start with a known Sunday, 00:00:00.
      last_gps_time = ptime{ start, milliseconds(gps_timestamp)}; // calculate a day/time given the known Sunday offset.

    }

    /// remember the value of the last timestamp encountered.
    template< typename iterator>
    void handle( timestamp, iterator begin, iterator end)
    {
        ++begin;
        unsigned long result = *begin++;
        result = (result << 8) + * begin++;
        result = (result << 8) + * begin++;
        last_timestamp = result;

        if (first_timestamp == 0)
        {
            first_timestamp = result;
        }
    }

    template< typename iterator>
    void handle( precalc_distance_data, iterator begin, iterator end)
    {
        ++begin;
        last_distance = bytes_to_numbers::get_big_endian<uint32_t>(begin)/1000.0;

    }

    template< typename iterator>
    void handle( high_res_timer, iterator begin, iterator end)
    {
        if (end-begin > 2 and *(begin+1) == 64)
        {
            output_line( *(begin+1));
        }
    }

//    ~event_position()
//    {
//        out << epilog();
//    }

private:
    std::string prolog()
    {
        std::string prolog{"run"};
        for ( const auto field : { "type", "distance", "time", "reltime", "longitude", "latitude"})
        {
            prolog = prolog + separator + field;
        }
        return prolog + '\n';
    }

    void output_line( unsigned int event_type)
    {
        out
            << sourceName << separator
            << event_type << separator
            << last_distance << separator
            << last_gps_time.time_of_day() << separator
            << (last_timestamp - first_timestamp)/100.0 << separator
            << lastposition.longitude << separator
            << lastposition.latitude
            << '\n'
            ;
    }

    std::string epilog()
    {
        return "";
    }

    struct position
    {
        double longitude;
        double latitude;
    };

    position lastposition = {};
    std::string sourceName;
    std::ostream &out;
    std::string separator = ",";
    boost::posix_time::ptime last_gps_time;
    double last_distance = 0.0;
    unsigned long last_timestamp = 0;
    unsigned long first_timestamp = 0;
};

#endif /* KML_WRITER_HPP_ */


#include <LightBulb/Teaching/TeachingLessonLinearInput.hpp>
#include <Mocks/MockNeuralNetwork.hpp>
#include <Mocks/MockActivationOrder.hpp>
#include <Mocks/MockFeedForwardNetworkTopology.hpp>
#include <Mocks/MockNetworkTopology.hpp>
#include <Mocks/MockActivationFunction.hpp>
#include <Eigen/Dense>
#include "Mocks/MockNeuronDescription.hpp"
#include <LightBulb/Function/InputFunction/AbstractInputFunction.hpp>

using namespace LightBulb;

class TeachingLessonLinearInputTest : public testing::Test {
public:
	TeachingLessonLinearInput* teachingLesson;
	TeachingInput<>* teachingInput;
	Vector<>* teachingPattern;
	MockNeuralNetwork* neuralNetwork;
	MockNetworkTopology* networkTopology;
	MockActivationOrder* activationOrder;
	MockActivationFunction* activationFunction;
	MockNeuronDescription* neuronDescription;
	Vector<> neuralNetworkOutput;
	void SetUp() {
		neuralNetwork = new MockNeuralNetwork();
		networkTopology = new MockNetworkTopology();
		activationOrder = new MockActivationOrder();
		neuronDescription = new MockNeuronDescription();
		activationFunction = new MockActivationFunction();
		EXPECT_CALL(*neuralNetwork, getNetworkTopology()).WillRepeatedly(testing::ReturnRef(*networkTopology));
		EXPECT_CALL(*networkTopology, getOutputNeuronDescription()).WillRepeatedly(testing::ReturnRef(*neuronDescription));
		EXPECT_CALL(*neuronDescription, getActivationFunction()).WillRepeatedly(testing::ReturnRef(*activationFunction));

		teachingInput = new TeachingInput<>(3);
		teachingInput->set(0, 1);
		teachingInput->set(1, 2);
		teachingInput->set(2, 3);
		teachingPattern = new Vector<>(2);
		teachingPattern->getEigenValueForEditing()[0] = 9;
		teachingPattern->getEigenValueForEditing()[1] = 8;
		teachingLesson = new TeachingLessonLinearInput(teachingPattern, teachingInput);
	}

	void setUpNeuralNetworkCalculateCall()
	{
		neuralNetworkOutput.getEigenValueForEditing().resize(3);
		neuralNetworkOutput.getEigenValueForEditing()(0) = -1;
		neuralNetworkOutput.getEigenValueForEditing()(1) = -1;
		neuralNetworkOutput.getEigenValueForEditing()(2) = -1;
		EXPECT_CALL(*neuralNetwork, calculateWithoutOutputCopy(testing::Ref(*teachingPattern), testing::Ref(*activationOrder), true)).WillOnce(testing::ReturnRef(neuralNetworkOutput));
	}

	virtual ~TeachingLessonLinearInputTest()
	{
		delete teachingLesson;
		delete neuralNetwork;
		delete activationOrder;
		delete networkTopology;
	}
};

TEST_F(TeachingLessonLinearInputTest, getTeachingInput)
{
	EXPECT_EQ(*teachingInput, teachingLesson->getTeachingInput(*activationFunction));
}

TEST_F(TeachingLessonLinearInputTest, getTeachingPattern)
{
	EXPECT_EQ(*teachingPattern, teachingLesson->getTeachingPattern());
}

TEST_F(TeachingLessonLinearInputTest, tryLesson)
{
	setUpNeuralNetworkCalculateCall();
	const Vector<>& returnedValue = teachingLesson->tryLesson(*neuralNetwork, *activationOrder);
	EXPECT_EQ(neuralNetworkOutput.getEigenValue().size(), returnedValue.getEigenValue().size());
	EXPECT_EQ(neuralNetworkOutput.getEigenValue()[0], returnedValue.getEigenValue()[0]);
	EXPECT_EQ(neuralNetworkOutput.getEigenValue()[1], returnedValue.getEigenValue()[1]);
	EXPECT_EQ(neuralNetworkOutput.getEigenValue()[2], returnedValue.getEigenValue()[2]);
}

TEST_F(TeachingLessonLinearInputTest, getErrormap)
{
	setUpNeuralNetworkCalculateCall();
	Vector<> expected(3);
	expected.getEigenValueForEditing()[0] = 2;
	expected.getEigenValueForEditing()[1] = 3;
	expected.getEigenValueForEditing()[2] = 4;
	const Vector<>& returnedValue = teachingLesson->getErrorVector(*neuralNetwork, *activationOrder);
	EXPECT_EQ(expected.getEigenValue()[0], returnedValue.getEigenValue()[0]);
	EXPECT_EQ(expected.getEigenValue()[1], returnedValue.getEigenValue()[1]);
	EXPECT_EQ(expected.getEigenValue()[2], returnedValue.getEigenValue()[2]);
}

TEST_F(TeachingLessonLinearInputTest, getSpecificError)
{
	setUpNeuralNetworkCalculateCall();
	auto returnedValue = teachingLesson->getSpecificError(*neuralNetwork, *activationOrder);
	EXPECT_EQ(14.5, returnedValue);
}


/**
 * \ file GainMaxCompressorFilter.cpp
 */

#include <iostream>

#include <ATK/Dynamic/GainMaxCompressorFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>
#include <ATK/Core/Utilities.h>

#include <gtest/gtest.h>

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

constexpr gsl::index PROCESSSIZE = 64;

TEST(GainMaxCompressorFilter, softness_test)
{
  ATK::GainFilter<ATK::GainMaxCompressorFilter<double>> filter;
  filter.set_softness(0.5);
  ASSERT_EQ(filter.get_softness(), 0.5);
}

TEST(GainMaxCompressorFilter, softness_range_test)
{
  ATK::GainFilter<ATK::GainMaxCompressorFilter<double>> filter;
  ASSERT_THROW(filter.set_softness(-0.000001), ATK::RuntimeError);
}

TEST(GainMaxCompressorFilter, maxreduc_test)
{
  ATK::GainFilter<ATK::GainMaxCompressorFilter<double>> filter;
  filter.set_max_reduction(0.5);
  ASSERT_EQ(filter.get_max_reduction(), 0.5);
}

TEST(GainMaxCompressorFilter, maxreduc_db_test)
{
  ATK::GainFilter<ATK::GainMaxCompressorFilter<double>> filter;
  filter.set_max_reduction_db(20);
  ASSERT_EQ(filter.get_max_reduction(), 100);
}

TEST(GainMaxCompressorFilter, maxreduc_range_test)
{
  ATK::GainFilter<ATK::GainMaxCompressorFilter<double>> filter;
  ASSERT_THROW(filter.set_max_reduction(-0.000001), ATK::RuntimeError);
}

TEST(GainMaxCompressorFilter, const_1_test)
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainMaxCompressorFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(10);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(1, outdata[i], 0.1);
  }
}

TEST(GainMaxCompressorFilter, const_0_test)
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 0;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainMaxCompressorFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(outdata[i], 1, 0.1);
  }
}

TEST(GainMaxCompressorFilter, const_1_threshold_05_ratio_2_test)
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainMaxCompressorFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(0.5);
  filter.set_ratio(2);
  filter.set_softness(1);
  filter.set_max_reduction(0.0001);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(0.836990714, outdata[i], 0.1);
  }
}

TEST(GainMaxCompressorFilter, const_1_threshold_05_ratio_4_test)
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainMaxCompressorFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(0.5);
  filter.set_ratio(2);
  filter.set_softness(1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(0.836990654, outdata[i], 0.1);
  }
}

TEST(GainMaxCompressorFilter, const_1_threshold_05_ratio_4_max_reduction_test)
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainMaxCompressorFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(0.5);
  filter.set_ratio(4);
  filter.set_softness(0.0001);
  filter.set_max_reduction(.1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_GE(outdata[i], .1 - std::numeric_limits<float>::epsilon());
  }
}