#include "maps.hpp"

#include <algorithm>
#include <iostream>
#include <random>
#include <vector>

#include <Eigen/Core>

#include "perlinnoise.hpp"

using namespace mocka;

void
Maps::randomMapGenerate()
{

  std::default_random_engine eng(info.seed);

  double _resolution = 1 / info.scale;

  double _x_l = -info.sizeX / (2 * info.scale);
  double _x_h = info.sizeX / (2 * info.scale);
  double _y_l = -info.sizeY / (2 * info.scale);
  double _y_h = info.sizeY / (2 * info.scale);
  double _h_l = 0;
  double _h_h = info.sizeZ / info.scale;

  double _w_l, _w_h;
  int    _ObsNum;

  info.nh_private->param("width_min", _w_l, 0.6);
  info.nh_private->param("width_max", _w_h, 1.5);
  info.nh_private->param("obstacle_number", _ObsNum, 10);

  std::uniform_real_distribution<double> rand_x;
  std::uniform_real_distribution<double> rand_y;
  std::uniform_real_distribution<double> rand_w;
  std::uniform_real_distribution<double> rand_h;

  pcl::PointXYZ pt_random;

  rand_x = std::uniform_real_distribution<double>(_x_l, _x_h);
  rand_y = std::uniform_real_distribution<double>(_y_l, _y_h);
  rand_w = std::uniform_real_distribution<double>(_w_l, _w_h);
  rand_h = std::uniform_real_distribution<double>(_h_l, _h_h);

  for (int i = 0; i < _ObsNum; i++)
  {
    double x, y;
    x = rand_x(eng);
    y = rand_y(eng);

    double w, h;
    w = rand_w(eng);
    h = rand_h(eng);

    int widNum = ceil(w / _resolution);
    int heiNum = ceil(h / _resolution);

    int rl, rh, sl, sh;
    rl = -widNum / 2;
    rh = widNum / 2;
    sl = -widNum / 2;
    sh = widNum / 2;

    for (int r = rl; r < rh; r++)
      for (int s = sl; s < sh; s++)
      {
        for (int t = 0; t < heiNum; t++)
        {
          if ((r - rl) * (r - rh + 1) * (s - sl) * (s - sh + 1) * t *
                (t - heiNum + 1) ==
              0)
          {
            pt_random.x = x + r * _resolution;
            pt_random.y = y + s * _resolution;
            pt_random.z = t * _resolution;
            info.cloud->points.push_back(pt_random);
          }
        }
      }
  }

  info.cloud->width    = info.cloud->points.size();
  info.cloud->height   = 1;
  info.cloud->is_dense = true;

  pcl2ros();
}

void
Maps::pcl2ros()
{
  pcl::toROSMsg(*info.cloud, *info.output);
  info.output->header.frame_id = "world";
  ROS_INFO("finish: infill %lf%%",
           info.cloud->width / (1.0 * info.sizeX * info.sizeY * info.sizeZ));
}

void
Maps::perlin3D()
{
  double complexity;
  double fill;
  int    fractal;
  double attenuation;

  info.nh_private->param("complexity", complexity, 0.142857);
  info.nh_private->param("fill", fill, 0.38);
  info.nh_private->param("fractal", fractal, 1);
  info.nh_private->param("attenuation", attenuation, 0.5);

  info.cloud->width  = info.sizeX * info.sizeY * info.sizeZ;
  info.cloud->height = 1;
  info.cloud->points.resize(info.cloud->width * info.cloud->height);

  PerlinNoise noise(info.seed);

  std::vector<double>* v = new std::vector<double>;
  v->reserve(info.cloud->width);
  for (int i = 0; i < info.sizeX; ++i)
  {
    for (int j = 0; j < info.sizeY; ++j)
    {
      for (int k = 0; k < info.sizeZ; ++k)
      {
        double tnoise = 0;
        for (int it = 1; it <= fractal; ++it)
        {
          int    dfv = pow(2, it);
          double ta  = attenuation / it;
          tnoise += ta * noise.noise(dfv * i * complexity,
                                     dfv * j * complexity,
                                     dfv * k * complexity);
        }
        v->push_back(tnoise);
      }
    }
  }
  std::sort(v->begin(), v->end());
  int    tpos = info.cloud->width * (1 - fill);
  double tmp  = v->at(tpos);
  ROS_INFO("threshold: %lf", tmp);

  int pos = 0;
  for (int i = 0; i < info.sizeX; ++i)
  {
    for (int j = 0; j < info.sizeY; ++j)
    {
      for (int k = 0; k < info.sizeZ; ++k)
      {
        double tnoise = 0;
        for (int it = 1; it <= fractal; ++it)
        {
          int    dfv = pow(2, it);
          double ta  = attenuation / it;
          tnoise += ta * noise.noise(dfv * i * complexity,
                                     dfv * j * complexity,
                                     dfv * k * complexity);
        }
        if (tnoise > tmp)
        {
          info.cloud->points[pos].x =
            i / info.scale - info.sizeX / (2 * info.scale);
          info.cloud->points[pos].y =
            j / info.scale - info.sizeY / (2 * info.scale);
          info.cloud->points[pos].z = k / info.scale;
          pos++;
        }
      }
    }
  }
  info.cloud->width = pos;
  ROS_INFO("the number of points before optimization is %d", info.cloud->width);
  info.cloud->points.resize(info.cloud->width * info.cloud->height);
  pcl2ros();
}

void
Maps::recursiveDivision(int xl, int xh, int yl, int yh, Eigen::MatrixXi& maze)
{
  ROS_INFO(
    "generating maze with width %d , height %d", xh - xl + 1, yh - yl + 1);

  if (xl < xh - 3 && yl < yh - 3)
  { // the remaining area is larger than or equal to 5*5, need to add both x
    // wall and y wall
    bool valid = false; // used to judge whether the wall selection is valid
    int  xm    = 0;
    int  ym    = 0;
    ROS_INFO("entered 5*5 mode");
    while (valid == false)
    {
      xm = (std::rand() % (xh - xl - 1) + xl +
            1); // generating random number between xl+1 and xh-1(pointless to
                // add a wall at the sides)
      ym = (std::rand() % (yh - yl - 1) + yl +
            1); // generating random number between yl+1 and yh-1(pointless to
                // add a wall at the sides)
      if (xl - 1 >= 0)
      { // there is a point at xl-1,ym
        if (maze(xl - 1, ym) == 0)
        { // this is an opening,need to change random number
          continue;
        }
      }

      else if (xh + 1 <= maze.cols() - 1)
      { // there is a point at xh+1,ym
        if (maze(xh + 1, ym) == 0)
        { // this is an opening,need to change random number
          continue;
        }
      }

      else if (yl - 1 >= 0)
      { // there is a point at xm,yl-1
        if (maze(xm, yl - 1) == 0)
        { // this is an opening,need to change random number
          continue;
        }
      }

      else if (yh + 1 <= maze.rows() - 1)
      { // there is a point at xm,yh+1
        if (maze(xm, yh + 1) == 0)
        { // this is an opening,need to change random number
          continue;
        }
      }

      valid = true;

    } // xm and ym are now the valid coordinate of the center of the wall
    for (int i = xl; i <= xh; i++)
    {
      maze(i, ym) = 1;
    }
    for (int j = yl; j <= yh; j++)
    {
      maze(xm, j) = 1;
    } // adding walls around the center point
    int d1 = std::rand() % (xm - xl) + xl;
    int d2 = std::rand() % (xh - xm) + xm + 1;
    int d3 = std::rand() % (ym - yl) + yl;
    int d4 =
      std::rand() % (yh - ym) + ym + 1; // generating four possible door points

    int decision = std::rand() % 4; // random selection of three doors
    switch (decision)
    {
      case 0:
        maze(d1, ym) = 0;
        maze(d2, ym) = 0;
        maze(xm, d3) = 0;
        break;

      case 1:
        maze(d1, ym) = 0;
        maze(d2, ym) = 0;
        maze(xm, d4) = 0;
        break;

      case 2:
        maze(d2, ym) = 0;
        maze(xm, d3) = 0;
        maze(xm, d4) = 0;
        break;

      case 3:
        maze(d1, ym) = 0;
        maze(xm, d3) = 0;
        maze(xm, d4) = 0;
        break;
    } // the doors are opened for this cell
    if (yl - 1 >= 0)
    {
      if (maze(xm, yl - 1) == 0)
      {
        maze(xm, yl) = 0;
      }
    }

    if (yh + 1 <= maze.rows() - 1)
    {
      if (maze(xm, yh + 1) == 0)
      {
        maze(xm, yh) = 0;
      }
    }

    if (xl - 1 >= 0)
    {
      if (maze(xl - 1, ym) == 0)
      {
        maze(xl, ym) = 0;
      }
    }

    if (xh + 1 <= maze.cols() - 1)
    {
      if (maze(xh + 1, ym) == 0)
      {
        maze(xh, ym) = 0;
      }
    }

    std::cout << maze << std::endl;
    recursiveDivision(xl, xm - 1, yl, ym - 1, maze);
    recursiveDivision(xm + 1, xh, yl, ym - 1, maze);
    recursiveDivision(xl, xm - 1, ym + 1, yh, maze);
    recursiveDivision(xm + 1, xh, ym + 1, yh, maze);

    ROS_INFO("finished generating maze with width %d , height %d",
             xh - xl + 1,
             yh - yl + 1);
    std::cout << maze << std::endl;
    return;
  } // when the remaining area is larger than or equal to 5*5

  else if (xl < xh - 2 && yl < yh - 2)
  {
    // bool valid     = false; // used to judge whether the wall selection is valid
    int  xm        = 0;
    int  ym        = 0;
    int  doorcount = 0;
    xm             = (std::rand() % (xh - xl - 1) + xl +
          1); // generating random number between xl+1 and xh-1(pointless to
                          // add a wall at the sides)
    ym =
      (std::rand() % (yh - yl - 1) + yl +
       1); // generating random number between yl+1 and yh-1(pointless to
           // add a wall at the sides)
           // xm and ym are now the valid coordinate of the center of the wall
    for (int i = xl; i <= xh; i++)
    {
      maze(i, ym) = 1;
    }
    for (int j = yl; j <= yh; j++)
    {
      maze(xm, j) = 1;
    } // adding walls around the center point
    if (yl - 1 >= 0)
    {
      if (maze(xm, yl - 1) == 0)
      {
        maze(xm, yl) = 0;
        doorcount++;
      }
    }

    if (yh + 1 <= maze.rows() - 1)
    {
      if (maze(xm, yh + 1) == 0)
      {
        maze(xm, yh) = 0;
        doorcount++;
      }
    }

    if (xl - 1 >= 0)
    {
      if (maze(xl - 1, ym) == 0)
      {
        maze(xl, ym) = 0;
        doorcount++;
      }
    }

    if (xh + 1 <= maze.cols() - 1)
    {
      if (maze(xh + 1, ym) == 0)
      {
        maze(xh, ym) = 0;
        doorcount++;
      }
    }

    int d1 = std::rand() % (xm - xl) + xl;
    int d2 = std::rand() % (xh - xm) + xm + 1;
    int d3 = std::rand() % (ym - yl) + yl;
    int d4 =
      std::rand() % (yh - ym) + ym + 1; // generating four possible door points

    int decision = std::rand() % 4; // random selection of three doors
    switch (decision)
    {
      case 0:
        maze(d1, ym) = 0;
        maze(d2, ym) = 0;
        maze(xm, d3) = 0;
        break;

      case 1:
        maze(d1, ym) = 0;
        maze(d2, ym) = 0;
        maze(xm, d4) = 0;
        break;

      case 2:
        maze(d2, ym) = 0;
        maze(xm, d3) = 0;
        maze(xm, d4) = 0;
        break;

      case 3:
        maze(d1, ym) = 0;
        maze(xm, d3) = 0;
        maze(xm, d4) = 0;
        break;
    } // the doors are opened for this cell
    std::cout << maze << std::endl;

    ROS_INFO("finished generating maze with width %d , height %d",
             xh - xl + 1,
             yh - yl + 1);
    std::cout << maze << std::endl;
    return;
  }

  else if (xl < xh - 1 && yl < yh - 2)
  { // the case of 3*4+
    ROS_INFO("entered 3*4+ mode");
    int doorcount = 0;
    int ym        = 0;
    for (int i = yl; i <= yh; i++)
    {
      maze(xl + 1, i) = 1;
    } // filling a center wall
    if (yl - 1 >= 0)
    {
      if (maze(xl + 1, yl - 1) == 0)
      {
        maze(xl + 1, yl) = 0;
        doorcount++;
      }
    }
    if (yh + 1 <= maze.rows() - 1)
    {
      if (maze(xl + 1, yh + 1) == 0)
      {
        maze(xl + 1, yh) = 0;
        doorcount++;
      }
    } // opening doors if the wall blocks the old doors
    if (doorcount == 0)
    {
      ym               = std::rand() % (yh - yl + 1) + yl;
      maze(xl + 1, ym) = 0;
    }
  } // the case of 4+*3
  //
  else if (xl < xh - 2 && yl < yh - 1)
  { // the case of 4+*3
    ROS_INFO("entered 4+*3 mode");
    int doorcount = 0;
    int xm        = 0;
    for (int i = xl; i <= xh; i++)
    {
      maze(i, yl + 1) = 1;
    } // filling a center wall
    if (xl - 1 >= 0)
    {
      if (maze(xl - 1, yl + 1) == 0)
      {
        maze(xl, yl + 1) = 0;
        doorcount++;
      }
    }
    if (xh + 1 <= maze.cols() - 1)
    {
      if (maze(xh + 1, yl + 1) == 0)
      {
        maze(xh, yl + 1) = 0;
        doorcount++;
      }
    } // opening doors if the wall blocks the old doors
    if (doorcount == 0)
    {
      xm               = std::rand() % (xh - xl + 1) + xl;
      maze(xm, yl + 1) = 0;
    }
  } // the case of 4+*3

  else if (xl < xh - 1 && yl < yh - 1)
  { // the case of 3*3
    maze(xl + 1, yl + 1) = 1;
    return;
  }
  else
  {
    ROS_INFO("finished generating maze with width %d , height %d",
             xh - xl + 1,
             yh - yl + 1);
    return;
  }
}

void
Maps::recursizeDivisionMaze(Eigen::MatrixXi& maze)
{
  //! @todo all bugs here...
  int sx = maze.rows();
  int sy = maze.cols();

  int px, py;

  if (sx > 5)
    px = (std::rand() % (sx - 3) + 1);
  else
    return;

  if (sy > 5)
    py = (std::rand() % (sy - 3) + 1);
  else
    return;

  ROS_INFO("debug %d %d %d %d", sx, sy, px, py);

  int x1, x2, y1, y2;

  if (px != 1)
    x1 = (std::rand() % (px - 1) + 1);
  else
    x1 = 1;

  if ((sx - px - 3) > 0)
    x2 = (std::rand() % (sx - px - 3) + px + 1);
  else
    x2 = px + 1;

  if (py != 1)
    y1 = (std::rand() % (py - 1) + 1);
  else
    y1 = 1;

  if ((sy - py - 3) > 0)
    y2 = (std::rand() % (sy - py - 3) + py + 1);
  else
    y2 = py + 1;
  ROS_INFO("%d %d %d %d", x1, x2, y1, y2);

  if (px != 1 && px != (sx - 2))
  {
    for (int i = 1; i < (sy - 1); ++i)
    {
      if (i != y1 && i != y2)
        maze(px, i) = 1;
    }
  }
  if (py != 1 && py != (sy - 2))
  {
    for (int i = 1; i < (sx - 1); ++i)
    {
      if (i != x1 && i != x2)
        maze(i, py) = 1;
    }
  }
  switch (std::rand() % 4)
  {
    case 0:
      maze(x1, py) = 1;
      break;
    case 1:
      maze(x2, py) = 1;
      break;
    case 2:
      maze(px, y1) = 1;
      break;
    case 3:
      maze(px, y2) = 1;
      break;
  }

  if (px > 2 && py > 2)
  {
    Eigen::MatrixXi sub = maze.block(0, 0, px + 1, py + 1);
    recursizeDivisionMaze(sub);
    maze.block(0, 0, px, py) = sub;
  }
  if (px > 2 && (sy - py - 1) > 2)
  {
    Eigen::MatrixXi sub = maze.block(0, py, px + 1, sy - py);
    recursizeDivisionMaze(sub);
    maze.block(0, py, px + 1, sy - py) = sub;
  }
  if (py > 2 && (sx - px - 1) > 2)
  {
    Eigen::MatrixXi sub = maze.block(px, 0, sx - px, py + 1);
    recursizeDivisionMaze(sub);
    maze.block(px, 0, sx - px, py + 1) = sub;
  }
  if ((sx - px - 1) > 2 && (sy - py - 1) > 2)
  {

    Eigen::MatrixXi sub = maze.block(px, py, sy - px, sy - py);

    recursizeDivisionMaze(sub);
    maze.block(px, py, sy - px, sy - py) = sub;
  }
}

void
Maps::maze2D()
{
  double width;
  int    type;
  int    addWallX;
  int    addWallY;
  info.nh_private->param("road_width", width, 1.0);
  info.nh_private->param("add_wall_x", addWallX, 0);
  info.nh_private->param("add_wall_y", addWallY, 0);
  info.nh_private->param("maze_type", type, 1);

  int mx = info.sizeX / (width * info.scale);
  int my = info.sizeY / (width * info.scale);

  Eigen::MatrixXi maze(mx, my);
  maze.setZero();

  switch (type)
  {
    case 1:
      recursiveDivision(0, maze.cols() - 1, 0, maze.rows() - 1, maze);
      break;
  }

  if (addWallX)
  {
    for (int i = 0; i < mx; ++i)
    {
      maze(i, 0)      = 1;
      maze(i, my - 1) = 1;
    }
  }
  if (addWallY)
  {
    for (int i = 0; i < my; ++i)
    {
      maze(0, i)      = 1;
      maze(mx - 1, i) = 1;
    }
  }

  std::cout << maze << std::endl;

  for (int i = 0; i < mx; ++i)
  {
    for (int j = 0; j < my; ++j)
    {
      if (maze(i, j))
      {
        for (int ii = 0; ii < width * info.scale; ++ii)
        {
          for (int jj = 0; jj < width * info.scale; ++jj)
          {
            for (int k = 0; k < info.sizeZ; ++k)
            {
              pcl::PointXYZ pt_random;
              pt_random.x =
                i * width + ii / info.scale - info.sizeX / (2.0 * info.scale);
              pt_random.y =
                j * width + jj / info.scale - info.sizeY / (2.0 * info.scale);
              pt_random.z = k / info.scale;
              info.cloud->points.push_back(pt_random);
            }
          }
        }
      }
    }
  }
  info.cloud->width    = info.cloud->points.size();
  info.cloud->height   = 1;
  info.cloud->is_dense = true;
  pcl2ros();
}

Maps::BasicInfo
Maps::getInfo() const
{
  return info;
}

void
Maps::setInfo(const BasicInfo& value)
{
  info = value;
}

Maps::Maps()
{
}

void
Maps::generate(int type)
{
  switch (type)
  {
    default:
    case 1:
      perlin3D();
      break;
    case 2:
      randomMapGenerate();
      break;
    case 3:
      std::srand(info.seed);
      maze2D();
      break;
    case 4: // generating 3d maze
      std::srand(info.seed);
      Maze3DGen();
      break;
  }
}

pcl::PointXYZ
MazePoint::getPoint()
{
  return point;
}

int
MazePoint::getPoint1()
{
  return point1;
}

int
MazePoint::getPoint2()
{
  return point2;
}

double
MazePoint::getDist1()
{
  return dist1;
}

double
MazePoint::getDist2()
{
  return dist2;
}

void
MazePoint::setPoint(pcl::PointXYZ p)
{
  point = p;
}

void
MazePoint::setPoint1(int p)
{
  point1 = p;
}

void
MazePoint::setPoint2(int p)
{
  point2 = p;
}

void
MazePoint::setDist1(double set)
{
  dist1 = set;
}

void
MazePoint::setDist2(double set)
{
  dist2 = set;
}

void
Maps::Maze3DGen()
{
  // getting required info parameters from the given node
  int    numNodes;
  double connectivity;
  int    nodeRad;
  int    roadRad;

  info.nh_private->param("numNodes", numNodes, 10);
  info.nh_private->param("connectivity", connectivity, 0.5);
  info.nh_private->param("nodeRad", nodeRad, 3);
  info.nh_private->param("roadRad", roadRad, 2);
  ROS_INFO("received parameters : numNodes: %d connectivity: "
           "%f nodeRad: %d roadRad: %d",
           numNodes,
           connectivity,
           nodeRad,
           roadRad);
  // generating random points
  std::vector<pcl::PointXYZ> base;

  for (int i = 0; i < numNodes; i++)
  {
    double rx = std::rand() / RAND_MAX +
                (std::rand() % info.sizeX) / info.scale -
                info.sizeX / (2 * info.scale);
    double ry = std::rand() / RAND_MAX +
                (std::rand() % info.sizeY) / info.scale -
                info.sizeY / (2 * info.scale);
    double rz = std::rand() / RAND_MAX +
                (std::rand() % info.sizeZ) / info.scale -
                info.sizeZ / (2 * info.scale);
    ROS_INFO("point: x: %f , y: %f , z: %f", rx, ry, rz);

    pcl::PointXYZ pt_random;
    pt_random.x = rx;
    pt_random.y = ry;
    pt_random.z = rz;
    base.push_back(pt_random);
  } // generating random cores in the space

  for (int i = 0; i < info.sizeX; i++)
  {
    for (int j = 0; j < info.sizeY; j++)
    {
      for (int k = 0; k < info.sizeZ; k++)
      { // for every scaled coordinate points
        pcl::PointXYZ test;
        test.x = i / info.scale - info.sizeX / (2 * info.scale);
        test.y = j / info.scale - info.sizeY / (2 * info.scale);
        test.z = k / info.scale -
                 info.sizeZ /
                   (2 * info.scale); // marking the corresponding point location

        MazePoint mp;
        mp.setPoint(test);
        mp.setPoint2(-1);
        mp.setPoint1(-1);
        mp.setDist1(10000.0);
        mp.setDist2(100000.0); // setting super large starting values
        for (int ii = 0; ii < numNodes; ii++)
        {
          double dist =
            std::sqrt((base[ii].x - test.x) * (base[ii].x - test.x) +
                      (base[ii].y - test.y) * (base[ii].y - test.y) +
                      (base[ii].z - test.z) * (base[ii].z - test.z));
          if (dist < mp.getDist1())
          {

            mp.setDist2(mp.getDist1());
            mp.setDist1(dist);

            mp.setPoint2(mp.getPoint1());
            mp.setPoint1(ii);
          }
          else if (dist < mp.getDist2())
          {
            mp.setDist2(dist);
            mp.setPoint2(ii);
          } // finding the distances to the nearest two cores
        }
        if (std::abs(mp.getDist2() - mp.getDist1()) < 1 / info.scale)
        { // the tested location is on one of the middle planes
          if ((mp.getPoint1() + mp.getPoint2()) >
                int((1 - connectivity) * numNodes) &&
              (mp.getPoint1() + mp.getPoint2()) <
                int((1 + connectivity) * numNodes))
          { // this is a holed wall
            double judge =
              std::sqrt((base[mp.getPoint1()].x - base[mp.getPoint2()].x) *
                          (base[mp.getPoint1()].x - base[mp.getPoint2()].x) +
                        (base[mp.getPoint1()].y - base[mp.getPoint2()].y) *
                          (base[mp.getPoint1()].y - base[mp.getPoint2()].y) +
                        (base[mp.getPoint1()].z - base[mp.getPoint2()].z) *
                          (base[mp.getPoint1()].z - base[mp.getPoint2()].z));
            if (mp.getDist1() + mp.getDist2() - judge >=
                roadRad / (info.scale * 3))
            {
              info.cloud->points.push_back(mp.getPoint());
            }
          }
          else
          {
            info.cloud->points.push_back(mp.getPoint());
          }
        }
      }
    }
  }

  info.cloud->width  = info.cloud->points.size();
  info.cloud->height = 1;
  ROS_INFO("the number of points before optimization is %d", info.cloud->width);
  info.cloud->points.resize(info.cloud->width * info.cloud->height);
  pcl2ros();
}


#pragma once

////////////////////////////////////////////////////////////////////////////////
#include <polyfem/Common.hpp>
#include <polyfem/LinearSolver.hpp>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <memory>
////////////////////////////////////////////////////////////////////////////////

namespace polyfem {

///
/// @brief         { Solve a linear system Ax = b with Dirichlet boundary
///                conditions. For each Dirichlet node i, we want to ensure that
///                x[i] = b[i]. The implementation follows
///                http://www.math.colostate.edu/~bangerth/videos.676.21.65.html
///
///                For memory efficiency, this function updates in place the
///                matrix of the linear system. We also return the modified rhs
///                to the user }
///
/// @param[in]     solver           { Linear solver class to use for solving the
///                                 system }
/// @param[in,out] A                { Matrix of the linear system without
///                                 boundary conditions. Output: modified
///                                 matrix. }
/// @param[in,out] b                { Right-hand side of the linear system.
///                                 Output: modified rhs for the equivalent
///                                 system. }
/// @param[in]     dirichlet_nodes  { List of ids of Dirichlet nodes }
/// @param[in,out] x                { Unknown vector }
///
Eigen::Vector4d dirichlet_solve(LinearSolver &solver, StiffnessMatrix &A,
	Eigen::VectorXd &b, const std::vector<int> &dirichlet_nodes, Eigen::VectorXd &x, const std::string &save_path = "",
	bool compute_spectrum = false);

} // namespace polyfem


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 complex.arithmetic toolbox - rec/scalar Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of complex.arithmetic components in scalar mode
//////////////////////////////////////////////////////////////////////////////
#include <nt2/include/functions/rec.hpp>

#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/basic.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <boost/dispatch/functor/meta/call.hpp>

#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/mzero.hpp>
#include <nt2/sdk/complex/meta/as_dry.hpp>
#include <nt2/sdk/complex/meta/as_imaginary.hpp>

NT2_TEST_CASE_TPL ( rec_real__1_0,  BOOST_SIMD_REAL_TYPES)
{
  using boost::simd::rec;
  using boost::simd::tag::rec_;
  typedef typename boost::dispatch::meta::as_integer<T>::type iT;
  typedef typename std::complex<T> cT;
  typedef typename boost::dispatch::meta::call<rec_(cT)>::type r_t;
  typedef typename boost::simd::meta::scalar_of<r_t>::type sr_t;
  typedef typename boost::simd::meta::scalar_of<r_t>::type ssr_t;
  typedef typename nt2::meta::as_dry<T>::type dT;

  // return type conformity test
  NT2_TEST_EXPR_TYPE( nt2::rec(cT(1))
                    , boost::mpl::_
                    , (cT)
                    );

  double ulpd;
  ulpd=0.0;

  // std::cout << nt2::type_id(nt2::I<T>()) << std::endl;
  // specific values tests
   NT2_TEST_EQUAL(nt2::rec(cT(1)), T(1));
   NT2_TEST_EQUAL(nt2::rec(cT(nt2::Inf<T>(), nt2::Inf<T>())), cT(nt2::Zero<cT>()));
   NT2_TEST_EQUAL(nt2::rec(cT(nt2::Minf<T>(), nt2::Minf<T>())), cT(nt2::Zero<cT>()));
   NT2_TEST_EQUAL(nt2::rec(cT(nt2::Inf<T>(), nt2::Zero<T>())), cT(nt2::Zero<cT>()));
   NT2_TEST_EQUAL(nt2::rec(cT(nt2::Zero<T>(), nt2::Inf<T>())), cT(nt2::Zero<cT>()));
   NT2_TEST_EQUAL(nt2::rec(cT(nt2::Minf<T>(), nt2::Zero<T>())), cT(nt2::Zero<cT>()));
   NT2_TEST_EQUAL(nt2::rec(cT(nt2::Zero<T>(), nt2::Minf<T>())), cT(nt2::Zero<cT>()));

   NT2_TEST_EQUAL(nt2::rec(cT(nt2::Zero<T>(), nt2::Zero<T>())), cT(nt2::Inf<cT>()));
   NT2_TEST_EQUAL(nt2::rec(cT(nt2::Mzero<T>(), nt2::Zero<T>())), cT(nt2::Minf<cT>()));

   NT2_TEST_EQUAL(nt2::rec(cT(nt2::Nan<T>(), nt2::Nan<T>())), cT(nt2::Nan<T>(), nt2::Nan<T>()));
   NT2_TEST_EQUAL(nt2::rec(cT(nt2::Nan<T>(), nt2::One<T>())), cT(nt2::Nan<T>(), nt2::Nan<T>()));
   NT2_TEST_EQUAL(nt2::rec(cT(nt2::One<T>(), nt2::Nan<T>())), cT(nt2::Nan<T>(), nt2::Nan<T>()));
   NT2_TEST_EQUAL(nt2::rec(cT(nt2::Nan<T>(), nt2::Zero<T>())), cT(nt2::Nan<cT>()));
   NT2_TEST_EQUAL(nt2::rec(cT(nt2::Zero<T>(), nt2::Nan<T>())), cT(nt2::Zero<T>(), nt2::Nan<T>()));

   std::complex < T > a(1, 0);
   NT2_TEST_EQUAL(nt2::rec(a), nt2::One<T>());
   std::complex < T > b(1, 1);
   NT2_TEST_ULP_EQUAL(nt2::rec(b), cT(0.5, -0.5), 0);
   //    NT2_TEST_ULP_EQUAL(rec(nt2::Inf<dT>()), dT(nt2::Zero<T>()), 0);
   //    NT2_TEST_ULP_EQUAL(rec(nt2::Minf<dT>()), dT(nt2::Zero<T>()), 0);
   //    NT2_TEST_ULP_EQUAL(rec(nt2::Mone<dT>()), dT(nt2::Mone<T>()), 0);
   //    NT2_TEST_ULP_EQUAL(rec(nt2::Nan<dT>()), dT(nt2::Nan<T>()), 0);
   //    NT2_TEST_ULP_EQUAL(rec(nt2::One<dT>()), dT(nt2::One<T>()), 0);
   //    NT2_TEST_ULP_EQUAL(rec(nt2::Zero<dT>()), dT(nt2::Inf<T>()), 0);
} // end of test for floating_


#include "catch.hpp"

#include <blitzml/base/common.h>
#include <blitzml/dataset/sparse_dataset.h>
#include <blitzml/dataset/dense_dataset.h>
#include <blitzml/dataset/sparse_binary_dataset.h>

using std::vector;

using namespace BlitzML;

TEST_CASE( "test_sparse_column", "[column]" ) {
  index_t indices[5] = {1, 3, 5, 7, 9};
  float data[5] = {-1.0, 2.6, 8.1, -0.1, 0.2};
  SparseColumn<float> col(indices, data, 5, 12);

  vector<value_t> v(12, 2.0);
  v[1] = 0.1; v[3] = -5.0; v[5] = 2.0; v[7] = 0.8; v[9] = -0.1;
  REQUIRE( col.inner_product(v) == Approx(3.) );

  vector<value_t> w(12, 7.0);
  w[1] = 0.6; w[3] = 0.4; w[5] = -0.1; w[7] = 10.; w[9] = 0.5;
  REQUIRE( col.weighted_inner_product(v, w) == Approx(-7.69) );
  REQUIRE( col.weighted_norm_sq(w) == Approx(-3.137) );

  col.add_multiple(v, 2.8);
  REQUIRE( v[0] == 2.0 );
  REQUIRE( v[1] == Approx(-2.7) );
  REQUIRE( v[7] == Approx(0.52) );
  REQUIRE( v[9] == Approx(0.46) );
  REQUIRE( v[11] == 2.0 );
  col.add_multiple(v, -2.8);

  col.weighted_add_multiple(v, w, -2.8);
  REQUIRE( v[0] == 2.0 );
  REQUIRE( v[1] == Approx(1.78) );
  REQUIRE( v[7] == Approx(3.6) );
  REQUIRE( v[9] == Approx(-0.38) );
  REQUIRE( v[11] == 2.0 );

  REQUIRE( col.sum() == Approx(9.8) );
  REQUIRE( col.mean() == Approx(9.8 / 12.) );
  REQUIRE( col.l2_norm_sq() == Approx(73.42) );

  REQUIRE( col.indices_begin()[0] == 1 );
  REQUIRE( col.indices_begin()[4] == 9 );
}

TEST_CASE( "empty_sparse_column", "[column]" ) {
  index_t* indices;
  int* data;
  SparseColumn<int> col(indices, data, 0, 100);

  vector<value_t> v(100, 2.0);
  vector<value_t> w(100, 4.0);
  REQUIRE( col.inner_product(v) == 0. );
  REQUIRE( col.weighted_inner_product(v, w) == 0. );
  REQUIRE( col.weighted_norm_sq(w) == 0. );
  REQUIRE( col.sum() == 0. );
  REQUIRE( col.mean() == 0. );
  REQUIRE( col.l2_norm_sq() == 0. );
  REQUIRE( col.indices_begin() == col.indices_end() );
}

TEST_CASE( "sparse_column_types", "[column]" ) {
  index_t indices[2] = {0, 2};
  double data_double[2] = {1.0, 3.0};
  float data_float[2] = {1.0, 3.0};
  int data_int[2] = {1.0, 3.0};
  bool data_bool[2] = {true, false};

  SparseColumn<double> col_double(indices, data_double, 2, 3);
  SparseColumn<float> col_float(indices, data_float, 2, 3);
  SparseColumn<int> col_int(indices, data_int, 2, 3);
  SparseColumn<bool> col_bool(indices, data_bool, 2, 3);

  REQUIRE( col_int.mean() == 4./3 );
  REQUIRE( col_int.mean() == 4./3 );
  REQUIRE( col_int.mean() == 4./3 );
  REQUIRE( col_bool.mean() == 1./3 );
}

TEST_CASE( "test_binary_column", "[column]" ) {
  index_t indices[5] = {1, 3, 5, 7, 9};
  SparseBinaryColumn col(indices, 5, 12);

  vector<value_t> v(12, 2.0);
  v[1] = 0.1; v[3] = -5.0; v[5] = 2.0; v[7] = 0.8; v[9] = -0.1;
  REQUIRE( col.inner_product(v) == Approx(-2.2) );

  vector<value_t> w(12, 7.0);
  w[1] = 0.6; w[3] = 0.4; w[5] = -0.1; w[7] = 10.; w[9] = 0.5;
  REQUIRE( col.weighted_inner_product(v, w) == Approx(5.81) );
  REQUIRE( col.weighted_norm_sq(w) == Approx(11.4) );

  col.add_multiple(v, 2.8);
  REQUIRE( v[0] == 2.0 );
  REQUIRE( v[1] == Approx(2.9) );
  REQUIRE( v[7] == Approx(3.6) );
  REQUIRE( v[9] == Approx(2.7) );
  REQUIRE( v[11] == 2.0 );
  col.add_multiple(v, -2.8);

  col.weighted_add_multiple(v, w, -2.8);
  REQUIRE( v[0] == 2.0 );
  REQUIRE( v[1] == Approx(-1.58) );
  REQUIRE( v[7] == Approx(-27.2) );
  REQUIRE( v[9] == Approx(-1.5) );
  REQUIRE( v[11] == 2.0 );

  REQUIRE( col.sum() == Approx(5) );
  REQUIRE( col.mean() == Approx(5. / 12.) );
  REQUIRE( col.l2_norm_sq() == Approx(5.) );

  REQUIRE( col.indices_begin()[0] == 1 );
  REQUIRE( col.indices_begin()[4] == 9 );

  index_t* empty_indices;
  SparseBinaryColumn col_empty(empty_indices, 0, 10);
  REQUIRE( col_empty.weighted_inner_product(v, w) == 0. );
  REQUIRE( col_empty.mean() == 0. );
  REQUIRE( col_empty.indices_begin() == col_empty.indices_end() );
}

TEST_CASE( "dense_column", "[column]" ) {
  float data[4] = {0.5, -0.1, 2.9, -8.1};
  DenseColumn<float> col(data, 4);

  vector<value_t> v(4, 0.);
  v[0] = -1.0; v[1] = 3.0; v[2] = 9.9; v[3] = -0.1;
  vector<value_t> w(4, 0.);
  w[0] = 2.2; w[1] = -0.2; w[2] = 0.0; w[3] = -0.9;

  REQUIRE( col.inner_product(v) == Approx(28.72) );
  REQUIRE( col.weighted_inner_product(v, w) == Approx(-1.769) );
  REQUIRE( col.weighted_norm_sq(w) == Approx( -58.501 ) );

  col.add_multiple(v, 5.5);
  REQUIRE( v[0] == Approx(1.75) );
  REQUIRE( v[2] == Approx(25.85) );
  REQUIRE( v[3] == Approx(-44.65) );
  col.add_multiple(v, -5.5);

  col.weighted_add_multiple(v, w, -1.5);
  REQUIRE( v[0] == Approx(-2.65) );
  REQUIRE( v[2] == Approx(9.9) );
  REQUIRE( v[3] == Approx(-11.035) );

  REQUIRE( col.sum() == Approx(-4.8) );
  REQUIRE( col.mean() == Approx(-4.8/4.) );
  REQUIRE( col.l2_norm_sq() == Approx(74.28) );
}

TEST_CASE( "dense_column_types", "[column]" ) {
  double data_double[2] = {0., 1.};
  float data_float[2] = {0., 1.};
  int data_int[2] = {0., 1.};
  bool data_bool[2] = {false, true};

  DenseColumn<double> col_double(data_double, 2);
  DenseColumn<float> col_float(data_float, 2);
  DenseColumn<int> col_int(data_int, 2);
  DenseColumn<bool> col_bool(data_bool, 2);

  REQUIRE( col_double.mean() == 0.5 );
  REQUIRE( col_float.mean() == 0.5 );
  REQUIRE( col_int.mean() == 0.5 );
  REQUIRE( col_bool.mean() == 0.5 );

  vector<value_t> v(2, 1.0); v[1] = 2.5;
  REQUIRE( col_double.inner_product(v) == 2.5 );
  REQUIRE( col_float.inner_product(v) == 2.5 );
  REQUIRE( col_int.inner_product(v) == 2.5 );
  REQUIRE( col_bool.inner_product(v) == 2.5 );
}


#ifndef UnitConvert_Unit_hpp
#define UnitConvert_Unit_hpp

/** @file Unit.hpp
  * @brief
  * @author C.D. Clark III
  * @date 09/03/18
  */

#include <boost/optional.hpp>

#include "./Dimension.hpp"

namespace UnitConvert {
/**
 * A class representing a unit. A unit has a dimension, a scale, and possibly an
 * offset.
 */
class Unit
{
 protected:
  Dimension m_Dimension;
  double m_Scale;
  boost::optional<double> m_Offset;

 public:

  Unit():m_Dimension(BaseDimension<Dimension::Name::Dimensionless>()),m_Scale(1){}
  Unit(double s, const Dimension& d);
  Unit(double s, double o, const Dimension& d);

  /**
   * Return the unit's scale.
   * This is the "size" of the unit compared to the unit consisting of base
   * units only.
   */
  double
  scale() const;

  /**
   * Return the dimension of this unit.
   */
  const Dimension&
  dimension() const;

  /**
   * Reuturn the offset for this dimension.
   *
   * The offset is always expressed in the scale of the unit. It is the
   * value that must be subtracted to make the unit an absolute unit.
   */
  double
  offset() const;

  /**
   * Return true if this is an offset unit
   */
  bool
  is_offset() const;

  /**
   * Output operator for the unit.
   */
  friend std::ostream&
  operator<<(std::ostream& out, const Unit& unit);

  /**
   * Return the unit resulting from multiplying this unit by another.
   */
  Unit operator*(const Unit& other) const;

  /**
   * Return the unit resulting from multiplying this unit by a scale.
   */
  Unit operator*(const double& scale) const;

  /**
   * Free function version of operator* to support scaling factor
   * in front of units.
   */
  friend Unit operator*(const double& scale, const Unit& other)
  {
    return other * scale;
  }

  /**
   * Multiplying this unit by another.
   */
  Unit&
  operator*=(const Unit& other);

  /**
   * Scale this unit
   */
  Unit&
  operator*=(const double& scale);

  /**
   * Return the unit resulting from dividing this unit by another.
   */
  Unit
  operator/(const Unit& other) const;

  /**
   * Return the unit resulting from dividing this unit by a scale factor.
   */
  Unit
  operator/(const double& scale) const;

  Unit&
  operator/=(const Unit& other);

  Unit&
  operator/=(const double& scale);

  /**
   * Return the unit resulting from adding an offset to this unit.
   *
   * The resulting unit will have the same scale.
   */
  Unit
  operator+(const double& offset) const;

  Unit&
  operator+=(const double& offset);

  /**
   * Return the unit resulting from subtracting an offset to this unit.
   *
   * The resulting unit will have the same scale.
   */
  Unit
  operator-(const double& offset) const;

  Unit&
  operator-=(const double& offset);
};

/**
 * A class for constructing base units. Base units are units for one of the base
 * dimensions
 * that have a scale of 1 and a zero offset.
 */
template <Dimension::Name DIM>
class BaseUnit : public Unit
{
 public:
  BaseUnit() : Unit(1., BaseDimension<DIM>{}) {}
};

}

#endif // include protector


#define ARMA_DONT_USE_WRAPPER

#include <armadillo>

arma::mat calc_asymptotic_variance(const arma::mat& Sigma_ll,
                                   const arma::mat& Sigma_ls,
                                   const arma::mat& Sigma_ss,
                                   const arma::field <arma::mat >& Sigma_ss_blockwise,
                                   double sigma2_s,
                                   unsigned int n,
                                   const arma::mat& Xl_test,
                                   const arma::mat& Xs_test);

arma::mat calc_var_betal(const arma::mat& Sigma_ll,
                         const arma::mat& Sigma_ls,
                         const arma::mat& Sigma_ss,
                         const arma::mat& A_inverse,
                         unsigned int n);

  arma::mat calc_var_betas(const arma::mat& Sigma_ss,
                           const arma::mat& Sigma_ls,
                           const arma::mat& A_inverse,
                           double sigma2_s,
                           unsigned int n,
                           const arma::mat& var_bl);

  arma::mat calc_A_inverse(const arma::field <arma::mat >& field,
                           double sigma2_s,
                           unsigned int n);

arma::mat BlockDiag( const arma::field<arma::mat>& x );

arma::mat ConcatenateColumns( const arma::field<arma::mat>& x );

arma::field <arma::mat> assembleMatrices(const arma::field < arma::mat>& field);


#include "teca_descriptive_statistics.h"

#include "teca_cartesian_mesh.h"
#include "teca_table.h"
#include "teca_array_collection.h"
#include "teca_variant_array.h"
#include "teca_metadata.h"

#include <algorithm>
#include <iostream>
#include <string>
#include <set>
#include <cmath>

#if defined(TECA_HAS_BOOST)
#include <boost/program_options.hpp>
#endif

using std::string;
using std::vector;
using std::set;
using std::cerr;
using std::endl;

//#define TECA_DEBUG

namespace internal
{
template <typename num_t>
void quartiles(const num_t *ptr, size_t n, num_t &lq, num_t &med, num_t &uq)
{
    size_t nb = n*sizeof(num_t);
    num_t *tmp = static_cast<num_t*>(malloc(nb));
    memcpy(tmp, ptr, nb);
    size_t n25 = n/4;
    size_t n50 = n/2;
    size_t n75 = (n*3)/4;
    std::partial_sort(tmp, tmp+n75+1, tmp+n);
    lq = tmp[n25];
    med = tmp[n50];
    uq = tmp[n75];
    free(tmp);
}

template <typename num_t>
void quartiles2(const num_t *ptr, size_t n, num_t &lq, num_t &med, num_t &uq)
{
    size_t nb = n*sizeof(num_t);
    num_t *tmp = static_cast<num_t*>(malloc(nb));
    memcpy(tmp, ptr, nb);

    size_t n25 = n/4;
    std::nth_element(tmp, tmp+n25, tmp+n);
    lq = tmp[n25];

    size_t n50 = n/2;
    std::nth_element(tmp, tmp+n50, tmp+n);
    med = tmp[n50];

    size_t n75 = (n*3)/4;
    std::nth_element(tmp, tmp+n75, tmp+n);
    uq = tmp[n75];

    free(tmp);
}

template <typename num_t>
num_t min(const num_t *ptr, size_t n)
{
    num_t min = std::numeric_limits<num_t>::max();
    for (size_t i = 0; i < n; ++i)
        min = min > ptr[i] ? ptr[i] : min;
    return min;
}

template <typename num_t>
num_t max(const num_t *ptr, size_t n)
{
    num_t max = std::numeric_limits<num_t>::lowest();
    for (size_t i = 0; i < n; ++i)
        max = max < ptr[i] ? ptr[i] : max;
    return max;
}

template <typename num_t>
num_t sum(const num_t *ptr, size_t n)
{
    num_t s = num_t();
    for (size_t i = 0; i < n; ++i)
        s += ptr[i];
    return s;
}

template <typename num_t>
num_t var(const num_t *ptr, size_t n, num_t av)
{
    if (!n) return num_t();
    num_t v = num_t();
    for (size_t i = 0; i < n; ++i)
    {
        num_t d = ptr[i] - av;
        v += d*d;
    }
    v /= num_t(n);
    return v;
}
};

// --------------------------------------------------------------------------
teca_descriptive_statistics::teca_descriptive_statistics()
{
    this->set_number_of_input_connections(1);
    this->set_number_of_output_ports(1);
}

// --------------------------------------------------------------------------
teca_descriptive_statistics::~teca_descriptive_statistics()
{}

#if defined(TECA_HAS_BOOST)
// --------------------------------------------------------------------------
void teca_descriptive_statistics::get_properties_description(
    const string &prefix, options_description &global_opts)
{
    options_description opts("Options for "
        + (prefix.empty()?"teca_descriptive_statistics":prefix));

    opts.add_options()
        TECA_POPTS_GET(std::vector<std::string>, prefix, dependent_variables,
            "list of arrays to compute statistics for")
        ;

    global_opts.add(opts);
}

// --------------------------------------------------------------------------
void teca_descriptive_statistics::set_properties(
    const string &prefix, variables_map &opts)
{
    TECA_POPTS_SET(opts, std::vector<std::string>, prefix, dependent_variables)
}
#endif

// --------------------------------------------------------------------------
void teca_descriptive_statistics::get_dependent_variables(
    const teca_metadata &request, std::vector<std::string> &dep_vars)
{
    dep_vars = this->dependent_variables;

    if (dep_vars.empty())
    {
        std::string key = "teca_descriptive_statistics::dependent_variables";
        if (request.has(key))
            request.get(key, dep_vars);
    }
}

// --------------------------------------------------------------------------
std::vector<teca_metadata>
teca_descriptive_statistics::get_upstream_request(
    unsigned int port, const std::vector<teca_metadata> &input_md,
    const teca_metadata &request)
{
#ifdef TECA_DEBUG
    cerr << teca_parallel_id()
        << "teca_descriptive_statistics::get_upstream_request" << endl;
#endif
    (void)port;
    (void)input_md;

    vector<teca_metadata> up_reqs;

    // copy the incoming request to preserve the downstream
    // requirements and add the arrays we need
    teca_metadata req(request);

    std::set<std::string> arrays;
    if (req.has("arrays"))
        req.get("arrays", arrays);

    // intercept request for our output
    //arrays.erase(this->get_derived_variable(request));

    // get the names of the arrays we need to request
    std::vector<std::string> dep_vars;
    this->get_dependent_variables(req, dep_vars);

    size_t n = dependent_variables.size();
    for (size_t i = 0; i < n; ++i)
        arrays.insert(dep_vars[i]);

    req.set("arrays", arrays);
    up_reqs.push_back(req);

    return up_reqs;
}

// --------------------------------------------------------------------------
const_p_teca_dataset teca_descriptive_statistics::execute(
    unsigned int port,
    const std::vector<const_p_teca_dataset> &input_data,
    const teca_metadata &request)
{
#ifdef TECA_DEBUG
    cerr << teca_parallel_id() << "teca_descriptive_statistics::execute" << endl;
#endif
    (void)port;

    // get the input mesh
    const_p_teca_cartesian_mesh in_mesh
        = std::dynamic_pointer_cast<const teca_cartesian_mesh>(input_data[0]);

    if (!in_mesh)
    {
        TECA_ERROR("dataset is not a teca_cartesian_mesh")
        return nullptr;
    }

    // set up the output
    p_teca_table table = teca_table::New();
    table->declare_columns("step", long(), "time", double());

    std::string calendar;
    in_mesh->get_calendar(calendar);
    table->set_calendar(calendar);

    std::string time_units;
    in_mesh->get_time_units(time_units);
    table->set_time_units(time_units);

    unsigned long step;
    in_mesh->get_time_step(step);
    table << step;

    double time;
    in_mesh->get_time(time);
    table << time;

    // dependent variables
    std::vector<std::string> dep_var_names;
    this->get_dependent_variables(request, dep_var_names);

    // for each variable
    size_t n_dep_vars = dep_var_names.size();
    for (size_t i = 0; i < n_dep_vars; ++i)
    {
        const std::string &dep_var_name = dep_var_names[i];

        // get the array
        const_p_teca_variant_array dep_var
            = in_mesh->get_point_arrays()->get(dep_var_name);
        if (!dep_var)
        {
            TECA_ERROR("dependent variable " << i << " \""
                << dep_var_name << "\" not present.")
            return nullptr;
        }

        TEMPLATE_DISPATCH(const teca_variant_array_impl,
            dep_var.get(),

            size_t n = dep_var->size();
            const NT *pv = static_cast<const TT*>(dep_var.get())->get();

            // compute stats
            NT mn = internal::min(pv, n);
            NT mx = internal::max(pv, n);
            NT av = internal::sum(pv, n)/NT(n);
            NT vr = internal::var(pv, n, av);
            NT lq; NT med; NT uq;
            internal::quartiles2(pv, n, lq, med, uq);

            // add to output table
            table->declare_columns(
                "min " + dep_var_name, NT(), "max " + dep_var_name, NT(),
                "avg " + dep_var_name, NT(), "var " + dep_var_name, NT(),
                "low_q " + dep_var_name, NT(), "med " + dep_var_name, NT(),
                "up_q " + dep_var_name, NT());

            table << mn << mx << av << vr << lq << med << uq;
            )
    }

    return table;
}


//==============================================================================
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/exponential/include/functions/log1p.hpp>

#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>
#include <nt2/sdk/meta/as_floating.hpp>

#include <nt2/include/constants/eps.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/smallestposval.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/log_2.hpp>

NT2_TEST_CASE_TPL ( log1p,  NT2_REAL_TYPES)
{
  using nt2::log1p;
  using nt2::tag::log1p_;

  typedef typename nt2::meta::call<log1p_(T)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_ULP_EQUAL(log1p(nt2::Nan<T>()), nt2::Nan<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(log1p(nt2::Minf<T>()), nt2::Nan<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(log1p(nt2::Mone<T>()), nt2::Minf<r_t>(), 0.5);
#endif
  NT2_TEST_ULP_EQUAL(log1p(nt2::Eps<T>()), nt2::Eps<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(log1p(nt2::Eps<T>()), nt2::Eps<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(log1p(nt2::One<T>()), nt2::Log_2<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(log1p(nt2::Zero<T>()), nt2::Zero<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(log1p(nt2::Smallestposval<T>()), nt2::Smallestposval<T>(), 0.5);
}


#include "graph/graph.hpp"

#include "heuristics/mst.hpp"
#include "heuristics/null.hpp"
#include "heuristics/shortest_path.hpp"

#include <tuple>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN
#define BOOST_TEST_ALTERNATIVE_INIT_API
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE(heuristics_null) {
	awesome::NullHeuristic::MapGraph map{{"0", "1", {1}},
	                                     {"1", "2", {2}},
	                                     {"2", "3", {3}},
	                                     {"3", "4", {4}},
	                                     {"4", "0", {5}},
	                                     {"0", "2", {1}},
	                                     {"0", "3", {2}},
	                                     {"1", "3", {1}},
	                                     {"1", "4", {3}},
	                                     {"2", "4", {4}}};

	awesome::NullHeuristic::Graph psg;
	auto degradedMap = map;

	awesome::NullHeuristic heur(psg, map);
	BOOST_CHECK_EQUAL(heur(degradedMap, psg["0"]), 0);

	degradedMap.removeNode(degradedMap["0"]);

	BOOST_CHECK_EQUAL(heur(degradedMap, psg["1"]), 0);
}

BOOST_AUTO_TEST_CASE(heuristics_shortest_path_basic) {
	awesome::ShortestPathHeuristic::MapGraph map{{"0", "1", {1}},
	                                             {"1", "2", {2}},
	                                             {"2", "3", {3}},
	                                             {"3", "4", {4}},
	                                             {"4", "0", {5}},
	                                             {"0", "2", {1}},
	                                             {"0", "3", {2}},
	                                             {"1", "3", {1}},
	                                             {"1", "4", {3}},
	                                             {"2", "4", {4}}};

	awesome::ShortestPathHeuristic::Graph psg;
	auto degradedMap = map;

	awesome::ShortestPathHeuristic heur(psg, map);
	BOOST_CHECK_EQUAL(heur(degradedMap, psg["0"]), 4);

	degradedMap.removeNode(degradedMap["0"]);

	BOOST_CHECK_EQUAL(heur(degradedMap, psg["1"]), 3);
}

BOOST_AUTO_TEST_CASE(heuristics_mst_basic) {
	awesome::MSTHeuristic::MapGraph map{{"0", "1", {1}},
	                                    {"1", "2", {2}},
	                                    {"2", "3", {3}},
	                                    {"3", "4", {4}},
	                                    {"4", "0", {5}},
	                                    {"0", "2", {1}},
	                                    {"0", "3", {2}},
	                                    {"1", "3", {1}},
	                                    {"1", "4", {3}},
	                                    {"2", "4", {4}}};

	awesome::MSTHeuristic::Graph psg;
	auto degradedMap = map;

	awesome::MSTHeuristic heur(psg, map);
	BOOST_CHECK_EQUAL(heur(degradedMap, psg["0"]), 6);

	degradedMap.removeNode(degradedMap["0"]);

	BOOST_CHECK_EQUAL(heur(degradedMap, psg["1"]), 6);
}


// Copyright (c) 2015-2020 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#ifndef hardy_weinberg_model_hpp
#define hardy_weinberg_model_hpp

#include <vector>
#include <unordered_map>
#include <functional>

#include <boost/optional.hpp>

#include "core/types/haplotype.hpp"
#include "core/types/indexed_haplotype.hpp"
#include "core/types/genotype.hpp"

namespace octopus {

class HardyWeinbergModel
{
public:
    using LogProbability = double;
    using GenotypeReference = std::reference_wrapper<const Genotype<IndexedHaplotype<>>>;
    using HaplotypeFrequencyVector = std::vector<double>;

    HardyWeinbergModel() = default;

    HardyWeinbergModel(IndexedHaplotype<> reference);
    HardyWeinbergModel(HaplotypeFrequencyVector haplotype_frequencies);

    HardyWeinbergModel(const HardyWeinbergModel&)            = default;
    HardyWeinbergModel& operator=(const HardyWeinbergModel&) = default;
    HardyWeinbergModel(HardyWeinbergModel&&)                 = default;
    HardyWeinbergModel& operator=(HardyWeinbergModel&&)      = default;

    ~HardyWeinbergModel() = default;

    void set_frequencies(HaplotypeFrequencyVector haplotype_frequencies);

    HaplotypeFrequencyVector& frequencies() noexcept;
    const HaplotypeFrequencyVector& frequencies() const noexcept;

    LogProbability evaluate(const Genotype<IndexedHaplotype<>>& genotype) const;
    LogProbability evaluate(const std::vector<Genotype<IndexedHaplotype<>>>& genotypes) const;
    LogProbability evaluate(const std::vector<GenotypeReference>& genotypes) const;

private:
    boost::optional<IndexedHaplotype<>> reference_;
    mutable HaplotypeFrequencyVector haplotype_frequencies_;
    mutable bool empirical_;
};

} // namespace octopus

#endif


#define BOOST_TEST_MODULE test_fmsub
#include <boost/test/included/unit_test.hpp>
#include <boost/mpl/list.hpp>

#include <mave/mave.hpp>
#include <tests/generate_random_matrices.hpp>
#include <tests/tolerance.hpp>

typedef boost::mpl::list<
    mave::vector<double, 3>,    mave::vector<float, 3>,
    mave::matrix<double, 3, 3>, mave::matrix<float, 3, 3>
    > test_targets;

constexpr std::size_t N = 12000;

BOOST_AUTO_TEST_CASE_TEMPLATE(fmsub_1arg, T, test_targets)
{
    std::mt19937 mt(123456789);
    const auto scalars  = mave::test::generate_random<typename T::value_type>(N, mt);
    const auto vectors1 = mave::test::generate_random<T>(N, mt);
    const auto vectors2 = mave::test::generate_random<T>(N, mt);

    for(std::size_t i=0; i<N; ++i)
    {
        const auto  s  = scalars.at(i);
        const auto& v1 = vectors1.at(i);
        const auto& v2 = vectors2.at(i);
        const auto  v3 = mave::fmsub(s, v1, v2);

        for(std::size_t j=0; j<v1.size(); ++j)
        {
            BOOST_TEST(v3[j] == std::fma(s, v1[j], -v2[j]),
                       mave::test::tolerance<typename T::value_type>());
        }
        BOOST_TEST(v1.diagnosis());
        BOOST_TEST(v2.diagnosis());
        BOOST_TEST(v3.diagnosis());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(fmsub_2arg, T, test_targets)
{
    std::mt19937 mt(123456789);
    const auto scalars  = mave::test::generate_random<typename T::value_type>(N, mt);
    const auto vectors1 = mave::test::generate_random<T>(N, mt);
    const auto vectors2 = mave::test::generate_random<T>(N, mt);

    for(std::size_t i=0; i<N; i+=2)
    {
        const auto  s1  = scalars.at(i);
        const auto  s2  = scalars.at(i+1);

        const auto& v11 = vectors1.at(i);
        const auto& v12 = vectors1.at(i+1);
        const auto& v21 = vectors2.at(i);
        const auto& v22 = vectors2.at(i+1);

        const auto  v3  = mave::fmsub(std::make_tuple( s1,  s2),
                                             std::tie(v11, v12),
                                             std::tie(v21, v22));

        const auto& v31 = std::get<0>(v3);
        const auto& v32 = std::get<1>(v3);

        for(std::size_t j=0; j<v11.size(); ++j)
        {
            BOOST_TEST(v31[j] == std::fma(s1, v11[j], -v21[j]),
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v32[j] == std::fma(s2, v12[j], -v22[j]),
                       mave::test::tolerance<typename T::value_type>());
        }

        BOOST_TEST(v11.diagnosis());
        BOOST_TEST(v12.diagnosis());
        BOOST_TEST(v21.diagnosis());
        BOOST_TEST(v22.diagnosis());
        BOOST_TEST(v31.diagnosis());
        BOOST_TEST(v32.diagnosis());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(fmsub_3arg, T, test_targets)
{
    std::mt19937 mt(123456789);
    const auto scalars  = mave::test::generate_random<typename T::value_type>(N, mt);
    const auto vectors1 = mave::test::generate_random<T>(N, mt);
    const auto vectors2 = mave::test::generate_random<T>(N, mt);

    for(std::size_t i=0; i<N; i+=3)
    {
        const auto  s1  = scalars.at(i);
        const auto  s2  = scalars.at(i+1);
        const auto  s3  = scalars.at(i+2);

        const auto& v11 = vectors1.at(i);
        const auto& v12 = vectors1.at(i+1);
        const auto& v13 = vectors1.at(i+2);
        const auto& v21 = vectors2.at(i);
        const auto& v22 = vectors2.at(i+1);
        const auto& v23 = vectors2.at(i+2);

        const auto  v3  = mave::fmsub(std::make_tuple( s1,  s2,  s3),
                                             std::tie(v11, v12, v13),
                                             std::tie(v21, v22, v23));

        const auto& v31 = std::get<0>(v3);
        const auto& v32 = std::get<1>(v3);
        const auto& v33 = std::get<2>(v3);

        for(std::size_t j=0; j<v11.size(); ++j)
        {
            BOOST_TEST(v31[j] == std::fma(s1, v11[j], -v21[j]),
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v32[j] == std::fma(s2, v12[j], -v22[j]),
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v33[j] == std::fma(s3, v13[j], -v23[j]),
                       mave::test::tolerance<typename T::value_type>());
        }

        BOOST_TEST(v11.diagnosis());
        BOOST_TEST(v12.diagnosis());
        BOOST_TEST(v13.diagnosis());

        BOOST_TEST(v21.diagnosis());
        BOOST_TEST(v22.diagnosis());
        BOOST_TEST(v23.diagnosis());

        BOOST_TEST(v31.diagnosis());
        BOOST_TEST(v32.diagnosis());
        BOOST_TEST(v33.diagnosis());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(fmsub_4arg, T, test_targets)
{
    std::mt19937 mt(123456789);
    const auto scalars  = mave::test::generate_random<typename T::value_type>(N, mt);
    const auto vectors1 = mave::test::generate_random<T>(N, mt);
    const auto vectors2 = mave::test::generate_random<T>(N, mt);

    for(std::size_t i=0; i<N; i+=4)
    {
        const auto  s1  = scalars.at(i);
        const auto  s2  = scalars.at(i+1);
        const auto  s3  = scalars.at(i+2);
        const auto  s4  = scalars.at(i+3);

        const auto& v11 = vectors1.at(i);
        const auto& v12 = vectors1.at(i+1);
        const auto& v13 = vectors1.at(i+2);
        const auto& v14 = vectors1.at(i+3);
        const auto& v21 = vectors2.at(i);
        const auto& v22 = vectors2.at(i+1);
        const auto& v23 = vectors2.at(i+2);
        const auto& v24 = vectors2.at(i+3);

        const auto  v3  = mave::fmsub(std::make_tuple( s1,  s2,  s3,  s4),
                                             std::tie(v11, v12, v13, v14),
                                             std::tie(v21, v22, v23, v24));

        const auto& v31 = std::get<0>(v3);
        const auto& v32 = std::get<1>(v3);
        const auto& v33 = std::get<2>(v3);
        const auto& v34 = std::get<3>(v3);

        for(std::size_t j=0; j<v11.size(); ++j)
        {
            BOOST_TEST(v31[j] == std::fma(s1, v11[j], -v21[j]),
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v32[j] == std::fma(s2, v12[j], -v22[j]),
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v33[j] == std::fma(s3, v13[j], -v23[j]),
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v34[j] == std::fma(s4, v14[j], -v24[j]),
                       mave::test::tolerance<typename T::value_type>());
        }

        BOOST_TEST(v11.diagnosis());
        BOOST_TEST(v12.diagnosis());
        BOOST_TEST(v13.diagnosis());
        BOOST_TEST(v14.diagnosis());

        BOOST_TEST(v21.diagnosis());
        BOOST_TEST(v22.diagnosis());
        BOOST_TEST(v23.diagnosis());
        BOOST_TEST(v24.diagnosis());

        BOOST_TEST(v31.diagnosis());
        BOOST_TEST(v32.diagnosis());
        BOOST_TEST(v33.diagnosis());
        BOOST_TEST(v34.diagnosis());
    }
}


#ifndef BEARING_HPP
#define BEARING_HPP

#include <boost/assert.hpp>
#include <string>

namespace osrm
{
namespace util
{

namespace bearing
{
inline std::string get(const double heading)
{
    BOOST_ASSERT(heading >= 0);
    BOOST_ASSERT(heading <= 360);

    if (heading <= 22.5)
    {
        return "N";
    }
    if (heading <= 67.5)
    {
        return "NE";
    }
    if (heading <= 112.5)
    {
        return "E";
    }
    if (heading <= 157.5)
    {
        return "SE";
    }
    if (heading <= 202.5)
    {
        return "S";
    }
    if (heading <= 247.5)
    {
        return "SW";
    }
    if (heading <= 292.5)
    {
        return "W";
    }
    if (heading <= 337.5)
    {
        return "NW";
    }
    return "N";
}

// Checks whether A is between B-range and B+range, all modulo 360
// e.g. A = 5, B = 5, range = 10 == true
//      A = -6, B = 5, range = 10 == false
//      A = -2, B = 355, range = 10 == true
//      A = 6, B = 355, range = 10 == false
//      A = 355, B = -2, range = 10 == true
//
// @param A the bearing to check, in degrees, 0-359, 0=north
// @param B the bearing to check against, in degrees, 0-359, 0=north
// @param range the number of degrees either side of B that A will still match
// @return true if B-range <= A <= B+range, modulo 360
inline bool CheckInBounds(const int A, const int B, const int range)
{

    if (range >= 180)
        return true;
    if (range <= 0)
        return false;

    // Map both bearings into positive modulo 360 space
    const int normalized_B = (B < 0) ? (B % 360) + 360 : (B % 360);
    const int normalized_A = (A < 0) ? (A % 360) + 360 : (A % 360);

    if (normalized_B - range < 0)
    {
        return (normalized_B - range + 360 <= normalized_A && normalized_A < 360) ||
               (0 <= normalized_A && normalized_A <= normalized_B + range);
    }
    else if (normalized_B + range > 360)
    {
        return (normalized_B - range <= normalized_A && normalized_A < 360) ||
               (0 <= normalized_A && normalized_A <= normalized_B + range - 360);
    }
    else
    {
        return normalized_B - range <= normalized_A && normalized_A <= normalized_B + range;
    }
}

inline double reverseBearing(const double bearing)
{
    if (bearing >= 180)
        return bearing - 180.;
    return bearing + 180;
}
}
}
}

#endif // BEARING_HPP


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_GENERIC_TWO_SPLIT_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_GENERIC_TWO_SPLIT_HPP_INCLUDED
#include <boost/simd/arithmetic/functions/two_split.hpp>
#include <boost/simd/include/constants/splitfactor.hpp>
#include <boost/simd/include/functions/multiplies.hpp>
#include <boost/simd/include/functions/minus.hpp>
#include <boost/fusion/include/std_pair.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::two_split_
                                    , tag::cpu_
                                    , (A0)
                                    , (generic_< floating_<A0> >)
                                      (generic_< floating_<A0> >)
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE
    result_type operator()(A0 const& a0,A0& a2) const
    {
      result_type a1;
      boost::simd::two_split(a0, a1, a2);
      return a1;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::two_split_, tag::cpu_
                                    , (A0)
                                    , (generic_< floating_<A0> >)
                                    )
  {
    typedef std::pair<A0,A0>                                   result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      result_type res;
      boost::simd::two_split( a0, res.first, res.second );
      return res;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::two_split_, tag::cpu_
                                    , (A0)
                                    , (generic_< floating_<A0> >)
                                      (generic_< floating_<A0> >)
                                      (generic_< floating_<A0> >)
                                    )
  {
    typedef void result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a, A0 & r0,A0 & r1) const
    {
      A0 const c = boost::simd::Splitfactor<A0>()*a;
      A0 const c1 = c-a;
      r0 = c-c1;
      r1 = a-r0;
    }
  };
} } }

#endif


#include <stan/math/fwd.hpp>
#include <gtest/gtest.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/math/distributions.hpp>

TEST(ProbDistributionsLkjCorr, fvar_double) {
  using stan::math::fvar;
  boost::random::mt19937 rng;
  int K = 4;
  Eigen::Matrix<fvar<double>, Eigen::Dynamic, Eigen::Dynamic> Sigma(K, K);
  Sigma.setZero();
  Sigma.diagonal().setOnes();
  for (int i = 0; i < K * K; i++)
    Sigma(i).d_ = 1.0;
  fvar<double> eta = stan::math::uniform_rng(0, 2, rng);
  fvar<double> f = stan::math::do_lkj_constant(eta, K);
  EXPECT_FLOAT_EQ(f.val_, stan::math::lkj_corr_log(Sigma, eta).val_);
  EXPECT_FLOAT_EQ(2.5177896, stan::math::lkj_corr_log(Sigma, eta).d_);
  eta = 1.0;
  f = stan::math::do_lkj_constant(eta, K);
  EXPECT_FLOAT_EQ(f.val_, stan::math::lkj_corr_log(Sigma, eta).val_);
  EXPECT_FLOAT_EQ(f.d_, stan::math::lkj_corr_log(Sigma, eta).d_);
}

TEST(ProbDistributionsLkjCorrCholesky, fvar_double) {
  using stan::math::fvar;
  boost::random::mt19937 rng;
  int K = 4;
  Eigen::Matrix<fvar<double>, Eigen::Dynamic, Eigen::Dynamic> Sigma(K, K);
  Sigma.setZero();
  Sigma.diagonal().setOnes();
  for (int i = 0; i < K * K; i++)
    Sigma(i).d_ = 1.0;
  fvar<double> eta = stan::math::uniform_rng(0, 2, rng);
  fvar<double> f = stan::math::do_lkj_constant(eta, K);
  EXPECT_FLOAT_EQ(f.val_, stan::math::lkj_corr_cholesky_log(Sigma, eta).val_);
  EXPECT_FLOAT_EQ(6.7766843, stan::math::lkj_corr_cholesky_log(Sigma, eta).d_);
  eta = 1.0;
  f = stan::math::do_lkj_constant(eta, K);
  EXPECT_FLOAT_EQ(f.val_, stan::math::lkj_corr_cholesky_log(Sigma, eta).val_);
  EXPECT_FLOAT_EQ(3, stan::math::lkj_corr_cholesky_log(Sigma, eta).d_);
}

TEST(ProbDistributionsLkjCorr, fvar_fvar_double) {
  using stan::math::fvar;
  boost::random::mt19937 rng;
  int K = 4;
  Eigen::Matrix<fvar<fvar<double> >, Eigen::Dynamic, Eigen::Dynamic> Sigma(K,
                                                                           K);
  Sigma.setZero();
  Sigma.diagonal().setOnes();
  for (int i = 0; i < K * K; i++)
    Sigma(i).d_.val_ = 1.0;
  fvar<fvar<double> > eta = stan::math::uniform_rng(0, 2, rng);
  fvar<fvar<double> > f = stan::math::do_lkj_constant(eta, K);
  EXPECT_FLOAT_EQ(f.val_.val_, stan::math::lkj_corr_log(Sigma, eta).val_.val_);
  EXPECT_FLOAT_EQ(2.5177896, stan::math::lkj_corr_log(Sigma, eta).d_.val_);
  eta = 1.0;
  f = stan::math::do_lkj_constant(eta, K);
  EXPECT_FLOAT_EQ(f.val_.val_, stan::math::lkj_corr_log(Sigma, eta).val_.val_);
  EXPECT_FLOAT_EQ(f.d_.val_, stan::math::lkj_corr_log(Sigma, eta).d_.val_);
}

TEST(ProbDistributionsLkjCorrCholesky, fvar_fvar_double) {
  using stan::math::fvar;
  boost::random::mt19937 rng;
  int K = 4;
  Eigen::Matrix<fvar<fvar<double> >, Eigen::Dynamic, Eigen::Dynamic> Sigma(K,
                                                                           K);
  Sigma.setZero();
  Sigma.diagonal().setOnes();
  for (int i = 0; i < K * K; i++)
    Sigma(i).d_.val_ = 1.0;
  fvar<fvar<double> > eta = stan::math::uniform_rng(0, 2, rng);
  fvar<fvar<double> > f = stan::math::do_lkj_constant(eta, K);
  EXPECT_FLOAT_EQ(f.val_.val_,
                  stan::math::lkj_corr_cholesky_log(Sigma, eta).val_.val_);
  EXPECT_FLOAT_EQ(6.7766843,
                  stan::math::lkj_corr_cholesky_log(Sigma, eta).d_.val_);
  eta = 1.0;
  f = stan::math::do_lkj_constant(eta, K);
  EXPECT_FLOAT_EQ(f.val_.val_,
                  stan::math::lkj_corr_cholesky_log(Sigma, eta).val_.val_);
  EXPECT_FLOAT_EQ(3, stan::math::lkj_corr_cholesky_log(Sigma, eta).d_.val_);
}


#pragma once

#include "Camera.hh"
#include "../eigen_util.hh"
#include <Eigen/Geometry>

namespace kt84 {

struct CameraFree : public Camera {
    Eigen::Vector3d eye, up;

    CameraFree()
        : eye(Eigen::Vector3d::UnitZ())
        , up(Eigen::Vector3d::UnitY())
    {}

    void init(const Eigen::Vector3d& eye_, const Eigen::Vector3d& center_, const Eigen::Vector3d& up_) {
        eye    = eye_   ;
        center = center_;
        up     = up_    ;
    }
    Eigen::Vector3d get_eye() const { return eye; }
    Eigen::Vector3d get_up () const { return up ; }

    void mouse_move(int x, int y) {
        if (auto_flip_y) y = height - y;
        const int viewport_size = (width + height) / 2;

        if (drag_mode == DragMode::NONE)
            return;

        Eigen::Vector2i pos(x, y);
        static const double PI = 2.0 * std::asin(1.0);
        switch (drag_mode) {
        case DragMode::ROTATE:
            {
                double theta_x = (2 * PI * (pos.x() - prev_pos.x())) / viewport_size;
                double theta_y = (2 * PI * (pos.y() - prev_pos.y())) / viewport_size;

                Eigen::AngleAxisd rot_hrz(-theta_x, up  );
                auto center_to_eye_new = rot_hrz * center_to_eye();

                Eigen::Vector3d left = center_to_eye_new.cross(up).normalized();
                Eigen::AngleAxisd rot_vrt(-theta_y, left);
                center_to_eye_new = rot_vrt * center_to_eye_new;
                up = rot_vrt * up;

                eye = center + center_to_eye_new;
            }
            break;
        case DragMode::PAN:
            {
                double len = center_to_eye().norm();

                Eigen::Vector3d trans_x(eye_to_center().cross(up));
                Eigen::Vector3d trans_y(up);
                trans_x.normalize();
                trans_y.normalize();
                trans_x *= -len * (pos.x() - prev_pos.x()) / viewport_size;
                trans_y *= -len * (pos.y() - prev_pos.y()) / viewport_size;

                eye    += trans_x + trans_y;
                center += trans_x + trans_y;
            }
            break;
        case DragMode::ZOOM:
            {
                auto delta = eye_to_center() * (pos.y() - prev_pos.y()) / static_cast<double>(viewport_size);
                eye -= delta;
            }
            break;
        }
        prev_pos = pos;
    }

    void update_center(const Eigen::Vector3d& center_new) {
        // find the point closest to center_new between center and eye. update center and r accordingly.
        // center + t * center_to_eye = center_new
        auto d = center_to_eye();
        double t = d.dot(center_new - center) / d.squaredNorm();
        center += t * d;
    }
    void snap_to_canonical() {
        auto d = center_to_eye();
        double dr = d.norm();
        int d_axis = eigen_util::closest_axis(d);
        d[(d_axis + 1) % 3] = d[(d_axis + 2) % 3] = 0;
        d[d_axis] = d[d_axis] < 0 ? -dr : dr;
        eye = center + d;
        int up_axis = eigen_util::closest_axis(up);
        up[(up_axis + 1) % 3] = up[(up_axis + 2) % 3] = 0;
        up[up_axis] = up[up_axis] < 0 ? -1 : 1;
    }
};

}


#ifndef CPP_OCTREE_HPP
#define CPP_OCTREE_HPP

#include <vector>
#include <bitset>

#include <algorithm>

#include <iostream>

#include <Eigen/Core>

namespace detail {

  // a fast linear (morton) octree
  template<class T>
  struct cell {

    static constexpr std::size_t brute_force_threshold = 256;

    static constexpr std::size_t max_level = (8 * sizeof(T)) / 3;
    using coord = std::bitset<max_level>;

    static constexpr T resolution() { return 1ul << max_level; }

    static constexpr std::size_t size = 3 * max_level;
    using bits_type = std::bitset<size>;
    bits_type bits;

    explicit cell(const bits_type& bits={}) noexcept : bits(bits) { }

    // decode a cell code as individual coordinate codes
    template<class Func>
    void decode(Func&& func) const {
      coord x, y, z;

      auto value = bits.to_ulong();
      for(std::size_t i = 0; i < max_level; ++i) {
        x[i] = value & 4;
        y[i] = value & 2;
        z[i] = value & 1;

        value >>= 3;
      }

      func(x, y, z);
    }

    // encode cell from x y z codes
    static cell encode(const coord& x, const coord& y, const coord& z) {
      T value = 0;
      for(std::size_t i = 0; i < max_level; ++i) {
        const std::size_t j = max_level - 1 - i;
        value <<= 3;

        value |= x[j] * 4 + y[j] * 2 + z[j] * 1;
      }

      return cell(value);
    }

    // encode a cell at a given level
    static cell encode(bool x, bool y, bool z, std::size_t level) {
      bits_type result = 0;
      result[2] = x;
      result[1] = y;
      result[0] = z;

      return cell(result << (3 * level));
    }

    // generate possible delta (-1, 0, 1) for a cell given maximum value
    template<class Func>
    static void iter_delta(T c, T max, const Func& func) {
      if(c) func(-1);
      func(0);
      if(c < max) func(1);
    }

    // iterate cell neighbors at given level (cell must be admissible at this
    // level i.e. zero lower-order bits)
    template<class Func>
    void neighbors(std::size_t level, Func&& func) const {
      const T max = 1ul << (max_level - level);

      decode([&](coord x, coord y, coord z) {
          const auto cx = (x >> level).to_ulong();
          const auto cy = (y >> level).to_ulong();
          const auto cz = (z >> level).to_ulong();

          iter_delta(cx, max, [&](int dx) {
              const auto rx = (cx + dx) << level;
              const auto ax = std::abs(dx);

              iter_delta(cy, max, [&](int dy) {
                  const auto ry = (cy + dy) << level;
                  const auto ay = std::abs(dy);

                  iter_delta(cz, max, [&](int dz) {
                      const auto rz = (cz + dz) << level;
                      const auto az = std::abs(dz);
                      if(ax + ay + az) {
                        func(rx, ry, rz);
                      }
                    });
                });
            });
        });
    }

    template<class Func>
    void children(std::size_t level, const Func& func) const noexcept {
      const T first = bits.to_ulong();
      const T incr = 1ul << (3 * level);
      const T last = first + (incr << 3);

      for(T value = first, next; value != last; value = next) {
        next = value + incr;
        func(cell(value), cell(next - 1));
      }
    }

    // debugging
    friend std::ostream& operator<<(std::ostream& out, const cell& self) {
      self.decode([&](coord x, coord y, coord z) {
          out << "(" << x << ", " << y << ", " << z << ")";
        });
      return out;
    }

  };

  template<class T>
  using coord = typename cell<T>::coord;

  // a fixed-size (dynamically chosen) sorted array
  template<class Key, class Value>
  class sorted_array {

    struct item_type {
      Key key;
      Value value;

      bool operator<(const item_type& other) const { return key < other.key; }

      friend bool operator<(const item_type& self, const Key& key) { return self.key < key; }
      friend bool operator<(const Key& key, const item_type& self) { return key < self.key; }
    };

    std::vector<item_type> items;
  public:
    sorted_array(std::size_t size, const Key& key = {}, const Value& value = {})
      : items(size, item_type{key, value}) { };

    std::size_t size() const { return items.size(); }

    void insert(const Key& key, const Value& value) {
      items.pop_back();

      auto it = std::upper_bound(items.begin(), items.end(), key);
      items.emplace(it, key, value);
    }

    auto begin() const -> decltype(items.begin()) { return items.begin(); }
    auto end() const -> decltype(items.begin()) { return items.begin(); }

    auto back() const -> decltype(items.back()) { return items.back(); }
  };

  // brute force 1-nn
  template<class Real, class Distance, class Iterator>
  static Iterator find_nearest(const Distance& distance, Real& best, Iterator first, Iterator last) noexcept {
    Iterator res = last;

    for(Iterator it = first; it != last; ++it) {
      const Real d = distance(*it);
      if(d < best) {
        best = d;
        res = it;
      }
    }

    return res;
  }

  // brute force k-nn
  template<class Real, class Distance, class Iterator>
  static void find_nearest(sorted_array<Real, Iterator>& result, const Distance& distance,
                           Iterator first, Iterator last) noexcept {
    for(Iterator it = first; it != last; ++it) {
      const Real d = distance(*it);
      if(d < result.back().key) {
        result.insert(d, it);
      }
    }
  }

  // octree based 1-nn
  template<class Real, class Distance, class Iterator, class T>
  static Iterator find_nearest(const Distance& distance, Real& best, Iterator first, Iterator last,
                               const cell<T>& origin, std::size_t level = cell<T>::max_level) noexcept {
    const std::size_t size = last - first;

    // base case
    if(size <= cell<T>::brute_force_threshold || !level) {
      return find_nearest(distance, best, first, last);
    }

    // recursive case
    struct chunk_type {
      Real d;
      cell<T> c;
      Iterator begin, end;

      inline bool operator<(const chunk_type& other) const { return d < other.d; }
    };

    const std::size_t next_level = level - 1;

    chunk_type chunks[8];
    std::size_t non_empty = 0;

    origin.children(next_level, [&](cell<T> lower, cell<T> upper) noexcept {
        assert(upper.bits.to_ulong() > lower.bits.to_ulong());

        // find range for lower/upper cell corners
        const Iterator begin = std::lower_bound(first, last, lower);
        if(begin == last) return; // no point found in subcell

        const Iterator end = std::upper_bound(begin, last, upper);

        assert(end >= begin);
        chunks[non_empty++] = {distance(lower, next_level), lower, begin, end};
      });

    // paranoid sanity checks
    assert(non_empty <= 8);
    assert(chunks[0].begin == first);
    assert(chunks[count - 1].end == last);

    // order non-empty subcells by distance
    std::sort(chunks, chunks + non_empty);

    Iterator result = last;
    for(auto it = chunks, end = chunks + non_empty; it != end; ++it) {
      if(it->d < best) {
        const Real old_best = best;
        const Iterator sub = find_nearest(distance, best, it->begin, it->end, it->c, next_level);

        if(best < old_best) {
          result = sub;
        }
      }
    }

    return result;
  };

  // // octree based k-nn
  // template<class Real, class Distance, class Iterator, class T>
  // static void find_nearest(sorted_array<Real, Iterator>& result, const Distance& distance,
  //                          Iterator first, Iterator last,
  //                          const cell<T>& origin, std::size_t level = cell<T>::max_level) noexcept {
  //     // range size
  //     const std::size_t size = last - first;

  //     // base case
  //     if( (level == 0) || size <= cell<T>::brute_force_threshold) {
  //         find_nearest(result, distance, first, last);
  //     }

  //     // recursive case: split cell
  //     const std::size_t next_level = level - 1;

  //     // subcell info TODO rename
  //     struct chunk_type {
  //         Real d;
  //         cell<T> c;
  //         Iterator begin, end;

  //         bool operator<(const chunk_type& other) const { return d < other.d; }
  //     };

  //     chunk_type chunks[8];
  //     std::size_t count = 0;

  //     // obtain subcell info, skip if empty
  //     for(cell<T> c : typename cell<T>::children(origin, next_level)) {
  //         const Iterator begin = std::lower_bound(first, last, c);
  //         if(begin == last) continue; // no point found in subcell

  //         // note: we want upper bound since we may have duplicate cells in the
  //         // range (e.g. many points in same leafs)
  //         const Iterator end = std::lower_bound(begin, last, c.next(next_level));

  //         chunks[count] = {distance(c, next_level), c, begin, end};
  //         ++count;
  //     }

  //     assert(count <= 8);

  //     // process subcells by distance to query point
  //     std::sort(chunks, chunks + count);

  //     for(auto it = chunks, end = chunks + count; it != end; ++it) {
  //         // don't visit subcell if our furthest guess is closer than the subcell
  //         if(it->d < result.back().key) {
  //             find_nearest(result, distance, it->begin, it->end, it->c, next_level);
  //         }
  //     }

  //     return result;
  // };

}

}

template<class Real=double, class T=unsigned long>
class octree {
  struct item;

  using data_type = std::vector<item>;
  data_type data;

public:
  struct distance;

  // TODO use traits
  using real = Real;
  using vec3 = Eigen::Matrix<real, 3, 1>;

  using cell = detail::cell<T>;
  using coord = detail::coord<T>;

  // preallocate octree data
  void reserve(std::size_t count) { data.reserve(count); }

  // clear octree data
  void clear() { data.clear(); }

  // number of octree cells
  std::size_t size() const { return data.size(); }

  static cell hash(const vec3& p) {
    auto s = (p * cell::resolution()).template cast<T>();
    return cell::encode(s.x(), s.y(), s.z());
  }

  static vec3 origin(const cell& c) {
    vec3 res;
    c.decode([&](const coord& x, const coord& y, const coord& z) {
        res = {x.to_ulong(), y.to_ulong(), z.to_ulong()};
      });

    return res / cell::resolution();
  }

  // append a point to the octree
  void add(const vec3* p) {
    data.push_back({hash(*p), p});
  }

  // sort octree cells
  void sort() { std::sort(data.begin(), data.end()); }

  // nearest-neighbor search
  const vec3* nearest(const vec3& query) const {
    if(data.empty()) return nullptr;

    real best = std::numeric_limits<real>::max();
    auto it = find_nearest(distance{query}, best, data.begin(), data.end(), cell(0));
    assert(it != data.end());

    return it->p;
  }

  // template<class OutputIterator>
  // void nearest(OutputIterator out, const vec3& query, std::size_t count = 1) const {
  //     assert(count >= data.size());

  //     using iterator = typename data_type::iterator;

  //     detail::sorted_array<real, iterator> knn(count, std::numeric_limits<real>::max());
  //     find_nearest(knn, distance{query}, data.begin(), data.end(), cell(0));

  //     for(auto& it : knn) {
  //         *out++ = it.value.p;
  //     }
  // }

};

template<class Real, class T>
struct octree<Real, T>::item {
  cell c;
  const vec3* p;

  friend inline bool operator<(const T& c, const item& self) {
    return c < self.c.bits.to_ulong();
  }

  friend inline bool operator<(const item& self, const T& c) {
    return self.c.bits.to_ulong() < c;
  }

  friend inline bool operator<(const cell& c, const item& self) {
    return c.bits.to_ulong() < self.c.bits.to_ulong();
  }

  friend inline bool operator<(const item& self, const cell& c) {
    return self.c.bits.to_ulong() < c.bits.to_ulong();
  }

  friend inline bool operator<(const item& lhs, const item& rhs) {
    return lhs.c.bits.to_ulong() < rhs.c.bits.to_ulong();
  }
};

template<class Real, class T>
struct octree<Real, T>::distance {
  const vec3 query;
  const vec3 scaled_query;

  // mutable std::size_t point_count = 0, box_count = 0;

  // ~distance() {
  //   std::clog << " point distances: " << point_count
  //             << " box distances: " << box_count << std::endl;
  // }

  distance(const vec3& query)
    : query(query),
      scaled_query(query * cell::resolution()) { }

  // distance to point
  real operator()(const vec3& p) const noexcept {
    // ++point_count;
    return (query - p).squaredNorm();
  }

  real operator()(const item& i) const noexcept {
    return operator()(*i.p);
  }

  // compute scaled_query - proj(scaled_query) onto cell c at given level
  vec3 project(const cell& c, std::size_t level) const {
    vec3 res;
    c.decode([&](coord sx, coord sy, coord sz) {
        const vec3 low{sx.to_ulong(), sy.to_ulong(), sz.to_ulong()};
        const vec3 local = scaled_query - low;
        res = local.array() - local.array().min(1ul << level).max(0);
      });
    return res;
  }

  // distance to cell
  real operator()(const cell& c, std::size_t level) const noexcept {
    // ++box_count;
    constexpr real factor = 1.0 / (cell::resolution() * cell::resolution());
    return project(c, level).squaredNorm() * factor;
  }
};

#endif


// Copyright 2020 Arthur Sonzogni. All rights reserved.
// Use of this source code is governed by the MIT license that can be found in
// the LICENSE file.

#include <iostream>
#include <map>
#include <queue>
#include <sstream>
#include <string>
#include <vector>
#include "screen/Screen.h"
#include "translator/Translator.h"
#include "translator/graph_planar/GraphPlanarLexer.h"
#include "translator/graph_planar/GraphPlanarParser.h"

// With emscripten, you may need to execute in the src directory.
// # ln -s /usr/include/boost/ boost
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>
#include <boost/graph/chrobak_payne_drawing.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/make_biconnected_planar.hpp>
#include <boost/graph/make_connected.hpp>
#include <boost/graph/make_maximal_planar.hpp>
#include <boost/graph/planar_canonical_ordering.hpp>
#include <boost/graph/properties.hpp>
#include <boost/property_map/property_map.hpp>

using Graph = boost::adjacency_list<boost::vecS,
                                    boost::vecS,
                                    boost::undirectedS,
                                    boost::property<boost::vertex_index_t, int>,
                                    boost::property<boost::edge_index_t, int>>;

using Embedding =
    std::vector<std::vector<boost::graph_traits<Graph>::edge_descriptor>>;

using Coordinates = struct {
  size_t x;
  size_t y;
};

using StraightLineDrawing = std::vector<Coordinates>;

struct Box {
  int left;
  int right;
  int top;
  int bottom;
  static Box Union(Box A, Box B);
  static Box Translate(Box A, int x, int y);
};

void InitializeEdgeIndex(Graph& graph) {
  boost::property_map<Graph, boost::edge_index_t>::type e_index =
      boost::get(boost::edge_index, graph);
  boost::graph_traits<Graph>::edges_size_type edge_count = 0;
  boost::graph_traits<Graph>::edge_iterator ei, ei_end;
  for (boost::tie(ei, ei_end) = boost::edges(graph); ei != ei_end; ++ei)
    boost::put(e_index, *ei, edge_count++);
}

bool ComputePlanarEmbedding(const Graph& graph, Embedding& embedding) {
  embedding = Embedding(boost::num_vertices(graph));
  return boost::boyer_myrvold_planarity_test(
      boost::boyer_myrvold_params::graph = graph,
      boost::boyer_myrvold_params::embedding = embedding.data());
}

class GraphPlanar;

struct DrawnEdge {
  int x;
  int vertex_up;
  int vertex_down;
  int y_up;
  int y_down;

  void Draw(Screen& screen, GraphPlanar& graph);
};

struct DrawnVertex {
  int left;
  int right;
  int y;
  std::wstring text;
  std::vector<DrawnEdge> edges;

  void Draw(Screen& screen);
};

enum class Arrow {
  RIGHT,
  LEFT_RIGHT,
  NONE,
  LEFT,
};

struct Edge {
  int from;
  int to;
  Arrow arrow;
};

enum class ArrowStyle {
  NONE,
  LINE,
  ARROW,
};

struct Node {
  int id;
  std::vector<Node> childs;
};

class GraphPlanar : public Translator {
 public:
  virtual ~GraphPlanar() = default;
  std::map<int, std::map<int, ArrowStyle>> arrow_style;

 private:
  const char* Name() final { return "Graph - planar"; }
  const char* Identifier() final { return "GraphPlanar"; }
  const char* Description() final {
    return "Build a graph from node and edges";
  }
  std::vector<Translator::OptionDescription> Options() final;
  std::vector<Translator::Example> Examples() final;
  std::string Translate(const std::string& input,
                        const std::string& options_string) final;

  void Read(const std::string& input);
  void ReadGraph(GraphPlanarParser::GraphContext* graph);
  void ReadEdges(GraphPlanarParser::EdgesContext* edges);
  int ReadNode(GraphPlanarParser::NodeContext* node);
  Arrow ReadArrow(GraphPlanarParser::ArrowContext* arrow);

  void Write();
  void ComputeArrowStyle();

  bool ascii_only_;
  std::string output_;

  std::map<std::wstring, int> name_to_id;
  std::vector<std::wstring> id_to_name;
  int next_id = 0;

  std::vector<Edge> vertex;
};

std::vector<Translator::OptionDescription> GraphPlanar::Options() {
  return {
      {
          "ascii_only",
          {
              "false",
              "true",
          },
          "false",
          "Use the full unicode charset or only ASCII.",
          Widget::Checkbox,
      },
  };
}

std::vector<Translator::Example> GraphPlanar::Examples() {
  return {
      {
          "if then else loop",
          "if -> \"then A\" -> end\n"
          "if -> \"then B\" -> end\n"
          "end -> loop -> if",
      },
      {
          "test",
          "A -- B\n"
          "A -- C\n"
          "A -- D -- G\n"
          "B -- Z\n"
          "C -- Z",
      },
  };
}

std::string GraphPlanar::Translate(const std::string& input,
                                   const std::string& options_string) {
  *this = GraphPlanar();
  auto options = SerializeOption(options_string);
  ascii_only_ = (options["ascii_only"] == "true");

  Read(input);
  Write();
  return output_;
}

void GraphPlanar::Read(const std::string& input) {
  antlr4::ANTLRInputStream input_stream(input);

  // Lexer.
  GraphPlanarLexer lexer(&input_stream);
  antlr4::CommonTokenStream tokens(&lexer);
  tokens.fill();

  GraphPlanarParser parser(&tokens);
  ReadGraph(parser.graph());
}

void GraphPlanar::ReadGraph(GraphPlanarParser::GraphContext* graph) {
  for (GraphPlanarParser::EdgesContext* edges : graph->edges()) {
    ReadEdges(edges);
  }
}

void GraphPlanar::ReadEdges(GraphPlanarParser::EdgesContext* edges) {
  std::vector<int> nodes;
  std::vector<Arrow> arrows;
  for (GraphPlanarParser::NodeContext* node : edges->node()) {
    nodes.push_back(ReadNode(node));
  }
  for (GraphPlanarParser::ArrowContext* arrow : edges->arrow()) {
    arrows.push_back(ReadArrow(arrow));
  }
  for (int i = 0; i < arrows.size(); ++i) {
    vertex.push_back(Edge{nodes[i], nodes[i + 1], arrows[i]});
  }
}

int GraphPlanar::ReadNode(GraphPlanarParser::NodeContext* node) {
  std::wstring label;
  if (auto id = node->ID()) {
    label = to_wstring(id->getSymbol()->getText());
  } else if (auto text = node->STRING()) {
    label = to_wstring(text->getSymbol()->getText());
    // Remove the two '"'.
    label = label.substr(1, label.size() - 2);
  } else {
    // NOTREACHED();
  }

  auto it = name_to_id.find(label);
  if (it != name_to_id.end()) {
    return it->second;
  } else {
    name_to_id[label] = next_id;
    id_to_name.push_back(label);
    return next_id++;
  }
}

Arrow GraphPlanar::ReadArrow(GraphPlanarParser::ArrowContext* arrow) {
  if (arrow->RIGHT_ARROW())
    return Arrow::RIGHT;
  if (arrow->NONE_ARROW())
    return Arrow::NONE;
  if (arrow->LEFT_RIGHT_ARROW())
    return Arrow::LEFT_RIGHT;
  if (arrow->LEFT_ARROW())
    return Arrow::LEFT;
  // NOTREACHED();
  return Arrow::RIGHT;
}

void GraphPlanar::ComputeArrowStyle() {
  // Compute ArrowStyle
  for (auto& v : vertex) {
    switch (v.arrow) {
      case Arrow::RIGHT:
        arrow_style[v.from][v.to] = ArrowStyle::ARROW;
        arrow_style[v.to][v.from] = ArrowStyle::LINE;
        break;
      case Arrow::LEFT_RIGHT:
        arrow_style[v.from][v.to] = ArrowStyle::ARROW;
        arrow_style[v.to][v.from] = ArrowStyle::ARROW;
        break;
      case Arrow::LEFT:
        arrow_style[v.from][v.to] = ArrowStyle::LINE;
        arrow_style[v.to][v.from] = ArrowStyle::ARROW;
        break;
      case Arrow::NONE:
        arrow_style[v.from][v.to] = ArrowStyle::LINE;
        arrow_style[v.to][v.from] = ArrowStyle::LINE;
        break;
    }
  }
}

void GraphPlanar::Write() {
  ComputeArrowStyle();

  if (id_to_name.size() <= 2) {
    return;
  }

  int num_vertices = id_to_name.size();

  // Create a graph.
  Graph graph(num_vertices);
  for (auto& it : vertex)
    add_edge(it.from, it.to, graph);
  InitializeEdgeIndex(graph);

  // Make it connected.
  boost::make_connected(graph);
  InitializeEdgeIndex(graph);

  // Make it biconnected.
  Embedding embedding;
  bool is_planar = ComputePlanarEmbedding(graph, embedding);
  if (!is_planar) {
    output_ = "Graph is not planar.\n";
    return;
  }

  boost::make_biconnected_planar(graph, embedding.data());
  InitializeEdgeIndex(graph);
  Graph biconnected_graph(graph);

  ComputePlanarEmbedding(graph, embedding);
  boost::make_maximal_planar(graph, embedding.data());
  InitializeEdgeIndex(graph);

  // Find a canonical ordering.
  ComputePlanarEmbedding(graph, embedding);
  std::vector<boost::graph_traits<Graph>::vertex_descriptor> ordering;
  boost::planar_canonical_ordering(graph, embedding.data(),
                                   std::back_inserter(ordering));

  std::vector<size_t> inverse_ordering(num_vertices);
  for (int i = 0; i < inverse_ordering.size(); ++i) {
    inverse_ordering[ordering[i]] = i;
  }

  StraightLineDrawing straight_line_drawing(num_vertices);

  // Compute the straight line drawing
  boost::chrobak_payne_straight_line_drawing(graph, embedding, ordering.begin(),
                                             ordering.end(),
                                             straight_line_drawing.data());

  auto compare_with = [&](int i) {
    return [&, i](int a, int b) {
      int a_dx = straight_line_drawing[a].x - straight_line_drawing[i].x;
      int a_dy = straight_line_drawing[a].y - straight_line_drawing[i].y;
      int b_dx = straight_line_drawing[b].x - straight_line_drawing[i].x;
      int b_dy = straight_line_drawing[b].y - straight_line_drawing[i].y;
      return a_dx * b_dy - b_dx * a_dy < 0;
    };
  };

  // Compute children.
  std::vector<std::vector<int>> children(num_vertices);
  for (int i = 0; i < num_vertices; ++i) {
    auto& down = children[i];
    auto adjacent = boost::adjacent_vertices(i, graph);
    for (auto j = adjacent.first; j != adjacent.second; ++j) {
      if (inverse_ordering[i] < inverse_ordering[*j]) {
        down.push_back(*j);
      }
    }
    std::sort(down.begin(), down.end(), compare_with(i));
  }

  // Compute Y.
  std::vector<size_t> y(num_vertices, 0);
  for (size_t i : ordering) {
    for (size_t j : children[i]) {
      if (inverse_ordering[i] < inverse_ordering[j]) {
        y[j] = std::max(y[j], y[i] + 1);
      }
    }
  }

  std::vector<DrawnVertex> drawn_vertices(num_vertices);
  std::vector<bool> is_drawn(num_vertices, false);
  std::vector<int> x(y.size(), -1);

  std::function<void(int)> refresh_x = [&](int y) {
    int i = y + 1;
    while (i < x.size() && x[i] < x[y]) {
      x[i] = x[y];
      ++i;
    };
  };

  auto Draw = [&]() {
    // Determine the screen dimension
    int width = 0;
    int height = 0;
    for (int i = 0; i < num_vertices; ++i) {
      if (!is_drawn[i])
        continue;
      width = std::max(width, drawn_vertices[i].right + 1);
      height = std::max(height, 3 * drawn_vertices[i].y + 3);
    }
    Screen screen(width, height);
    for (int i = 0; i < num_vertices; ++i) {
      if (!is_drawn[i])
        continue;
      drawn_vertices[i].Draw(screen);
    }
    for (int i = 0; i < num_vertices; ++i) {
      if (!is_drawn[i])
        continue;
      for (auto& edge : drawn_vertices[i].edges) {
        edge.Draw(screen, *this);
      }
    }

    if (ascii_only_)
      screen.ASCIIfy(1);
    output_ += screen.ToString();
  };

  std::function<void(int)> DrawNode = [&](int i) -> void {
    if (is_drawn[i])
      return;
    DrawnVertex& vertex = drawn_vertices[i];
    int child_left = -1;
    int child_right = -1;
    // Draw our childrens.
    for (auto j : children[i]) {
      DrawNode(j);

      auto& child = drawn_vertices[j];
      DrawnEdge edge;
      edge.vertex_up = i;
      edge.vertex_down = j;
      edge.y_up = y[i] + 1;
      edge.y_down = y[j] - 1;

      // There are no real edge between those two vertex. in this case, do not
      // draw anything.
      if (arrow_style[i][j] == ArrowStyle::NONE) {
        continue;
      }

      // Make the child bigger if we need to.
      child.right = std::max(child.right, x[y[j] - 1] + 3);
      x[y[j]] = child.right;
      refresh_x(y[j]);

      edge.x = child.left + 1;
      edge.x = std::max(edge.x, x[y[i]] + 2);
      if (y[j] != y[i] + 1) {
        edge.x = std::max(edge.x, x[child.y - 1] + 1);
      }
      vertex.edges.push_back(edge);

      x[y[i] + 1] = std::max(x[y[i] + 1], edge.x);
      refresh_x(y[i] + 1);

      if (child_left == -1) {
        child_left = edge.x - 1;
      }
      child_right = edge.x + 1;
    }

    // Draw ourself.
    vertex.text = id_to_name[i];
    vertex.y = y[i];
    vertex.left = std::max(0, std::max(child_left, x[y[i]] + 1));
    vertex.right =
        std::max(child_right, (int)(vertex.left + vertex.text.size() + 1));
    x[y[i]] = vertex.right;
    refresh_x(y[i]);

    // Mark ourself as drawn.
    is_drawn[i] = true;
  };

  for (int i : ordering) {
    if (y[i] == 0) {
      DrawNode(i);
    }
  }

  return Draw();
}

void DrawnVertex::Draw(Screen& screen) {
  screen.DrawBox(left, 3 * y, right - left + 1, 3);
  int text_position = left + 1 + (right - left - 1 - text.size()) / 2;
  screen.DrawText(text_position, 3 * y + 1, text);
}

void DrawnEdge::Draw(Screen& screen, GraphPlanar& graph) {
  int top = 3 * y_up - 1;
  int bottom = 3 * y_down + 3;

  screen.DrawVerticalLine(top + 1, bottom - 1, x);

  if (graph.arrow_style[vertex_down][vertex_up] == ArrowStyle::LINE)
    screen.DrawPixel(x, top, L'┬');
  else
    screen.DrawPixel(x, top, L'△');

  if (graph.arrow_style[vertex_up][vertex_down] == ArrowStyle::LINE)
    screen.DrawPixel(x, bottom, L'┴');
  else
    screen.DrawPixel(x, bottom, L'▽');
}

std::unique_ptr<Translator> GraphPlanarTranslator() {
  return std::make_unique<GraphPlanar>();
}