#include "clean.h"
#include "subdiv_weights.h"
#include "Mesh.h"
extern "C"
{
#include "subdivision_skinning_wrapper.h"
}
#include <igl/Camera.h>
#include <igl/MouseController.h>
#include <igl/REDRUM.h>
#include <igl/ReAntTweakBar.h>
#include <igl/STR.h>
#include <igl/bone_parents.h>
#include <igl/centroid.h>
#include <igl/colon.h>
#include <igl/colon.h>
#include <igl/draw_beach_ball.h>
#include <igl/draw_floor.h>
#include <igl/draw_skeleton_3d.h>
#include <igl/draw_skeleton_vector_graphics.h>
#include <igl/forward_kinematics.h>
#include <igl/get_seconds.h>
#include <igl/lbs_matrix.h>
#include <igl/material_colors.h>
#include <igl/next_filename.h>
#include <igl/normalize_row_sums.h>
#include <igl/pathinfo.h>
#include <igl/polygon_mesh_to_triangle_mesh.h>
#include <igl/quat_to_mat.h>
#include <igl/readDMAT.h>
#include <igl/readTGF.h>
#include <igl/readOBJ.h>
#include <igl/list_to_matrix.h>
#include <igl/remove_unreferenced.h>
#include <igl/report_gl_error.h>
#include <igl/snap_to_canonical_view_quat.h>
#include <igl/snap_to_fixed_up.h>
#include <igl/trackball.h>
#include <igl/two_axis_valuator_fixed_up.h>
#include <igl/writeDMAT.h>
#include <igl/writeOBJ.h>
#include <igl/png/render_to_png_async.h>
#include <igl/writeMESH.h>
#include <igl/writeOFF.h>
#include <igl/writeTGF.h>

#include <Eigen/Core>
#include <Eigen/Geometry>

#ifdef __APPLE__
#include <GLUT/glut.h>
#else
#include <GL/glut.h>
#endif

#include <iostream>
#include <fstream>

#include <string>
#include <vector>
#include <queue>
#include <stack>
#include <iostream>
#include <iomanip>
#include <unistd.h>

#define VERBOSE
#define DEBUG_WEIGHTS

enum SkelStyleType
{
  SKEL_STYLE_TYPE_3D = 0,
  SKEL_STYLE_TYPE_VECTOR_GRAPHICS = 1,
  NUM_SKEL_STYLE_TYPE = 2
}skel_style;
bool show_skeleton = true;

double fps = 0.;
bool force_anim = false;
bool floor_visible = true;
bool render_to_png_on_next = false;
bool render_to_png_on_anim = false;

Eigen::MatrixXd M;
Mesh c(false),r(true),l(false); // coarse, refined, lbs

Eigen::Vector3d Vmid;
double bbd = 1.0;
igl::Camera camera;

Eigen::MatrixXd C;
Eigen::MatrixXi BE;
Eigen::VectorXi P,RP;

struct State
{
  igl::MouseController mouse;
  Eigen::MatrixXf colors;
} s;

bool wireframe = false;

// See README for descriptions
enum RotationType
{
  ROTATION_TYPE_IGL_TRACKBALL = 0,
  ROTATION_TYPE_TWO_AXIS_VALUATOR_FIXED_UP = 1,
  NUM_ROTATION_TYPES = 2,
} rotation_type = ROTATION_TYPE_TWO_AXIS_VALUATOR_FIXED_UP;

std::stack<State> undo_stack;
std::stack<State> redo_stack;

bool is_rotating = false;
bool centroid_is_visible = false;
int down_x,down_y;
igl::Camera down_camera;
std::string output_prefix;

struct CameraAnimation
{
  bool is_animating = false;
  double DURATION = 0.5;
  double start_time = 0;
  Eigen::Quaterniond from_quat,to_quat;
} canim;

typedef std::vector<
Eigen::Quaterniond,
  Eigen::aligned_allocator<Eigen::Quaterniond> > RotationList;

struct PoseAnimation
{
  bool is_animating = false;
  double DURATION = 2;
  double start_time = 0;
  RotationList pose;
  int count = 0;
} panim;

int width,height;
Eigen::Vector4f light_pos(-0.1,-0.1,0.9,0);

#define REBAR_NAME "temp.rbr"
igl::ReTwBar rebar;

// Subdiv library pointers
void* engine;
void* eval;

bool show_cage_on_drag = true;

void push_undo()
{
  undo_stack.push(s);
  // Clear
  redo_stack = std::stack<State>();
}

// No-op setter, does nothing
void TW_CALL no_op(const void * /*value*/, void * /*clientData*/)
{
}

void TW_CALL set_rotation_type(const void * value, void * clientData)
{
  using namespace Eigen;
  using namespace std;
  using namespace igl;
  const RotationType old_rotation_type = rotation_type;
  rotation_type = *(const RotationType *)(value);
  if(rotation_type == ROTATION_TYPE_TWO_AXIS_VALUATOR_FIXED_UP &&
    old_rotation_type != ROTATION_TYPE_TWO_AXIS_VALUATOR_FIXED_UP)
  {
    push_undo();
    canim.from_quat = camera.m_rotation_conj;
    snap_to_fixed_up(canim.from_quat,canim.to_quat);
    // start animation
    canim.start_time = get_seconds();
    canim.is_animating = true;
  }
}
void TW_CALL get_rotation_type(void * value, void *clientData)
{
  RotationType * rt = (RotationType *)(value);
  *rt = rotation_type;
}

void reshape(int width, int height)
{
  ::width = width;
  ::height = height;
  glViewport(0,0,width,height);
  // Send the new window size to AntTweakBar
  TwWindowSize(width, height);
  camera.m_aspect = (double)width/(double)height;
  s.mouse.reshape(width,height);
}

void push_scene()
{
  using namespace igl;
  using namespace std;
  glMatrixMode(GL_PROJECTION);
  glPushMatrix();
  glLoadIdentity();
  gluPerspective(camera.m_angle,camera.m_aspect,camera.m_near,camera.m_far);
  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
  glLoadIdentity();
  gluLookAt(
    camera.eye()(0), camera.eye()(1), camera.eye()(2),
    camera.at()(0), camera.at()(1), camera.at()(2),
    camera.up()(0), camera.up()(1), camera.up()(2));
}

void push_object()
{
  using namespace igl;
  glPushMatrix();
  glScaled(2./bbd,2./bbd,2./bbd);
  glTranslated(-Vmid(0),-Vmid(1),-Vmid(2));
}

void pop_object()
{
  glPopMatrix();
}

void pop_scene()
{
  glMatrixMode(GL_PROJECTION);
  glPopMatrix();
  glMatrixMode(GL_MODELVIEW);
  glPopMatrix();
}

// Set up double-sided lights
void lights()
{
  using namespace std;
  using namespace Eigen;
  glEnable(GL_LIGHTING);
  glLightModelf(GL_LIGHT_MODEL_TWO_SIDE,GL_TRUE);
  glEnable(GL_LIGHT0);
  glEnable(GL_LIGHT1);
  float WHITE[4] =  {0.8,0.8,0.8,1.};
  float GREY[4] =  {0.4,0.4,0.4,1.};
  float BLACK[4] =  {0.,0.,0.,1.};
  Vector4f pos = light_pos;
  glLightfv(GL_LIGHT0,GL_AMBIENT,GREY);
  glLightfv(GL_LIGHT0,GL_DIFFUSE,WHITE);
  glLightfv(GL_LIGHT0,GL_SPECULAR,BLACK);
  glLightfv(GL_LIGHT0,GL_POSITION,pos.data());
  pos(0) *= -1;
  pos(1) *= -1;
  pos(2) *= -1;
  glLightfv(GL_LIGHT1,GL_AMBIENT,GREY);
  glLightfv(GL_LIGHT1,GL_DIFFUSE,WHITE);
  glLightfv(GL_LIGHT1,GL_SPECULAR,BLACK);
  glLightfv(GL_LIGHT1,GL_POSITION,pos.data());
}

template <typename DerivedQ, typename DerivedU, typename DerivedUN>
void update_refined(
  const Eigen::Matrix<subdivision_evaluator_real_t,Eigen::Dynamic,Eigen::Dynamic> & TT,
  const Eigen::PlainObjectBase<DerivedQ> & Q,
  subdivision_evaluator_real_t * vs_modified,
  Eigen::PlainObjectBase<DerivedU> & U,
  Eigen::PlainObjectBase<DerivedUN> & UN)
{
  using namespace Eigen;
  using namespace igl;

  // Retrieve refined mesh
  const int num_refined_vs = num_refined_vertices_of_subdivision_evaluator( eval );
  subdivision_evaluator_real_t* refined_vs = new subdivision_evaluator_real_t[ num_refined_vs*3 ];
  get_refined_vertices_of_subdivision_skinning_engine_with_control_vertices( engine, 3, vs_modified, refined_vs );
  const auto & UT = Map<Matrix<subdivision_evaluator_real_t,Dynamic,Dynamic> >
    (refined_vs,3,num_refined_vs);
  U = UT.cast<double>().transpose();
  // Clean up
  delete[] refined_vs;
}

void display()
{
  using namespace igl;
  using namespace std;
  using namespace Eigen;
  //const float back[4] = {0.75, 0.75, 0.75,0};
  const float back[4] = {1.,1.,1.,0};
  glClearColor(back[0],back[1],back[2],0);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  if(canim.is_animating)
  {
    double t = (get_seconds() - canim.start_time)/canim.DURATION;
    if(t > 1)
    {
      t = 1;
      canim.is_animating = false;
    }
    Quaterniond q = canim.from_quat.slerp(t,canim.to_quat).normalized();
    camera.orbit(q.conjugate());
  }

  RotationList dQ;
  if(panim.is_animating)
  {
    double t =
      render_to_png_on_anim ?
      double(panim.count)/(30.*panim.DURATION) :
      (get_seconds() - panim.start_time)/panim.DURATION;
    if(t > 1)
    {
      t = 1;
      panim.is_animating = false;
    }
    const auto & ease = [](const double t)
    {
      return 3.*t*t-2.*t*t*t;
    };
    double f = (t<0.5?ease(2.*t):ease(2.-2.*t));
    dQ.resize(panim.pose.size());
    for(int e = 0;e<(int)panim.pose.size();e++)
    {
      dQ[e] = panim.pose[e].slerp(f,Quaterniond::Identity()).normalized();
    }
    panim.count++;
  }else
  {
    dQ = s.mouse.rotations();
  }
  RotationList vQ;
  vector<Vector3d> vT;
  forward_kinematics(C,BE,P,dQ,vQ,vT);

  // Vertical stack, row-major --> horizontal stack of transposes, col-major
  Matrix<subdivision_evaluator_real_t,Dynamic,Dynamic> TT(4,4*BE.rows());
  // draw_skeleton needs a different stack
  MatrixXd T(BE.rows()*(3+1),3);
  for(int w = 0;w<BE.rows();w++)
  {
    Affine3d a = Affine3d::Identity();
    a.translate(vT[w]);
    a.rotate(vQ[w]);
    TT.block(0,w*4,4,4) =
      a.matrix().transpose().cast<subdivision_evaluator_real_t>();
    T.block(w*(3+1),0,3+1,3) = a.matrix().transpose().block(0,0,3+1,3);
  }
  /////////////////////////////////////////////////////////////////////////
  // Tell subdiv lib about transformations and get modified cage, refined
  /////////////////////////////////////////////////////////////////////////
  if(c.stale || r.stale)
  {
    const subdivision_evaluator_real_t* transforms = TT.data();
    subdivision_evaluator_real_t* vs_modified = new subdivision_evaluator_real_t[ c.V.size() ];
    // Retrieve control mesh
    compute_control_mesh_vertices_given_transforms_for_subdivision_skinning_engine( engine, (subdivision_evaluator_real_t*)transforms, vs_modified );
    const auto & CUT = Map<Matrix<subdivision_evaluator_real_t,Dynamic,Dynamic> >
      (vs_modified,3,c.V.rows());
    c.U = CUT.cast<double>().transpose();
    if((!s.mouse.is_widget_down() && r.must_update && show_cage_on_drag) ||
      r.force_visible)
    {
      if(r.stale)
      {
        update_refined(TT,r.Q,vs_modified,r.U,r.N);
      }
      r.must_update = false;
    }
    delete[] vs_modified;
  }

  glEnable(GL_DEPTH_TEST);
  glDepthFunc(GL_LEQUAL);
  glEnable(GL_NORMALIZE);
  lights();
  push_scene();

  double y_boost = 0;
  // Draw a nice floor
  if(floor_visible)
  {
    if(c.U.rows() > 0)
    {
      y_boost = 2.*(c.U.col(1).minCoeff()-c.V.col(1).minCoeff())/bbd;
    }
    if(panim.is_animating && render_to_png_on_anim)
    {
      glTranslated(0,-y_boost,0);
    }
    glEnable(GL_DEPTH_TEST);
    glPushMatrix();
    double floor_offset = -2./bbd*(c.V.col(1).maxCoeff()-Vmid(1));
    floor_offset += y_boost;
    glTranslated(0,floor_offset,0);
    const float GREY[4] = {0.5,0.5,0.6,1.0};
    const float DARK_GREY[4] = {0.2,0.2,0.3,1.0};
    glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);
    glEnable(GL_CULL_FACE);
    glCullFace(GL_BACK);
    static GLuint floor_dl = 0;
    if(!glIsList(floor_dl))
    {
      floor_dl = glGenLists(1);
      glNewList(floor_dl,GL_COMPILE_AND_EXECUTE);
      draw_floor(GREY,DARK_GREY);
      glEndList();
    }else
    {
      glCallList(floor_dl);
    }
    glDisable(GL_CULL_FACE);
    glPopMatrix();
  }

  push_object();

  const auto & draw_skeleton = [](const MatrixXd & T)
  {
    switch(skel_style)
    {
      default:
      case SKEL_STYLE_TYPE_3D:
      {
        draw_skeleton_3d(C,BE,T,s.colors);
        break;
      }
      case SKEL_STYLE_TYPE_VECTOR_GRAPHICS:
        draw_skeleton_vector_graphics(C,BE,T);
        break;
    }
  };
  // Set material properties
  glDisable(GL_COLOR_MATERIAL);
  glMaterialfv(GL_FRONT, GL_AMBIENT,GOLD_AMBIENT);
  glMaterialfv(GL_FRONT, GL_DIFFUSE,GOLD_DIFFUSE);
  glMaterialfv(GL_FRONT, GL_SPECULAR,GOLD_SPECULAR);
  glMaterialf (GL_FRONT, GL_SHININESS, 128);
  glMaterialfv(GL_BACK, GL_AMBIENT,SILVER_AMBIENT);
  glMaterialfv(GL_BACK, GL_DIFFUSE,FAST_GREEN_DIFFUSE);
  glMaterialfv(GL_BACK, GL_SPECULAR,SILVER_SPECULAR);
  glMaterialf (GL_BACK, GL_SHININESS, 128);
  if(wireframe)
  {
    glPolygonMode(GL_FRONT_AND_BACK,GL_LINE);
  }
  glLineWidth(1.0);

  if((r.must_update && show_cage_on_drag) || c.force_visible)
  {
    if(c.stale)
    {
      c.N.conservativeResize(c.Q.rows(),c.N.cols());
    }
    c.draw_and_cache();
  }

  if((!r.must_update && show_cage_on_drag) || r.force_visible)
  {
    r.draw_and_cache();
  }

  if(l.force_visible)
  {
    // Set material properties
    glDisable(GL_COLOR_MATERIAL);
    glMaterialfv(GL_FRONT, GL_AMBIENT,SILVER_AMBIENT);
    glMaterialfv(GL_FRONT, GL_DIFFUSE,FAST_RED_DIFFUSE);
    glMaterialfv(GL_FRONT, GL_SPECULAR,SILVER_SPECULAR);
    glMaterialf (GL_FRONT, GL_SHININESS, 128);
    glMaterialfv(GL_BACK, GL_AMBIENT,SILVER_AMBIENT);
    glMaterialfv(GL_BACK, GL_DIFFUSE,SILVER_DIFFUSE);
    glMaterialfv(GL_BACK, GL_SPECULAR,SILVER_SPECULAR);
    glMaterialf (GL_BACK, GL_SHININESS, 128);
    if(l.stale)
    {
      l.U = M*T;
    }
    l.draw_and_cache();
  }

  glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);
  glDisable(GL_DEPTH_TEST);
  if(show_skeleton)
  {
    draw_skeleton(T);
  }

  if(centroid_is_visible && c.U.rows() > 0)
  {
    Vector3d cen;
    centroid(c.U,c.F,cen);
    glEnable(GL_DEPTH_TEST);
    glPushMatrix();
    glTranslated(cen(0),cen(1),cen(2));
    glScaled(bbd/2.,bbd/2.,bbd/2.);
    glScaled(0.1,0.1,0.1);
    glEnable(GL_POLYGON_OFFSET_FILL);
    glPolygonOffset(0,-100000);
    draw_beach_ball();
    glDisable(GL_POLYGON_OFFSET_FILL);
    glPopMatrix();
  }

  // Mouse is always on top
  glDisable(GL_DEPTH_TEST);
  if(!panim.is_animating)
  {
    s.mouse.draw();
  }
  pop_object();
  pop_scene();

  report_gl_error();

  bool was_rendering_to_png = false;
  if(render_to_png_on_next || (render_to_png_on_anim && panim.is_animating))
  {
    was_rendering_to_png = true;
    string prefix = STR(getenv("HOME")<<"/Desktop/"<< "subdivgui-");
    string filename;
    next_filename(prefix,4,".png",filename);
    GLint viewport[4];
    glGetIntegerv(GL_VIEWPORT,viewport);
    render_to_png_async(filename.c_str(),viewport[2],viewport[3],true,false);
    if(render_to_png_on_next)
    {
      glClearColor(1,1,1,1);
      glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
      glFlush();
      usleep(1000*1);
      render_to_png_on_next = false;
    }
  }

  TwDraw();
  glutSwapBuffers();

  if(canim.is_animating ||
    panim.is_animating ||
    force_anim ||
    was_rendering_to_png)
  {
    r.must_update = true;
    c.stale = l.stale = r.stale = true;
    glutPostRedisplay();
  }
  {
    static int count = 0;
    static double start = get_seconds();
    if(++count > 10)
    {
      double now = get_seconds();
      fps = double(count)/(now-start);
      count = 0;
      start = now;
    }
  }
}

void mouse_wheel(int wheel, int direction, int mouse_x, int mouse_y)
{
  using namespace std;
  using namespace igl;
  using namespace Eigen;
  GLint viewport[4];
  glGetIntegerv(GL_VIEWPORT,viewport);
  if(wheel == 0 && TwMouseMotion(mouse_x, viewport[3] - mouse_y))
  {
    static double mouse_scroll_y = 0;
    const double delta_y = 0.125*direction;
    mouse_scroll_y += delta_y;
    TwMouseWheel(mouse_scroll_y);
    return;
  }
  push_undo();

  if(wheel==0)
  {
    // factor of zoom change
    double s = (1.-0.01*direction);
    //// FOV zoom: just widen angle. This is hardly ever appropriate.
    //camera.m_angle *= s;
    //camera.m_angle = min(max(camera.m_angle,1),89);
    camera.push_away(s);
  }else
  {
    // Dolly zoom:
    camera.dolly_zoom((double)direction*1.0);
  }
  glutPostRedisplay();
}

void mouse(int glutButton, int glutState, int mouse_x, int mouse_y)
{
  using namespace std;
  using namespace Eigen;
  using namespace igl;
  bool tw_using = TwEventMouseButtonGLUT(glutButton,glutState,mouse_x,mouse_y);
  const int mod = (glutButton <=2 ? glutGetModifiers() : 0);
  const bool option_down = mod & GLUT_ACTIVE_ALT;
  switch(glutButton)
  {
    case GLUT_RIGHT_BUTTON:
    case GLUT_LEFT_BUTTON:
    {
      push_scene();
      push_object();
      switch(glutState)
      {
        case 1:
        {
          // up
          const bool mouse_was_selecting = s.mouse.is_selecting();
          is_rotating = false;
          s.mouse.up(mouse_x,mouse_y);
          glutSetCursor(GLUT_CURSOR_INHERIT);
          if(mouse_was_selecting)
          {
            s.mouse.set_selection_from_last_drag(C,BE,P,RP);
            MouseController::VectorXb S;
            MouseController::propogate_to_descendants_if(
              s.mouse.selection(),P,S);
            vector<size_t> selected_weights;
            for(size_t s = 0;s<S.rows();s++)
            {
              if(S(s))
              {
                selected_weights.push_back(s);
              }
            }
            if(selected_weights.size() > 0)
            {
              r.selected_weights = selected_weights;
              if(r.show_weights)
              {
                r.stale = true;
              }
            }
            MouseController::color_if(S,MAYA_SEA_GREEN,MAYA_VIOLET,s.colors);
          }
          break;
        }
        case 0:
          if(!tw_using)
          {
            down_x = mouse_x;
            down_y = mouse_y;
            if(option_down || glutButton==GLUT_RIGHT_BUTTON)
            {
              glutSetCursor(GLUT_CURSOR_CYCLE);
              // collect information for trackball
              is_rotating = true;
              down_camera = camera;
            }else
            {
              push_undo();
              s.mouse.down(mouse_x,mouse_y);
              if(s.mouse.is_widget_down())
              {
                r.must_update = true;
                c.stale = l.stale = r.stale = true;
              }
            }
          }
        break;
      }
      pop_object();
      pop_scene();
      break;
    }
    // Scroll down
    case 3:
    {
      mouse_wheel(0,-1,mouse_x,mouse_y);
      break;
    }
    // Scroll up
    case 4:
    {
      mouse_wheel(0,1,mouse_x,mouse_y);
      break;
    }
    // Scroll left
    case 5:
    {
      mouse_wheel(1,-1,mouse_x,mouse_y);
      break;
    }
    // Scroll right
    case 6:
    {
      mouse_wheel(1,1,mouse_x,mouse_y);
      break;
    }
  }
  glutPostRedisplay();
}

void mouse_drag(int mouse_x, int mouse_y)
{
  using namespace igl;
  using namespace std;
  using namespace Eigen;

  push_scene();
  push_object();
  if(is_rotating)
  {
    glutSetCursor(GLUT_CURSOR_CYCLE);
    Quaterniond q;
    switch(rotation_type)
    {
      case ROTATION_TYPE_IGL_TRACKBALL:
      {
        // Rotate according to trackball
        igl::trackball(
          width,
          height,
          2.0,
          down_camera.m_rotation_conj,
          down_x,
          down_y,
          mouse_x,
          mouse_y,
          q);
          break;
      }
      case ROTATION_TYPE_TWO_AXIS_VALUATOR_FIXED_UP:
      {
        // Rotate according to two axis valuator with fixed up vector
        two_axis_valuator_fixed_up(
          width, height,
          2.0,
          down_camera.m_rotation_conj,
          down_x, down_y, mouse_x, mouse_y,
          q);
        break;
      }
      default:
        break;
    }
    camera.orbit(q.conjugate());
  }else if(s.mouse.drag(mouse_x,mouse_y))
  {
    if(s.mouse.is_widget_down())
    {
      c.stale = l.stale = r.stale = true;
    }
  }
  pop_object();
  pop_scene();
  glutPostRedisplay();
}

void init_relative()
{
  using namespace Eigen;
  using namespace igl;
  using namespace std;
  const auto Vmax = c.V.colwise().maxCoeff();
  const auto Vmin = c.V.colwise().minCoeff();
  Vmid = 0.5*(Vmax + Vmin);
  bbd = (Vmax-Vmin).norm();
  camera.push_away(2);
}

void undo()
{
  using namespace std;
  if(!undo_stack.empty())
  {
    redo_stack.push(s);
    s = undo_stack.top();
    undo_stack.pop();
    s.mouse.reshape(width,height);
  }
}

void redo()
{
  using namespace std;
  if(!redo_stack.empty())
  {
    undo_stack.push(s);
    s = redo_stack.top();
    redo_stack.pop();
    s.mouse.reshape(width,height);
  }
}

bool save()
{
  using namespace std;
  using namespace igl;
  using namespace Eigen;
  string output_filename;
  next_filename(output_prefix,4,".dmat",output_filename);
  MatrixXd T;
  forward_kinematics(C,BE,P,s.mouse.rotations(),T);
  if(writeDMAT(output_filename,T))
  {
    cout<<GREENGIN("Current pose written to "+output_filename+".")<<endl;
    return true;
  }else
  {
    cout<<REDRUM("Writing to "+output_filename+" failed.")<<endl;
    return false;
  }
}

void key(unsigned char key, int mouse_x, int mouse_y)
{
  using namespace std;
  using namespace igl;
  using namespace Eigen;
  r.must_update = true;
  c.stale = l.stale = r.stale = true;
  int mod = glutGetModifiers();
  const bool command_down = GLUT_ACTIVE_COMMAND & mod;
  const bool shift_down = GLUT_ACTIVE_SHIFT & mod;
  switch(key)
  {
    // ESC
    case char(27):
      rebar.save(REBAR_NAME);
    // ^C
    case char(3):
      exit(EXIT_SUCCESS);
    case 'a':
    {
      panim.is_animating = !panim.is_animating;
      panim.pose = s.mouse.rotations();
      panim.start_time = get_seconds();
      panim.count = 0;
      break;
    }
    case 'D':
    case 'd':
    {
      push_undo();
      s.mouse.clear_selection();
      break;
    }
    case 'P':
    case 'p':
    {
      render_to_png_on_next = true;
      break;
    }
    case 'R':
    {
      push_undo();
      s.mouse.reset_selected_rotations();
      break;
    }
    case 'r':
    {
      push_undo();
      s.mouse.reset_rotations();
      break;
    }
    //case 'S':
    //case 's':
    //{
    //  save();
    //  break;
    //}
    case 'z':
    case 'Z':
      is_rotating = false;
      if(command_down)
      {
        if(shift_down)
        {
          redo();
        }else
        {
          undo();
        }
        break;
      }else
      {
        push_undo();
        Quaterniond q;
        snap_to_canonical_view_quat(camera.m_rotation_conj,1.0,q);
        camera.orbit(q.conjugate());
      }
      break;
    default:
      if(!TwEventKeyboardGLUT(key,mouse_x,mouse_y))
      {
        cout<<"Unknown key command: "<<key<<" "<<int(key)<<endl;
      }
  }

  glutPostRedisplay();
}

int main(int argc, char * argv[])
{
  using namespace std;
  using namespace Eigen;
  using namespace igl;
  string filename = "torus.obj";
  string skel_filename = "torus.tgf";
  string weights_filename = "";
  output_prefix = "";
  int comp_level = 1;
  int eval_level = 3;
  switch(argc)
  {
    case 7:
      output_prefix = argv[6];
      //fall through
    case 6:
      weights_filename = argv[5];
    case 5:
      eval_level = atoi(argv[4]);
    case 4:
      comp_level = atoi(argv[3]);
      //fall through
    case 3:
      skel_filename = argv[2];
      // Read and prepare mesh
      filename = argv[1];
      break;
    default:
      cerr<<"Usage:"<<endl<<"    ./example model.obj skeleton.tgf "<<
        "[comp_level] [eval_level] [weights.dmat] [pose-prefix]"<<endl;
      cout<<endl<<"Opening default rig..."<<endl;
      weights_filename = "torus-weights.dmat";
  }
  if(comp_level>eval_level)
  {
    cerr<<"comp_level ("<<comp_level<<") must be <= eval_level ("<<eval_level
      <<")"<<endl;
    return EXIT_FAILURE;
  }

  // print key commands
  cout<<"[Click] and [drag]     Select a bone/Use onscreen widget to rotate bone."<<endl;
  cout<<"⌥ +[Click] and [drag]  Rotate secene."<<endl;
  cout<<"[,]                    Change scene rotation mouse UI method."<<endl;
  cout<<"A                      Force tight animation rendering loop."<<endl;
  cout<<"a                      Animate from current pose to rest pose."<<endl;
  cout<<"C,c                    Visualize centroid."<<endl;
  cout<<"D,d                    Deselect all."<<endl;
  cout<<"F,f                    Show floor."<<endl;
  cout<<"G                      Force show coarse cage."<<endl;
  cout<<"g                      Force show refined mesh."<<endl;
  cout<<"H,h                    Show coarse cage while dragging."<<endl;
  cout<<"l                      Show wireframe on all."<<endl;
  cout<<"L                      Show wireframe on cage."<<endl;
  cout<<"P,p                    Render next frame to png file."<<endl;
  cout<<"R                      Reset selected rotation(s)."<<endl;
  cout<<"r                      Reset all rotations."<<endl;
  cout<<"S,s                    Save current pose."<<endl;
  cout<<"V,v                    Render directly skinned \"limit\" surface."<<endl;
  cout<<"Z,z                    Snap to canonical view."<<endl;
  cout<<"⌘ Z                    Undo."<<endl;
  cout<<"⇧ ⌘ Z                  Redo."<<endl;
  cout<<"^C,ESC                 Exit (without saving)."<<endl;

  // Load subdivision cage
  string dir,_1,_2,name;
  pathinfo(filename,dir,_1,_2,name);
  {
    vector<vector<int > > vCQ,vCF;
    vector<vector<double> > vCV;
    if(!readOBJ(filename,vCV,vCQ))
    {
      return EXIT_FAILURE;
    }
    list_to_matrix(vCV,c.V);
    polygon_mesh_to_triangle_mesh(vCQ,c.F);
    if(!list_to_matrix(vCQ,4,-1,c.Q))
    {
      cerr<<"Error: "<<filename<<" contains high valence facet."<<endl;
      return EXIT_FAILURE;
    }
  }

  if(output_prefix.size() == 0)
  {
    output_prefix = dir+"/"+name+"-pose-";
  }

  {
    string output_filename;
    next_filename(output_prefix,4,".dmat",output_filename);
    cout<<BLUEGIN("Output set to start with "<<output_filename)<<endl;
  }

  /////////////////////////////////////////////////////////////////////////
  // Tell subdiv lib about cage
  /////////////////////////////////////////////////////////////////////////
  const Matrix<subdivision_evaluator_real_t,Dynamic,Dynamic> CVT =
    c.V.cast<subdivision_evaluator_real_t>().transpose();
  const subdivision_evaluator_real_t * vs = CVT.data();
  const int num_vs = c.V.rows();
  const MatrixXi CQT = c.Q.cast<int>().transpose();
  const int * faces = CQT.data();
  const int num_faces = c.Q.rows();
  eval = new_subdivision_evaluator(
   num_vs, (subdivision_evaluator_real_t*)vs, num_faces, (int*)faces, eval_level);

  /////////////////////////////////////////////////////////////////////////
  // Ask subdiv lib for refined mesh
  /////////////////////////////////////////////////////////////////////////
  const int num_refined_faces = num_refined_quad_faces_of_subdivision_evaluator( eval );
  int* refined_faces = new int[ num_refined_faces*4 ];
  get_refined_quad_faces_of_subdivision_evaluator( eval, refined_faces );
  const auto & QT = Map<MatrixXi>(refined_faces,4,num_refined_faces);
  r.Q = QT.transpose();
  polygon_mesh_to_triangle_mesh(r.Q,r.F);
  delete[] refined_faces;

  const int num_refined_vs = num_refined_vertices_of_subdivision_evaluator( eval );
  subdivision_evaluator_real_t* refined_vs = new subdivision_evaluator_real_t[ num_refined_vs*3 ];
  get_refined_vertices_of_subdivision_evaluator( eval, refined_vs );
  const auto & VT = Map<Matrix<subdivision_evaluator_real_t,Dynamic,Dynamic> >
    (refined_vs,3,num_refined_vs);
  r.V = VT.cast<double>().transpose();
  delete[] refined_vs;

  /////////////////////////////////////////////////////////////////////////
  // Load skeleton and load/compute weights
  /////////////////////////////////////////////////////////////////////////
  // Read in skeleton and precompute hierarchy
  readTGF(skel_filename,C,BE);
  // initialize mouse interface
  s.mouse.set_size(BE.rows());
  // Rigid parts (not used)
  colon<int>(0,BE.rows()-1,RP);
  assert(RP.size() == BE.rows());
  // Bone parents
  bone_parents(BE,P);
  if(weights_filename.size() == 0 || !file_exists(weights_filename.c_str()))
  {
    cout<<YELLOWGIN("Computing weights...")<<endl;
    //robust_bbw(V,F,C,BE,r.W);
    subdiv_weights(c.V,c.Q,C,BE,comp_level,eval_level,WEIGHTS_TYPE_BBW,r.W);
    if(weights_filename.size() > 0)
    {
      if(writeDMAT(weights_filename,r.W))
      {
        cout<<GREENGIN("Saved weights to "<<weights_filename)<<endl;
      }else
      {
        cerr<<REDRUM("Failed to save weights to "<<weights_filename)<<endl;
      }
    }
  }else
  {
    // Read in weights and precompute LBS matrix
    readDMAT(weights_filename,r.W);
  }

  assert(r.W.rows() == r.V.rows() && "#W must match #r.V");
  // Use refined mesh for lbs
  l = r;
  lbs_matrix(l.V,r.W,M);
  if(r.W.cols() != BE.rows())
  {
    cerr<<REDRUM("# cols in weights ("<<r.W.cols()<<
      ") != # bones ("<<BE.rows()<<")")<<endl;
    return EXIT_FAILURE;
  }

  if(num_refined_vs != r.W.rows())
  {
    cerr<<REDRUM("# rows in weights ("<<r.W.rows()<<
      ") != # refined vertices ("<<
      num_refined_vs<<") at level ("<<
      eval_level<<")")<<endl;
    return EXIT_FAILURE;
  }

  /////////////////////////////////////////////////////////////////////////
  // Tell subdiv lib about weights
  /////////////////////////////////////////////////////////////////////////
  const Matrix<subdivision_evaluator_real_t,Dynamic,Dynamic> WT =
    r.W.cast<subdivision_evaluator_real_t>().transpose();
  const subdivision_evaluator_real_t * weights = WT.data();
  engine = new_subdivision_skinning_engine( eval, r.W.cols(), weights );

  // Handle degenerate quads before draw (these only occur in coarse)
  for(int i = 0;i<c.Q.rows();i++)
  {
    if(c.Q(i,3) == -1)
    {
      c.Q(i,3) = c.Q(i,2);
    }
  }

  // initialize some camera settings
  init_relative();

  // Init glut
  glutInit(&argc,argv);
  if( !TwInit(TW_OPENGL, NULL) )
  {
    // A fatal error occured
    fprintf(stderr, "AntTweakBar initialization failed: %s\n", TwGetLastError());
    return EXIT_FAILURE;
  }
  // Create a tweak bar
  rebar.TwNewBar("TweakBar");
  rebar.TwAddVarRO("fps", TW_TYPE_DOUBLE,&fps,"");
  rebar.TwAddVarRW("force_anim", TW_TYPE_BOOLCPP,&force_anim,"key=A");
  rebar.TwAddVarRW("camera_rotation", TW_TYPE_QUAT4D,
    camera.m_rotation_conj.coeffs().data(), "open readonly=true");
  TwType RotationTypeTW = ReTwDefineEnumFromString("RotationType",
    "igl_trackball,two-a...-fixed-up");
  rebar.TwAddVarCB( "rotation_type", RotationTypeTW,
    set_rotation_type,get_rotation_type,NULL,"keyIncr=] keyDecr=[");
  rebar.TwAddVarRW("wireframe", TW_TYPE_BOOLCPP,&wireframe,"key=l");
  rebar.TwAddVarRW("show_cage_on_drag", TW_TYPE_BOOLCPP,&show_cage_on_drag,
    "keyIncr=H keyDecr=h");
  rebar.TwAddVarRW("c_force_visible", TW_TYPE_BOOLCPP,&c.force_visible,"key=G");
  rebar.TwAddVarRW("c_wire_frame", TW_TYPE_BOOLCPP,&c.wireframe,"key=L");
  rebar.TwAddVarRW("r_force_visible", TW_TYPE_BOOLCPP,&r.force_visible,"key=g");
  rebar.TwAddVarRW("l_force_visible", TW_TYPE_BOOLCPP,&l.force_visible,
    "keyIncr=V keyDecr=v");
  rebar.TwAddVarRW("floor_visible", TW_TYPE_BOOLCPP,&floor_visible,"keyIncr=f keyDecr=F");
  rebar.TwAddVarRW("render_to_png_on_anim", TW_TYPE_BOOLCPP,&render_to_png_on_anim,"key='CTRL+a'",false);
  rebar.TwAddVarRW("centroid_is_visible", TW_TYPE_BOOLCPP,&centroid_is_visible,
    "keyIncr=C keyDecr=c label='centroid visible?'");
  TwType SkelStyleTypeTW = ReTwDefineEnumFromString("SkelStyleType",
    "3d,vector-graphics");
  rebar.TwAddVarRW("style",SkelStyleTypeTW,&skel_style,"");
  rebar.TwAddVarRW("r_show_weights", TW_TYPE_BOOLCPP,&r.show_weights,"key=w");
  rebar.TwAddVarRW("show_skeleton", TW_TYPE_BOOLCPP,&show_skeleton,"keyIncr=s keyDecr=S");
  rebar.load(REBAR_NAME);

  // Init antweakbar
  glutInitDisplayString( "rgba depth double samples>=8 ");
  glutInitWindowSize(glutGet(GLUT_SCREEN_WIDTH)/2.0,glutGet(GLUT_SCREEN_HEIGHT)/2.0);
  glutCreateWindow("skeleton-poser");
  glutDisplayFunc(display);
  glutReshapeFunc(reshape);
  glutKeyboardFunc(key);
  glutMouseFunc(mouse);
  glutMotionFunc(mouse_drag);
  glutPassiveMotionFunc((GLUTmousemotionfun)TwEventMouseMotionGLUT);
  glutMainLoop();

  // Clean up
  delete_subdivision_skinning_engine( engine );
  delete_subdivision_evaluator( eval );
  return EXIT_SUCCESS;
}


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

// ---
// Author: Zachary Kann

// Contains information about groups of atoms as defined in
// supplied .ndx file. This allows the properties of a subset
// of the atoms to be treated with the same ease as the entire
// set of atoms. This class includes postion, velocity, and mass
// information for individual atoms as well as for molecules
// defined in the .top file.

#include <cassert>
#include <armadillo>
#include "z_constants.hpp"
#include "z_molecule.hpp"
#include "z_gromacs.hpp"
#include "z_string.hpp"
#include "z_histogram.hpp"
#include "z_vec.hpp"
#ifndef _Z_PARTICLE_GROUP_HPP_
#define _Z_PARTICLE_GROUP_HPP_

//See description above
class ParticleGroup {
 public:
  ParticleGroup(std::string name, std::vector<int> indices)
      : indices_(indices), group_size_(indices.size()),
        field_filename_(name+"_electric_field.dat"), name_(name) {};

  ParticleGroup() {};

  // Creates .gro file
  virtual void WriteGro(const std::string& gro, const arma::rowvec& box,
                        const std::string description) = 0;

  // Deletes a molecule from the group. Sends references to the
  // center of mass position and velocity in case the user wishes
  // to replace it with another molecule.
  virtual void RemoveMolecule(const int molecule_id,
                              arma::rowvec& old_com_position,
                              arma::rowvec& old_com_velocity) = 0;

  // Wrapper for the above function which throws away references
  // to position/velocity vectors. Used if replacement is not planned.
  inline void RemoveMolecule(const int molecule_id) {
    arma::rowvec position(DIMS);
    arma::rowvec velocity(DIMS);
    RemoveMolecule(molecule_id, position, velocity);
  }

  // Adds a molecule to the group at the given position.
  // Currently only supports monatomic molecules.
  virtual void AddMolecule(const int molecule_id, const Molecule& mol_to_add,
                           const arma::rowvec& position,
                           const arma::rowvec& velocity) = 0;

  // Removes old molecule and adds new one at same position.
  // TODO(Zak) Separate into versions for all_atoms group and smaller groups.
  virtual inline void ReplaceMolecule(const Molecule& new_molecule,
                                      const int mol_to_remove) = 0;

  // Resets center of mass position and velocity of all molecules.
  virtual inline void ZeroCom() = 0;

  // Sets center of mass position and velocity of all molecules.
  virtual void UpdateCom() = 0;

  inline void ZeroElectricField() {
    electric_fields_ = arma::zeros(group_size_, DIMS);
  }

  void CalculateElectricField(const ParticleGroup& other_group,
                              const arma::rowvec& box,
                              const arma::imat& nearby_molecules,
                              arma::rowvec& dx);

  void CalculateElectricField(const ParticleGroup& other_group,
                              const arma::rowvec& box,
                              const double cutoff_squared,
                              arma::rowvec& dx);

  void MarkNearbyAtoms(const ParticleGroup& other_group,
                              const arma::rowvec& box,
                              const double cutoff_squared,
                              arma::imat& nearby_atoms) const;

  // Mutators
  inline void set_has_molecules(const bool has_mol) {
    has_molecules_ = has_mol;
  }
  inline void set_name(const std::string& name) {
    name_ = name;
  }

  inline void set_position(const int i, const rvec& position) {
    positions_.row(i) = RvecToRow(position);
  }

  inline void set_velocity(const int i, const rvec& velocity) {
    velocities_.row(i) = RvecToRow(velocity);
  }

  inline void set_positions(const rvec x_in[]) {
    int i = 0;
    for (std::vector<int>::iterator i_atom = indices_.begin();
         i_atom != indices_.begin(); ++i_atom, ++i) {
      set_position(i, x_in[*i_atom]);
    }
  }

  inline void set_velocities(const rvec v_in[]) {
    int i = 0;
    for (std::vector<int>::iterator i_atom = indices_.begin();
         i_atom != indices_.begin(); ++i_atom, ++i) {
      set_velocity(i, v_in[*i_atom]);
    }
  }

  inline void set_phase_space(const rvec x_in[], const rvec v_in[]) {
    int i = 0;
    for (std::vector<int>::iterator i_atom = indices_.begin();
         i_atom != indices_.begin(); ++i_atom, ++i) {
      set_position(i, x_in[*i_atom]);
      set_velocity(i, v_in[*i_atom]);
    }
  }

  inline void set_gammas(const double gamma) {
    for (int i_atom = 0; i_atom < group_size_; i_atom++) {
      index_to_gamma_.push_back(gamma);
    }
  }

  virtual inline void set_ion_gammas(const double cation_gamma,
                                     const double anion_gamma) = 0;

  // Accessors
  inline double kFieldCutoffSquared() const { return kFieldCutoffSquared_; }
  inline bool has_molecules() const { return has_molecules_; }
  inline bool field_check() const { return field_check_; }
  inline std::string name() const { return name_; }
  inline std::vector<int>::const_iterator begin() const {
    return indices_.begin();
  }
  inline std::vector<int>::const_iterator end() const { return indices_.end(); }
  inline int size() const { return group_size_; }
  inline std::vector<int> indices() const { return indices_; }
  inline int indices(int i) const { return indices_[i]; }
  virtual inline int num_molecules() const = 0;
  inline arma::mat positions() const { return positions_; }
  inline arma::mat velocities() const { return velocities_; }
  virtual inline arma::mat com_positions() const = 0;
  virtual inline arma::mat com_velocities() const = 0;
  inline arma::rowvec position(int atom) const { return positions_.row(atom); }
  inline arma::rowvec velocity(int atom) const { return velocities_.row(atom); }
  inline arma::rowvec electric_field(int atom) const {
    return electric_fields_.row(atom);
  }
  inline double mass(int atom) const { return index_to_mass_[atom]; }
  inline double sigma(int atom) const { return index_to_sigma_[atom]; }
  inline double epsilon(int atom) const { return index_to_epsilon_[atom]; }
  inline double charge(int atom) const { return index_to_charge_[atom]; }
  inline double gamma(int atom) const { return index_to_gamma_[atom]; }

  virtual inline arma::rowvec com_position(int molecule) const = 0;

  virtual inline arma::rowvec com_velocity(int molecule) const = 0;

  inline double position(int atom, int dim) const {
    return positions_(atom, dim);
  }

  inline double velocity(int atom, int dim) const {
    return velocities_(atom, dim);
  }

  inline arma::rowvec velocity_xy(int atom) const {
    return velocities_(atom, arma::span(0,1));
  }

  virtual inline double com_position(int molecule, int dim) const = 0;

  virtual inline double com_velocity(int molecule, int dim) const = 0;

  virtual inline arma::rowvec com_velocity_xy(int molecule) const = 0;

  virtual inline int molecular_index(int atom) const = 0;

  // Used to be molecule_mass
  virtual inline double molecular_mass(int molecule) const = 0;

  virtual inline double molecular_charge(int molecule) const = 0;

  virtual inline int molecular_index_to_molecule(int molecule) const = 0;

  inline int index_to_molecule(int index) const {
    return index_to_molecule_[index];
  }

  inline std::string atom_name(int index) const {
    return atom_names_[index];
  }

  inline std::string molecule_name(int index) const {
    return molecule_names_[index];
  }

  inline void WriteElectricField() {
    assert(field_file_.is_open());
    for (int i_atom = 0; i_atom < group_size_; i_atom++) {
      for (int i_dim = 0; i_dim < DIMS; i_dim++)
        field_file_ << std::scientific << electric_fields_(i_atom,i_dim) << " ";
    }
    field_file_ << std::endl;
  }

  inline void ReadElectricField(std::fstream& field_file) {
    std::string line;
    assert(field_file.is_open());
    getline(field_file, line);
    const std::vector<std::string> split_line = Split(line, ' ');
    std::vector<std::string>::const_iterator i_split = split_line.begin();
    for (int i_atom = 0; i_atom < group_size_; i_atom++) {
      for (int i_dim = 0; i_dim < DIMS; i_dim++) {
        electric_fields_(i_atom,i_dim) = atof((*i_split).c_str());
        i_split++;
      }
    }
  }

  void FindClusters(const double cutoff_distance_squared,
                    const arma::rowvec& box, Histogram& clusters) const;

  // Finds the k atoms that are nearest to some point. The atoms are returned
  // in distance order.
  void FindNearestk(const arma::rowvec& point, const arma::rowvec& box,
                    const int exclude_molecule, const int k, arma::rowvec& dx,
                    arma::rowvec& r2, arma::irowvec& nearest) const;

  inline void OpenFieldFile() {
    field_file_.open(field_filename_.c_str(), std::fstream::in);
    if (!field_file_.is_open()) {
      field_file_.open(field_filename_.c_str(), std::fstream::out);
      field_check_ = false;
    } else {
      std::cout << "Using previous electric field data from file: " <<
                 field_filename_ << "." << std::endl;
      field_check_ = true;
    }
  }

  virtual inline void PermanentDipole(const int molecule,
                                      const arma::rowvec& box,
                                      arma::rowvec& dipole,
                                      const bool zero_first = false) = 0;

  virtual inline void InducedDipole(const int molecule, arma::rowvec& dipole,
                                    const bool zero_first = false) const = 0;

  virtual inline void SetElectricField(const ParticleGroup& other_group,
                                       const arma::rowvec& box) = 0;

  virtual inline void UpdateElectricField(const ParticleGroup& other_group,
                                          const arma::rowvec& box) = 0;

 protected:
  std::vector<double> index_to_gamma_;
  std::vector<int> index_to_molecule_;
  std::vector<std::string> molecule_names_;
  arma::mat positions_;
  arma::mat velocities_;
  arma::mat electric_fields_;
  std::vector<int> indices_;
  std::vector<std::string> atom_names_;
  std::vector<double> index_to_mass_;
  std::vector<double> index_to_sigma_;
  std::vector<double> index_to_epsilon_;
  std::vector<double> index_to_charge_;
  int group_size_;
  bool has_molecules_;
  arma::imat nearby_molecules;
  arma::rowvec dx;
  bool field_check_;
  std::vector<bool> included_in_field_;
  std::fstream field_file_;
  std::string field_filename_;

  // Indices of atoms for use in finding whether molecule is nearby.
  // Primarily for electric field calculations.
  std::vector<int> molecule_position_markers_;

 private:
  static const double kFieldCutoff_ = 0.7831;
  static const double kFieldCutoffSquared_ = 0.61324561;
  std::string name_;

  // Takes care of the atom-level removal for RemoveMolecule
  virtual void RemoveAtom(const int index) = 0;

  // Takes care of the atom-level addition for AddMolecule
  virtual void AddAtom(const Atom atom_to_add, const int molecule_id,
                       const arma::rowvec& position,
                       const arma::rowvec& velocity) = 0;

  void FindCluster(const double cutoff_distance_squared,
                   const arma::rowvec& box, arma::rowvec& dx,
                   arma::irowvec& in_current_cluster,
                   arma::irowvec& in_any_cluster,
                   const int start_atom) const;

  virtual inline void CalculateMolecularCharges (
      const std::vector<double>& molecular_index_to_charge) = 0;

};

#endif


#define BOOST_TEST_MODULE "test_omp_3spn2_cross_stacking_interaction"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <test/util/utility.hpp>

#include <mjolnir/math/math.hpp>
#include <mjolnir/math/constants.hpp>
#include <mjolnir/util/make_unique.hpp>
#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/core/VerletList.hpp>
#include <mjolnir/omp/System.hpp>
#include <mjolnir/omp/ThreeSPN2BasePairInteraction.hpp>
#include <mjolnir/omp/ThreeSPN2CrossStackingInteraction.hpp>

BOOST_AUTO_TEST_CASE(ThreeSPN2CrossStackingIntearction_numerical_diff)
{
    namespace test = mjolnir::test;

    constexpr double tol = 1e-4;
    mjolnir::LoggerManager::set_default_logger(
            "test_omp_3spn2_cross_stacking_interaction.log");

    using seq_traits_type  = mjolnir::SimulatorTraits      <double, mjolnir::UnlimitedBoundary>;
    using omp_traits_type  = mjolnir::OpenMPSimulatorTraits<double, mjolnir::UnlimitedBoundary>;

    using real_type        = typename omp_traits_type::real_type;
    using matrix33_type    = typename omp_traits_type::matrix33_type;
    using coordinate_type  = typename omp_traits_type::coordinate_type;
    using boundary_type    = typename omp_traits_type::boundary_type;
    using topology_type    = mjolnir::Topology;

    using potential_type       = mjolnir::ThreeSPN2CrossStackingPotential<real_type>;

    using omp_system_type      = mjolnir::System<omp_traits_type>;
    using omp_interaction_type = mjolnir::ThreeSPN2CrossStackingInteraction<omp_traits_type>;

    using seq_system_type      = mjolnir::System<seq_traits_type>;
    using seq_interaction_type = mjolnir::ThreeSPN2CrossStackingInteraction<seq_traits_type>;

    using omp_parameter_list_type = mjolnir::ThreeSPN2CrossStackingParameterList<omp_traits_type>;
    using seq_parameter_list_type = mjolnir::ThreeSPN2CrossStackingParameterList<seq_traits_type>;
    using omp_parameter_type      = typename omp_parameter_list_type::parameter_type;
    using seq_parameter_type      = typename seq_parameter_list_type::parameter_type;

    using omp_partition_type      = mjolnir::VerletList<omp_traits_type, potential_type>;
    using seq_partition_type      = mjolnir::VerletList<seq_traits_type, potential_type>;

    // those actually does not depends on traits.
    using base_kind            = typename omp_parameter_list_type::base_kind;
    using ignore_group_type    = typename omp_parameter_list_type::ignore_group_type;
    using ignore_molecule_type = typename omp_parameter_list_type::ignore_molecule_type;

    constexpr real_type pi = mjolnir::math::constants<real_type>::pi();

    const int max_number_of_threads = omp_get_max_threads();
    BOOST_TEST_WARN(max_number_of_threads > 2);
    BOOST_TEST_MESSAGE("maximum number of threads = " << omp_get_max_threads());

    {
        using unit_type = mjolnir::unit::constants<real_type>;
        using phys_type = mjolnir::physics::constants<real_type>;
        const std::string energy = "kcal/mol";
        const std::string length = "angstrom";
        phys_type::set_kB(phys_type::kB() * (unit_type::J_to_cal() / 1000.0) *
                          unit_type::avogadro_constant());
        phys_type::set_eps0(phys_type::eps0() * (1000.0 / unit_type::J_to_cal()) /
                            unit_type::avogadro_constant());
        phys_type::set_energy_unit(energy);

        phys_type::set_eps0(phys_type::eps0() / unit_type::m_to_angstrom());

        phys_type::set_m_to_length(unit_type::m_to_angstrom());
        phys_type::set_length_to_m(unit_type::angstrom_to_m());

        phys_type::set_L_to_volume(1e-3 * std::pow(unit_type::m_to_angstrom(), 3));
        phys_type::set_volume_to_L(1e+3 * std::pow(unit_type::angstrom_to_m(), 3));

        phys_type::set_length_unit(length);
    }

    //       Si   Bi   Bj   Sj
    //  5'    o -- o===o -- o     3'
    //  ^    /      \ /      \    |
    //  | P o        X        o P |
    //  |    \      / \      /    v
    //  3'    o -- o===o -- o     5'
    //       Bj_next   Bj_next
    //
    // Required test cases are the combination of the following conditions
    //
    // CrossStacking
    // theta3:
    //  1. theta < pi/2K
    //  2. pi/2K < theta < pi/K
    //  3. pi/K  < theta
    // thetaCS_i:
    //  1. theta < pi/2K
    //  2. pi/2K < theta < pi/K
    //  3. pi/K  < theta
    // thetaCS_j:
    //  1. theta < pi/2K
    //  2. pi/2K < theta < pi/K
    //  3. pi/K  < theta
    // thetaCS_j:
    //  1. theta < pi/2K
    //  2. pi/2K < theta < pi/K
    //  3. pi/K  < theta
    // ri_jnext
    //  1. (r < r0)
    //  2. (r0 < r)
    // rj_inext
    //  1. (r < r0)
    //  2. (r0 < r)
    //
    // Oh, okay, we have 3^4 * 2^2 = 324 test configurations.
    //
    // Also, this interaction is a sum of two, BasePairing and CrossStacking.
    // This makes numeric differentiation unstable.

    std::mt19937 rng(123456789);

    for(int num_thread=1; num_thread<=max_number_of_threads; ++num_thread)
    {
        omp_set_num_threads(num_thread);
        BOOST_TEST_MESSAGE("maximum number of threads = " << omp_get_max_threads());

        const std::vector<base_kind> bases{base_kind::A, base_kind::T, base_kind::G, base_kind::G};
        //        0    1   9    8
        //  5'    o -- o===o -- o     3'
        //  ^    /      \ /      \    |
        //  | 2 o        X        o 7 |
        //  |    \      / \      /    v
        //  3'    o -- o===o -- o     5'
        //        3    4   6    5

        constexpr auto invalid = potential_type::invalid();

        omp_parameter_list_type omp_parameter_list(
            mjolnir::ThreeSPN2CrossStackingGlobalPotentialParameter<real_type>{}, {
                {1, omp_parameter_type{bases.at(0), 0, 4, invalid}},
                {9, omp_parameter_type{bases.at(1), 8, invalid, 6}},
                {4, omp_parameter_type{bases.at(2), 3, invalid, 1}},
                {6, omp_parameter_type{bases.at(3), 5, 9, invalid}},
            }, {}, ignore_molecule_type("Nothing"), ignore_group_type({})
        );
        seq_parameter_list_type seq_parameter_list(
            mjolnir::ThreeSPN2CrossStackingGlobalPotentialParameter<real_type>{}, {
                {1, seq_parameter_type{bases.at(0), 0, 4, invalid}},
                {9, seq_parameter_type{bases.at(1), 8, invalid, 6}},
                {4, seq_parameter_type{bases.at(2), 3, invalid, 1}},
                {6, seq_parameter_type{bases.at(3), 5, 9, invalid}},
            }, {}, ignore_molecule_type("Nothing"), ignore_group_type({})
        );
        mjolnir::ThreeSPN2BasePairParameterList<omp_traits_type> bp_parameter_list(
            mjolnir::ThreeSPN2BasePairGlobalPotentialParameter<real_type>{}, {}, {},
            ignore_molecule_type("Nothing"), ignore_group_type({}));

        omp_interaction_type omp_interaction(potential_type{},
            omp_parameter_list_type(omp_parameter_list),
            mjolnir::SpatialPartition<omp_traits_type, potential_type>(
                mjolnir::make_unique<omp_partition_type>()));

        seq_interaction_type seq_interaction(potential_type{},
            seq_parameter_list_type(seq_parameter_list),
            mjolnir::SpatialPartition<seq_traits_type, potential_type>(
                mjolnir::make_unique<seq_partition_type>()));

        omp_system_type omp_sys(10, boundary_type{});
        seq_system_type seq_sys(10, boundary_type{});

        test::clear_everything(omp_sys);
        test::clear_everything(seq_sys);
        topology_type topol(10);

        topol.add_connection(0, 1, "bond");
        topol.add_connection(0, 2, "bond");
        topol.add_connection(2, 3, "bond");
        topol.add_connection(3, 4, "bond");

        topol.add_connection(5, 6, "bond");
        topol.add_connection(5, 7, "bond");
        topol.add_connection(7, 8, "bond");
        topol.add_connection(8, 9, "bond");

        topol.add_connection(0, 2, "next_nucl");
        topol.add_connection(1, 2, "next_nucl");
        topol.add_connection(0, 3, "next_nucl");
        topol.add_connection(1, 3, "next_nucl");
        topol.add_connection(0, 4, "next_nucl");
        topol.add_connection(1, 4, "next_nucl");

        topol.add_connection(5, 7, "next_nucl");
        topol.add_connection(6, 7, "next_nucl");
        topol.add_connection(5, 8, "next_nucl");
        topol.add_connection(6, 8, "next_nucl");
        topol.add_connection(5, 9, "next_nucl");
        topol.add_connection(6, 9, "next_nucl");

        topol.construct_molecules();

        omp_interaction.initialize(omp_sys, topol);
        seq_interaction.initialize(seq_sys, topol);

        // paramter_lists in the interactions will be initialized via
        // interaction.initialize(), but parameter_list here will not.
        potential_type pot;
        omp_parameter_list.initialize(omp_sys, topol, pot);

        const auto bp_kind      = omp_parameter_list.bp_kind (bases.at(0), bases.at(1));
        const auto cs_i_kind    = omp_parameter_list.cs5_kind(bases.at(0), bases.at(3));
        const auto cs_j_kind    = omp_parameter_list.cs3_kind(bases.at(1), bases.at(2));

        const auto rbp          =  bp_parameter_list.r0(bp_kind) + 0.2;
        const auto rcs_i_0      = omp_parameter_list.r0(cs_i_kind);
        const auto rcs_j_0      = omp_parameter_list.r0(cs_j_kind);
        const auto theta1       =  bp_parameter_list.theta1_0(bp_kind);
        const auto theta2       =  bp_parameter_list.theta2_0(bp_kind);
        const auto theta3_0     = omp_parameter_list.theta3_0(bp_kind);
        const auto thetaCS_i_0  = omp_parameter_list.thetaCS_0(cs_i_kind);
        const auto thetaCS_j_0  = omp_parameter_list.thetaCS_0(cs_j_kind);

        const auto pi_over_K_BP =  bp_parameter_list.pi_over_K_BP();
        const auto pi_over_K_CS = omp_parameter_list.pi_over_K_CS();

        for(const auto rcsi : {rcs_i_0 - 0.2, rcs_i_0 + 0.5})
        {
        for(const auto rcsj : {rcs_j_0 - 0.2, rcs_j_0 + 0.5})
        {
        for(const auto theta3 : {theta3_0 + pi_over_K_BP * 0.1, theta3_0 + pi_over_K_BP * 0.6, theta3_0 + pi_over_K_BP * 1.1})
        {
        for(const auto thetaCS_i : {thetaCS_i_0 - pi_over_K_CS * 0.1, thetaCS_i_0 - pi_over_K_CS * 0.6, thetaCS_i_0 - pi_over_K_CS * 1.1})
        {
        for(const auto thetaCS_j : {thetaCS_j_0 + pi_over_K_CS * 0.1, thetaCS_j_0 + pi_over_K_CS * 0.6, thetaCS_j_0 + pi_over_K_CS * 1.1})
        {
            BOOST_TEST_REQUIRE(0.0 <= theta3   );BOOST_TEST_REQUIRE(theta3    <= pi);
            BOOST_TEST_REQUIRE(0.0 <= thetaCS_i);BOOST_TEST_REQUIRE(thetaCS_i <= pi);
            BOOST_TEST_REQUIRE(0.0 <= thetaCS_j);BOOST_TEST_REQUIRE(thetaCS_j <= pi);

            // generate positions...
            //        0    1   9    8
            //  5'    o -- o===o -- o     3'
            //  ^    /      \ /      \    |
            //  | 2 o        X        o 7 |
            //  |    \      / \      /    v
            //  3'    o -- o===o -- o     5'
            //        3    4   6    5

            omp_sys.position(1) = coordinate_type(0.0, 0.0, 0.0);
            omp_sys.position(9) = coordinate_type(0.0, 0.0, 0.0);

            omp_sys.position(0) = coordinate_type(4.0 * std::cos(theta1),
                                                  4.0 * std::sin(theta1), 0.0);
            omp_sys.position(8) = coordinate_type(4.0 * std::cos(pi - theta2),
                                                  4.0 * std::sin(pi - theta2), 0.0);
            // rotate around x axis
            // to make angle between 0->1 and 8->9 theta3
            {
                real_type best_phi = 0.0;
                real_type dtheta   = std::numeric_limits<real_type>::max();
                const auto bck = omp_sys;
                for(int i=-999; i<1000; ++i)
                {
                    const real_type phi = pi * 0.001 * i;
                    const matrix33_type rot(
                       1.0,           0.0,           0.0,
                       0.0, std::cos(phi), -std::sin(phi),
                       0.0, std::sin(phi),  std::cos(phi));

                    omp_sys.position(8) = rot * omp_sys.position(8);
                    omp_sys.position(9) = rot * omp_sys.position(9);

                    const auto v01 = omp_sys.position(1) - omp_sys.position(0);
                    const auto v89 = omp_sys.position(9) - omp_sys.position(8);
                    const auto theta_0189 = std::acos(mjolnir::math::clamp<real_type>(
                         mjolnir::math::dot_product(v01, v89) /
                        (mjolnir::math::length(v01) * mjolnir::math::length(v89)),
                        -1.0, 1.0));

                    BOOST_TEST_MESSAGE("dtheta = " << dtheta << ", current configuration = " << std::abs(theta_0189 - theta3));

                    if(std::abs(theta_0189 - theta3) < dtheta)
                    {
                        dtheta   = std::abs(theta_0189 - theta3);
                        best_phi = phi;
                    }
                    omp_sys = bck;
                }

                BOOST_TEST_MESSAGE("best_phi = " << best_phi << ", dtheta = " << dtheta);

                {
                    const matrix33_type rot(
                            1.0,                0.0,                 0.0,
                            0.0, std::cos(best_phi), -std::sin(best_phi),
                            0.0, std::sin(best_phi),  std::cos(best_phi));
                    omp_sys.position(8) = rot * omp_sys.position(8);
                    omp_sys.position(9) = rot * omp_sys.position(9);
                }

                mjolnir::math::X(omp_sys.position(4)) += rbp;
                mjolnir::math::X(omp_sys.position(8)) += rbp;
                mjolnir::math::X(omp_sys.position(9)) += rbp;
            }

            {
                const auto v01 = omp_sys.position(1) - omp_sys.position(0);
                const auto v89 = omp_sys.position(9) - omp_sys.position(8);
                const auto n4 = mjolnir::math::cross_product(coordinate_type(0.0, 0.0, 1.0), v89);
                const auto n6 = mjolnir::math::cross_product(coordinate_type(0.0, 0.0, 1.0), v01);

                const auto n4_reg = n4 / mjolnir::math::length(n4);
                const auto n6_reg = n6 / mjolnir::math::length(n6);

                coordinate_type v16 = v01 / mjolnir::math::length(v01);
                coordinate_type v94 = v89 / mjolnir::math::length(v89);
                {
                    const auto cos_theta = std::cos(pi - thetaCS_i);
                    const auto sin_theta = std::sin(pi - thetaCS_i);

                    BOOST_TEST_REQUIRE(mjolnir::math::length(n6_reg) == 1.0, boost::test_tools::tolerance(1e-8));
                    const auto nx = mjolnir::math::X(n6_reg);
                    const auto ny = mjolnir::math::Y(n6_reg);
                    const auto nz = mjolnir::math::Z(n6_reg);
                    const matrix33_type rot_16(
                        cos_theta + nx*nx*(1-cos_theta),    nx*ny*(1-cos_theta) - nz*sin_theta, nx*nz*(1-cos_theta) + ny*sin_theta,
                        ny*nx*(1-cos_theta) + nz*sin_theta, cos_theta + ny*ny*(1-cos_theta),    ny*nz*(1-cos_theta) - nx*sin_theta,
                        nz*nx*(1-cos_theta) - ny*sin_theta, nz*ny*(1-cos_theta) + nx*sin_theta, cos_theta + nz*nz*(1-cos_theta));
                    BOOST_TEST_REQUIRE(mjolnir::math::length(v16) == 1.0, boost::test_tools::tolerance(1e-8));
                    v16 = rot_16 * v16;
                    BOOST_TEST_REQUIRE(mjolnir::math::length(v16) == 1.0, boost::test_tools::tolerance(1e-8));
                }
                {
                    const auto cos_theta = std::cos(pi - thetaCS_j);
                    const auto sin_theta = std::sin(pi - thetaCS_j);

                    BOOST_TEST_REQUIRE(mjolnir::math::length(n4_reg) == 1.0, boost::test_tools::tolerance(1e-8));
                    const auto nx = mjolnir::math::X(n4_reg);
                    const auto ny = mjolnir::math::Y(n4_reg);
                    const auto nz = mjolnir::math::Z(n4_reg);

                    const matrix33_type rot_94(
                        cos_theta + nx*nx*(1-cos_theta),    nx*ny*(1-cos_theta) - nz*sin_theta, nx*nz*(1-cos_theta) + ny*sin_theta,
                        ny*nx*(1-cos_theta) + nz*sin_theta, cos_theta + ny*ny*(1-cos_theta),    ny*nz*(1-cos_theta) - nx*sin_theta,
                        nz*nx*(1-cos_theta) - ny*sin_theta, nz*ny*(1-cos_theta) + nx*sin_theta, cos_theta + nz*nz*(1-cos_theta));

                    BOOST_TEST_REQUIRE(mjolnir::math::length(v94) == 1.0, boost::test_tools::tolerance(1e-8));
                    v94 = rot_94 * v94;
                    BOOST_TEST_REQUIRE(mjolnir::math::length(v94) == 1.0, boost::test_tools::tolerance(1e-8));
                }
                omp_sys.position(4) = omp_sys.position(9) + v94 * rcsj;
                omp_sys.position(6) = omp_sys.position(1) + v16 * rcsi;
            }

            // check the configurations are okay
            {
                const auto v19 = omp_sys.position(9) - omp_sys.position(1);
                BOOST_TEST_REQUIRE(mjolnir::math::length(v19) == rbp,
                           boost::test_tools::tolerance(0.01));

                const auto v01 = omp_sys.position(1) - omp_sys.position(0);
                const auto v89 = omp_sys.position(9) - omp_sys.position(8);
                {
                    const auto dot = mjolnir::math::dot_product(v01, v89);
                    const auto cos_theta = dot /
                        (mjolnir::math::length(v01) * mjolnir::math::length(v89));
                    const auto theta = std::acos(cos_theta);
                    BOOST_TEST_REQUIRE(theta == theta3, boost::test_tools::tolerance(0.01));
                }

                {
                    const auto dot = mjolnir::math::dot_product(-v01, v19);
                    const auto cos_theta = dot /
                        (mjolnir::math::length(v01) * mjolnir::math::length(v19));
                    const auto theta = std::acos(cos_theta);
                    BOOST_TEST_REQUIRE(theta == theta1, boost::test_tools::tolerance(0.01));
                }
                {
                    const auto dot = mjolnir::math::dot_product(v89, v19);
                    const auto cos_theta = dot /
                        (mjolnir::math::length(v89) * mjolnir::math::length(v19));
                    const auto theta = std::acos(cos_theta);
                    BOOST_TEST_REQUIRE(theta == theta2, boost::test_tools::tolerance(0.01));
                }

                const auto v61 = omp_sys.position(1) - omp_sys.position(6);
                BOOST_TEST_REQUIRE(mjolnir::math::length(v61) == rcsi,
                                   boost::test_tools::tolerance(0.01));
                {
                    const auto dot = mjolnir::math::dot_product(
                            v01 / mjolnir::math::length(v01),
                            v61 / mjolnir::math::length(v61));
                    const auto theta = std::acos(dot);
                    BOOST_TEST_REQUIRE(theta == thetaCS_i,
                            boost::test_tools::tolerance(0.01));
                }
                const auto v49 = omp_sys.position(9) - omp_sys.position(4);
                BOOST_TEST_REQUIRE(mjolnir::math::length(v49) == rcsj,
                           boost::test_tools::tolerance(0.01));
                {
                    const auto dot = mjolnir::math::dot_product(
                            v89 / mjolnir::math::length(v89),
                            v49 / mjolnir::math::length(v49));
                    const auto theta = std::acos(dot);
                    BOOST_TEST_REQUIRE(theta == thetaCS_j, boost::test_tools::tolerance(0.01));
                }
            }

            // positions generated!
            // ... and then rotate random direction to remove special axis

            test::apply_random_rotation(omp_sys, rng);
            test::apply_random_perturbation(omp_sys, rng, 0.01);
            test::clear_force(omp_sys);
            test::clear_force(seq_sys);

            for(std::size_t idx=0; idx<10; ++idx)
            {
                seq_sys.position(idx) = omp_sys.position(idx);
            }

            test::check_force_consistency              (omp_sys, omp_interaction, seq_sys, seq_interaction, tol);
            test::check_force_and_energy_consistency   (omp_sys, omp_interaction, seq_sys, seq_interaction, tol);
            test::check_force_and_virial_consistency   (omp_sys, omp_interaction, seq_sys, seq_interaction, tol);
            test::check_force_energy_virial_consistency(omp_sys, omp_interaction, seq_sys, seq_interaction, tol);
            test::check_energy_consistency             (omp_sys, omp_interaction, seq_sys, seq_interaction, tol);

        } // thetaCS_j
        } // thetaCS_i
        } // theta3
        } // rcsj
        } // rcsi
    }
}


#ifndef STAN_MATH_TORSTEN_SOLVE_BDF_HPP
#define STAN_MATH_TORSTEN_SOLVE_BDF_HPP

#include <Eigen/Dense>
#include <stan/math/torsten/to_array_2d.hpp>
#include <stan/math/torsten/ev_manager.hpp>
#include <stan/math/torsten/pmx_population_check.hpp>
#include <stan/math/torsten/ev_solver.hpp>
#include <stan/math/torsten/pmx_ode_model.hpp>
#include <stan/math/torsten/pmx_check.hpp>
#include <stan/math/torsten/pmx_solve_ode.hpp>
#include <stan/math/torsten/pmx_solve_group_ode.hpp>
#include <stan/math/torsten/dsolve/pmx_ode_integrator.hpp>
#include <vector>

namespace torsten {

/**
 * Computes the predicted amounts in each compartment at each event
 * for a general compartment model, defined by a system of ordinary
 * differential equations. Uses the torsten::pmx_integrate_ode_bdf
 * function.
 *
 * @tparam Ts types of parameters, see pmx_solve_ode for
 *         details. The last type of Ts... is
 *         ostream* as required by stan
 * @tparam F type of ODE system function.
 * @param[in] f functor for base ordinary differential equation that defines
 *            compartment model.
 * @param[in] nCmt number of compartments in model
 * @return a matrix with predicted amount in each compartment
 *         at each event.
 *
 */
  template <typename F, typename... Ts>
  auto pmx_solve_bdf(const F& f, const int nCmt, Ts... args) {
    return PMXSolveODE<dsolve::PMXOdeIntegrator<dsolve::PMXVariadicOdeSystem, dsolve::PMXCvodesIntegrator<CV_BDF, CV_STAGGERED>>>::solve(f, nCmt, args...);
  }

  /*
   * For backward compatibility we keep old version of
   * return type using transpose. This is less efficient and
   * will be decomissioned in formal release.
   */
  template <typename T0, typename T1, typename T2, typename T3,
            typename T_par, typename T_biovar, typename T_tlag,
            typename F>
  stan::matrix_return_t<T0, T1, T2, T3, T_par, T_biovar, T_tlag>
  generalOdeModel_bdf(const F& f,
                       const int nCmt,
                      const std::vector<T0>& time,
                      const std::vector<T1>& amt,
                      const std::vector<T2>& rate,
                      const std::vector<T3>& ii,
                      const std::vector<int>& evid,
                      const std::vector<int>& cmt,
                      const std::vector<int>& addl,
                      const std::vector<int>& ss,
                       const std::vector<T_par>& pMatrix,
                       const std::vector<T_biovar>& biovar,
                       const std::vector<T_tlag>& tlag,
                       double rel_tol = 1e-6,
                       double abs_tol = 1e-6,
                       long int max_num_steps = 1e6,
                       std::ostream* msgs = 0) {
    auto x = pmx_solve_bdf(f, nCmt,
                            time, amt, rate, ii, evid, cmt, addl, ss,
                            pMatrix, biovar, tlag,
                            rel_tol, abs_tol, max_num_steps,
                            msgs);
    return x.transpose();
  }

  /*
   * For population models, more often we use ragged arrays
   * to describe the entire population, so in addition we need the arrays of
   * the length of each individual's data. The size of that
   * vector is the size of the population.
   */
  template <typename F, typename... Ts>
  auto pmx_solve_group_bdf(const F& f, const int nCmt,
                           const std::vector<int>& len, Ts... args) {
    return PMXSolveGroupODE<dsolve::PMXOdeIntegrator<dsolve::PMXVariadicOdeSystem, dsolve::PMXCvodesIntegrator<CV_BDF, CV_STAGGERED>>>::solve(f, nCmt, len, args...);
  }
}

#endif


#include "tsdfcuda.h"
#include <Eigen2/Dense>
#include <cmath>
#include <fstream>
#include <thread>
#include <chrono>

#include <GL/glew.h>
#include <GL/freeglut.h>
#include "StdAfx.h"
#include "Depth.h"
#include "Base.h"

#include <wrap/ply/plylib.h>
#include <wrap/ply/plystuff.h>
#include <wrap/io_trimesh/import.h>
#include <wrap/io_trimesh/export_ply.h>
#include <wrap/ply/plylib.cpp>
#include <vcg/complex/algorithms/clean.h>
#include "Timer.h"
#define DEBUG_SDF 0
#define RATIO 0.75
#define BILATERIAL 0
#define MEDIUM 1
typedef unsigned char BYTE;

#define DEPTH_RAW_WIDTH             512
#define DEPTH_RAW_HEIGHT            424
#define DEPTH_NORM_FOCAL_LENGTH_X (0.72113f)
#define DEPTH_NORM_FOCAL_LENGTH_Y (0.870799f)
#define DEPTH_NORM_PRINCIPAL_POINT_X (0.50602675f)
#define DEPTH_NORM_PRINCIPAL_POINT_Y (0.499133f)
int w = DEPTH_RAW_WIDTH;
int h = DEPTH_RAW_HEIGHT;
Timer ttime;
const int3 volres = make_int3(512,512,512);
const float trunc_dist_factor = 2.0;
loo::BoundingBox bbox(make_float3(-1000,-1000,-1000), make_float3(1000,1000 ,1000));
const loo::ImageIntrinsics K(DEPTH_NORM_FOCAL_LENGTH_X * w, DEPTH_NORM_FOCAL_LENGTH_Y * h, w /2, h /2);
loo::Image<float, loo::TargetHost, loo::Manage> h_rawdepth(w,h);
loo::Image<float, loo::TargetDevice, loo::Manage> d_tsdf_depth(w,h);
#if BILATERIAL
loo::Image<float, loo::TargetDevice, loo::Manage> d_tsdf_depth_bi(w,h);
#elif MEDIUM
loo::Image<float, loo::TargetDevice, loo::Manage> d_tsdf_depth_mi(w,h);
#endif
loo::Image<float4, loo::TargetDevice, loo::Manage> d_tsdf_normal(w,h);
loo::Image<float4, loo::TargetDevice, loo::Manage> d_tsdf_vertex(w,h);
loo::BoundedVolume<loo::SDF_t, loo::TargetDevice, loo::Manage> vol(volres.x, volres.y, volres.z, bbox);
const float trunc_dist = trunc_dist_factor*length(vol.VoxelSizeUnits());
loo::Image<float, loo::TargetDevice, loo::Manage> d_raycast_render(w, h);
loo::Image<float, loo::TargetDevice, loo::Manage> d_raycast_depth(w, h);
loo::Image<float4, loo::TargetDevice, loo::Manage> d_raycast_norm(w, h);
loo::Image<float,  loo::TargetHost, loo::Manage> h_raycast_depth(w, h);
loo::Image<float4, loo::TargetHost, loo::Manage> h_raycast_normal(w,h);
loo::Image<float,  loo::TargetHost, loo::Manage> h_raycast_render(w,h);
loo::Mat<float,3,4> T_tsdf_cw;
loo::Mat<float,3,4> T_raycast_cw;
Depth * getDepth;
GLdisplay * displayGL;
bool if_resetVolume = false;
int displayidx = 0;

typedef Eigen::Matrix<float, 4,4 ,Eigen::RowMajor> Mat4;
void setVMatrix(float3 eyepose, float3 eyecenter, float3 upvector, loo::Mat<float,3,4>& matout)
{
	float3 Pc = eyepose;
	float3 Zc = normalize(Pc - eyecenter);
	float3 Yc = normalize(upvector);
	float3 Xc = normalize(cross(Yc, Zc));
	Yc = cross(Zc, Xc);
	Mat4 Vmatrix;
	Vmatrix.setIdentity();
	Vmatrix(0,0) = Xc.x;
	Vmatrix(0,1) = Xc.y;
	Vmatrix(0,2) = Xc.z;
	Vmatrix(1,0) = Yc.x;
	Vmatrix(1,1) = Yc.y;
	Vmatrix(1,2) = Yc.z;
	Vmatrix(2,0) = Zc.x;
	Vmatrix(2,1) = Zc.y;
	Vmatrix(2,2) = Zc.z;
	Vmatrix(0,3) = -dot(Xc, Pc);
	Vmatrix(1,3) = -dot(Yc, Pc);
	Vmatrix(2,3) = -dot(Zc, Pc);
	std::cout<<Vmatrix<<std::endl<<std::endl;
	Eigen::Matrix<float,3,4, Eigen::RowMajor> Vmat = Vmatrix.inverse().block<3,4>(0,0);
	for(int r = 0 ; r < 3 ; r ++)
	{
		for(int c = 0 ; c < 4 ; c++)
		{
			matout(r,c) = Vmat(r,c);
		}
	}
	std::cout<<Vmat<<std::endl;
}

void timer( int value )
{
	glutPostRedisplay();
	glutTimerFunc( 33, timer, 0 );
}
void MeshOutput();
void keyboard( unsigned char key, int x, int y )
{

	switch (key)
	{
	case 27:
		exit(0);
		break;
	case 9:
		if_resetVolume = true;
		glutPostRedisplay();
		break;
	case 13:
		MeshOutput();
		break;
	}
}

void tsdfprocessing()
{
	float raycast_near = 1000;
	float raycast_far = 3000;
	float depth_clip_near = 1000;
	float depth_clip_far = 3000;
	float bigs = 3, bigr = 10;
	int biwin = 2;
	float max_w = 1000;
	float mincostheta = 0.1;
	bool subpix = true;

	//TSDF FUSION
	d_tsdf_depth.CopyFrom(h_rawdepth);
#if BILATERIAL
	loo::BilateralFilter<float, float>(d_tsdf_depth_bi, d_tsdf_depth, bigs, bigr, biwin);
	loo::DepthToVbo<float>(d_tsdf_vertex, d_tsdf_depth_bi, K);
#elif MEDIUM
	loo::MediumFilter<float, float>(d_tsdf_depth_mi, d_tsdf_depth, biwin, depth_clip_near, depth_clip_far);
	loo::DepthToVbo<float>(d_tsdf_vertex, d_tsdf_depth_mi, K);
#else
	loo::DepthToVbo<float>(d_tsdf_vertex, d_tsdf_depth, K);
#endif
	loo::NormalsFromVbo(d_tsdf_normal, d_tsdf_vertex);
#if MEDIUM
	loo::SdfFuse(vol, d_tsdf_depth_mi, d_tsdf_normal, T_tsdf_cw, K, trunc_dist, max_w, mincostheta);
#else
	loo::SdfFuse(vol, d_tsdf_depth, d_tsdf_normal, T_tsdf_cw, K, trunc_dist, max_w, mincostheta);
#endif

	//RAYCASTING
	loo::RaycastSdf(d_raycast_depth, d_raycast_norm, d_raycast_render, vol, T_raycast_cw, K, raycast_near, raycast_far, 0, subpix );
	h_raycast_depth.CopyFrom(d_raycast_depth);
	h_raycast_normal.CopyFrom(d_raycast_norm);
	h_raycast_render.CopyFrom(d_raycast_render);
}

void render()
{
	if(if_resetVolume)
	{
		loo::SdfReset(vol, trunc_dist);
		if_resetVolume = false;
	}
	getDepth->Update(h_rawdepth.ptr, 1000, 3000);
	tsdfprocessing();
	displayGL->display(h_rawdepth.ptr, h_raycast_normal.ptr, h_raycast_render.ptr);
}

void MeshOutput()
{
	std::vector<float3> verts;
	std::vector<int3> faces;
	ttime.Start();
	loo::SaveMesh("volume.ply", vol, verts, faces, trunc_dist);
	ttime.Print("MC");
	vcg::CMeshO cm;
	cm.vert.resize(verts.size());
	cm.vn = cm.vert.size();
	cm.face.resize(faces.size());
	cm.fn = cm.face.size();
	int a = cm.vert.size();
	int b = 0;

	ttime.Start();
	std::cout<<cm.vert.size()<<std::endl;
	Concurrency::parallel_for(0, (int) cm.vert.size(), [&](int k)
	{
		cm.vert.at(k).P() = vcg::Point3f(verts[k].x, verts[k].y, verts[k].z);
	});

	CVertexO * V_start = &cm.vert[0];

	for(int k = 0 ; k < cm.face.size(); k++)
	{
		cm.face[k].setvptr(0, V_start + faces[k].x);
		cm.face[k].setvptr(2, V_start + faces[k].y);
		cm.face[k].setvptr(1, V_start + faces[k].z);
	}
	//Mesh Clean by VCG
	tri::UpdateNormal<CMeshO>::PerVertex(cm);
	tri::UpdateNormal<CMeshO>::NormalizePerVertex(cm);
	int dup = tri::Clean<CMeshO>::RemoveDuplicateVertex(cm);
	int cls = tri::Clean<CMeshO>::MergeCloseVertex(cm, 2);
	int unref = vcg::tri::Clean<CMeshO>::RemoveUnreferencedVertex(cm);
	printf("1. Remove %d duplicate Vertex\n2. Merge %d Vertex\n3. Remove %d unreference vertex\n", dup, cls, unref);

	CMeshO cm2;
	tri::Append<CMeshO, CMeshO>::MeshCopy(cm2, cm);

	const int minCC= (int)(cm2.vert.size() * 0.01f);
	tri::UpdateTopology<CMeshO>::FaceFace(cm2);
	std::pair<int,int> delInfo=tri::Clean<CMeshO>::RemoveSmallConnectedComponentsSize(cm2,minCC);
	printf("4. remove %d component out of %d component\n", delInfo.first, delInfo.second);
	tri::io::ExporterPLY<CMeshO>::Save(cm2, "tsdfply.ply", vcg::tri::io::Mask::IOM_VERTNORMAL, true);
	ttime.Print("vcg");
}

int main(int argc, char** argv )
{
	setVMatrix(make_float3(0,0,-2000), make_float3(0,0,0), make_float3(0,1,0), T_tsdf_cw);
	T_raycast_cw = T_tsdf_cw;
	T_raycast_cw (2,2) *= -1;
	loo::SdfReset(vol, trunc_dist);
	getDepth = new Depth();
	getDepth->InitializeDefaultSensor();

	std::cout<<"\n----------thread sleep----------\n"<<std::endl;
	std::this_thread::sleep_for(std::chrono::seconds(1));
	std::cout<<"\n----------thread weakup----------\n"<<std::endl;

	displayGL = new GLdisplay(DEPTH_RAW_WIDTH, DEPTH_RAW_HEIGHT);
	glutInit( &argc, argv );
	glutInitDisplayMode( GLUT_DOUBLE | GLUT_RGB |GLUT_DEPTH );
	glutInitWindowSize( displayGL ->gl_Windows_Width * RATIO, displayGL -> gl_Windows_Height * RATIO);
	glutInitWindowPosition( 100, 100 );
	glutCreateWindow( argv[0] );

	glewInit();
	displayGL->init();
	glutDisplayFunc( render );
	glutKeyboardFunc( keyboard );
	glutTimerFunc(16, timer, 0);
	glutMainLoop();

	delete getDepth;
	getDepth = NULL;
	return 0;
}


//==============================================================================
//         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_OPERATOR_FUNCTIONS_SIMD_VMX_ALTIVEC_MULTIPLIES_HPP_INCLUDED
#define BOOST_SIMD_OPERATOR_FUNCTIONS_SIMD_VMX_ALTIVEC_MULTIPLIES_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_VMX_SUPPORT

#include <boost/simd/operator/functions/multiplies.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/constants/mzero.hpp>
#include <boost/dispatch/meta/as_unsigned.hpp>
#include <boost/dispatch/meta/downgrade.hpp>
#include <boost/dispatch/meta/upgrade.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( multiplies_
                                    , boost::simd::tag::vmx_
                                    , (A0)
                                    , ((simd_<single_<A0>,boost::simd::tag::vmx_>))
                                      ((simd_<single_<A0>,boost::simd::tag::vmx_>))
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return vec_madd(a0(),a1(), boost::simd::Mzero<A0>()());
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( multiplies_
                                    , boost::simd::tag::vmx_, (A0)
                                    , ((simd_<type16_<A0>,boost::simd::tag::vmx_>))
                                      ((simd_<type16_<A0>,boost::simd::tag::vmx_>))
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return vec_mladd(a0(),a1(),Zero<A0>()());
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( multiplies_
                                    , boost::simd::tag::vmx_, (A0)
                                    , ((simd_<type8_<A0>,boost::simd::tag::vmx_>))
                                      ((simd_<type8_<A0>,boost::simd::tag::vmx_>))
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename dispatch::meta::upgrade<A0>::type::native_type uptype;
      uptype l = vec_mule(a0(),a1());
      uptype r = vec_mulo(a0(),a1());
      return vec_mergel(vec_pack(l,l),vec_pack(r,r));
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( multiplies_
                                    , boost::simd::tag::vmx_, (A0)
                                    , ((simd_<uint32_<A0>,boost::simd::tag::vmx_>))
                                      ((simd_<uint32_<A0>,boost::simd::tag::vmx_>))
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename dispatch::meta::downgrade<A0,unsigned>::type s_t;

      // reinterpret as u16
      s_t short0 = bitwise_cast<s_t>(a0);
      s_t short1 = bitwise_cast<s_t>(a1);

      // shifting constant
      typename A0::native_type shift_ = vec_splat_u32(-16);

      A0 sf = vec_rl(a1(), shift_);

      // Compute high part of the product
      A0 high    = vec_msum ( short0()
                            , bitwise_cast<s_t>(sf)()
                            , vec_splat_u32(0)
                            );

      // Complete by adding low part of the 16 bits product
      return vec_add( vec_sl(high(), shift_)
                    , vec_mulo(short0(), short1())
                    );
    }
  };
} } }

#endif
#endif