/* Copyright (c) 2016, Julian Straub <jstraub@csail.mit.edu> Licensed
 * under the MIT license. See the license file LICENSE.
 */
#include <thread>
#include <pangolin/pangolin.h>
#include <pangolin/video/video_record_repeat.h>
#include <pangolin/gl/gltexturecache.h>
#include <pangolin/gl/glpixformat.h>
#include <pangolin/handler/handler_image.h>
#include <pangolin/utils/file_utils.h>
#include <pangolin/utils/timer.h>
#include <pangolin/gl/gl.h>
#include <pangolin/gl/glcuda.h>
#include <pangolin/gl/gldraw.h>

#include <Eigen/Dense>
#include <tdp/camera/rig.h>
#include <tdp/data/image.h>
#include <tdp/data/managed_image.h>
#include <tdp/data/managed_pyramid.h>
#include <tdp/data/managed_volume.h>
#include <tdp/data/pyramid.h>
#include <tdp/data/volume.h>
#include <tdp/data/circular_buffer.h>
#include <tdp/gl/gl_draw.h>
#include <tdp/gui/gui_base.hpp>
#include <tdp/gui/quickView.h>
#include <tdp/icp/icp.h>
#include <tdp/icp/icpRot.h>
#include <tdp/icp/icpGrad3d.h>
#include <tdp/icp/icpTexture.h>
#include <tdp/manifold/SE3.h>
#include <tdp/nvidia/helper_cuda.h>
#include <tdp/preproc/depth.h>
#include <tdp/preproc/normals.h>
#include <tdp/preproc/pc.h>
#include <tdp/utils/Stopwatch.h>
#include <tdp/inertial/imu_factory.h>
#include <tdp/inertial/imu_interpolator.h>
#include <tdp/manifold/SO3.h>
#include <tdp/preproc/grad.h>
#include <tdp/preproc/grey.h>
#include <tdp/preproc/mask.h>
#include <tdp/preproc/curvature.h>
#include <tdp/geometry/cosy.h>
#include <tdp/gl/shaders.h>
#include <tdp/utils/colorMap.h>
#include <tdp/camera/photometric.h>

typedef tdp::CameraPoly3f CameraT;
//typedef tdp::Cameraf CameraT;

namespace tdp {

void ExtractNormals(const Image<Vector3fda>& pc,
    const Image<uint8_t>& mask, uint32_t W,
    ManagedHostImage<Vector3fda>& pc_c,
    ManagedHostImage<Vector3fda>& n_c) {
  size_t numObs = 0;
  for (size_t i=0; i<mask.Area(); ++i) {
    if (mask[i] && tdp::IsValidData(pc[i])) numObs++;
  }
  pc_c.Reinitialise(numObs);
  n_c.Reinitialise(numObs);
  size_t j=0;
  for (size_t i=0; i<mask.Area(); ++i) {
    if (mask[i] && tdp::IsValidData(pc[i])) {
      uint32_t u0 = i%mask.w_;
      uint32_t v0 = i/mask.w_;
      pc_c[j] = pc(u0,v0);
      uint32_t Wscaled = floor(W*pc_c[j](2));
      if (!tdp::NormalViaScatter(pc, u0, v0, Wscaled, n_c[j++])) {
        std::cout << "problem with normal computation" << std::endl;
        std::cout << u0 << " " << v0 << std::endl;
        std::cout << pc_c[j].transpose() << std::endl;
      }
    }
  }
}

void IcpPlanes(
    const Image<Vector3fda>& pc_c,
    const Image<Vector3fda>& n_c,
    const Image<Vector3fda>& pc_m,
    const Image<Vector3fda>& n_m,
    uint32_t maxIt,
    float angleThr,
    float distThr,
    float p2plThr,
    bool verbose,
    SE3f& T_mc,
    Eigen::Matrix<float,6,6>& Sigma_mc,
    std::vector<std::pair<size_t, size_t>>& assoc
    ) {
  assoc.clear();
  Eigen::Matrix<float,6,6> A;
  Eigen::Matrix<float,6,1> b;
  Eigen::Matrix<float,6,1> Ai;
  float dotThr = cos(angleThr*M_PI/180.);
  for (size_t it = 0; it < maxIt; ++it) {
    A = Eigen::Matrix<float,6,6>::Zero();
    b = Eigen::Matrix<float,6,1>::Zero();
    Ai = Eigen::Matrix<float,6,1>::Zero();
    float bi = 0.;
    float err = 0.;
    uint32_t numInl = 0;
    for (size_t i=0; i<pc_m.Area(); ++i) {
      for (size_t j=0; j<pc_c.Area(); ++j) {
        Eigen::Vector3f pc_c_in_m = T_mc*pc_c[j];
        float dist = (pc_m[i] - pc_c_in_m).norm();
        if (dist < distThr*pc_m[i](2)) {
          Eigen::Vector3f n_m_in_c = T_mc.rotation().Inverse()*n_m[i];
          if (fabs(n_m_in_c.dot(n_c[j])) > dotThr) {
            float p2pl = n_m[i].dot(pc_m[i] - pc_c_in_m);
            if (fabs(p2pl) < p2plThr) {
              // match
              Ai.topRows<3>() = pc_c[j].cross(n_m_in_c);
              Ai.bottomRows<3>() = n_m_in_c;
              bi = p2pl;
              A += Ai * Ai.transpose();
              b += Ai * bi;
              numInl ++;
              err += p2pl;
              assoc.emplace_back(i,j);
            }
          }
        }
      }
    }
    Eigen::Matrix<float,6,1> x = Eigen::Matrix<float,6,1>::Zero();
    if (numInl > 10) {
      // solve for x using ldlt
      x = (A.cast<double>().ldlt().solve(b.cast<double>())).cast<float>();
      T_mc = T_mc * tdp::SE3f::Exp_(x);
    }
    if (verbose) {
      std::cout << " it " << it
        << ": err=" << err
        << "\t# inliers: " << numInl
        << "\t|x|: " << x.topRows(3).norm()*180./M_PI
        << " " <<  x.bottomRows(3).norm()
        << std::endl;
    }
    if (x.norm() < 1e-4) break;
  }
  Sigma_mc = A.inverse();
}

}

int main( int argc, char* argv[] )
{
  std::string input_uri = "openni2://";
  std::string output_uri = "pango://video.pango";
  std::string calibPath = "";
  std::string imu_input_uri = "";
  std::string tsdfOutputPath = "tsdf.raw";

  if( argc > 1 ) {
    input_uri = std::string(argv[1]);
    calibPath = (argc > 2) ? std::string(argv[2]) : "";
//    imu_input_uri =  (argc > 3)? std::string(argv[3]) : "";
  }

  pangolin::Uri uri = pangolin::ParseUri(input_uri);
  if (!uri.scheme.compare("file")) {
    std::cout << uri.scheme << std::endl;
    if (pangolin::FileExists(uri.url+std::string("imu.pango"))
     && pangolin::FileExists(uri.url+std::string("video.pango"))) {
//      imu_input_uri = input_uri + std::string("imu.pango");
      tsdfOutputPath = uri.url + tsdfOutputPath;
      input_uri = input_uri + std::string("video.pango");
    } else if (pangolin::FileExists(uri.url+std::string("video.pango"))) {
      input_uri = input_uri + std::string("video.pango");
    }
  }

  std::cout << input_uri << std::endl;
  std::cout << imu_input_uri << std::endl;

  Stopwatch::getInstance().setCustomSignature(82043984912);

  // Open Video by URI
  pangolin::VideoRecordRepeat video(input_uri, output_uri);
  const size_t num_streams = video.Streams().size();

  if(num_streams == 0) {
    pango_print_error("No video streams from device.\n");
    return 1;
  }

  // optionally connect to IMU if it is found.
//  tdp::ImuInterface* imu = nullptr;
//  if (imu_input_uri.size() > 0)
//    imu = tdp::OpenImu(imu_input_uri);
//  if (imu) imu->Start();
//  tdp::ImuInterpolator imuInterp(imu,nullptr);
//  imuInterp.Start();

  tdp::GuiBase gui(1200,800,video);
  gui.container().SetLayout(pangolin::LayoutEqual);

  tdp::Rig<CameraT> rig;
  if (calibPath.size() > 0) {
    rig.FromFile(calibPath,false);
    std::vector<pangolin::VideoInterface*>& streams = video.InputStreams();
    rig.CorrespondOpenniStreams2Cams(streams);
  } else {
    return 2;
  }

  size_t w = video.Streams()[gui.iRGB[0]].Width();
  size_t h = video.Streams()[gui.iRGB[0]].Height();
  size_t wc = (w+w%64); // for convolution
  size_t hc = rig.NumCams()*(h+h%64);
  wc += wc%64;
  hc += hc%64;

  tdp::Camera<float> camView(Eigen::Vector4f(220,220,319.5,239.5));
  // Define Camera Render Object (for view / scene browsing)
  pangolin::OpenGlRenderState s_cam(
      pangolin::ProjectionMatrix(640,480,420,420,319.5,239.5,0.1,1000),
      pangolin::ModelViewLookAt(0,0.5,-3, 0,0,0, pangolin::AxisNegY)
      );
  // Add named OpenGL viewport to window and provide 3D Handler
  pangolin::View& viewPc3D = pangolin::CreateDisplay()
    .SetHandler(new pangolin::Handler3D(s_cam));
  gui.container().AddDisplay(viewPc3D);

  pangolin::View& viewAssoc = pangolin::CreateDisplay()
    .SetHandler(new pangolin::Handler3D(s_cam));
  gui.container().AddDisplay(viewAssoc);

  pangolin::View& containerTracking = pangolin::Display("tracking");
  containerTracking.SetLayout(pangolin::LayoutEqual);
  tdp::QuickView viewModel(wc, hc);
  containerTracking.AddDisplay(viewModel);
  tdp::QuickView viewCurrent(wc, hc);
  containerTracking.AddDisplay(viewCurrent);
  tdp::QuickView viewMask(wc, hc);
  containerTracking.AddDisplay(viewMask);
  gui.container().AddDisplay(containerTracking);

  containerTracking.Show(false);

  pangolin::View& plotters = pangolin::Display("plotters");
  plotters.SetLayout(pangolin::LayoutEqualVertical);
//  pangolin::DataLog logInliers;
//  pangolin::Plotter plotInliers(&logInliers, -100.f,1.f, 0, 130000.f,
//      10.f, 0.1f);
//  plotters.AddDisplay(plotInliers);
//  pangolin::DataLog logRmse;
//  pangolin::Plotter plotRmse(&logRmse, -100.f,1.f, 0.f,0.2f, 0.1f, 0.1f);
//  plotters.AddDisplay(plotRmse);
  pangolin::DataLog logdH;
  pangolin::Plotter plotdH(&logdH, -100.f,1.f, .5f,1.5f, .1f, 0.1f);
  plotters.AddDisplay(plotdH);
  pangolin::DataLog logObs;
  pangolin::Plotter plotObs(&logObs, -100.f,1.f, 0.0f,1000.f, .1f, 0.1f);
  plotters.AddDisplay(plotObs);
  pangolin::DataLog logEntropy;
  pangolin::Plotter plotH(&logEntropy, -100.f,1.f, -80.f,-40.f, .1f, 0.1f);
  plotters.AddDisplay(plotH);

  pangolin::DataLog logAdaptiveEntropy;
  pangolin::Plotter plotAdaptiveH(&logAdaptiveEntropy, -100.f,1.f, -40.f,40.f, .1f, 0.1f);
  plotters.AddDisplay(plotAdaptiveH);
  gui.container().AddDisplay(plotters);

  tdp::ManagedHostImage<float> d(wc, hc);
  tdp::ManagedHostImage<tdp::Vector3bda> n2D(wc,hc);
  memset(n2D.ptr_,0,n2D.SizeBytes());
  tdp::ManagedHostImage<tdp::Vector3fda> n2Df(wc,hc);
  tdp::ManagedHostImage<tdp::Vector3fda> n(wc,hc);
  tdp::ManagedHostImage<tdp::Vector3bda> rgb(wc,hc);
  tdp::ManagedHostImage<tdp::Vector3bda> rgb_m(wc,hc);
  tdp::ManagedHostImage<tdp::Vector3fda> pc(wc, hc);

  tdp::ManagedHostImage<tdp::Vector3fda> pcFull_m(wc, hc);

  tdp::ManagedDeviceImage<tdp::Vector3bda> cuRgb(wc,hc);
  tdp::ManagedDeviceImage<float> cuGrey(wc,hc);

  tdp::ManagedDeviceImage<uint16_t> cuDraw(wc, hc);
  tdp::ManagedDeviceImage<float> cuD(wc, hc);

  tdp::ManagedDevicePyramid<uint8_t,3> cuPyrMask(wc, hc);
  tdp::ManagedDeviceImage<uint8_t> cuMask(wc, hc);
  tdp::ManagedHostImage<uint8_t> mask(wc, hc);

  // ICP stuff
  tdp::ManagedDevicePyramid<tdp::Vector3fda,3> pcs_m(wc,hc);
  tdp::ManagedDevicePyramid<tdp::Vector3fda,3> pcs_c(wc,hc);

  pangolin::GlBuffer vbo(pangolin::GlArrayBuffer,wc*hc,GL_FLOAT,3);
  pangolin::GlBuffer cbo(pangolin::GlArrayBuffer,wc*hc,GL_UNSIGNED_BYTE,3);

  tdp::ManagedHostImage<tdp::Vector3fda> pc_c;
  tdp::ManagedHostImage<tdp::Vector3fda> n_c;

  tdp::ManagedHostImage<tdp::Vector3fda> pc_m;
  tdp::ManagedHostImage<tdp::Vector3fda> n_m;

  pangolin::Var<float> depthSensorScale("ui.depth sensor scale",1e-3,1e-4,1e-3);
  pangolin::Var<float> dMin("ui.d min",0.10,0.0,0.1);
  pangolin::Var<float> dMax("ui.d max",6.,0.1,10.);

  pangolin::Var<float> subsample("ui.subsample %",0.001,0.0001,.01);

  pangolin::Var<float> scale("ui.scale %",0.1,0.1,1);

  pangolin::Var<bool> automaticKFs("ui.automatic Ks",true,true);
  pangolin::Var<bool> addKf("ui.add Kf",false,true);

  pangolin::Var<bool> icpReset("ui.reset icp",true,false);
  pangolin::Var<bool> icpFixed("ui.icp Fixed",true,true);
  pangolin::Var<float> angleThr("ui.angle Thr",15, 0, 90);
  pangolin::Var<float> p2plThr("ui.p2pl Thr",0.01,0,0.3);
  pangolin::Var<float> distThr("ui.dist Thr",0.1,0,0.3);
  pangolin::Var<float> logdHThr("ui.log dH Thr",0.90,0.8,1.1);
  pangolin::Var<float> relLogHChange("ui.rel log dH ", 1.e-2,1.e-3,1e-2);
  pangolin::Var<int> maxIt("ui.max iter",20, 1, 20);

  pangolin::Var<int>   W("ui.W ",8,1,15);
  pangolin::Var<int>   dispLvl("ui.disp lvl",0,0,2);

  pangolin::Var<bool> showPlanes("ui.show planes",false,true);
  pangolin::Var<bool> showPcModel("ui.show model",false,true);
  pangolin::Var<bool> showPcCurrent("ui.show current",false,true);
  pangolin::Var<bool> showFullPc("ui.show full",false,true);

  tdp::SE3f T_wc_0;
  tdp::SE3f T_wc = T_wc_0;
  tdp::SE3f T_wKf = T_wc;
  std::vector<tdp::SE3f> T_wcs;
  tdp::SE3f T_mc; // current to model
  Eigen::Matrix<float,6,6> Sigma_mc;
  std::vector<float> logHs;

  gui.verbose = true;
  if (gui.verbose) std::cout << "starting main loop" << std::endl;

  pangolin::GlBuffer vbo_w(pangolin::GlArrayBuffer,1000000,GL_FLOAT,3);
  tdp::ManagedHostCircularBuffer<tdp::Vector3fda> pc_w(1000000);
  pc_w.Fill(tdp::Vector3fda(NAN,NAN,NAN));

  std::vector<std::pair<size_t, size_t>> assoc;
  assoc.reserve(10000);

  size_t numKfs = 0;
  size_t numKfsPrev = 0;

  size_t frame = 0;
  // Stream and display video
  while(!pangolin::ShouldQuit())
  {
    if (pangolin::Pushed(icpReset)) {
      pcs_m.CopyFrom(pcs_c);
      pcFull_m.CopyFrom(pcs_m.GetImage(0));
      pc_m.ResizeCopyFrom(pc_c);
      n_m.ResizeCopyFrom(n_c);
      rgb_m.CopyFrom(rgb);
      T_mc = tdp::SE3f();
    }

    if (!gui.paused() && frame > 0 && (addKf || frame==1)) {
      pcs_m.CopyFrom(pcs_c);
      pcFull_m.CopyFrom(pcs_m.GetImage(0));
      pc_m.ResizeCopyFrom(pc_c);
      n_m.ResizeCopyFrom(n_c);
      rgb_m.CopyFrom(rgb);
      T_wKf = T_wc;
      T_mc = tdp::SE3f();
      numKfs ++;
      if (automaticKFs)
        addKf = false;
    }

    if (!gui.paused() && frame > 0) {
      for (size_t i=0; i<pc_c.Area(); ++i) pc_c[i] = T_wc * pc_c[i];
      pc_w.Insert(pc_c);
      vbo_w.Upload(pc_w.ptr_, pc_w.SizeBytes(), 0);
    }

    glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
    glColor3f(1.0f, 1.0f, 1.0f);

    gui.NextFrames();

    int64_t t_host_us_d = 0;
    if (gui.verbose) std::cout << "setup pyramids" << std::endl;
    TICK("Setup Pyramids");
    if (gui.verbose) std::cout << "collect d" << std::endl;
    rig.CollectD(gui, dMin, dMax, cuDraw, cuD, t_host_us_d);
    if (gui.verbose) std::cout << "compute pc" << std::endl;
    rig.ComputePc(cuD, true, pcs_c);
    if (gui.verbose) std::cout << "collect rgb" << std::endl;
    rig.CollectRGB(gui, rgb) ;

    if (!gui.paused() || subsample.GuiChanged()) {
      TICK("mask");
      tdp::RandomMaskCpu(mask, subsample, W*dMax);
      cuMask.CopyFrom(mask);
      tdp::ConstructPyramidFromImage(cuMask, cuPyrMask);
      TOCK("mask");
    }
    TOCK("Setup Pyramids");

    pc.CopyFrom(pcs_c.GetImage(0));

    TICK("normals");
    ExtractNormals(pc, mask, W, pc_c, n_c);
    TOCK("normals");
    if (icpFixed) {
      TICK("icp");
      IcpPlanes( pc_c, n_c, pc_m, n_m,
          maxIt, angleThr, distThr, p2plThr, gui.verbose,
          T_mc, Sigma_mc, assoc);
      TOCK("icp");
      logObs.Log(pc_c.Area(), assoc.size());
    } else {

      assoc.clear();
      Eigen::Matrix<float,6,6> A;
      Eigen::Matrix<float,6,1> b;
      Eigen::Matrix<float,6,1> Ai;
      float dotThr = cos(angleThr*M_PI/180.);

      std::vector<size_t> id_c(pc_c.Area());
      std::iota(id_c.begin(), id_c.end(), 0);
      std::random_shuffle(id_c.begin(), id_c.end());

      std::vector<size_t> id_m(pc_m.Area());
      std::iota(id_m.begin(), id_m.end(), 0);
      std::random_shuffle(id_m.begin(), id_m.end());
      uint32_t numObs = 0;
      uint32_t numInlPrev = 0;
      for (size_t it = 0; it < maxIt; ++it) {
        A = Eigen::Matrix<float,6,6>::Zero();
        b = Eigen::Matrix<float,6,1>::Zero();
        Ai = Eigen::Matrix<float,6,1>::Zero();
        float bi = 0.;
        float err = 0.;
        float logHprev = 1e10;
        uint32_t numInl = 0;
        numObs = 0;

        for (size_t j : id_c) {
          for (size_t i : id_m) {
//        for (size_t i=0; i<pc_m.Area(); ++i) {
//          for (size_t j=0; j<pc_c.Area(); ++j) {
            Eigen::Vector3f pc_c_in_m = T_mc*pc_c[j];
            float dist = (pc_m[i] - pc_c_in_m).norm();
            if (dist < distThr*pc_m[i](2)) {
              Eigen::Vector3f n_m_in_c = T_mc.rotation().Inverse()*n_m[i];
              if (fabs(n_m_in_c.dot(n_c[j])) > dotThr) {
                float p2pl = n_m[i].dot(pc_m[i] - pc_c_in_m);
                if (fabs(p2pl) < p2plThr) {
                  Ai.topRows<3>() = pc_c[j].cross(n_m_in_c);
                  Ai.bottomRows<3>() = n_m_in_c;
                  bi = p2pl;
                  A += Ai * Ai.transpose();
                  b += Ai * bi;
                  err += p2pl;
                  assoc.emplace_back(i,j);
                  // if this gets expenseive I could use
                  // https://en.wikipedia.org/wiki/Matrix_determinant_lemma
                  numInl ++;
                }
              }
            }
          }
          if (numInl > numInlPrev) {
            float logH = -((A.eigenvalues()).array().log().sum()).real();
//            std::cout << numObs << " " << numInl
//              << " logH " << logH << " delta "
//              << (logHprev-logH) << std::endl;
            if ((logHprev - logH) < relLogHChange && numObs > 6) {
              std::cout << numInl << " logH " << logH << " delta "
                << (logHprev-logH) << std::endl;
              break;
            }
            logAdaptiveEntropy.Log(logH);
            logHprev = logH;
            numObs ++;
          }
          numInlPrev = numInl;
        }
        Eigen::Matrix<float,6,1> x = Eigen::Matrix<float,6,1>::Zero();
        if (numInl > 10) {
          // solve for x using ldlt
          x = (A.cast<double>().ldlt().solve(b.cast<double>())).cast<float>();
          T_mc = T_mc * tdp::SE3f::Exp_(x);
        }
        if (gui.verbose) {
          std::cout << " it " << it
            << ": err=" << err
            << "\t# inliers: " << numInl
            << "\t|x|: " << x.topRows(3).norm()*180./M_PI
            << " " <<  x.bottomRows(3).norm()
            << std::endl;
        }
        if (x.norm() < 1e-4) break;
      }
      Sigma_mc = A.inverse();
      logObs.Log(numObs, numInlPrev);
      plotAdaptiveH.ScrollView(numObs,0);
    }

    float logH = ((Sigma_mc.eigenvalues()).array().log().sum()).real();
    std::cout << " H " << logH  << std::endl;
    if (numKfs > numKfsPrev) {
      logHs.push_back(logH);
    }
    if (numKfsPrev > 0) {
      float dH = logH / logHs.back();
      if (dH < logdHThr && automaticKFs) {
        addKf = true;
      }
      std::cout << " dH " << dH << " logHs[-1] " << logHs.back() << std::endl;
      logdH.Log(dH, logdHThr);
      logEntropy.Log(logH, logHs.back());
    }

    if (!gui.paused()) {
      T_wc = T_wKf*T_mc;
      T_wcs.push_back(T_wc);
    }

    frame ++;
    numKfsPrev = numKfs;

    if (gui.verbose) std::cout << "draw 3D" << std::endl;
    TICK("Draw 3D");
    glEnable(GL_DEPTH_TEST);
    if (viewPc3D.IsShown()) {
      viewPc3D.Activate(s_cam);

      pangolin::glDrawAxis(T_wc.matrix(), 0.05f);
      glColor4f(1.,1.,0.,0.6);
      glDrawPoses(T_wcs,20);

      if (showFullPc) {
        glColor4f(0.,1.,1.,0.6);
        pangolin::RenderVbo(vbo_w);
      }

      pangolin::glSetFrameOfReference(T_wKf.matrix());
      vbo.Reinitialise(pangolin::GlArrayBuffer, pc_m.Area(), GL_FLOAT,
          3, GL_DYNAMIC_DRAW);
      vbo.Upload(pc_m.ptr_, pc_m.SizeBytes(), 0);
      glColor3f(1,0,0);
      pangolin::RenderVbo(vbo);
      pangolin::glUnsetFrameOfReference();

      // draw sampled pc points and mean curvature
//      vbo.Reinitialise(pangolin::GlArrayBuffer, pc_c.Area(), GL_FLOAT, 3,
//          GL_DYNAMIC_DRAW);
//      vbo.Upload(pc_c.ptr_, pc_c.SizeBytes(), 0);
//      glColor3f(1,0,0);
      pangolin::glSetFrameOfReference(T_wc.matrix());
//      pangolin::RenderVbo(vbo);
      glColor3f(1,1,0);
      for (size_t i=0; i<n_c.Area(); ++i) {
        tdp::glDrawLine(pc_c[i], pc_c[i] + scale*n_c[i]);
      }
      pangolin::glUnsetFrameOfReference();
      if (showPlanes) {
        for (size_t i=0; i<n_c.Area(); ++i) {
          Eigen::Matrix3f R = tdp::OrthonormalizeFromYZ(
              Eigen::Vector3f(0,1,0), n_c[i].normalized());
          tdp::SE3f T(R, pc_c[i]);
          pangolin::glSetFrameOfReference((T_wc*T).matrix());
          pangolin::glDrawAxis(0.05f);
          pangolin::glDraw_z0(0.01,10);
          pangolin::glUnsetFrameOfReference();
        }
      }

      // render current camera second in the propper frame of
      // reference
      if (showPcCurrent) {
        vbo.Reinitialise(pangolin::GlArrayBuffer, pc.Area(), GL_FLOAT,
            3, GL_DYNAMIC_DRAW);
        vbo.Upload(pc.ptr_, pc.SizeBytes(), 0);
        pangolin::glSetFrameOfReference(T_wc.matrix());
        if(dispLvl == 0){
          cbo.Upload(rgb.ptr_, rgb.SizeBytes(), 0);
          pangolin::RenderVboCbo(vbo, cbo, true);
        } else {
          glColor3f(1,0,0);
          pangolin::RenderVbo(vbo);
        }
        pangolin::glUnsetFrameOfReference();
      }
    }

    if (viewAssoc.IsShown()) {
      viewAssoc.Activate(s_cam);

      pangolin::glSetFrameOfReference((T_mc).matrix());
      pangolin::glDrawAxis(0.1f);
      vbo.Reinitialise(pangolin::GlArrayBuffer, pc_c.Area(), GL_FLOAT,
          3, GL_DYNAMIC_DRAW);
      vbo.Upload(pc_c.ptr_, pc_c.SizeBytes(), 0);
      glColor3f(0,1,0);
      pangolin::RenderVbo(vbo);

      if (showPcCurrent) {
        vbo.Reinitialise(pangolin::GlArrayBuffer, pc.Area(), GL_FLOAT,
            3, GL_DYNAMIC_DRAW);
        vbo.Upload(pc.ptr_, pc.SizeBytes(), 0);
        cbo.Upload(rgb.ptr_, rgb.SizeBytes(), 0);
        pangolin::RenderVboCbo(vbo, cbo, true);
      }

      pangolin::glUnsetFrameOfReference();

      pangolin::glDrawAxis(0.3f);
      vbo.Reinitialise(pangolin::GlArrayBuffer, pc_m.Area(), GL_FLOAT,
          3, GL_DYNAMIC_DRAW);
      vbo.Upload(pc_m.ptr_, pc_m.SizeBytes(), 0);
      glColor3f(1,0,0);
      pangolin::RenderVbo(vbo);

      if (showPcModel) {
        vbo.Reinitialise(pangolin::GlArrayBuffer, pcFull_m.Area(), GL_FLOAT,
            3, GL_DYNAMIC_DRAW);
        vbo.Upload(pcFull_m.ptr_, pcFull_m.SizeBytes(), 0);
        cbo.Upload(rgb_m.ptr_, rgb_m.SizeBytes(), 0);
        pangolin::RenderVboCbo(vbo, cbo, true);
      }

      glColor4f(0,1,1,0.3);
      for (const auto& ass : assoc) {
        tdp::Vector3fda pc_c_in_m = T_mc*pc_c[ass.second];
        tdp::glDrawLine(pc_m[ass.first], pc_c_in_m);
      }

    }

    TOCK("Draw 3D");
    if (gui.verbose) std::cout << "draw 2D" << std::endl;
    TICK("Draw 2D");
    glLineWidth(1.5f);
    glDisable(GL_DEPTH_TEST);

    if (containerTracking.IsShown()) {
      if (viewModel.IsShown()) {
        viewModel.SetImage(rgb_m);
      }
      if (viewCurrent.IsShown()) {
        viewCurrent.SetImage(rgb);
      }
      if (viewMask.IsShown()) {
        viewMask.SetImage(mask);
      }
    }
    plotdH.ScrollView(1,0);
    plotH.ScrollView(1,0);
    plotObs.ScrollView(1,0);

    TOCK("Draw 2D");

    if (gui.verbose) std::cout << "finished one iteration" << std::endl;
    // leave in pixel orthographic for slider to render.
    pangolin::DisplayBase().ActivatePixelOrthographic();
    if(video.IsRecording()) {
      pangolin::glRecordGraphic(pangolin::DisplayBase().v.w-14.0f,
          pangolin::DisplayBase().v.h-14.0f, 7.0f);
    }
    Stopwatch::getInstance().sendAll();
    pangolin::FinishFrame();
  }

//  imuInterp.Stop();
//  if (imu) imu->Stop();
//  delete imu;
//  std::this_thread::sleep_for(std::chrono::microseconds(500));
  return 0;
}


#include <math.h>
#include <stdio.h>
#include <stdlib.h>

#include <Eigen/Geometry>
#include <arc_utilities/arc_helpers.hpp>
#include <arc_utilities/eigen_typedefs.hpp>
#include <map>
#include <vector>

#ifndef ABB_IRB1600_145_FK_FAST_HPP
#define ABB_IRB1600_145_FK_FAST_HPP

namespace ABB_IRB1600_145_FK_FAST {
const size_t ABB_IRB1600_145_NUM_ACTIVE_JOINTS = 6;
const size_t ABB_IRB1600_145_NUM_LINKS = 8;

const std::string ABB_IRB1600_145_ACTIVE_JOINT_1_NAME = "joint_1";
const std::string ABB_IRB1600_145_ACTIVE_JOINT_2_NAME = "joint_2";
const std::string ABB_IRB1600_145_ACTIVE_JOINT_3_NAME = "joint_3";
const std::string ABB_IRB1600_145_ACTIVE_JOINT_4_NAME = "joint_4";
const std::string ABB_IRB1600_145_ACTIVE_JOINT_5_NAME = "joint_5";
const std::string ABB_IRB1600_145_ACTIVE_JOINT_6_NAME = "joint_6";

const std::string ABB_IRB1600_145_LINK_1_NAME = "link_0";
const std::string ABB_IRB1600_145_LINK_2_NAME = "link_1";
const std::string ABB_IRB1600_145_LINK_3_NAME = "link_2";
const std::string ABB_IRB1600_145_LINK_4_NAME = "link_3";
const std::string ABB_IRB1600_145_LINK_5_NAME = "link_4";
const std::string ABB_IRB1600_145_LINK_6_NAME = "link_5";
const std::string ABB_IRB1600_145_LINK_7_NAME = "link_6";
const std::string ABB_IRB1600_145_LINK_8_NAME = "link_7";

inline Eigen::Isometry3d Get_base_joint1_LinkJointTransform(const double joint_val) {
  Eigen::Translation3d pre_joint_translation(0.0, 0.0, 0.1245);
  Eigen::Quaterniond pre_joint_rotation(1.0, 0.0, 0.0, 0.0);
  Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
  Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
  Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
  return (pre_joint_transform * joint_transform);
}

inline Eigen::Isometry3d Get_link_1_joint_2_LinkJointTransform(const double joint_val) {
  Eigen::Translation3d pre_joint_translation(0.15, -0.1395, 0.362);
  Eigen::Quaterniond pre_joint_rotation(1.0, 0.0, 0.0, 0.0);
  Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
  Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitY()));
  Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
  return (pre_joint_transform * joint_transform);
}

inline Eigen::Isometry3d Get_link_2_joint_3_LinkJointTransform(const double joint_val) {
  Eigen::Translation3d pre_joint_translation(0.0, 0.028, 0.7);
  Eigen::Quaterniond pre_joint_rotation(1.0, 0.0, 0.0, 0.0);
  Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
  Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitY()));
  Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
  return (pre_joint_transform * joint_transform);
}

inline Eigen::Isometry3d Get_link_3_joint_4_LinkJointTransform(const double joint_val) {
  Eigen::Translation3d pre_joint_translation(0.314, 0.107, 0.0);
  Eigen::Quaterniond pre_joint_rotation(1.0, 0.0, 0.0, 0.0);
  Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
  Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitX()));
  Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
  return (pre_joint_transform * joint_transform);
}

inline Eigen::Isometry3d Get_link_4_joint_5_LinkJointTransform(const double joint_val) {
  Eigen::Translation3d pre_joint_translation(0.286, 0.0, 0.0);
  Eigen::Quaterniond pre_joint_rotation(1.0, 0.0, 0.0, 0.0);
  Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
  Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitY()));
  Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
  return (pre_joint_transform * joint_transform);
}

inline Eigen::Isometry3d Get_link_5_joint_6_LinkJointTransform(const double joint_val) {
  Eigen::Translation3d pre_joint_translation(0.0, 0.0, 0.0);
  Eigen::Quaterniond pre_joint_rotation(1.0, 0.0, 0.0, 0.0);
  Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
  Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitX()));
  Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
  return (pre_joint_transform * joint_transform);
}

inline Eigen::Isometry3d Get_Fixed_link_6_joint_tool_LinkJointTransform(void) {
  Eigen::Translation3d pre_joint_translation(0.065, 0.0, 0.0);
  Eigen::Quaterniond pre_joint_rotation(0.7071067811865476, 0.0, 0.7071067811865476, 0.0);
  Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  return pre_joint_transform;
}

inline EigenHelpers::VectorIsometry3d GetLinkTransforms(
    const std::vector<double>& configuration, const Eigen::Isometry3d& base_transform = Eigen::Isometry3d::Identity()) {
  assert(configuration.size() == ABB_IRB1600_145_NUM_ACTIVE_JOINTS);
  EigenHelpers::VectorIsometry3d link_transforms(ABB_IRB1600_145_NUM_LINKS);
  link_transforms[0] = base_transform;
  link_transforms[1] = link_transforms[0] * Get_base_joint1_LinkJointTransform(configuration[0]);
  link_transforms[2] = link_transforms[1] * Get_link_1_joint_2_LinkJointTransform(configuration[1]);
  link_transforms[3] = link_transforms[2] * Get_link_2_joint_3_LinkJointTransform(configuration[2]);
  link_transforms[4] = link_transforms[3] * Get_link_3_joint_4_LinkJointTransform(configuration[3]);
  link_transforms[5] = link_transforms[4] * Get_link_4_joint_5_LinkJointTransform(configuration[4]);
  link_transforms[6] = link_transforms[5] * Get_link_5_joint_6_LinkJointTransform(configuration[5]);
  link_transforms[7] = link_transforms[6] * Get_Fixed_link_6_joint_tool_LinkJointTransform();
  return link_transforms;
}

inline EigenHelpers::VectorIsometry3d GetLinkTransforms(
    std::map<std::string, double> configuration,
    const Eigen::Isometry3d& base_transform = Eigen::Isometry3d::Identity()) {
  std::vector<double> configuration_vector(ABB_IRB1600_145_NUM_ACTIVE_JOINTS);
  configuration_vector[0] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_1_NAME];
  configuration_vector[1] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_2_NAME];
  configuration_vector[2] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_3_NAME];
  configuration_vector[3] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_4_NAME];
  configuration_vector[4] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_5_NAME];
  configuration_vector[5] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_6_NAME];
  return GetLinkTransforms(configuration_vector, base_transform);
}

inline EigenHelpers::MapStringIsometry3d GetLinkTransformsMap(
    const std::vector<double>& configuration, const Eigen::Isometry3d& base_transform = Eigen::Isometry3d::Identity()) {
  const EigenHelpers::VectorIsometry3d link_transforms = GetLinkTransforms(configuration, base_transform);
  EigenHelpers::MapStringIsometry3d link_transforms_map;
  link_transforms_map[ABB_IRB1600_145_LINK_1_NAME] = link_transforms[0];
  link_transforms_map[ABB_IRB1600_145_LINK_2_NAME] = link_transforms[1];
  link_transforms_map[ABB_IRB1600_145_LINK_3_NAME] = link_transforms[2];
  link_transforms_map[ABB_IRB1600_145_LINK_4_NAME] = link_transforms[3];
  link_transforms_map[ABB_IRB1600_145_LINK_5_NAME] = link_transforms[4];
  link_transforms_map[ABB_IRB1600_145_LINK_6_NAME] = link_transforms[5];
  link_transforms_map[ABB_IRB1600_145_LINK_7_NAME] = link_transforms[6];
  link_transforms_map[ABB_IRB1600_145_LINK_8_NAME] = link_transforms[7];
  return link_transforms_map;
}

inline EigenHelpers::MapStringIsometry3d GetLinkTransformsMap(
    const std::map<std::string, double>& configuration,
    const Eigen::Isometry3d& base_transform = Eigen::Isometry3d::Identity()) {
  const EigenHelpers::VectorIsometry3d link_transforms = GetLinkTransforms(configuration, base_transform);
  EigenHelpers::MapStringIsometry3d link_transforms_map;
  link_transforms_map[ABB_IRB1600_145_LINK_1_NAME] = link_transforms[0];
  link_transforms_map[ABB_IRB1600_145_LINK_2_NAME] = link_transforms[1];
  link_transforms_map[ABB_IRB1600_145_LINK_3_NAME] = link_transforms[2];
  link_transforms_map[ABB_IRB1600_145_LINK_4_NAME] = link_transforms[3];
  link_transforms_map[ABB_IRB1600_145_LINK_5_NAME] = link_transforms[4];
  link_transforms_map[ABB_IRB1600_145_LINK_6_NAME] = link_transforms[5];
  link_transforms_map[ABB_IRB1600_145_LINK_7_NAME] = link_transforms[6];
  link_transforms_map[ABB_IRB1600_145_LINK_8_NAME] = link_transforms[7];
  return link_transforms_map;
}
}  // namespace ABB_IRB1600_145_FK_FAST

#endif  // ABB_IRB1600_145_FK_FAST_HPP


#include <boost/circular_buffer.hpp>
#include <iostream>
#include <fstream>
#include <sstream>
#include <algorithm>
#include <range/v3/algorithm.hpp>
#include <range/v3/numeric.hpp>
#include <range/v3/view.hpp>
#include "../aoc2020/matrix.h"
#include "../aoc2020/mypair.h"
namespace views = ranges::views;

template<int N>
int firstgreaterthan(FixedMatrix<int, N, N> &m, int dst)
{
  Mypair dir[4] = {
    Mypair{ -1, 0 },
    Mypair{ 0, -1 },
    Mypair{ 1, 0 },
    Mypair{ 0, 1 }
  };
  int di = 0;
  Mypair src{ N / 2, N / 2 };
  m(src.x, src.y) = 1;
  std::cout << m(src.x, src.y) << std::endl;
  m(src.x, src.y + 1) = 1;
  std::cout << m(src.x, src.y + 1) << std::endl;

  for (Mypair cur = src + Mypair{ -1, 1 };; cur += dir[di]) {
    int sum = 0;

    for (int i = -1; i < 2; ++i) {
      for (int j = -1; j < 2; ++j) {
        auto t = cur + Mypair{ i, j };
        sum += m(t.x, t.y);
      }
    }
    m(cur.x, cur.y) = sum;
    if (sum > dst) {
      return sum;
    }
    std::cout << m(cur.x, cur.y) << std::endl;
    if (m(cur.x, cur.y) >= dst) {
      return m(cur.x, cur.y);
    }

    auto diff = cur - src;
    if (diff.y > 0) {
      if (diff.x + 1 == diff.y) {
        di = 0;
      } else if (diff.y == -diff.x) {
        di = 1;
      }
    } else {
      if (diff.y == diff.x) {
        di = 2;
      } else if (diff.y == -diff.x) {
        di = 3;
      }
    }
  }
  return 0;
}

int main(int argc, char **argv)
{
  FixedMatrix<int, 600, 600> m;
  std::cout << firstgreaterthan(m, 277678) << std::endl;
}


// Copyright (c) 2021 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#pragma once
#include <gsl/gsl_spline2d.h>

#include <Eigen/Core>
#include <functional>
#include <memory>

#include "pyinterp/detail/gsl/accelerator.hpp"

namespace pyinterp::detail::gsl {

/// Interpolate a 1-D function
class Interpolate2D {
 public:
  /// Interpolate a 1-D function
  ///
  /// @param xsize grid points in the X direction
  /// @param ysize grid points in the Y direction
  /// @param type fitting model
  /// @param acc Accelerator
  Interpolate2D(const size_t xsize, const size_t ysize,
                const gsl_interp2d_type* type, Accelerator xacc,
                Accelerator yacc)
      : workspace_(
            std::unique_ptr<gsl_spline2d, std::function<void(gsl_spline2d*)>>(
                gsl_spline2d_alloc(type, xsize, ysize),
                [](gsl_spline2d* ptr) { gsl_spline2d_free(ptr); })),
        xacc_(std::move(xacc)),
        yacc_(std::move(yacc)) {}

  /// Returns the name of the interpolation type used
  [[nodiscard]] inline auto name() const noexcept -> std::string {
    return gsl_spline2d_name(workspace_.get());
  }

  /// Return the minimum number of points required by the interpolation
  [[nodiscard]] inline auto min_size() const noexcept -> size_t {
    return gsl_spline2d_min_size(workspace_.get());
  }

  /// Return the interpolated value of z for a given point x, y
  [[nodiscard]] inline auto evaluate(const Eigen::VectorXd& xa,
                                     const Eigen::VectorXd& ya,
                                     const Eigen::MatrixXd& za, const double x,
                                     const double y) -> double {
    init(xa, ya, za);
    return gsl_spline2d_eval(workspace_.get(), x, y, xacc_, yacc_);
  }

 private:
  std::unique_ptr<gsl_spline2d, std::function<void(gsl_spline2d*)>> workspace_;
  Accelerator xacc_;
  Accelerator yacc_;

  /// Initializes the interpolation object
  inline void init(const Eigen::VectorXd& xa, const Eigen::VectorXd& ya,
                   const Eigen::MatrixXd& za) noexcept {
    xacc_.reset();
    xacc_.reset();
    gsl_spline2d_init(workspace_.get(), xa.data(), ya.data(), za.data(),
                      xa.size(), ya.size());
  }
};

}  // namespace pyinterp::detail::gsl


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.
//
// Written by Jiahu Deng and Peter Gottschling

#ifndef MTL_DILATED_INT_INCLUDE
#define MTL_DILATED_INT_INCLUDE

#include <boost/numeric/mtl/detail/masked_dilation_tables.hpp>
#include <iostream>

namespace mtl { namespace dilated {

template <typename T>
struct even_bits
{
    static T const value = T(-1) / T(3);
};

template <typename T>
struct odd_bits
{
    static T const value = ~even_bits<T>::value;
};

// And is mostly used with original mask
template <typename T, T BitMask, bool Normalized>
struct masking
{
    inline void operator() (T& x) const
    {
	x &= BitMask;
    }
};

// Or is mostly used with complementary mask
template <typename T, T BitMask>
struct masking<T, BitMask, false>
{
    inline void operator() (T& x) const
    {
	static T const anti_mask = ~BitMask;
	x |= anti_mask;
    }
};

template <typename T, T BitMask>
struct last_bit;

template <typename T, T BitMask, bool IsZero>
struct last_bit_helper {
    static T const tmp = BitMask >> 1;
    static T const value = BitMask & 1 ? 1 : last_bit_helper<T, tmp, tmp == 0>::value << 1;
};

template <typename T, T BitMask>
struct last_bit_helper<T, BitMask, true> {
    static T const value = 0;
};

template <typename T, T BitMask>
struct last_bit
{
    static T const value = last_bit_helper<T, BitMask, BitMask == 0>::value;
};

template <typename T, T BitMask, bool Normalized>
struct dilated_int
{
    typedef T                                       value_type;
    typedef dilated_int<T, BitMask, Normalized>     self;

    typedef masking<T, BitMask, Normalized>         clean_carry;
    typedef masking<T, BitMask, !Normalized>        init_carry;

    static T const       bit_mask = BitMask,
	                 anti_mask = ~BitMask,
	                 dilated_zero = Normalized ? 0 : anti_mask,
			 dilated_one = dilated_zero + last_bit<T, bit_mask>::value;
protected:
    // masked_dilation_tables<T, bit_mask>   mask_tables;
    // masked_dilation_tables<T, anti_mask>  anti_tables; probably not needed

// will be protected later
public:
    T i;

    void dilate(T x)
    {
		static const T to_switch_on = Normalized ? 0 : anti_mask;
		// auto aa = mask<bit_mask>(x) | to_switch_on;
		// auto ab = mask<static_cast<T>(bit_mask)>(x) | to_switch_on
		i = mask<bit_mask>(x) | to_switch_on;
    }

public:

    // Default constructor
    dilated_int()
    {
	i = Normalized ? 0 : anti_mask;
    }

    // Only works for odd and even bits and 4-byte-int at this point !!!!!!!!!!!!!!!!!!!
    explicit dilated_int(T x)
    {
	dilate(x);
    }

    // Only works for odd and even bits and 4-byte-int at this point !!!!!!!!!!!!!!!!!!!
    T undilate()
    {
	return unmask<bit_mask>(i);
    }

    T dilated_value() const
    {
	return i;
    }

    self& operator= (self const& x)
    {
	i = x.i;
	return *this;
    }

    self& operator= (T x)
    {
	dilate(x);
	return *this;
    }

    self& operator++ ()
    {
	static T const x = Normalized ? bit_mask : T(-1);
	i -= x;
	clean_carry()(i);
	return *this;
    }

    self operator++ (int)
    {
	self tmp(*this);
	++*this;
	return tmp;
    }

    self& operator+= (self const& x)
    {
	init_carry()(i);
	i+= x.i;
	clean_carry()(i);
	return *this;
    }

    self operator+ (self const& x)
    {
	self tmp(*this);
	return tmp += x;
    }

    self& operator-- ()
    {
	i -= dilated_one;
	clean_carry()(i);
	return *this;
    }

    self operator-- (int)
    {
	self tmp(*this);
	--*this;
	return tmp;
    }

    self& operator-= (self const& x)
    {
	i -= x.i;
	clean_carry()(i);
	return *this;
    }

    self operator- (self const& x) const
    {
	self tmp(*this);
	return tmp -= x;
    }

    // advance in both directions, special care is needed for negative values
    self& advance(long inc)
    {
	value_type incv(inc >= 0 ? inc : -inc);
	self incd(incv);
	if (inc >= 0)
	    operator+=(incd);
	else
	    operator-=(incd);
	return *this;
    }

    bool operator== (self const& x) const
    {
	return i == x.i;
    }

    bool operator!= (self const& x) const
    {
	return i != x.i;
    }

    bool operator<= (self const& x) const
    {
	return i <= x.i;
    }

    bool operator< (self const& x) const
    {
	return i < x.i;
    }

    bool operator>= (self const& x) const
    {
	return i >= x.i;
    }

    bool operator> (self const& x) const
    {
	return i > x.i;
    }

};

} // namespace mtl::dilated

using dilated::dilated_int;

} // namespace mtl

template <typename T, T BitMask, bool Normalized>
inline std::ostream& operator<< (std::ostream& os, mtl::dilated::dilated_int<T, BitMask, Normalized> d)
{
    os.setf(std::ios_base::hex, std::ios_base::basefield);
    return os << d.i;
}

#endif // MTL_DILATED_INT_INCLUDE


#pragma once

#include <algorithm>
#include <map>
#include <vector>
#include <boost/functional/hash.hpp>

#include "nifty/xtensor/xtensor.hxx"
#include "nifty/parallel/threadpool.hxx"

namespace nifty {
namespace tools {

    template<class UV_ARRAY, class INDICATORS, class SIZES>
    void computeMergeVotes(const xt::xexpression<UV_ARRAY> & uvIdsExp,
                           const xt::xexpression<INDICATORS> & indicatorsExp,
                           const xt::xexpression<SIZES> & sizesExp,
                           std::map<std::pair<typename UV_ARRAY::value_type,
                                              typename UV_ARRAY::value_type>,
                                    std::pair<std::size_t, std::size_t>> & mergeVotes,
                           const bool weightEdges=false) {
        const auto & uvIds = uvIdsExp.derived_cast();
        const auto & indicators = indicatorsExp.derived_cast();
        const auto & sizes = sizesExp.derived_cast();

        typedef typename UV_ARRAY::value_type NodeType;
        typedef typename std::pair<NodeType, NodeType> UvType;
        mergeVotes.clear();

        // TODO parallelize ?!
        const std::size_t nPairs = uvIds.shape()[0];
        for(std::size_t i = 0; i < nPairs; ++i) {
            const NodeType u = uvIds(i, 0);
            const NodeType v = uvIds(i, 1);
            const UvType uv(u, v);

            auto voteIt = mergeVotes.find(uv);
            if(voteIt == mergeVotes.end()) {
                voteIt = mergeVotes.insert(voteIt, std::make_pair(uv,
                                                                  std::make_pair(0, 0)));
            }

            const auto indicator = indicators(i);
            if(weightEdges) {
                auto size = sizes(i);
                voteIt->second.first += indicator * size;
                voteIt->second.second += size;
            } else {
                voteIt->second.first += indicator;
                voteIt->second.second += 1;
            }

        }
    }

    template<class UV_ARRAY, class VOTES>
    void mergeMergeVotes(const xt::xexpression<UV_ARRAY> & uvIdsExp,
                         const xt::xexpression<VOTES> & votesExp,
                         std::map<std::pair<typename UV_ARRAY::value_type,
                                            typename UV_ARRAY::value_type>,
                                  std::pair<std::size_t, std::size_t>> & votesOut) {

        const auto & uvIds = uvIdsExp.derived_cast();
        const auto & votes = votesExp.derived_cast();

        typedef typename UV_ARRAY::value_type NodeType;
        typedef typename std::pair<NodeType, NodeType> UvType;
        votesOut.clear();

        // TODO parallelize ?!
        const std::size_t nPairs = uvIds.shape()[0];
        for(std::size_t i = 0; i < nPairs; ++i) {
            const NodeType u = uvIds(i, 0);
            const NodeType v = uvIds(i, 1);
            const UvType uv(u, v);

            // find this node pair in the out dataset
            auto voteIt = votesOut.find(uv);
            if(voteIt == votesOut.end()) {
                voteIt = votesOut.insert(voteIt, std::make_pair(uv,
                                                                std::make_pair(0, 0)));
            }

            // add the current votes
            voteIt->second.first += votes(i, 0);
            voteIt->second.second += votes(i, 1);
        }

    }

}
}


///////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_

//
// Compare arithmetic results using fixed_int to GMP results.
//

#ifdef _MSC_VER
#  define _SCL_SECURE_NO_WARNINGS
#endif

#include <boost/multiprecision/cpp_int.hpp>
#include "test.hpp"
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>

template <class T>
T generate_random(unsigned bits_wanted)
{
   static boost::random::mt19937 gen;
   typedef boost::random::mt19937::result_type random_type;

   T max_val;
   unsigned digits;
   if(std::numeric_limits<T>::is_bounded && (bits_wanted == (unsigned)std::numeric_limits<T>::digits))
   {
      max_val = (std::numeric_limits<T>::max)();
      digits = std::numeric_limits<T>::digits;
   }
   else
   {
      max_val = T(1) << bits_wanted;
      digits = bits_wanted;
   }

   unsigned bits_per_r_val = std::numeric_limits<random_type>::digits - 1;
   while((random_type(1) << bits_per_r_val) > (gen.max)()) --bits_per_r_val;

   unsigned terms_needed = digits / bits_per_r_val + 1;

   T val = 0;
   for(unsigned i = 0; i < terms_needed; ++i)
   {
      val *= (gen.max)();
      val += gen();
   }
   val %= max_val;
   return val;
}

template <class Number>
void test_signed_overflow(Number a, Number b, const boost::mpl::true_&)
{
   a = -a;
   BOOST_CHECK_THROW(Number(a * b), std::overflow_error);
   ++a;
   BOOST_CHECK(Number(a * b) >= (std::numeric_limits<Number>::min)());
}
template <class Number>
void test_signed_overflow(Number a, Number b, const boost::mpl::false_&)
{
}

template <class Number>
void test()
{
   using namespace boost::multiprecision;
   typedef Number test_type;

   if(std::numeric_limits<test_type>::is_bounded)
   {
      test_type val = (std::numeric_limits<test_type>::max)();
#ifndef BOOST_NO_EXCEPTIONS
      BOOST_CHECK_THROW(++val, std::overflow_error);
      val = (std::numeric_limits<test_type>::max)();
      BOOST_CHECK_THROW(test_type(1 + val), std::overflow_error);
      BOOST_CHECK_THROW(test_type(val + 1), std::overflow_error);
      BOOST_CHECK_THROW(test_type(2 * val), std::overflow_error);
      val /= 2;
      val += 1;
      BOOST_CHECK_THROW(test_type(2 * val), std::overflow_error);

      if(std::numeric_limits<test_type>::is_signed)
      {
         val = (std::numeric_limits<test_type>::min)();
         BOOST_CHECK_THROW(--val, std::overflow_error);
         val = (std::numeric_limits<test_type>::min)();
         BOOST_CHECK_THROW(test_type(val - 1), std::overflow_error);
         BOOST_CHECK_THROW(test_type(2 * val), std::overflow_error);
         val /= 2;
         val -= 1;
         BOOST_CHECK_THROW(test_type(2 * val), std::overflow_error);
      }
      else
      {
         val = (std::numeric_limits<test_type>::min)();
         BOOST_CHECK_THROW(--val, std::range_error);
         val = (std::numeric_limits<test_type>::min)();
         BOOST_CHECK_THROW(test_type(val - 1), std::range_error);
      }
#endif
      //
      // Test overflow in random values:
      //
      for(unsigned bits = 30; bits < std::numeric_limits<test_type>::digits; bits += 30)
      {
         for(unsigned i = 0; i < 100; ++i)
         {
            val = static_cast<test_type>(generate_random<cpp_int>(bits));
            test_type val2 = 1 + (std::numeric_limits<test_type>::max)() / val;
            BOOST_CHECK_THROW(test_type(val2 * val), std::overflow_error);
            test_signed_overflow(val2, val, boost::mpl::bool_<std::numeric_limits<test_type>::is_signed>());
            --val2;
            BOOST_CHECK(cpp_int(val2) * cpp_int(val) <= cpp_int((std::numeric_limits<test_type>::max)()));
            BOOST_CHECK(val2 * val <= (std::numeric_limits<test_type>::max)());
            val2 = (std::numeric_limits<test_type>::max)() - val;
            ++val2;
            BOOST_CHECK_THROW(test_type(val2 + val), std::overflow_error);
            BOOST_CHECK((val2 - 1) + val == (std::numeric_limits<test_type>::max)());
            if(std::numeric_limits<test_type>::is_signed)
            {
               val2 = (std::numeric_limits<test_type>::min)() + val;
               --val2;
               BOOST_CHECK_THROW(test_type(val2 - val), std::overflow_error);
               ++val2;
               BOOST_CHECK(val2 - val == (std::numeric_limits<test_type>::min)());
            }
            unsigned shift = std::numeric_limits<test_type>::digits - msb(val);
            BOOST_CHECK_THROW((val << shift) > 0, std::overflow_error);
         }
      }
   }

#ifndef BOOST_NO_EXCEPTIONS
   if(std::numeric_limits<test_type>::is_signed)
   {
      test_type a = -1;
      test_type b = 1;
      BOOST_CHECK_THROW(test_type(a | b), std::range_error);
      BOOST_CHECK_THROW(test_type(a & b), std::range_error);
      BOOST_CHECK_THROW(test_type(a ^ b), std::range_error);
   }
   else
   {
      // Constructing from a negative value is not allowed:
      BOOST_CHECK_THROW(test_type(-2), std::range_error);
      BOOST_CHECK_THROW(test_type("-2"), std::range_error);
   }
   if(std::numeric_limits<test_type>::digits < std::numeric_limits<long long>::digits)
   {
      long long llm = (std::numeric_limits<long long>::max)();
      test_type t;
      BOOST_CHECK_THROW(t = llm, std::range_error);
      BOOST_CHECK_THROW(t = static_cast<test_type>(llm), std::range_error);
      unsigned long long ullm = (std::numeric_limits<unsigned long long>::max)();
      BOOST_CHECK_THROW(t = ullm, std::range_error);
      BOOST_CHECK_THROW(t = static_cast<test_type>(ullm), std::range_error);

      static const checked_uint512_t big = (std::numeric_limits<checked_uint512_t>::max)();
      BOOST_CHECK_THROW(t = static_cast<test_type>(big), std::range_error);
   }
   //
   // String errors:
   //
   BOOST_CHECK_THROW(test_type("12A"), std::runtime_error);
   BOOST_CHECK_THROW(test_type("0658"), std::runtime_error);

   if(std::numeric_limits<test_type>::is_signed)
   {
      BOOST_CHECK_THROW(test_type(-2).str(0, std::ios_base::hex), std::runtime_error);
      BOOST_CHECK_THROW(test_type(-2).str(0, std::ios_base::oct), std::runtime_error);
   }
#endif
}

int main()
{
   using namespace boost::multiprecision;

   test<number<cpp_int_backend<0, 0, signed_magnitude, checked> > >();
   test<checked_int512_t>();
   test<checked_uint512_t>();
   test<number<cpp_int_backend<32, 32, signed_magnitude, checked, void> > >();
   test<number<cpp_int_backend<32, 32, unsigned_magnitude, checked, void> > >();

   //
   // We also need to test type with "odd" bit counts in order to ensure full code coverage:
   //
   test<number<cpp_int_backend<528, 528, signed_magnitude, checked, void> > >();
   test<number<cpp_int_backend<528, 528, unsigned_magnitude, checked, void> > >();
   test<number<cpp_int_backend<48, 48, signed_magnitude, checked, void> > >();
   test<number<cpp_int_backend<48, 48, unsigned_magnitude, checked, void> > >();
   return boost::report_errors();
}


//=======================================================================
// Copyright (c) 2013 Piotr Wygocki
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file knapsack_utils.hpp
 * @brief
 * @author Piotr Wygocki
 * @version 1.0
 * @date 2013-10-07
 */
#ifndef PAAL_KNAPSACK_UTILS_HPP
#define PAAL_KNAPSACK_UTILS_HPP

#include "paal/utils/type_functions.hpp"
#include "paal/utils/less_pointees.hpp"
#include "paal/utils/functors.hpp"

#include <boost/iterator/filter_iterator.hpp>
#include <boost/optional.hpp>
#include <boost/range/adaptor/filtered.hpp>
#include <boost/range/algorithm/max_element.hpp>
#include <boost/range/algorithm/min_element.hpp>
#include <boost/range/numeric.hpp>

namespace paal {

/**
 * @brief density functor, for given value and size
 *
 * @tparam Value
 * @tparam Size
 */
template <typename Value, typename Size> struct density {

    /// constructor
    density(Value value, Size size) : m_value(value), m_size(size) {}

    /// operator()
    template <typename ObjectRef> double operator()(ObjectRef obj) const {
        return double(m_value(obj)) / double(m_size(obj));
    }

  private:
    Value m_value;
    Size m_size;
};

/**
 * @brief make for density
 *
 * @tparam Value
 * @tparam Size
 * @param value
 * @param size
 *
 * @return
 */
template <typename Value, typename Size>
density<Value, Size> make_density(Value value, Size size) {
    return density<Value, Size>(value, size);
}

namespace detail {
template <typename Functor, typename Range>
using FunctorOnRangePValue =
    puretype(std::declval<Functor>()(*std::begin(std::declval<Range>())));

// definition of basic types
template <typename Objects, typename ObjectSizeFunctor,
          typename ObjectValueFunctor>
struct knapsack_base {
    typedef detail::FunctorOnRangePValue<ObjectSizeFunctor, Objects> SizeType;
    typedef detail::FunctorOnRangePValue<ObjectValueFunctor, Objects> ValueType;
    typedef puretype(*std::begin(std::declval<Objects>())) ObjectType;
    using PureObjRef = typename boost::range_reference<Objects>::type;
    // add_const is unfortunately needed by function_output_iterator
    // TODO it supposed to be changed in boost...
    typedef typename std::conditional<
        std::is_reference<PureObjRef>::value,
        typename std::add_lvalue_reference<typename std::add_const<
            typename std::remove_reference<PureObjRef>::type>::type>::type,
        typename std::add_const<PureObjRef>::type>::type ObjectRef;
    typedef std::pair<ValueType, SizeType> return_type;
};

// various tags
struct integral_value_and_size_tag {};
struct integral_value_tag {};
struct integral_size_tag {};
struct non_integral_value_and_size_tag {};

struct arithmetic_size_tag {};
struct Nonarithmetic_size_tag {};

struct zero_one_tag {};
struct unbounded_tag {};

struct retrieve_solution_tag {};
struct no_retrieve_solution_tag {};

template <typename GetSize, typename GetValue, typename Objects,
          typename OutputIterator>
class knapsack_data {
  public:
    using traits = knapsack_base<Objects, GetSize, GetValue>;
    using size = typename traits::SizeType;
    using value = typename traits::ValueType;
    using objects = Objects;
    using object_ref = typename traits::ObjectRef;
    using object_iter = typename boost::range_iterator<Objects>::type;
    using return_type = typename traits::return_type;

    knapsack_data(Objects objects, size capacity, GetSize get_size,
                  GetValue get_value, OutputIterator & out)
        : m_objects(objects), m_capacity(capacity), m_get_size(get_size),
          m_get_value(get_value), m_out(out) {}

    size get_size(object_ref obj) const { return m_get_size(obj); }

    value get_value(object_ref obj) const { return m_get_value(obj); }

    GetSize get_size() const { return m_get_size; }

    GetValue get_value() const { return m_get_value; }

    objects get_objects() { return m_objects; }

    void out(object_ref obj) {
        *m_out = obj;
        ++m_out;
    }

    size get_capacity() const { return m_capacity; }

    OutputIterator & get_output_iter() const { return m_out; }

    density<GetValue, GetSize> get_density() {
        return make_density(m_get_value, m_get_size);
    }

  private:
    Objects m_objects;
    size m_capacity;
    GetSize m_get_size;
    GetValue m_get_value;
    OutputIterator & m_out;
};

template <typename GetSize, typename GetValue, typename Objects,
          typename OutputIterator,
          typename Size = FunctorOnRangePValue<GetSize, Objects>>
knapsack_data<GetSize, GetValue, Objects, OutputIterator>
make_knapsack_data(Objects &&objects, Size capacity, GetSize get_size,
                   GetValue get_value, OutputIterator & out) {
    return knapsack_data<GetSize, GetValue, Objects, OutputIterator>{
        std::forward<Objects>(objects), capacity, get_size, get_value, out
    };
}

} //!detail

}      //! paal
#endif // PAAL_KNAPSACK_UTILS_HPP


#include <boost/mpl/assert.hpp>
#include <nt2/sdk/config/types.hpp>
#include <nt2/sdk/meta/downgrade.hpp>
#include <nt2/sdk/simd/pack.hpp>
#include <boost/type_traits/is_same.hpp>

using boost::is_same;
using boost::mpl::_;
using nt2::meta::downgrade;

using nt2::uint16_t;
using nt2::uint8_t;
using nt2::simd::pack;

int main()
{
  BOOST_MPL_ASSERT(( is_same<uint8_t,        downgrade<uint16_t>::type>          ));
  BOOST_MPL_ASSERT(( is_same<int8_t,         downgrade<uint8_t, signed>::type>   ));
  BOOST_MPL_ASSERT(( is_same<float,          downgrade<double>::type>            ));
  BOOST_MPL_ASSERT(( is_same<pack<float, 4>, downgrade< pack<double, 2> >::type> ));
}


/*******************************************************************************
*
*  Filename    : RooFit_SimFit_Val.cc
*  Description : Validation of simfit method via signal injection
*  Author      : Yi-Mu "Enoch" Chen [ ensc@hep1.phys.ntu.edu.tw ]
*
*******************************************************************************/
#include "TstarAnalysis/LimitCalc/interface/Common.hpp"
#include "TstarAnalysis/LimitCalc/interface/MakeKeysPdf.hpp"
#include "TstarAnalysis/LimitCalc/interface/SampleRooFitMgr.hpp"
#include "TstarAnalysis/LimitCalc/interface/SimFit.hpp"
#include "TstarAnalysis/LimitCalc/interface/SimFitVal.hpp"
#include "TstarAnalysis/LimitCalc/interface/Template.hpp"

#include "ManagerUtils/Common/interface/Random.hpp"
#include "ManagerUtils/PlotUtils/interface/Common.hpp"
#include "ManagerUtils/PlotUtils/interface/RooFitUtils.hpp"
#include <boost/filesystem.hpp>
#include <boost/format.hpp>
#include <ctime>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>

#include "RooDataSet.h"
#include "RooRandom.h"

using namespace std;

/*******************************************************************************
*   Helper functions
*******************************************************************************/
void
RunGenFit(
  SampleRooFitMgr* mc,
  SampleRooFitMgr* sigmgr,
  SampleRooFitMgr* data
  )
{
  RooFitResult* bgconstrain = FitBackgroundTemplate( mc, "" );
  const double bkgnum       = data->DataSet()->sumEntries();
  const double signum
    = limnamer.CheckInput( "relmag" ) ? sigmgr->ExpectedYield() * limnamer.GetInput<double>( "relmag" ) :
      limnamer.GetInput<double>( "absmag" );

  vector<RooRealVar*> paramlist = mc->VarContains( "template" );
  const double param1           = paramlist[0]->getVal();
  const double param2           = paramlist[1]->getVal();

  RooAbsPdf* bkgfunc = mc->Pdf( "template" );
  RooAbsPdf* sigfunc = MakeSingleKeysPdf( sigmgr, "" );

  const string strgthtag = SigStrengthTag();
  const string filename  = limnamer.TextFileName( "valsimfit",  strgthtag );

  ofstream result;
  if( !boost::filesystem::exists( filename ) ){
    // If doesn't already exists create new file and print first fitting value.
    result.open( filename, ios_base::out );
    cout << "Writting central value to file " << filename << endl;
    result << boost::format( "%lf %lf %lf %lf" ) % bkgnum % signum % param1 % param2  << endl;
  } else {
    // Else open in append mode.
    cout << "Appending fit to file " << filename << endl;
    result.open( filename, ios_base::app );
  }

  // Setting random seed to present time to allow multiple reruns
  RooRandom::randomGenerator()->SetSeed( time( NULL ) );

  for( int i = 0; i < limnamer.GetInput<int>( "num" ); ++i ){
    const string pseudosetname = "pseudo" + mgr::RandomString( 6 );

    const double sigmax = std::max( signum*3, 10000. );
    const double sigmin = ( signum > 100 ) ? 0 : -sigmax;

    RooRealVar nb( "nb", "nb", bkgnum, 0, 3*bkgnum );
    RooRealVar ns( "ns", "ns", signum, sigmin, sigmax );

    RooRealVar& p1 = *paramlist[0];
    RooRealVar& p2 = *paramlist[1];

    p1.setConstant( kFALSE );
    p2.setConstant( kFALSE );

    // Resetting value
    p1 = param1;
    p2 = param2;

    RooAddPdf model(
      "model", "model",
      RooArgList( *bkgfunc, *sigfunc ),
      RooArgList( nb,       ns )
      );

    RooDataSet* set = model.generate( SampleRooFitMgr::x(), RooFit::Extended( kTRUE ) );

    RooFitResult* ans = NULL;
    unsigned iter = 0;
    while( !ans && iter < 5 ){
      ans = model.fitTo( *set,
        RooFit::Verbose( kFALSE ),
        RooFit::PrintLevel( -1 ),
        RooFit::PrintEvalErrors( -1 ),
        RooFit::Warnings( kFALSE ),
        RooFit::Save()
        );

      if( ans->status() ){
        delete ans;
        ans = NULL;
        ++iter;
      }
    }
    delete ans;

    result << boost::format(  "%20.12lf %20.12lf \t %20.12lf %20.12lf \t %20.12lf %20.12lf \t %20.12lf %20.12lf" )
      % nb.getValV() % nb.getError()
      % ns.getValV() % ns.getError()
      % p1.getValV() % p1.getError()
      % p2.getValV() % p2.getError()
           << endl;

    delete set;
  }

  delete bgconstrain;

  result.close();
}

/******************************************************************************/

string
SigStrengthTag()
{
  static boost::format secfmt( "%1%%2%" );
  return limnamer.CheckInput( "relmag" ) ?
         str( secfmt % "rel" % limnamer.GetInput<double>( "relmag" ) ) :
         str( secfmt % "abs" % limnamer.GetInput<double>( "absmag" ) );
}

/******************************************************************************/
string
RhoTag()
{
  static boost::format rhofmt( "forcerho%2%" );
  return limnamer.CheckInput( "forcerho" ) ?
         str( rhofmt % "rel" % limnamer.GetInput<double>( "forcerho" ) ) :
         "";
}


/*
-------------------------------------------------------------------------
   This file is part of BayesOpt, an efficient C++ library for
   Bayesian optimization.

   Copyright (C) 2011-2015 Ruben Martinez-Cantin <rmcantin@unizar.es>

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

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

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

#include <cstdio>
#include <cstdlib>
#include <stdexcept>
#include <boost/lexical_cast.hpp>

#include "kernelregressor.hpp"

#include "log.hpp"
#include "ublas_extra.hpp"

namespace bayesopt
{
  KernelRegressor::KernelRegressor(size_t dim, Parameters parameters,
				   const Dataset& data,
				   MeanModel& mean, randEngine& eng):
    NonParametricProcess(dim,parameters,data,mean,eng),
    mRegularizer(parameters.noise),
    mKernel(dim, parameters),
    mScoreType(parameters.sc_type),
    mLearnType(parameters.l_type),
    mLearnAll(parameters.l_all)
  { }

  KernelRegressor::~KernelRegressor(){}

  void KernelRegressor::updateSurrogateModel()
  {
    const vectord lastX = mData.getLastSampleX();
    vectord newK = computeCrossCorrelation(lastX);
    newK(newK.size()-1) += mRegularizer;   // We add it to the last element
    utils::cholesky_add_row(mL,newK);
    precomputePrediction();
  } // updateSurrogateModel

  void KernelRegressor::computeCholeskyCorrelation()
  {
    size_t nSamples = mData.getNSamples();
    mL.resize(nSamples,nSamples);

    //  const matrixd K = computeCorrMatrix();
    matrixd K(nSamples,nSamples);
    computeCorrMatrix(K);
    size_t line_error = utils::cholesky_decompose(K,mL);
    if (line_error)
      {
	throw std::runtime_error("Cholesky decomposition error at line " +
				 boost::lexical_cast<std::string>(line_error));
      }
  }

  matrixd KernelRegressor::computeDerivativeCorrMatrix(int dth_index)
  {
    const size_t nSamples = mData.getNSamples();
    matrixd corrMatrix(nSamples,nSamples);
    mKernel.computeDerivativeCorrMatrix(mData.mX,corrMatrix,dth_index);
    return corrMatrix;
  }

} //namespace bayesopt


/*

Copyright (c) 2015, Project OSRM contributors
All rights reserved.

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

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

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

*/

#ifndef HIDDEN_MARKOV_MODEL
#define HIDDEN_MARKOV_MODEL

#include "../util/integer_range.hpp"

#include <boost/assert.hpp>

#include <cmath>

#include <limits>
#include <vector>

namespace osrm
{
namespace matching
{
static const double log_2_pi = std::log(2. * M_PI);
static const double IMPOSSIBLE_LOG_PROB = -std::numeric_limits<double>::infinity();
static const double MINIMAL_LOG_PROB = std::numeric_limits<double>::lowest();
static const std::size_t INVALID_STATE = std::numeric_limits<std::size_t>::max();
} // namespace matching
} // namespace osrm

// closures to precompute log -> only simple floating point operations
struct EmissionLogProbability
{
    double sigma_z;
    double log_sigma_z;

    EmissionLogProbability(const double sigma_z) : sigma_z(sigma_z), log_sigma_z(std::log(sigma_z))
    {
    }

    double operator()(const double distance) const
    {
        return -0.5 * (osrm::matching::log_2_pi + (distance / sigma_z) * (distance / sigma_z)) -
               log_sigma_z;
    }
};

struct TransitionLogProbability
{
    double beta;
    double log_beta;
    TransitionLogProbability(const double beta) : beta(beta), log_beta(std::log(beta)) {}

    double operator()(const double d_t) const { return -log_beta - d_t / beta; }
};

template <class CandidateLists> struct HiddenMarkovModel
{
    std::vector<std::vector<double>> viterbi;
    std::vector<std::vector<std::pair<unsigned, unsigned>>> parents;
    std::vector<std::vector<float>> path_lengths;
    std::vector<std::vector<bool>> pruned;
    std::vector<std::vector<bool>> suspicious;
    std::vector<bool> breakage;

    const CandidateLists &candidates_list;
    const EmissionLogProbability &emission_log_probability;

    HiddenMarkovModel(const CandidateLists &candidates_list,
                      const EmissionLogProbability &emission_log_probability)
        : breakage(candidates_list.size()), candidates_list(candidates_list),
          emission_log_probability(emission_log_probability)
    {
        viterbi.resize(candidates_list.size());
        parents.resize(candidates_list.size());
        path_lengths.resize(candidates_list.size());
        suspicious.resize(candidates_list.size());
        pruned.resize(candidates_list.size());
        breakage.resize(candidates_list.size());
        for (const auto i : osrm::irange<std::size_t>(0u, candidates_list.size()))
        {
            const auto& num_candidates = candidates_list[i].size();
            // add empty vectors
            if (num_candidates > 0)
            {
                viterbi[i].resize(num_candidates);
                parents[i].resize(num_candidates);
                path_lengths[i].resize(num_candidates);
                suspicious[i].resize(num_candidates);
                pruned[i].resize(num_candidates);
            }
        }

        clear(0);
    }

    void clear(std::size_t initial_timestamp)
    {
        BOOST_ASSERT(viterbi.size() == parents.size() && parents.size() == path_lengths.size() &&
                     path_lengths.size() == pruned.size() && pruned.size() == breakage.size());

        for (const auto t : osrm::irange(initial_timestamp, viterbi.size()))
        {
            std::fill(viterbi[t].begin(), viterbi[t].end(), osrm::matching::IMPOSSIBLE_LOG_PROB);
            std::fill(parents[t].begin(), parents[t].end(), std::make_pair(0u, 0u));
            std::fill(path_lengths[t].begin(), path_lengths[t].end(), 0);
            std::fill(suspicious[t].begin(), suspicious[t].end(), true);
            std::fill(pruned[t].begin(), pruned[t].end(), true);
        }
        std::fill(breakage.begin() + initial_timestamp, breakage.end(), true);
    }

    std::size_t initialize(std::size_t initial_timestamp)
    {
        auto num_points = candidates_list.size();
        do
        {
            BOOST_ASSERT(initial_timestamp < num_points);

            for (const auto s : osrm::irange<std::size_t>(0u, viterbi[initial_timestamp].size()))
            {
                viterbi[initial_timestamp][s] =
                    emission_log_probability(candidates_list[initial_timestamp][s].second);
                parents[initial_timestamp][s] = std::make_pair(initial_timestamp, s);
                pruned[initial_timestamp][s] =
                    viterbi[initial_timestamp][s] < osrm::matching::MINIMAL_LOG_PROB;
                suspicious[initial_timestamp][s] = false;

                breakage[initial_timestamp] =
                    breakage[initial_timestamp] && pruned[initial_timestamp][s];
            }

            ++initial_timestamp;
        } while (initial_timestamp < num_points && breakage[initial_timestamp - 1]);

        if (initial_timestamp >= num_points)
        {
            return osrm::matching::INVALID_STATE;
        }

        BOOST_ASSERT(initial_timestamp > 0);
        --initial_timestamp;

        BOOST_ASSERT(breakage[initial_timestamp] == false);

        return initial_timestamp;
    }
};

#endif // HIDDEN_MARKOV_MODEL


#pragma once
#include <iostream>
#include <vector>
#include <string>
#include <map>
#include <algorithm>
#include <boost/thread/thread.hpp>
#include <boost/range/combine.hpp>
#include <boost/foreach.hpp>
#include <chrono>
#include <cstdlib>
#include <ctime>

#include "libscapi/include/comm/Comm.hpp"
#include "libscapi/include/primitives/DlogOpenSSL.hpp"
#include "libscapi/include/interactive_mid_protocols/CommitmentSchemePedersenTrapdoor.hpp"
#include "libscapi/include/mid_layer/ElGamalEnc.hpp"
#include "libscapi/include/infra/Common.hpp"

#include "bvsma/include/SchnorrSignature.hpp"
#include "bvsma/include/fileFunctions.hpp"

class Component;
class Signature;
class SchnorrSignature;

/* Print Functions */
void printOpenSSLECFpPoint(string namePoint, shared_ptr<GroupElement> elt);
void printVectIntMap(string nameMsg, map<int, vector<int>> vectIntMap);
void printVectBigintMap(string nameMsg, map<int, vector<biginteger>> vectBigintMap);
void printIntVect(string msg, vector<int> vectInt);
void printBigintVect(string nameMsg, vector<biginteger> vectBigint);
void printBoolVect(string nameMsg, vector<bool> vectBool);
void printByteVect(vector<byte> byteVect, string msg);

/* Convertion Functions */
vector<biginteger> toBigint(vector<shared_ptr<GroupElement>> vectGpElt);
vector<byte> gpEltVectToByteVect(vector<shared_ptr<GroupElement>> vectGpElt);
vector<byte> vectBigintToByteVect(vector<biginteger> vectBigint);
vector<biginteger> gpEltVectToBigintVect(vector<shared_ptr<GroupElement>> vectGpElt);
vector<shared_ptr<GroupElement>> strToVectGpElt(shared_ptr<OpenSSLDlogECFp> dlog, string str);

/* Permutation Functions */
vector<biginteger> vectPermutation(int seed, biginteger begin, biginteger end);
vector<int> vectIntPermutation(int seed, int begin, int end);

/* Generate Functions */
vector<biginteger> generateVectBigintFixed(biginteger mod, biginteger start, size_t len);
vector<biginteger> generateVectBigintRand(biginteger q, size_t len);
vector<shared_ptr<AsymmetricCiphertext>> generateVectElGamalCiphers(shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<ElGamalOnGroupElementEnc> elgamal, biginteger m, biginteger r, size_t len);
vector<shared_ptr<AsymmetricCiphertext>> generateVectElGamalCiphersFromVects(shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<ElGamalOnGroupElementEnc> elgamal, vector<biginteger> vectM, vector<biginteger> vectR);
vector<shared_ptr<AsymmetricCiphertext>> generateThresholdEncSet(int seed, shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<ElGamalOnGroupElementEnc> thresholdElgamal, biginteger T, size_t len);
biginteger generateChallenge(const vector<shared_ptr<GroupElement>>& vectGpElt, biginteger order);
biginteger generateHashForSign(const vector<biginteger>& bigintVect, const vector<shared_ptr<GroupElement>>& vectGpElt, biginteger order);
vector<int> generateSyntheticProbe(int seed, int tempDim, int colLen);
map<int, vector<int>> generateMapIntPermutation(int seed, size_t len, const vector<int>& vectBegin, const vector<int>& vectEnd);
map<int, vector<shared_ptr<AsymmetricCiphertext>>> generateCipherMapFixed(shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<ElGamalOnGroupElementEnc> elgamal, biginteger m, biginteger r, size_t rowLen, size_t colLen);
map<int, vector<Component>> generatePartialTemplate(biginteger userId, map<int, vector<int>> permutation, map<int, vector<shared_ptr<AsymmetricCiphertext>>> colEncMap, map<int, vector<shared_ptr<AsymmetricCiphertext>>> scoreEncMap);
map<int, vector<Component>> generateTemplate( SchnorrSignature schnorr, map<int, vector<Component>> partialTemplate);
map<int, vector<biginteger>> generateMapPermutation(int seed, size_t len, vector<biginteger> vectBegin, vector<biginteger> vectEnd);

/* Get and Extract Functions */
vector<shared_ptr<GroupElement>> getVectC1(vector<shared_ptr<AsymmetricCiphertext>> vectCipher);
vector<shared_ptr<GroupElement>> getVectC2(vector<shared_ptr<AsymmetricCiphertext>> vectCipher);
vector<int> getIndexCompFromPermutation(vector<int> vectInt, map<int,vector<int>> permutation);
vector<shared_ptr<AsymmetricCiphertext>> extractColEncFromTemp(vector<Component> vectComp);
vector<shared_ptr<AsymmetricCiphertext>> extractScoreEncFromTemp(vector<Component> vectComp);
vector<Component> pickSpecificComponents(vector<int> indexCompVect, map<int, vector<Component>> userTemplate);
vector<shared_ptr<AsymmetricCiphertext>> pickSpecificCiphers(vector<int> indexCompVect, map<int, vector<shared_ptr<AsymmetricCiphertext>>> userTemplate);

/* Verify Functions */
bool verifyVectCompSignatures(vector<Component> vectComp, SchnorrSignature schnorr);
vector<bool> verifyVectCompSignaturesVectBool(vector<Component> vectComp, SchnorrSignature schnorr);
bool verifyColEncSignatures(SchnorrSignature schnorr, vector<Component> vectComp);
bool verifyScoreEncSignatures(SchnorrSignature schnorr, vector<Component> vectComp);
bool areEqual(shared_ptr<GroupElement> elt1, shared_ptr<GroupElement> elt2);
biginteger hashToSignAndVerify(const vector<biginteger>& bigintVect, const vector<shared_ptr<GroupElement>>& vectGpElt, biginteger order);

/* ElGamal and Threshold ElGamal Functions */
map<int, vector<shared_ptr<AsymmetricCiphertext>>> encryptColIndices(shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<ElGamalOnGroupElementEnc> elgamal, vector<int> maxNFQ);
map<int, vector<shared_ptr<AsymmetricCiphertext>>> encryptRowsFromHELR(string dir, shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<ElGamalOnGroupElementEnc> elgamalTH, vector<int> vectQuantizedSample);
shared_ptr<AsymmetricCiphertext> inverseCipherElGamal(shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<AsymmetricCiphertext> cipher);
vector<shared_ptr<GroupElement>> partiallyDecryptVectCiphers(shared_ptr<ElGamalOnGroupElementEnc> elgamal, vector<shared_ptr<AsymmetricCiphertext>> vectCipher);
bool finalDecryptionANDMatch(shared_ptr<ElGamalOnGroupElementEnc> elgamalTH, vector<shared_ptr<AsymmetricCiphertext>> vectCipherBlinded, vector<shared_ptr<GroupElement>> vectPartialDec);
bool finalDecryptionANDMatchSH(shared_ptr<ElGamalOnGroupElementEnc> elgamalTH, vector<shared_ptr<AsymmetricCiphertext>> vectPartialDec);

/* Operation on Ciphers Functions */
vector<shared_ptr<AsymmetricCiphertext>> multEvenCipherVect(shared_ptr<ElGamalOnGroupElementEnc> elgamalTH, const vector<shared_ptr<AsymmetricCiphertext>>& vectCipher);
vector<shared_ptr<AsymmetricCiphertext>> multOddCipherVect(shared_ptr<ElGamalOnGroupElementEnc> elgamalTH, const vector<shared_ptr<AsymmetricCiphertext>>& vectCipher);
shared_ptr<AsymmetricCiphertext> multiplyCiphersOfVect(shared_ptr<ElGamalOnGroupElementEnc> elgamalTH, const vector<shared_ptr<AsymmetricCiphertext>>& vectCipher);
vector<shared_ptr<AsymmetricCiphertext>> subtractOneCipherFromCipherVect(shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<ElGamalOnGroupElementEnc> elgamal, vector<shared_ptr<AsymmetricCiphertext>> vectCipher, shared_ptr<AsymmetricCiphertext> cipher);
vector<shared_ptr<AsymmetricCiphertext>> inverseVectCipherElGamal(shared_ptr<OpenSSLDlogECFp> dlog, vector<shared_ptr<AsymmetricCiphertext>> vectCipher);
shared_ptr<AsymmetricCiphertext> subtractTwoCipher(shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<ElGamalOnGroupElementEnc> elgamal, shared_ptr<AsymmetricCiphertext> cipher1, shared_ptr<AsymmetricCiphertext> cipher2);
vector<shared_ptr<AsymmetricCiphertext>> subtractTwoCipherVect(shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<ElGamalOnGroupElementEnc> elgamal, vector<shared_ptr<AsymmetricCiphertext>> vectCipher1, vector<shared_ptr<AsymmetricCiphertext>> vectCipher2);
shared_ptr<AsymmetricCiphertext> blindCipherElGamal(shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<AsymmetricCiphertext> cipher, biginteger r);
vector<shared_ptr<AsymmetricCiphertext>> blindVectCipherElGamal(shared_ptr<OpenSSLDlogECFp> dlog, vector<shared_ptr<AsymmetricCiphertext>> vectCipher, vector<biginteger> rBlindVect);
vector<shared_ptr<AsymmetricCiphertext>> elgamalPartialDecryption(shared_ptr<ElGamalOnGroupElementEnc> elgamalTH, vector<shared_ptr<AsymmetricCiphertext>> vectCipher);

/* Send Functions */
void sendIntVect(shared_ptr<CommParty> channel, vector<int> index);
void sendBigint(shared_ptr<CommParty> channel, biginteger bigInt);
void sendVectBigint(shared_ptr<CommParty> channel, vector<biginteger> vecBigInt);
void sendGpElt(shared_ptr<CommParty> channel, shared_ptr<GroupElement> elt);
void sendVectGpElt(shared_ptr<CommParty> channel, vector<shared_ptr<GroupElement>> vectGpElt);
void sendElGamalPubKey(shared_ptr<CommParty> channel, shared_ptr<PublicKey> pubKey);
void sendElGamalCipher(shared_ptr<CommParty> channel, shared_ptr<AsymmetricCiphertext> cipher);
void sendVectElGamalCipher(shared_ptr<CommParty> channel, vector<shared_ptr<AsymmetricCiphertext>> vectCipher);
void sendMapElGamalCipher(shared_ptr<CommParty> channel, map<int, vector<shared_ptr<AsymmetricCiphertext>>> mapCipher);
void sendSignature(shared_ptr<CommParty> channel, Signature sig);
void sendComponent(shared_ptr<CommParty> channel, Component comp);
void sendVectComponent(shared_ptr<CommParty> channel, vector<Component> vectComp);
void sendMapOfVectComponent(shared_ptr<CommParty> channel, map<int, vector<Component>> mapOfVectComp);

/* Receive Functions */
vector<int> receiveIntVect(shared_ptr<CommParty> channel);
string receiveStr(shared_ptr<CommParty> channel);
biginteger receiveBigint(shared_ptr<CommParty> channel);
vector<biginteger> receiveVectBigint(shared_ptr<CommParty> channel) ;
shared_ptr<GroupElement> receiveGpElt(shared_ptr<CommParty> channel, shared_ptr<OpenSSLDlogECFp> dlog);
vector<shared_ptr<GroupElement>> receiveVectGpElt(shared_ptr<CommParty> channel, shared_ptr<OpenSSLDlogECFp> dlog);
shared_ptr<PublicKey> receiveElGamalPubKey(shared_ptr<CommParty> channel, shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<ElGamalOnGroupElementEnc> elgamal);
shared_ptr<AsymmetricCiphertext> receiveElGamalCipher(shared_ptr<CommParty> channel, shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<ElGamalOnGroupElementEnc> elgamal);
vector<shared_ptr<AsymmetricCiphertext>> receiveVectElGamalCipher(shared_ptr<CommParty> channel, shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<ElGamalOnGroupElementEnc> elgamal);
map<int, vector<shared_ptr<AsymmetricCiphertext>>> receiveMapElGamalCipher(shared_ptr<CommParty> channel, shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<ElGamalOnGroupElementEnc> elgamal);
Signature receiveSignature(shared_ptr<CommParty> channel);
Component receiveComponent(shared_ptr<CommParty> channel, shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<ElGamalOnGroupElementEnc> elgamal, shared_ptr<ElGamalOnGroupElementEnc> elgamalTH);
vector<Component> receiveVectComponent(shared_ptr<CommParty> channel, shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<ElGamalOnGroupElementEnc> elgamal, shared_ptr<ElGamalOnGroupElementEnc> elgamalTH);
map<int, vector<Component>> receiveMapOfVectComponent(shared_ptr<CommParty> channel, shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<ElGamalOnGroupElementEnc> elgamal, shared_ptr<ElGamalOnGroupElementEnc> elgamalTH);


// Chilitags
#include <chilitags/chilitags.hpp>
#include "chili_landmarks.h"

// OpenCV
// #include <opencv2/core/core.hpp>
// #include <opencv2/core/core_c.h>
#include <opencv2/highgui/highgui.hpp>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
// General
// #include <cmath>
// #include <cstring>
// #include <fstream>
// #include <iostream>
// #include <stdio.h>
// #include <string>
// #include <time.h>
#include <iostream>
#include <armadillo>
#include "highgui.h"
#include "imgproc.h"
using namespace std;
using namespace cv;
chili_landmarks::chili_landmarks()
{
}

chili_landmarks::~chili_landmarks()
{
}

void chili_landmarks::update(cv::Mat &cameraFrame, std::mutex &camframelock,
	arma::vec & bearpos, double &bearsize, bool &bearfound
	)
{
	// Paramaters for image aquisition
	int xRes = 640;
	int yRes = 480;
	int cameraIndex = 1;

	// Get camera feed
	cv::VideoCapture capture(cameraIndex);
	if (!capture.isOpened())
	{
		perror("Unable to initialise video capture.");
		exit(0);
	}

	// Set camera feed resolution
	capture.set(CV_CAP_PROP_FRAME_WIDTH, xRes);
	capture.set(CV_CAP_PROP_FRAME_HEIGHT, yRes);

	// Initialize variables to be used in loop
	cv::Mat inputImage;
	chilitags::Chilitags chilitags;
	// char tagInfo[256];

	// Window to display tracked chilitags
	//cv::namedWindow("DisplayChilitags");
	static const cv::Scalar COLOR(255, 0, 255);
	static const int SHIFT = 16;
	static const float PRECISION = 1<<SHIFT;

  const int h1 = 0;
  const int h2 = 100;
  const int s1 = 128;
  const int s2 = 255;
  const int v1 = 128;
  const int v2 = 255;
	arma::vec xbuf(10, arma::fill::zeros);
	arma::vec ybuf(10, arma::fill::zeros);
	arma::vec xcbuf(10, arma::fill::zeros);
	arma::vec ycbuf(10, arma::fill::zeros);
	int xind = 0, yind = 0;
	bool stable = false;

	// Main loop, exiting when 'q is pressed'
	for (int framecount = 0; 'q' != (char) cv::waitKey(1); framecount++)
	{
		// Read input image and parse for chilitags
		capture.read(inputImage);

		cv::Mat img = inputImage.clone();

		Mat hsv;
		cvtColor(img, hsv, COLOR_BGR2HSV);

		// in range masking
		Mat hsv2mask, hsv2;
		inRange(hsv, Scalar(h1, s1, v1), Scalar(h2, s2, v2), hsv2mask);
		//hsv.copyTo(hsv2, hsv2mask);

		arma::mat I;
		cvt_opencv2arma(hsv2mask, I);

		//blur
		//I = conv2(I, gauss2(7, 16));

		arma::rowvec r = arma::sum(I, 0);
		arma::colvec c = arma::sum(I, 1);
		double midx = arma::sum(r * arma::cumsum(arma::ones<arma::vec>(r.n_elem))) / arma::sum(r);
		double midy = arma::sum(c % arma::cumsum(arma::ones<arma::vec>(c.n_elem))) / arma::sum(c);
		stable = true;
		if (midx < 1 || (midx > (int)I.n_cols - 1) || arma::sum(r) == 0) {
			midx = 0;
		}
		if (midy < 1 || (midy > (int)I.n_rows - 1) || arma::sum(c) == 0) {
			midy = 0;
		}

		// remove noise
		if (midx == 0 || midy == 0) {
			stable = false;
		}
		xbuf[xind] = midx;
		ybuf[yind] = midy;
		midx = arma::mean(xbuf);
		midy = arma::mean(ybuf);
		if (sqrt(arma::var(xbuf) * arma::var(xbuf) + arma::var(ybuf) + arma::var(ybuf)) > 70.0) {
			stable = false;
		}

		Scalar color(255, 0, 0);
		//cout << "arma:: " << midx << ", " << midy << endl;
		//cout << "stable: " << stable << endl;
		circle(img, Point((int)midx, (int)midy), 4, color, 0);

		double covx = sqrt(arma::sum(r * arma::square(arma::cumsum(arma::ones<arma::vec>(r.n_elem)) - midx)) / arma::sum(r));
		double covy = sqrt(arma::sum(c % arma::square(arma::cumsum(arma::ones<arma::vec>(c.n_elem)) - midy)) / arma::sum(c));
		xcbuf[xind] = covx;
		ycbuf[yind] = covy;
		covx = arma::accu(xcbuf) / (int)xbuf.n_elem;
		covy = arma::accu(ycbuf) / (int)ybuf.n_elem;
		xind = (xind + 1) % (int)xbuf.n_elem;
		yind = (yind + 1) % (int)ybuf.n_elem;
		//double estw = covx * 3;
		//double esth = covy * 3;
		//blur the image to make it more accurate
		/*I = conv2(I, gauss2(7, 16));
		disp_image("convI", I);
		int minw = (int)midx;
		int maxw = (int)midx;
		for (int minw = (int)midx; minw >= 0; minw--) {
		  if (I(midy, minw) < 0.05) {
			break;
		  }
		}
		for (int maxw = (int)midx; maxw < 640; maxw++) {
		  if (I(midy, maxw) < 0.05) {
			break;
		  }
		}
		int minh = (int)midy;
		int maxh = (int)midy;
		for (int minh = (int)midy; minh >= 0; minh--) {
		  if (I(minh, midx) < 0.05) {
			break;
		  }
		}
		for (int maxh = (int)midy; maxh < 480; maxh++) {
		  if (I(maxh, midx) < 0.05) {
			break;
		  }
		}
		double covy = (double)(maxh - minh);
		double covx = (double)(maxw - minw);*/
		covx = (covx) * 3.3;
		covy = (covy) * 3.3;
		bearpos = arma::vec({ midx - 320, 479 - (midy - 240) });
		bearsize = 533 / (covx) * 12;
		bearfound = stable;

		rectangle(img, Rect(midx-covx/2,midy-covy/2,covx,covy), Scalar(0, 0, 255), 2);

		imshow("oldhsv", hsv);
		imshow("newhsv", hsv2mask);
		imshow("img", img);

    // put the image in ipcdb.h
    //camframelock.lock();
    //cameraFrame = inputImage;
    //camframelock.unlock();

		// Start time for processing
		// int64 startTime = cv::getTickCount();

		// Process chilitags
		chilitags::TagCornerMap detected_tags = chilitags.find(inputImage);

		// Measure the processing time needed for the detection
		// int64 endTime = cv::getTickCount();
		// float processingTime = 1000.0f*((float) endTime - startTime)/cv::getTickFrequency();

		// Reset tagInfo string
	    // std::memset(tagInfo, 0, 256);

	    // Clone input image to label chilitags on & display
		cv::Mat outputImage = inputImage.clone();

		for (int i=0; i<1024; i++)
		{
			this->tags[i][0] = 0;
		}

		for (const std::pair<int, chilitags::Quad> & tag : detected_tags)
		{
			// Get ID and corner points of chilitag
			int id = tag.first;
			const cv::Mat_<cv::Point2f> corners(tag.second);

			// Draw outline around detected chilitag
			// for (size_t i = 0; i < 4; ++i)
			// {
				// cv::line(outputImage, PRECISION*corners(i), PRECISION*corners((i+1)%4), COLOR, 1, CV_AA, SHIFT);
			// }

			// Calculate center based on two corner points
			cv::Point2f center = 0.5f*(corners(0) + corners(2));

			// Calculate x & y in local reference frame
			// Tag size = 9 inches (W)
			// Distance = 24 inches (D)
			// Pixel diagonal width = 200 px (P)
			// Focal length = F = (P x D) / W = 533.3333

			double Z = 60.0; // inches to the ceiling

			double delta_x = center.x - (xRes/2);
			double delta_y = center.y - (yRes/2);

			double u1 = corners(0).x - delta_x;
			double u2 = corners(1).x - delta_x;
			double u3 = corners(2).x - delta_x;
			double u4 = corners(3).x - delta_x;

			double v1 = corners(0).y - delta_y;
			double v2 = corners(1).y - delta_y;
			double v3 = corners(2).y - delta_y;
			double v4 = corners(3).y - delta_y;

			double x1 = u1 * Z / 533.333;
			double x2 = u2 * Z / 533.333;
			double x3 = u3 * Z / 533.333;
			double x4 = u4 * Z / 533.333;

			double y1 = v1 * Z / 533.333;
			double y2 = v2 * Z / 533.333;
			double y3 = v3 * Z / 533.333;
			double y4 = v4 * Z / 533.333;

			///////// AFTER THIS IS GOOD //////////

			double x_squared = pow(abs(corners(0).x - corners(2).x), 2);
			double y_squared = pow(abs(corners(0).y - corners(2).y), 2);
			double diagonal_size = sqrt(x_squared + y_squared);

			double distance_inches = 9 * 533.33 / diagonal_size;
			double x_inches = delta_x * distance_inches / 533.33;
			double y_inches = -delta_y * distance_inches / 533.33;
			//double z_inches = sqrt(pow(distance_inches, 2) - (pow(x_inches, 2) + pow(y_inches, 2)));

			this->tags[id][0] = 1;
			this->tags[id][1] = x_inches;
			this->tags[id][2] = y_inches;
			this->tags[id][3] = distance_inches;

			///////// BEFORE THIS IS GOOD //////////

			// printf("id: %d\t", id);
			// printf("x: %0.3f\t\t", tags[id][1]);
			// printf("y: %0.3f\n", tags[id][2]);

			// Write tagInfo to center of tag in displayed window
			// sprintf(tagInfo, "id: %d x:%2.2f y:%2.2f", id, x_inches, y_inches);
			// cv::putText(outputImage, tagInfo, center, cv::FONT_HERSHEY_SIMPLEX, 0.5, CV_RGB(0,255,0), 2.0);
		}

		// Write stats on the current frame (resolution and processing time) to display window
        // cv::putText(outputImage, cv::format("%dx%d %4.0f ms (press q to quit)", outputImage.cols, outputImage.rows, processingTime), cv::Point(32,32), cv::FONT_HERSHEY_SIMPLEX, 0.5f, COLOR);

        // cv::circle(outputImage, cv::Point(320, 240), 15, (0, 0, 0));

        // Display chilitags window
        //cv::imshow("DisplayChilitags", outputImage);
	}

	// Release all used resources
	capture.release();
	//cv::destroyWindow("DisplayChilitags");
}

// int main()
// {
//     // std::thread detect(update);
//     // detect.join();
// }