/* 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/gui/gui_base.hpp>
#include <tdp/gui/quickView.h>
#include <tdp/icp/icp.h>
#include <tdp/manifold/SE3.h>
#include <tdp/nvidia/helper_cuda.h>
#include <tdp/preproc/convolutionSeparable.h>
#include <tdp/preproc/depth.h>
#include <tdp/preproc/normals.h>
#include <tdp/preproc/pc.h>
#include <tdp/tsdf/tsdf.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/marching_cubes/marching_cubes.h>

#include <tdp/gl/render.h>

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

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]) : "";
  }

  // 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();

  Eigen::Matrix3f R_ir;
  R_ir << 0, 0,-1,
       0,-1, 0,
       -1, 0, 0;
  tdp::SE3f T_ir(R_ir,Eigen::Vector3f::Zero());

  tdp::GuiBase gui(1200,800,video);
  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 = h+h%64;
  float f = 550;
  float uc = (w-1.)/2.;
  float vc = (h-1.)/2.;

  size_t dTSDF = 512;
  size_t wTSDF = 512;
  size_t hTSDF = 512;

  CameraT camR(Eigen::Vector4f(f,f,uc,vc));
  CameraT camD(Eigen::Vector4f(f,f,uc,vc));

  if (calibPath.size() > 0) {
    tdp::Rig<CameraT> rig;
    rig.FromFile(calibPath,false);
    std::vector<pangolin::VideoInterface*>& streams = video.InputStreams();
    rig.CorrespondOpenniStreams2Cams(streams);
    // camera model for computing point cloud and normals
    camR = rig.cams_[rig.rgbStream2cam_[0]];
    camD = camR; //rig.cams_[rig.dStream2cam_[0]];
    if (rig.T_ris_.size() > 0) T_ir = rig.T_ris_[0];
  }

  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& viewN3D = pangolin::CreateDisplay()
    .SetHandler(new pangolin::Handler3D(s_cam));
  gui.container().AddDisplay(viewN3D);

  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::ManagedDeviceImage<tdp::Vector3bda> cuRgb(wc,hc);
  tdp::ManagedDeviceImage<float> cuGrey(wc,hc);
  tdp::ManagedDeviceImage<float> cuGreydv(wc,hc);
  tdp::ManagedDeviceImage<float> cuGreydu(wc,hc);
  tdp::ManagedDeviceImage<tdp::Vector3fda> cuGrad3D(wc,hc);

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

  tdp::ManagedDeviceImage<tdp::Vector3bda> cuRGBraw(w, h);
  tdp::ManagedDeviceImage<tdp::Vector3bda> cuRGB(w, h);

  tdp::ManagedHostVolume<tdp::TSDFval> TSDF(wTSDF, hTSDF, dTSDF);
  TSDF.Fill(tdp::TSDFval(-1.01,0.));
  tdp::ManagedDeviceVolume<tdp::TSDFval> cuTSDF(wTSDF, hTSDF, dTSDF);
  cuTSDF.CopyFrom(TSDF);

  tdp::ManagedHostImage<float> dEst(wc, hc);
//  tdp::ManagedDeviceImage<float> cuDEst(wc, hc);
//  dEst.Fill(0.);
//  tdp::CopyImage(dEst, cuDEst, cudaMemcpyHostToDevice);
  tdp::ManagedDeviceImage<float> cuDView(wc, hc);
  tdp::ManagedDeviceImage<tdp::Vector3fda> cuPcView(wc, hc);

  // ICP stuff
  tdp::ManagedHostPyramid<float,3> dPyr(wc,hc);
  tdp::ManagedHostPyramid<float,3> dPyrEst(wc,hc);
  tdp::ManagedDevicePyramid<float,3> cuDPyr(wc,hc);
  tdp::ManagedDevicePyramid<float,3> cuDPyrEst(wc,hc);
  tdp::ManagedDevicePyramid<tdp::Vector3fda,3> pcs_m(wc,hc);
  tdp::ManagedDevicePyramid<tdp::Vector3fda,3> pcs_c(wc,hc);
  tdp::ManagedDevicePyramid<tdp::Vector3fda,3> ns_m(wc,hc);
  tdp::ManagedDevicePyramid<tdp::Vector3fda,3> ns_c(wc,hc);
  tdp::ManagedDevicePyramid<tdp::Vector3fda,3> gs_m(wc,hc);
  tdp::ManagedDevicePyramid<tdp::Vector3fda,3> gs_c(wc,hc);
  tdp::ManagedDeviceImage<float> cuICPassoc_m(wc, hc);
  tdp::ManagedDeviceImage<float> cuICPassoc_c(wc, hc);
  tdp::ManagedHostImage<float> ICPassoc_m(wc, hc);
  tdp::ManagedHostImage<float> ICPassoc_c(wc, hc);

  tdp::ManagedDeviceImage<float> cuPcErr(wc, hc);
  tdp::ManagedDeviceImage<float> cuAngErr(wc, hc);
  tdp::ManagedHostImage<float> pcErr(wc, hc);
  tdp::ManagedHostImage<float> angErr(wc, hc);

  tdp::ManagedDeviceImage<tdp::Vector3fda> nEstdummy(wc,hc);

  pangolin::GlBufferCudaPtr cuPcbuf(pangolin::GlArrayBuffer, wc*hc,
      GL_FLOAT, 3, cudaGraphicsMapFlagsNone, GL_DYNAMIC_DRAW);

  tdp::ManagedHostImage<float> tsdfDEst(wc, hc);
  tdp::ManagedHostImage<float> tsdfSlice(wTSDF, hTSDF);
//  tdp::QuickView viewTsdfDEst(wc,hc);
//  gui.container().AddDisplay(viewTsdfDEst);

  tdp::ManagedHostImage<float> dispDepthPyr(dPyr.Width(0)+dPyr.Width(1), hc);
  tdp::QuickView viewDepthPyr(dispDepthPyr.w_,dispDepthPyr.h_);
  gui.container().AddDisplay(viewDepthPyr);

  tdp::ManagedDeviceImage<tdp::Vector3fda> cuDispNormalsPyr(ns_m.Width(0)+ns_m.Width(1), hc);
  tdp::ManagedDeviceImage<tdp::Vector3bda> cuDispNormals2dPyr(ns_m.Width(0)+ns_m.Width(1), hc);
  tdp::ManagedHostImage<tdp::Vector3bda> dispNormals2dPyr(ns_m.Width(0)+ns_m.Width(1), hc);

  tdp::QuickView viewNormalsPyr(dispNormals2dPyr.w_,dispNormals2dPyr.h_);
  gui.container().AddDisplay(viewNormalsPyr);

  tdp::QuickView viewICPassocM(wc,hc);
  gui.container().AddDisplay(viewICPassocM);
  tdp::QuickView viewICPassocC(wc,hc);
  gui.container().AddDisplay(viewICPassocC);

  tdp::QuickView viewAngErr(wc,hc);
  gui.container().AddDisplay(viewAngErr);
  tdp::QuickView viewPcErr(wc,hc);
  gui.container().AddDisplay(viewPcErr);

  tdp::QuickView viewTsdfSliveView(wTSDF,hTSDF);
  gui.container().AddDisplay(viewTsdfSliveView);

  pangolin::GlBuffer meshVbo;
  pangolin::GlBuffer meshCbo;
  pangolin::GlBuffer meshIbo;

  viewICPassocC.Show(true);
  viewICPassocM.Show(true);
  viewAngErr.Show(true);
  viewPcErr.Show( true);

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

  pangolin::Var<bool> dispNormalsPyrEst("ui.disp normal est", false, true);
//  pangolin::Var<bool> dispDepthPyrEst("ui.disp d pyr est", false,true);

  pangolin::Var<bool> runFusion("ui.run Fusion",true,true);

  pangolin::Var<bool>  resetTSDF("ui.reset TSDF", false, false);
  pangolin::Var<bool>  saveTSDF("ui.save TSDF", false, false);
  pangolin::Var<bool> fuseTSDF("ui.fuse TSDF",true,true);
  pangolin::Var<bool> fuseRgb("ui.fuse RGB",true,true);
  pangolin::Var<bool> normalsFromTSDF("ui.normals from TSDF",true,true);
  pangolin::Var<bool> pcFromTSDF("ui.pc from TSDF", true, true);
  pangolin::Var<bool> normalsFromDepthPyr("ui.n from depth pyr",false,true);

  pangolin::Var<float> tsdfMu("ui.mu",0.05,0.01,0.5);
  pangolin::Var<float> tsdfWThr("ui.w thr",25.,1.,20.);
  pangolin::Var<float> tsdfWMax("ui.w max",200.,1.,300.);
  pangolin::Var<int>   tsdfSliceD("ui.TSDF slice D",dTSDF/2,0,dTSDF-1);
  pangolin::Var<float> grid0x("ui.grid0 x",-3.0,-2.,0);
  pangolin::Var<float> grid0y("ui.grid0 y",-3.0,-2.,0);
  pangolin::Var<float> grid0z("ui.grid0 z",-3.0,-2.,0);
  pangolin::Var<float> gridEx("ui.gridE x", 3.0,2,3);
  pangolin::Var<float> gridEy("ui.gridE y", 3.0,2,3);
  pangolin::Var<float> gridEz("ui.gridE z", 3.0,2,3);

  pangolin::Var<bool>  runMarchingCubes("ui.run Marching Cubes", false, false);
  pangolin::Var<float> marchCubesfThr("ui.f Thr", 1.0,0.,1.);
  pangolin::Var<float> marchCubeswThr("ui.weight Thr", 0,0,10);

  pangolin::Var<bool> resetICP("ui.reset ICP",false,false);
  pangolin::Var<bool>  runICP("ui.run ICP", true, true);
  pangolin::Var<float> icpAngleThr_deg("ui.icp angle thr",15,0.,90.);
  pangolin::Var<float> icpDistThr("ui.icp dist thr",0.10,0.,1.);
  pangolin::Var<int>   icpIter0("ui.ICP iter lvl 0",10,0,10);
  pangolin::Var<int>   icpIter1("ui.ICP iter lvl 1",7,0,10);
  pangolin::Var<int>   icpIter2("ui.ICP iter lvl 2",5,0,10);

  pangolin::Var<bool>  icpImu("ui.use IMU to warm start ICP", false, true);

  pangolin::Var<bool> showIcpError("ui.show ICP",true,true);
  pangolin::Var<int>   icpErrorLvl("ui.ICP error vis lvl",0,0,2);

  pangolin::Var<bool> showPcModel("ui.show model",true,true);
  pangolin::Var<bool> showPcCurrent("ui.show current",true,true);
  pangolin::Var<bool> showPcView("ui.show overview",true,true);

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

  tdp::SE3f T_mo(Eigen::Matrix4f::Identity());
//  T_mo.rotation() = tdp::SO3f::Rz(M_PI/2.f);
  tdp::SE3f T_mo_0 = T_mo;
  tdp::SE3f T_mo_prev = T_mo_0;
//  tdp::SE3f T_wr_imu_prev;
  size_t numFused = 0;

  tdp::SE3f T_wG;
  std::vector<tdp::SE3f> T_mos;

  tdp::Vector3fda grid0(grid0x,grid0y,grid0z);
  tdp::Vector3fda gridE(gridEx,gridEy,gridEz);
  tdp::Vector3fda dGrid = gridE - grid0;

  tdp::ThreadedValue<bool> runWorker(true);
  std::thread workThread([&]() {
        while(runWorker.Get()) {
          if (pangolin::Pushed(saveTSDF)) {
            tdp::ManagedHostVolume<tdp::TSDFval> tmpTSDF(wTSDF, hTSDF, dTSDF);
            tmpTSDF.CopyFrom(cuTSDF, cudaMemcpyDeviceToHost);
            std::cout << "start writing TSDF to " << tsdfOutputPath << std::endl;
            tdp::TSDF::SaveTSDF(tmpTSDF, grid0, dGrid, T_wG, tsdfOutputPath);
            std::cout << "done writing TSDF to " << tsdfOutputPath << std::endl;
          }
          std::this_thread::sleep_for(std::chrono::microseconds(100));
        }
      });

  gui.verbose = true;
  // Stream and display video
  while(!pangolin::ShouldQuit())
  {
    if (!normalsFromTSDF) pcFromTSDF = false;
    if (runFusion.GuiChanged() && !runFusion) {
      T_mo_0 = tdp::SE3f();
      T_mo = T_mo_0;
      T_mo_prev = T_mo_0;
    }
    grid0 = tdp::Vector3fda (grid0x,grid0y,grid0z);
    gridE = tdp::Vector3fda (gridEx,gridEy,gridEz);
    dGrid = gridE - grid0;
    dGrid(0) /= (wTSDF-1);
    dGrid(1) /= (hTSDF-1);
    dGrid(2) /= (dTSDF-1);

    if (pangolin::Pushed(runMarchingCubes)) {
      TSDF.CopyFrom(cuTSDF);
      tdp::ComputeMesh(TSDF, grid0, dGrid,
          T_wG, meshVbo, meshCbo, meshIbo, marchCubeswThr, marchCubesfThr);
    }

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

    gui.NextFrames();

    tdp::Image<uint16_t> dRaw;
    tdp::Image<tdp::Vector3bda> rgbRaw;
    int64_t t_host_us_d = 0;
    int64_t t_host_us_rgb = 0;
    if (!gui.ImageD(dRaw, 0, &t_host_us_d) ||
        !gui.ImageRGB(rgbRaw, 0, &t_host_us_rgb)) {
      continue;
    }

//    tdp::SE3f T_wr_imu = T_ir.Inverse() * imuInterp.Ts_wi_[t_host_us_d * 1000] * T_ir;
//    std::cout << " depth frame at " << t_host_us_d << " us" << std::endl;

    if (gui.verbose) std::cout << "setup pyramids" << std::endl;
    TICK("Setup Pyramids");
    cuDraw.CopyFrom(dRaw);
    ConvertDepthGpu(cuDraw, cuD, depthSensorScale, tsdfDmin, tsdfDmax);
    cuRGBraw.CopyFrom(rgbRaw);

    // construct pyramid
    tdp::ConstructPyramidFromImage<float,3>(cuD, cuDPyr, 0.03);
    tdp::Depth2PCsGpu(cuDPyr,camD,pcs_c);
    // compute normals
    if (normalsFromDepthPyr) {
      tdp::Depth2NormalsViaDerivativePyr(cuDPyr,camD,ns_c);
    } else {
      tdp::Depth2Normals(cuDPyr,camD,ns_c);
    }

    if (!gui.ImageRGB(rgb,0)) continue;
    cuRgb.CopyFrom(rgb);
    tdp::Rgb2Grey(cuRgb,cuGrey);
    TOCK("Setup Pyramids");

//    if (icpImu && imu)
//      T_mo = (T_wr_imu * T_wr_imu_prev.Inverse()) * T_mo;
    if (runICP && (!runFusion || numFused > 30)) {
      if (gui.verbose) std::cout << "icp" << std::endl;
      TICK("ICP");
      std::vector<size_t> maxIt{icpIter0,icpIter1,icpIter2};
      tdp::ICP::ComputeProjective(pcs_m, ns_m, pcs_c, ns_c, T_mo,
          tdp::SE3f(), camD, maxIt, icpAngleThr_deg, icpDistThr,
          gui.verbose);
      TOCK("ICP");
//      std::cout << "T_mo update: " << std::endl
//        << T_mo * T_mo_prev.Inverse() << std::endl;
//      std::cout << "IMU : " << std::endl
//        << T_wr_imu * T_wr_imu_prev.Inverse() << std::endl;
    }
//    std::cout << "T_mo after ICP: " << std::endl
//      << T_mo  << std::endl;
    T_mos.push_back(T_mo);

    if (showIcpError) {
      tdp::Image<tdp::Vector3fda> pc_m = pcs_m.GetImage(icpErrorLvl);
      ICPassoc_c.Reinitialise(pc_m.w_, pc_m.h_);
      ICPassoc_m.Reinitialise(pc_m.w_, pc_m.h_);
      cuICPassoc_c.Reinitialise(pc_m.w_, pc_m.h_);
      cuICPassoc_m.Reinitialise(pc_m.w_, pc_m.h_);
      ICPassoc_c.Fill(-1.);
      ICPassoc_m.Fill(-1.);
      cuICPassoc_c.CopyFrom(ICPassoc_c);
      cuICPassoc_m.CopyFrom(ICPassoc_m);
      tdp::ICPVisualizeAssoc(pc_m, ns_m.GetImage(icpErrorLvl),
        pcs_c.GetImage(icpErrorLvl), ns_c.GetImage(icpErrorLvl), T_mo,
        tdp::ScaleCamera<float>(camD,pow(0.5,icpErrorLvl)),
        icpAngleThr_deg, icpDistThr, cuICPassoc_m,
        cuICPassoc_c);
    }

    if (runFusion && (fuseTSDF || numFused <= 30)) {
      if (gui.verbose) std::cout << "add to tsdf" << std::endl;
      TICK("Add To TSDF");
      if (fuseRgb) {
        tdp::TSDF::AddToTSDF(cuTSDF, cuD, cuRGBraw, T_mo, camD, grid0,
            dGrid, tsdfMu, tsdfWMax);
      } else {
        tdp::TSDF::AddToTSDF(cuTSDF, cuD, T_mo, camD, grid0, dGrid,
          tsdfMu, tsdfWMax);
      }
      numFused ++;
      TOCK("Add To TSDF");
    }

    if (runFusion) {
      if (gui.verbose) std::cout << "ray trace" << std::endl;
      TICK("Ray Trace TSDF");
      // first one not needed anymore
      tdp::TSDF::RayTraceTSDF(cuTSDF, pcs_m.GetImage(0),
            ns_m.GetImage(0), T_mo, camD, grid0, dGrid, tsdfMu, tsdfWThr);
      // get pc in model coordinate system
      tdp::CompletePyramid<tdp::Vector3fda,3>(pcs_m);
      TOCK("Ray Trace TSDF");
      if (normalsFromTSDF) {
        tdp::CompleteNormalPyramid<3>(ns_m);
      } else {
        if (normalsFromDepthPyr) {
          tdp::Depth2NormalsViaDerivativePyr(cuDPyrEst,camD,ns_m);
        } else {
          tdp::Depth2Normals(cuDPyrEst,camD,ns_m);
        }
      }
    }

    if (pangolin::Pushed(resetTSDF)) {
      TSDF.Fill(tdp::TSDFval(-1.01,0.));
      dEst.Fill(0.);
      cuTSDF.CopyFrom(TSDF);
      numFused = 0;
      T_mo = T_mo_0;
      T_mo_prev = T_mo;
      std::cout << "resetting ICP and TSDF" << std::endl;
    }
    if (pangolin::Pushed(resetICP)) {
      T_mo = T_mo_0;
      T_mo_prev = T_mo;
      std::cout << "resetting ICP" << std::endl;
    }

    if (gui.verbose) std::cout << "draw 3D" << std::endl;

    TICK("Draw 3D");
    // Render point cloud from viewpoint of origin
    if (showPcView) {
      tdp::SE3f T_mv;
      tdp::TSDF::RayTraceTSDF(cuTSDF, cuDView, nEstdummy, T_mv, camView, grid0,
          dGrid, tsdfMu, tsdfWThr);
      tdp::Depth2PCGpu(cuDView,camView,cuPcView);
    }

    glEnable(GL_DEPTH_TEST);
    if (viewPc3D.IsShown()) {
      viewPc3D.Activate(s_cam);

      Eigen::AlignedBox3f box(grid0,gridE);
      glColor4f(1,0,0,0.5f);
      pangolin::glDrawAlignedBox(box);

      // imu
//      pangolin::glDrawAxis(T_wr_imu.matrix(),0.2f);

      for (auto& T_moi : T_mos)
        pangolin::glDrawAxis(T_moi.matrix(),0.1f);

      tdp::RenderVboIboCbo(meshVbo, meshIbo, meshCbo);

      // render global view of the model first
      pangolin::glDrawAxis(0.1f);
      if (showPcView) {
        {
          pangolin::CudaScopedMappedPtr cuPcbufp(cuPcbuf);
          cudaMemset(*cuPcbufp,0,hc*wc*sizeof(tdp::Vector3fda));
          cudaMemcpy(*cuPcbufp, cuPcView.ptr_, cuPcView.SizeBytes(),
              cudaMemcpyDeviceToDevice);
        }
        glColor3f(0,0,1);
        pangolin::RenderVbo(cuPcbuf);
      }
      // render model and observed point cloud
      if (showPcModel) {
        {
          pangolin::CudaScopedMappedPtr cuPcbufp(cuPcbuf);
          cudaMemset(*cuPcbufp,0,hc*wc*sizeof(tdp::Vector3fda));
          tdp::Image<tdp::Vector3fda> pc0 = pcs_m.GetImage(icpErrorLvl);
          cudaMemcpy(*cuPcbufp, pc0.ptr_, pc0.SizeBytes(),
              cudaMemcpyDeviceToDevice);
        }
        glColor3f(0,1,0);
        pangolin::RenderVbo(cuPcbuf);
      }
      // render current camera second in the propper frame of
      // reference
      pangolin::glSetFrameOfReference(T_mo.matrix());
      if (showPcCurrent) {
        {
          pangolin::CudaScopedMappedPtr cuPcbufp(cuPcbuf);
          cudaMemset(*cuPcbufp,0,hc*wc*sizeof(tdp::Vector3fda));
          tdp::Image<tdp::Vector3fda> pc0 = pcs_c.GetImage(icpErrorLvl);
          cudaMemcpy(*cuPcbufp, pc0.ptr_, pc0.SizeBytes(),
              cudaMemcpyDeviceToDevice);
        }
        glColor3f(1,0,0);
        pangolin::RenderVbo(cuPcbuf);
      }
      pangolin::glUnsetFrameOfReference();
    }

    if (viewN3D.IsShown()) {
      viewN3D.Activate(s_cam);
      // render global view of the model first
      pangolin::glDrawAxis(0.1f);
      if (showPcView) {
        {
          pangolin::CudaScopedMappedPtr cuPcbufp(cuPcbuf);
          cudaMemset(*cuPcbufp,0,hc*wc*sizeof(tdp::Vector3fda));
          cudaMemcpy(*cuPcbufp, nEstdummy.ptr_, nEstdummy.SizeBytes(),
              cudaMemcpyDeviceToDevice);
        }
        glColor3f(0,0,1);
        pangolin::RenderVbo(cuPcbuf);
      }
      // render model and observed point cloud
      if (showPcModel) {
        {
          pangolin::CudaScopedMappedPtr cuPcbufp(cuPcbuf);
          cudaMemset(*cuPcbufp,0,hc*wc*sizeof(tdp::Vector3fda));
          tdp::Image<tdp::Vector3fda> pc0 = ns_m.GetImage(icpErrorLvl);
          cudaMemcpy(*cuPcbufp, pc0.ptr_, pc0.SizeBytes(),
              cudaMemcpyDeviceToDevice);
        }
        glColor3f(0,1,0);
        pangolin::RenderVbo(cuPcbuf);
      }
      Eigen::Matrix4f R_mo = T_mo.matrix();
      R_mo.topRightCorner(3,1).fill(0.);
      pangolin::glSetFrameOfReference(R_mo);
      // render current camera second in the propper frame of
      // reference
      pangolin::glDrawAxis(0.1f);
      if (showPcCurrent) {
        {
          pangolin::CudaScopedMappedPtr cuPcbufp(cuPcbuf);
          cudaMemset(*cuPcbufp,0,hc*wc*sizeof(tdp::Vector3fda));
          tdp::Image<tdp::Vector3fda> pc0 = ns_c.GetImage(icpErrorLvl);
          cudaMemcpy(*cuPcbufp, pc0.ptr_, pc0.SizeBytes(),
              cudaMemcpyDeviceToDevice);
        }
        glColor3f(1,0,0);
        pangolin::RenderVbo(cuPcbuf);
      }
      pangolin::glUnsetFrameOfReference();
    }
    TOCK("Draw 3D");

    TICK("Draw 2D");
    glLineWidth(1.5f);
    glDisable(GL_DEPTH_TEST);

//    if (viewTsdfDEst.IsShown()) {
//      tsdfDEst.CopyFrom(cuDEst);
//      viewTsdfDEst.SetImage(tsdfDEst);
//    }

    if (viewTsdfSliveView.IsShown()) {
      tdp::Image<tdp::TSDFval> cuTsdfSlice =
        cuTSDF.GetImage(std::min((int)cuTSDF.d_-1,tsdfSliceD.Get()));
      tdp::ManagedHostImage<tdp::TSDFval> tsdfSliceRaw(cuTsdfSlice.w_,
          cuTsdfSlice.h_);
      tsdfSliceRaw.CopyFrom(cuTsdfSlice);
      for (size_t i=0; i<tsdfSliceRaw.Area(); ++i)
        tsdfSlice[i] = tsdfSliceRaw[i].f;
      viewTsdfSliveView.SetImage(tsdfSlice);
    }

    if (viewDepthPyr.IsShown()) {
//      if (dispDepthPyrEst) {
//        tdp::PyramidToImage<float,3>(cuDPyrEst,dispDepthPyr,
//            cudaMemcpyDeviceToHost);
//      } else {
        tdp::PyramidToImage<float,3>(cuDPyr,dispDepthPyr);
//      }
      viewDepthPyr.SetImage(dispDepthPyr);
    }

    if (viewNormalsPyr.IsShown()) {
      if (dispNormalsPyrEst) {
        tdp::PyramidToImage<tdp::Vector3fda,3>(ns_m,cuDispNormalsPyr);
      } else {
        tdp::PyramidToImage<tdp::Vector3fda,3>(ns_c,cuDispNormalsPyr);
      }
      tdp::Normals2Image(cuDispNormalsPyr, cuDispNormals2dPyr);
      dispNormals2dPyr.CopyFrom(cuDispNormals2dPyr);
      viewNormalsPyr.SetImage(dispNormals2dPyr);
    }

    if (showIcpError) {
      if (viewICPassocM.IsShown()) {
        ICPassoc_m.CopyFrom(cuICPassoc_m);
        viewICPassocM.SetImage(ICPassoc_m);
      }
      if (viewICPassocC.IsShown()) {
        ICPassoc_c.CopyFrom(cuICPassoc_c);
        viewICPassocC.SetImage(ICPassoc_c);
      }
    }

    if (viewPcErr.IsShown()) {
      tdp::L2Distance(pcs_m.GetImage(0), pcs_c.GetImage(0), T_mo, cuPcErr);
      pcErr.CopyFrom(cuPcErr);
      viewPcErr.SetImage(pcErr);
    }
    if (viewAngErr.IsShown()) {
      tdp::AngularDeviation(ns_m.GetImage(0), ns_c.GetImage(0),
          T_mo, cuAngErr);
      angErr.CopyFrom(cuAngErr);
      viewAngErr.SetImage(angErr);
    }
    TOCK("Draw 2D");

    if (!runFusion) {
      for (size_t lvl=0; lvl<3; ++lvl) {
        tdp::Image<tdp::Vector3fda> pc = pcs_c.GetImage(lvl);
        tdp::Image<tdp::Vector3fda> n = ns_c.GetImage(lvl);
        tdp::Image<tdp::Vector3fda> g = gs_c.GetImage(lvl);
        tdp::TransformPc(T_mo, pc);
        tdp::TransformPc(T_mo.rotation(), n);
        tdp::TransformPc(T_mo.rotation(), g);
      }
      pcs_m.CopyFrom(pcs_c);
      ns_m.CopyFrom(ns_c);
      gs_m.CopyFrom(gs_c);
    }
    if (!gui.paused()) {
//      T_wr_imu_prev = T_wr_imu;
      T_mo_prev = T_mo;
    }

    // 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();
  }
  runWorker.Set(false);
  workThread.join();
  imuInterp.Stop();
  if (imu) imu->Stop();
  delete imu;
  return 0;
}


//######################################################################
//#   SDF_Fusion Module
//#
//#   Copyright (C) 2020 Siemens AG
//#   SPDX-License-Identifier: MIT
//#   Author 2020: This module has been developed by
//#                or under supervision of Slobodan Ilic
//#######################################################################

#ifndef TYPES_HPP
#define TYPES_HPP

#include <iostream>
#include <sstream>
#include <string>
#include <memory>

#include <Eigen/Dense>
#include <Eigen/LU>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

template <typename T>
struct array_deleter {
    void operator() (T const* p) {
        _mm_free((void*)p);
    }
};

template <typename T> void init( std::shared_ptr<T> data, T const val, int size );
template <> void init<float>( std::shared_ptr<float> data, float val, int size );
template <> void init<Eigen::Vector3f>( std::shared_ptr<Eigen::Vector3f> data, Eigen::Vector3f val, int size );

template <typename T>
struct voxel_cube {
    int size_x, size_y, size_z;
    float voxel_size;
    T default_value;
    //std::vector<T> data;
	std::shared_ptr<T> data;
    //std::valarray<T> data;

    voxel_cube(){}

    voxel_cube(int sx, int sy, int sz, float voxel_s, T const default_v) :
        size_x(sx), size_y(sy), size_z(sz), voxel_size(voxel_s), default_value(default_v) {
        //data.resize(sx * sy * sz, default_v);
		data = std::shared_ptr<T>( (T*) _mm_malloc( sx*sy*sz*sizeof(T), 16 ), array_deleter<T>() );
        if ( data == NULL ) std::cerr << "MEMORY ALLOCATION FAILED" << std::endl;
        init(data,default_v,sx*sy*sz);
    }

    //voxel_cube(voxel_cube<T> const& other) :
    //    size_x(other.size_x), size_y(other.size_y), size_z(other.size_z),
    //    voxel_size(other.voxel_size), default_value(other.default_value),data(other.data) {}

	inline T& at(int x, int y, int z) {return data.get()[x + size_x * y + size_x * size_y * z];}
    inline T at(int x, int y, int z) const {return data.get()[x + size_x * y + size_x * size_y * z];}

    inline T& at(int idx) {return data.get()[idx];}
    inline T at(int idx) const {return data.get()[idx];}

    operator std::string() const {
        std::stringstream out;
        for (T t : data) {
            out << t << " ";
        }
        return out.str();
    }

};

template <typename T>
voxel_cube<T> operator+( const voxel_cube<T>& left, const voxel_cube<T>& right ) {
    voxel_cube<T> result = left;
    for (int z = 0; z < result.size_z; ++z)
        for (int y = 0; y < result.size_y; ++y)
            for (int x = 0; x < result.size_x; ++x)
                result.at(x,y,z) += right.at(x,y,z);
    return result;
}

template <typename T>
void operator+= ( voxel_cube<T>& left, const voxel_cube<T>& right ) {
    for (int z = 0; z < left.size_z; ++z)
        for (int y = 0; y < left.size_y; ++y)
            for (int x = 0; x < left.size_x; ++x)
                left.at(x,y,z) += right.at(x,y,z);
}

template <typename T>
voxel_cube<T> operator-( const voxel_cube<T>& left, const voxel_cube<T>& right ) {
    voxel_cube<T> result = left;
    for (int z = 0; z < result.size_z; ++z)
        for (int y = 0; y < result.size_y; ++y)
            for (int x = 0; x < result.size_x; ++x)
                result.at(x,y,z) -= right.at(x,y,z);
    return result;
}

template <typename T>
voxel_cube<T> operator*( const voxel_cube<T>& left, const voxel_cube<T>& right ) {
    voxel_cube<T> result = left;
    for (int z = 0; z < result.size_z; ++z)
        for (int y = 0; y < result.size_y; ++y)
            for (int x = 0; x < result.size_x; ++x)
                result.at(x,y,z) *= right.at(x,y,z);
    return result;
}

template <typename T>
voxel_cube<T> operator/ ( voxel_cube<T>& grid, const voxel_cube<T>& divisor ) {
    voxel_cube<T> result = grid;
    for (int z = 0; z < result.size_z; ++z)
        for (int y = 0; y < result.size_y; ++y)
            for (int x = 0; x < result.size_x; ++x)
                result.at(x,y,z) /= divisor.at(x,y,z);
    return result;
}

template <typename T>
void operator/= ( voxel_cube<T>& grid, const T divisor ) {
    for (int z = 0; z < grid.size_z; ++z)
        for (int y = 0; y < grid.size_y; ++y)
            for (int x = 0; x < grid.size_x; ++x)
                grid.at(x,y,z) /= divisor;
}

template <typename T>
struct color_cube {

    voxel_cube<T> red, green, blue;

    color_cube() {}

    color_cube(voxel_cube<T> const& r,voxel_cube<T> const& g, voxel_cube<T>const& b)
        : red(r), green(g), blue(b) {}

    color_cube(color_cube const& o) : red(o.red), green(o.green), blue(o.blue) {}

    color_cube(int size_x, int size_y, int size_z, float voxel_size) :
        red  ( voxel_cube<T>(size_x, size_y, size_z, voxel_size, 0) ),
        green( voxel_cube<T>(size_x, size_y, size_z, voxel_size, 0) ),
        blue ( voxel_cube<T>(size_x, size_y, size_z, voxel_size, 0) ) {}
};

template <typename T>
color_cube<T> operator+( const color_cube<T>& left, const color_cube<T>& right ) {
    color_cube<T> result = left;
    result.red   += right.red;
    result.green += right.green;
    result.blue  += right.blue;
    return result;
}

template <typename T>
color_cube<T> operator-( const color_cube<T>& left, const color_cube<T>& right ) {
    color_cube<T> result = left;
    result.red   -= right.red;
    result.green -= right.green;
    result.blue  -= right.blue;
    return result;
}

template <typename T>
color_cube<T> operator*( const color_cube<T>& colors, const voxel_cube<T>& weights ) {
    color_cube<T> result = colors;
    result.red   *= weights;
    result.green *= weights;
    result.blue  *= weights;
    return result;
}

template <typename T>
void operator/= ( color_cube<T>& grid, const T divisor ) {
    grid.red   /= divisor;
    grid.green /= divisor;
    grid.blue  /= divisor;
}

struct sdf { // RGB SDF

    int size_x, size_y, size_z;
    float voxel_size;
    voxel_cube<float> distance_field, weights;
    color_cube<float> color_field;

    sdf() {}

    sdf(int x, int y, int z, float vox_size, float default_w=0.0f, float default_v=-1.0f) :
        size_x(x), size_y(y), size_z(z), voxel_size(vox_size),
        distance_field(voxel_cube<float>(x, y, z, vox_size, default_v)),
        weights(voxel_cube<float>(x, y, z, vox_size, default_w)),
        color_field(color_cube<float>(x,y,z,vox_size))
       { }

    sdf(voxel_cube<float> const& df, color_cube<float> const& cf)
        : size_x(df.size_x), size_y(df.size_y),size_z(df.size_z), voxel_size(df.voxel_size),
        distance_field(df), color_field(cf){}

    sdf(sdf const& o) : size_x(o.size_x), size_y(o.size_y), size_z(o.size_z), voxel_size(o.voxel_size),
    distance_field(o.distance_field), weights(o.weights), color_field(o.color_field){}

    float& weight(int x, int y, int z) {return weights.at(x,y,z);}

    float weight(int x, int y, int z) const {return weights.at(x,y,z);}

};

struct plain_sdf {
    int size_x, size_y, size_z;
    float voxel_size;
    voxel_cube<float> distance_field, weights;

	plain_sdf() {}

    plain_sdf(int x, int y, int z, float vox_size, float default_w, float default_v) :
        size_x(x), size_y(y), size_z(z), voxel_size(vox_size),
        distance_field(voxel_cube<float>(x, y, z, vox_size, default_v)),
        weights(voxel_cube<float>(x, y, z, vox_size, default_w))
       { }

    plain_sdf(plain_sdf const& o) : size_x(o.size_x), size_y(o.size_y), size_z(o.size_z), voxel_size(o.voxel_size),
    distance_field(o.distance_field), weights(o.weights) {}
};

#endif // TYPES_HPP


#include <utility>
#include <boost/gil/gil_all.hpp>
#include <boost/gil/extension/io/png_io.hpp>

int main() {
	//namespace gil = boost::gil;

	boost::gil::point2<ptrdiff_t> dim(400, 400);
	boost::gil::gray8_image_t img(dim);
	//boost::gil::any_image<boost::gil::gray8_image_t>::view_t view = boost::gil::view(img);
	auto view = boost::gil::view(img);
	//boost::gil::image<boost::gil::gray8_image_t> view = boost::gil::view(img);

	for (ptrdiff_t i = 0; i < dim.y; ++i) {
		for (ptrdiff_t j = 0; j < dim.x; ++j) {
			view(i, j) = 255 * (std::min(i, j) / static_cast<double>(dim.x));
		}
	}

	boost::gil::png_write_view("img.png", view);

    return 0;
}


/*=============================================================================
Copyright 2020 Syed Ali Hasan <alihasan9922@gmail.com>

Distributed under the Boost Software License, Version 1.0. (See accompanying
file License.txt or copy at https://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/

#ifndef BOOST_ASTRONOMY_GALACTIC_COORD_HPP
#define BOOST_ASTRONOMY_GALACTIC_COORD_HPP

#include <iostream>
#include <boost/static_assert.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/units/get_dimension.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/units/systems/si/plane_angle.hpp>
#include <boost/units/systems/si/dimensionless.hpp>
#include <boost/units/physical_dimensions/plane_angle.hpp>
#include <boost/astronomy/coordinate/coord_sys/coord_sys.hpp>

/**
 * To describe the relationships between stars or other celestial
 * objects within our own Galaxy is convenient to use the Galactic
 * Coordinate System.
 *
 * The center of our Milky Way is chosen to be the coordinate system’s
 * origin. The equatorial coordinates for the center of the Milky Way
 * for the standard epoch 1950.0 are right ascension 17h42m and
 * declination −28◦45′.
 *
 * Galactic Latitude
 * The plane containing the Sun and the center of the Milky Way is
 * extended until it intersects the celestial sphere to form a
 * “galactic equator.”
 * Galactic latitudes, denoted by b, are measured with respect to
 * this galactic equator. Galactic latitudes are in the range of ±90◦
 * with positive angles being north of the galactic plane and negative
 * angles being south of the galactic plane.
 *
 * Galactic Longitude
 * Galactic longitudes, denoted by l, are measured counterclockwise so as
 * to increase from 0◦ to 360◦ in the same direction as increasing
 * right ascension.
**/

namespace boost { namespace astronomy { namespace coordinate {

namespace bu = boost::units;
namespace bg = boost::geometry;

template
<
    typename CoordinateType = double,
    typename LatQuantity = bu::quantity<bu::si::plane_angle, CoordinateType>,
    typename LonQuantity = bu::quantity<bu::si::plane_angle, CoordinateType>
>
struct galactic_coord : public coord_sys
    <2, bg::cs::spherical<bg::radian>, CoordinateType>
{
  ///@cond INTERNAL
  BOOST_STATIC_ASSERT_MSG(
      ((std::is_same<typename bu::get_dimension<LatQuantity>::type,
          bu::plane_angle_dimension>::value) &&
       (std::is_same<typename bu::get_dimension<LonQuantity>::type,
           bu::plane_angle_dimension>::value)),
      "Latitude and Longitude must be of plane angle type");
  BOOST_STATIC_ASSERT_MSG((std::is_floating_point<CoordinateType>::value),
                          "CoordinateType must be a floating-point type");
  ///@endcond
public:
    typedef LatQuantity quantity1;
    typedef LonQuantity quantity2;

    //Default constructor
    galactic_coord() {}

    galactic_coord
    (
        LatQuantity const &Lat,
        LonQuantity const &Lon
    )
    {
        this->set_lat_lon(Lat, Lon);
    }

    //Create tuple of Galactic Latitude and Galactic Longitude
    std::tuple<LatQuantity, LonQuantity> get_lat_lon() const
    {
        return std::make_tuple(this->get_lat(), this->get_lon());
    }

    //Get Galactic Latitude
    LatQuantity get_lat() const
    {
        return static_cast<LatQuantity>
        (
            bu::quantity<bu::si::plane_angle, CoordinateType>::from_value
                    (bg::get<0>(this->point))
        );
    }

    //Get Galactic Longitude
    LonQuantity get_lon() const
    {
        return static_cast<LonQuantity>
        (
            bu::quantity<bu::si::plane_angle, CoordinateType>::from_value
                    (bg::get<1>(this->point))
        );
    }

    //Set value of Galactic Latitude and Galactic Longitude
    void set_lat_lon
    (
        LatQuantity const &Lat,
        LonQuantity const &Lon
    )
    {
        this->set_lat(Lat);
        this->set_lon(Lon);
    }

    //Set Galactic Latitude
    void set_lat(LatQuantity const &Lat)
    {
        bg::set<0>
            (
                this->point,
                static_cast<bu::quantity<bu::si::plane_angle, CoordinateType>>(Lat).value()
            );
    }

    //Set Galactic Longitude
    void set_lon(LonQuantity const &Lon)
    {
        bg::set<1>
            (
                this->point,
                static_cast<bu::quantity<bu::si::plane_angle, CoordinateType>>(Lon).value()
            );
    }

}; //galactic_coord

//Make Galactic Coordinate
template
<
    typename CoordinateType,
    template<typename Unit2, typename CoordinateType_> class LatQuantity,
    template<typename Unit1, typename CoordinateType_> class LonQuantity,
    typename Unit1,
    typename Unit2
>
galactic_coord
<
    CoordinateType,
    LonQuantity<Unit1, CoordinateType>,
    LatQuantity<Unit2, CoordinateType>
> make_galactic_coord
(
        LatQuantity<Unit2, CoordinateType> const &Lat,
        LonQuantity<Unit1, CoordinateType> const &Lon
)
{
    return galactic_coord
        <
            CoordinateType,
            LatQuantity<Unit2, CoordinateType>,
            LonQuantity<Unit1, CoordinateType>
        > (Lat, Lon);
}

//Print Galactic Coordinates
template
<
    typename CoordinateType,
    class LatQuantity,
    class LonQuantity
>
std::ostream &operator << (std::ostream &out, galactic_coord
        <CoordinateType, LatQuantity, LonQuantity> const &point) {
    out << "Galactic Coordinate (Galactic Latitude: "
        << point.get_lat() << ", Galactic Longitude: "
        << point.get_lon() << ")";

    return out;
}

}}}

#endif //BOOST_ASTRONOMY_GALACTIC_COORD_HPP


/**
 * \file SimpleOverdriveFilter.cpp
 */

#include "SimpleOverdriveFilter.h"

#include <boost/math/special_functions/sign.hpp>

#include <ATK/Utility/ScalarNewtonRaphson.h>

namespace ATK
{
  template<typename DataType_>
  class SimpleOverdriveFilter<DataType_>::SimpleOverdriveFunction
  {
  public:
    typedef DataType_ DataType;
  protected:
    DataType A;
    DataType B;
    DataType is;
    DataType vt;

    DataType oldy0;
    DataType oldexpy0;
    DataType oldinvexpy0;
    DataType oldy1;
    DataType oldexpy1;
    DataType oldinvexpy1;
  public:
    SimpleOverdriveFunction(DataType dt, DataType R, DataType C, DataType is, DataType vt)
    :is(is), vt(vt)
    {
      A = dt / (2 * C) + R;
      B = dt / (2 * C) - R;

      oldy0 = oldy1 = 0;
      oldexpy0 = oldinvexpy0 = oldexpy1 = oldinvexpy1 = 1;
    }

    std::pair<DataType, DataType> operator()(const DataType* ATK_RESTRICT input, DataType* ATK_RESTRICT output, DataType y1)
    {
      auto x0 = input[-1];
      auto x1 = input[0];
      auto y0 = output[-1];
      DataType expdiode_y1_p = std::exp(y1 / vt);
      DataType expdiode_y1_m = 1 / expdiode_y1_p;
      DataType expdiode_y0_p;
      DataType expdiode_y0_m;

	    if(y0 == oldy0)
	    {
	      expdiode_y0_p = oldexpy0;
	      expdiode_y0_m = oldinvexpy0;
	    }
	    else if(y0 == oldy1)
	    {
	      expdiode_y0_p = oldexpy1;
	      expdiode_y0_m = oldinvexpy1;
	    }
	    else
	    {
	      expdiode_y0_p = std::exp(y0 / vt);
	      expdiode_y0_m = 1 / expdiode_y0_p;
	    }

      oldy0 = y0;
      oldexpy0 = expdiode_y0_p;
      oldinvexpy0 = expdiode_y0_m;

      oldy1 = y1;
      oldexpy1 = expdiode_y1_p;
      oldinvexpy1 = expdiode_y1_m;

      std::pair<DataType, DataType> diode = std::make_pair(is * (expdiode_y1_p - expdiode_y1_m), is * (expdiode_y1_p + expdiode_y1_m) / vt);
      return std::make_pair(A * diode.first + y1 + x0 - x1 + B * is * (expdiode_y0_p - expdiode_y0_m) - y0, A * diode.second + 1);
    }

    DataType estimate(const DataType* ATK_RESTRICT input, DataType* ATK_RESTRICT output)
    {
      auto x0 = input[-1];
      auto x1 = input[0];
      auto y0 = output[-1];
      return affine_estimate(x0, x1, y0);
    }

    DataType id_estimate(DataType x0, DataType x1, DataType y0)
    {
      return y0;
    }

    DataType linear_estimate(DataType x0, DataType x1, DataType y0)
    {
      if(y0 == 0)
        return 0;
      auto sinh = (oldexpy1 - oldinvexpy1);
      return y0 * (x1 - x0 + y0 - is * sinh * B) / (is * sinh * A + y0);
    }

    DataType affine_estimate(DataType x0, DataType x1, DataType y0)
    {
      auto sinh = (oldexpy1 - oldinvexpy1);
      auto cosh = (oldexpy1 + oldinvexpy1);
      return (x1 - x0 + y0 - is * sinh * B - (sinh - y0 / vt * cosh) * is * A) / (is * cosh * A / vt + 1);
    }
  };

  template <typename DataType>
  SimpleOverdriveFilter<DataType>::SimpleOverdriveFilter()
  :TypedBaseFilter<DataType>(1, 1)
  {
    input_delay = 1;
    output_delay = 1;
  }

  template <typename DataType>
  SimpleOverdriveFilter<DataType>::~SimpleOverdriveFilter()
  {
  }

  template <typename DataType>
  void SimpleOverdriveFilter<DataType>::setup()
  {
    Parent::setup();
    optimizer.reset(new ScalarNewtonRaphson<SimpleOverdriveFunction>(SimpleOverdriveFunction(static_cast<DataType>(1. / input_sampling_rate),
      10000, static_cast<DataType>(22e-9), static_cast<DataType>(1e-12), static_cast<DataType>(26e-3))));
  }

  template <typename DataType>
  void SimpleOverdriveFilter<DataType>::process_impl(int64_t size) const
  {
    const DataType* ATK_RESTRICT input = converted_inputs[0];
    DataType* ATK_RESTRICT output = outputs[0];
    for(int64_t i = 0; i < size; ++i)
    {
      optimizer->optimize(input + i, output + i);
    }
  }

  template class SimpleOverdriveFilter<float>;
  template class SimpleOverdriveFilter<double>;

  template<typename DataType_>
  class BackwardSimpleOverdriveFilter<DataType_>::SimpleOverdriveFunction
  {
  public:
    typedef DataType_ DataType;
  protected:
    DataType A;
    DataType B;
    DataType is;
    DataType vt;

    DataType oldy0;
    DataType oldexpy0;
    DataType oldinvexpy0;
    DataType oldy1;
    DataType oldexpy1;
    DataType oldinvexpy1;
  public:
    SimpleOverdriveFunction(DataType dt, DataType R, DataType C, DataType is, DataType vt)
    :is(is), vt(vt)
    {
      A = dt / C + R;
      B = - R;

      oldy0 = oldy1 = 0;
      oldexpy0 = oldinvexpy0 = oldexpy1 = oldinvexpy1 = 1;
    }

    std::pair<DataType, DataType> operator()(const DataType* ATK_RESTRICT input, DataType* ATK_RESTRICT output, DataType y1)
    {
      auto x0 = input[-1];
      auto x1 = input[0];
      auto y0 = output[-1];
      DataType expdiode_y1_p = std::exp(y1 / vt);
      DataType expdiode_y1_m = 1 / expdiode_y1_p;
      DataType expdiode_y0_p;
      DataType expdiode_y0_m;

      if(y0 == oldy0)
      {
        expdiode_y0_p = oldexpy0;
        expdiode_y0_m = oldinvexpy0;
      }
      else if(y0 == oldy1)
      {
        expdiode_y0_p = oldexpy1;
        expdiode_y0_m = oldinvexpy1;
      }
      else
      {
        expdiode_y0_p = std::exp(y0 / vt);
        expdiode_y0_m = 1 / expdiode_y0_p;
      }

      oldy0 = y0;
      oldexpy0 = expdiode_y0_p;
      oldinvexpy0 = expdiode_y0_m;

      oldy1 = y1;
      oldexpy1 = expdiode_y1_p;
      oldinvexpy1 = expdiode_y1_m;

      std::pair<DataType, DataType> diode = std::make_pair(is * (expdiode_y1_p - expdiode_y1_m), is * (expdiode_y1_p + expdiode_y1_m) / vt);
      return std::make_pair(A * diode.first + (y1 + (x0 - x1 + B * is * (expdiode_y0_p - expdiode_y0_m) - y0)), A * diode.second + 1);
    }

    DataType estimate(const DataType* ATK_RESTRICT input, DataType* ATK_RESTRICT output)
    {
      auto x0 = input[-1];
      auto x1 = input[0];
      auto y0 = output[-1];
      return affine_estimate(x0, x1, y0);
    }

    DataType id_estimate(DataType x0, DataType x1, DataType y0)
    {
      return y0;
    }

    DataType linear_estimate(DataType x0, DataType x1, DataType y0)
    {
      if(y0 == 0)
        return 0;
      auto exp = std::exp(y0 / vt);
      auto sinh = (exp - 1/exp);
      return y0 * (x1 - x0 + y0 - is * sinh * B) / (is * sinh * A + y0);
    }

    DataType affine_estimate(DataType x0, DataType x1, DataType y0)
    {
      auto exp = std::exp(y0 / vt);
      auto sinh = (exp - 1/exp);
      auto cosh = (exp + 1/exp);
      return (x1 - x0 + y0 - is * sinh * B - (sinh - y0 / vt * cosh) * is * A) / (is * cosh * A / vt + 1);
    }
  };

  template <typename DataType>
  BackwardSimpleOverdriveFilter<DataType>::BackwardSimpleOverdriveFilter()
  :TypedBaseFilter<DataType>(1, 1)
  {
    input_delay = 1;
    output_delay = 1;
  }

  template <typename DataType>
  BackwardSimpleOverdriveFilter<DataType>::~BackwardSimpleOverdriveFilter()
  {
  }

  template <typename DataType>
  void BackwardSimpleOverdriveFilter<DataType>::setup()
  {
    Parent::setup();
    optimizer.reset(new ScalarNewtonRaphson<SimpleOverdriveFunction>(SimpleOverdriveFunction(static_cast<DataType>(1. / input_sampling_rate),
                                                                                10000, static_cast<DataType>(22e-9), static_cast<DataType>(1e-12), static_cast<DataType>(26e-3))));
  }

  template <typename DataType>
  void BackwardSimpleOverdriveFilter<DataType>::process_impl(int64_t size) const
  {
    const DataType* ATK_RESTRICT input = converted_inputs[0];
    DataType* ATK_RESTRICT output = outputs[0];
    for(int64_t i = 0; i < size; ++i)
    {
      optimizer->optimize(input + i, output + i);
    }
  }

  template class BackwardSimpleOverdriveFilter<float>;
  template class BackwardSimpleOverdriveFilter<double>;

  template <typename DataType>
  ForwardSimpleOverdriveFilter<DataType>::ForwardSimpleOverdriveFilter()
  :TypedBaseFilter<DataType>(1, 1)
  {
    input_delay = 1;
    output_delay = 1;
  }

  template <typename DataType>
  ForwardSimpleOverdriveFilter<DataType>::~ForwardSimpleOverdriveFilter()
  {
  }

  template <typename DataType>
  void ForwardSimpleOverdriveFilter<DataType>::setup()
  {
    Parent::setup();
  }

  template <typename DataType>
  void ForwardSimpleOverdriveFilter<DataType>::process_impl(int64_t size) const
  {
    DataType dt = static_cast<DataType>(1. / input_sampling_rate);
    DataType R = 10000;
    DataType C = static_cast<DataType>(22e-9);
    DataType is = static_cast<DataType>(1e-12);
    DataType vt = static_cast<DataType>(26e-3);

    const DataType* ATK_RESTRICT input = converted_inputs[0];
    DataType* ATK_RESTRICT output = outputs[0];
    for(int64_t i = 0; i < size; ++i)
    {
      output[i] = output[i-1] + (input[i] - input[i-1] - (4 * dt * is * C * sinh(output[i-1] / vt))) / (1 + 2 * is * R * vt * cosh(output[i-1] / vt));
    }
  }

  template class ForwardSimpleOverdriveFilter<float>;
  template class ForwardSimpleOverdriveFilter<double>;

}


/**
 * @file ann_layer_test.cpp
 * @author Marcus Edel
 *
 * Tests the ann layer modules.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>

#include <mlpack/methods/ann/layer/layer.hpp>
#include <mlpack/methods/ann/layer/layer_types.hpp>
#include <mlpack/methods/ann/init_rules/random_init.hpp>
#include <mlpack/methods/ann/init_rules/const_init.hpp>
#include <mlpack/methods/ann/init_rules/nguyen_widrow_init.hpp>
#include <mlpack/methods/ann/ffn.hpp>
#include <mlpack/methods/ann/rnn.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace mlpack::ann;

BOOST_AUTO_TEST_SUITE(ANNLayerTest);

// Helper function which calls the Reset function of the given module.
template<class T>
void ResetFunction(
    T& layer,
    typename std::enable_if<HasResetCheck<T, void(T::*)()>::value>::type* = 0)
{
  layer.Reset();
}

template<class T>
void ResetFunction(
    T& /* layer */,
    typename std::enable_if<!HasResetCheck<T, void(T::*)()>::value>::type* = 0)
{
  /* Nothing to do here */
}

// Approximate Jacobian and supposedly-true Jacobian, then compare them
// similarly to before.
template<typename ModuleType>
double JacobianTest(ModuleType& module,
                  arma::mat& input,
                  const double minValue = -2,
                  const double maxValue = -1,
                  const double perturbation = 1e-6)
{
  arma::mat output, outputA, outputB, jacobianA, jacobianB;

  // Initialize the input matrix.
  RandomInitialization init(minValue, maxValue);
  init.Initialize(input, input.n_rows, input.n_cols);

  // Initialize the module parameters.
  ResetFunction(module);

  // Initialize the jacobian matrix.
  module.Forward(std::move(input), std::move(output));
  jacobianA = arma::zeros(input.n_elem, output.n_elem);

  // Share the input paramter matrix.
  arma::mat sin = arma::mat(input.memptr(), input.n_rows, input.n_cols,
      false, false);

  for (size_t i = 0; i < input.n_elem; ++i)
  {
    double original = sin(i);
    sin(i) = original - perturbation;
    module.Forward(std::move(input), std::move(outputA));
    sin(i) = original + perturbation;
    module.Forward(std::move(input), std::move(outputB));
    sin(i) = original;

    outputB -= outputA;
    outputB /= 2 * perturbation;
    jacobianA.row(i) = outputB.t();
  }

  // Initialize the derivative parameter.
  arma::mat deriv = arma::zeros(output.n_rows, output.n_cols);

  // Share the derivative parameter.
  arma::mat derivTemp = arma::mat(deriv.memptr(), deriv.n_rows, deriv.n_cols,
      false, false);

  // Initialize the jacobian matrix.
  jacobianB = arma::zeros(input.n_elem, output.n_elem);

  for (size_t i = 0; i < derivTemp.n_elem; ++i)
  {
    deriv.zeros();
    derivTemp(i) = 1;

    arma::mat delta;
    module.Backward(std::move(input), std::move(deriv), std::move(delta));

    jacobianB.col(i) = delta;
  }

  return arma::max(arma::max(arma::abs(jacobianA - jacobianB)));
}

// Approximate Jacobian and supposedly-true Jacobian, then compare them
// similarly to before.
template<typename ModuleType>
double JacobianPerformanceTest(ModuleType& module,
                               arma::mat& input,
                               arma::mat& target,
                               const double eps = 1e-6)
{
  module.Forward(std::move(input), std::move(target));

  arma::mat delta;
  module.Backward(std::move(input), std::move(target), std::move(delta));

  arma::mat centralDifference = arma::zeros(delta.n_rows, delta.n_cols);
  arma::mat inputTemp = arma::mat(input.memptr(), input.n_rows, input.n_cols,
      false, false);

  arma::mat centralDifferenceTemp = arma::mat(centralDifference.memptr(),
      centralDifference.n_rows, centralDifference.n_cols, false, false);

  for (size_t i = 0; i < input.n_elem; ++i)
  {
    inputTemp(i) = inputTemp(i) + eps;
    double outputA = module.Forward(std::move(input), std::move(target));
    inputTemp(i) = inputTemp(i) - (2 * eps);
    double outputB = module.Forward(std::move(input), std::move(target));

    centralDifferenceTemp(i) = (outputA - outputB) / (2 * eps);
    inputTemp(i) = inputTemp(i) + eps;
  }

  return arma::max(arma::max(arma::abs(centralDifference - delta)));
}

// Simple numerical gradient checker.
template<class FunctionType>
double CheckGradient(FunctionType& function, const double eps = 1e-7)
{
  // Get gradients for the current parameters.
  arma::mat orgGradient, gradient, estGradient;
  function.Gradient(orgGradient);

  estGradient = arma::zeros(orgGradient.n_rows, orgGradient.n_cols);

  // Compute numeric approximations to gradient.
  for (size_t i = 0; i < orgGradient.n_elem; ++i)
  {
    double tmp = function.Parameters()(i);

    // Perturb parameter with a positive constant and get costs.
    function.Parameters()(i) += eps;
    double costPlus = function.Gradient(gradient);

    // Perturb parameter with a negative constant and get costs.
    function.Parameters()(i) -= (2 * eps);
    double costMinus = function.Gradient(gradient);

    // Restore the parameter value.
    function.Parameters()(i) = tmp;

    // Compute numerical gradients using the costs calculated above.
    estGradient(i) = (costPlus - costMinus) / (2 * eps);
  }

  // Estimate error of gradient.
  return arma::norm(orgGradient - estGradient) /
      arma::norm(orgGradient + estGradient);
}

/**
 * Simple add module test.
 */
BOOST_AUTO_TEST_CASE(SimpleAddLayerTest)
{
  arma::mat output, input, delta;
  Add<> module(10);
  module.Parameters().randu();

  // Test the Forward function.
  input = arma::zeros(10, 1);
  module.Forward(std::move(input), std::move(output));
  BOOST_REQUIRE_EQUAL(arma::accu(module.Parameters()), arma::accu(output));

  // Test the Backward function.
  module.Backward(std::move(input), std::move(output), std::move(delta));
  BOOST_REQUIRE_EQUAL(arma::accu(output), arma::accu(delta));

  // Test the forward function.
  input = arma::ones(10, 1);
  module.Forward(std::move(input), std::move(output));
  BOOST_REQUIRE_CLOSE(10 + arma::accu(module.Parameters()),
      arma::accu(output), 1e-3);

  // Test the backward function.
  module.Backward(std::move(input), std::move(output), std::move(delta));
  BOOST_REQUIRE_CLOSE(arma::accu(output), arma::accu(delta), 1e-3);
}

/**
 * Jacobian add module test.
 */
BOOST_AUTO_TEST_CASE(JacobianAddLayerTest)
{
  for (size_t i = 0; i < 5; i++)
  {
    const size_t elements = math::RandInt(2, 1000);
    arma::mat input;
    input.set_size(elements, 1);

    Add<> module(elements);
    module.Parameters().randu();

    double error = JacobianTest(module, input);
    BOOST_REQUIRE_LE(error, 1e-5);
  }
}

/**
 * Add layer numerically gradient test.
 */
BOOST_AUTO_TEST_CASE(GradientAddLayerTest)
{
  // Add function gradient instantiation.
  struct GradientFunction
  {
    GradientFunction()
    {
      input = arma::randu(10, 1);
      target = arma::mat("1");

      model = new FFN<NegativeLogLikelihood<>, NguyenWidrowInitialization>(
          input, target);
      model->Add<IdentityLayer<> >();
      model->Add<Add<> >(10);
      model->Add<LogSoftMax<> >();
    }

    ~GradientFunction()
    {
      delete model;
    }

    double Gradient(arma::mat& gradient) const
    {
      arma::mat output;
      double error = model->Evaluate(model->Parameters(), 0, 1);
      model->Gradient(model->Parameters(), 0, gradient, 1);
      return error;
    }

    arma::mat& Parameters() { return model->Parameters(); }

    FFN<NegativeLogLikelihood<>, NguyenWidrowInitialization>* model;
    arma::mat input, target;
  } function;

  BOOST_REQUIRE_LE(CheckGradient(function), 1e-4);
}

/**
 * Simple constant module test.
 */
BOOST_AUTO_TEST_CASE(SimpleConstantLayerTest)
{
  arma::mat output, input, delta;
  Constant<> module(10, 3.0);

  // Test the Forward function.
  input = arma::zeros(10, 1);
  module.Forward(std::move(input), std::move(output));
  BOOST_REQUIRE_EQUAL(arma::accu(output), 30.0);

  // Test the Backward function.
  module.Backward(std::move(input), std::move(output), std::move(delta));
  BOOST_REQUIRE_EQUAL(arma::accu(delta), 0);

  // Test the forward function.
  input = arma::ones(10, 1);
  module.Forward(std::move(input), std::move(output));
  BOOST_REQUIRE_EQUAL(arma::accu(output), 30.0);

  // Test the backward function.
  module.Backward(std::move(input), std::move(output), std::move(delta));
  BOOST_REQUIRE_EQUAL(arma::accu(delta), 0);
}

/**
 * Jacobian constant module test.
 */
BOOST_AUTO_TEST_CASE(JacobianConstantLayerTest)
{
  for (size_t i = 0; i < 5; i++)
  {
    const size_t elements = math::RandInt(2, 1000);
    arma::mat input;
    input.set_size(elements, 1);

    Constant<> module(elements, 1.0);

    double error = JacobianTest(module, input);
    BOOST_REQUIRE_LE(error, 1e-5);
  }
}

/**
 * Simple dropout module test.
 */
BOOST_AUTO_TEST_CASE(SimpleDropoutLayerTest)
{
  // Initialize the probability of setting a value to zero and the scale
  // parameter.
  const double p = 0.2;
  const double scale = 1.0 / (1.0 - p);

  // Initialize the input parameter.
  arma::mat input(1000, 1);
  input.fill(1 - p);

  Dropout<> module(p);
  module.Deterministic() = false;

  // Test the Forward function.
  arma::mat output;
  module.Forward(std::move(input), std::move(output));
  BOOST_REQUIRE_LE(
      arma::as_scalar(arma::abs(arma::mean(output) - (1 - p))), 0.05);

  // Test the Backward function.
  arma::mat delta;
  module.Backward(std::move(input), std::move(input), std::move(delta));
  BOOST_REQUIRE_LE(
      arma::as_scalar(arma::abs(arma::mean(delta) - (1 - p))), 0.05);

  // Test the Forward function.
  module.Deterministic() = true;
  module.Rescale() = false;
  module.Forward(std::move(input), std::move(output));
  BOOST_REQUIRE_EQUAL(arma::accu(input), arma::accu(output));

  // Test the Forward function.
  module.Rescale() = true;
  module.Forward(std::move(input), std::move(output));
  BOOST_REQUIRE_CLOSE(arma::accu(input) * scale, arma::accu(output), 1e-3);
}

/**
 * Perform dropout x times using ones as input, sum the number of ones and
 * validate that the layer is producing approximately the correct number of
 * ones.
 */
BOOST_AUTO_TEST_CASE(DropoutProbabilityTest)
{
  arma::mat input = arma::ones(1500, 1);
  const size_t iterations = 10;

  double probability[5] = { 0.1, 0.3, 0.4, 0.7, 0.8 };
  for (size_t trial = 0; trial < 5; ++trial)
  {
    double nonzeroCount = 0;
    for (size_t i = 0; i < iterations; ++i)
    {
      Dropout<> module(probability[trial]);
      module.Deterministic() = false;

      arma::mat output;
      module.Forward(std::move(input), std::move(output));

      // Return a column vector containing the indices of elements of X that
      // are non-zero, we just need the number of non-zero values.
      arma::uvec nonzero = arma::find(output);
      nonzeroCount += nonzero.n_elem;
    }
    const double expected = input.n_elem * (1 - probability[trial]) *
        iterations;
    const double error = fabs(nonzeroCount - expected) / expected;

    BOOST_REQUIRE_LE(error, 0.15);
  }
}

/*
 * Perform dropout with probability 1 - p where p = 0, means no dropout.
 */
BOOST_AUTO_TEST_CASE(NoDropoutTest)
{
  arma::mat input = arma::ones(1500, 1);
  Dropout<> module(0);
  module.Deterministic() = false;

  arma::mat output;
  module.Forward(std::move(input), std::move(output));

  BOOST_REQUIRE_EQUAL(arma::accu(output), arma::accu(input));
}

/**
 * Simple linear module test.
 */
BOOST_AUTO_TEST_CASE(SimpleLinearLayerTest)
{
  arma::mat output, input, delta;
  Linear<> module(10, 10);
  module.Parameters().randu();
  module.Reset();

  // Test the Forward function.
  input = arma::zeros(10, 1);
  module.Forward(std::move(input), std::move(output));
  BOOST_REQUIRE_CLOSE(arma::accu(
      module.Parameters().submat(100, 0, module.Parameters().n_elem - 1, 0)),
      arma::accu(output), 1e-3);

  // Test the Backward function.
  module.Backward(std::move(input), std::move(input), std::move(delta));
  BOOST_REQUIRE_EQUAL(arma::accu(delta), 0);
}

/**
 * Jacobian linear module test.
 */
BOOST_AUTO_TEST_CASE(JacobianLinearLayerTest)
{
  for (size_t i = 0; i < 5; i++)
  {
    const size_t inputElements = math::RandInt(2, 1000);
    const size_t outputElements = math::RandInt(2, 1000);

    arma::mat input;
    input.set_size(inputElements, 1);

    Linear<> module(inputElements, outputElements);
    module.Parameters().randu();

    double error = JacobianTest(module, input);
    BOOST_REQUIRE_LE(error, 1e-5);
  }
}

/**
 * Linear layer numerically gradient test.
 */
BOOST_AUTO_TEST_CASE(GradientLinearLayerTest)
{
  // Linear function gradient instantiation.
  struct GradientFunction
  {
    GradientFunction()
    {
      input = arma::randu(10, 1);
      target = arma::mat("1");

      model = new FFN<NegativeLogLikelihood<>, NguyenWidrowInitialization>(
          input, target);
      model->Add<IdentityLayer<> >();
      model->Add<Linear<> >(10, 2);
      model->Add<LogSoftMax<> >();
    }

    ~GradientFunction()
    {
      delete model;
    }

    double Gradient(arma::mat& gradient) const
    {
      arma::mat output;
      double error = model->Evaluate(model->Parameters(), 0, 1);
      model->Gradient(model->Parameters(), 0, gradient, 1);
      return error;
    }

    arma::mat& Parameters() { return model->Parameters(); }

    FFN<NegativeLogLikelihood<>, NguyenWidrowInitialization>* model;
    arma::mat input, target;
  } function;

  BOOST_REQUIRE_LE(CheckGradient(function), 1e-4);
}

/**
 * Simple linear no bias module test.
 */
BOOST_AUTO_TEST_CASE(SimpleLinearNoBiasLayerTest)
{
  arma::mat output, input, delta;
  LinearNoBias<> module(10, 10);
  module.Parameters().randu();
  module.Reset();

  // Test the Forward function.
  input = arma::zeros(10, 1);
  module.Forward(std::move(input), std::move(output));
  BOOST_REQUIRE_EQUAL(0, arma::accu(output));

  // Test the Backward function.
  module.Backward(std::move(input), std::move(input), std::move(delta));
  BOOST_REQUIRE_EQUAL(arma::accu(delta), 0);
}

/**
 * Jacobian linear no bias module test.
 */
BOOST_AUTO_TEST_CASE(JacobianLinearNoBiasLayerTest)
{
  for (size_t i = 0; i < 5; i++)
  {
    const size_t inputElements = math::RandInt(2, 1000);
    const size_t outputElements = math::RandInt(2, 1000);

    arma::mat input;
    input.set_size(inputElements, 1);

    LinearNoBias<> module(inputElements, outputElements);
    module.Parameters().randu();

    double error = JacobianTest(module, input);
    BOOST_REQUIRE_LE(error, 1e-5);
  }
}

/**
 * LinearNoBias layer numerically gradient test.
 */
BOOST_AUTO_TEST_CASE(GradientLinearNoBiasLayerTest)
{
  // LinearNoBias function gradient instantiation.
  struct GradientFunction
  {
    GradientFunction()
    {
      input = arma::randu(10, 1);
      target = arma::mat("1");

      model = new FFN<NegativeLogLikelihood<>, NguyenWidrowInitialization>(
          input, target);
      model->Add<IdentityLayer<> >();
      model->Add<LinearNoBias<> >(10, 2);
      model->Add<LogSoftMax<> >();
    }

    ~GradientFunction()
    {
      delete model;
    }

    double Gradient(arma::mat& gradient) const
    {
      arma::mat output;
      double error = model->Evaluate(model->Parameters(), 0, 1);
      model->Gradient(model->Parameters(), 0, gradient, 1);
      return error;
    }

    arma::mat& Parameters() { return model->Parameters(); }

    FFN<NegativeLogLikelihood<>, NguyenWidrowInitialization>* model;
    arma::mat input, target;
  } function;

  BOOST_REQUIRE_LE(CheckGradient(function), 1e-4);
}

/**
 * Jacobian negative log likelihood module test.
 */
BOOST_AUTO_TEST_CASE(JacobianNegativeLogLikelihoodLayerTest)
{
  for (size_t i = 0; i < 5; i++)
  {
    NegativeLogLikelihood<> module;
    const size_t inputElements = math::RandInt(5, 100);
    arma::mat input;
    RandomInitialization init(0, 1);
    init.Initialize(input, inputElements, 1);

    arma::mat target(1, 1);
    target(0) = math::RandInt(1, inputElements - 1);

    double error = JacobianPerformanceTest(module, input, target);
    BOOST_REQUIRE_LE(error, 1e-5);
  }
}

/**
 * Jacobian LeakyReLU module test.
 */
BOOST_AUTO_TEST_CASE(JacobianLeakyReLULayerTest)
{
  for (size_t i = 0; i < 5; i++)
  {
    const size_t inputElements = math::RandInt(2, 1000);

    arma::mat input;
    input.set_size(inputElements, 1);

    LeakyReLU<> module;

    double error = JacobianTest(module, input);
    BOOST_REQUIRE_LE(error, 1e-5);
  }
}

/**
 * Jacobian MultiplyConstant module test.
 */
BOOST_AUTO_TEST_CASE(JacobianMultiplyConstantLayerTest)
{
  for (size_t i = 0; i < 5; i++)
  {
    const size_t inputElements = math::RandInt(2, 1000);

    arma::mat input;
    input.set_size(inputElements, 1);

    MultiplyConstant<> module(3.0);

    double error = JacobianTest(module, input);
    BOOST_REQUIRE_LE(error, 1e-5);
  }
}

/**
 * Jacobian HardTanH module test.
 */
BOOST_AUTO_TEST_CASE(JacobianHardTanHLayerTest)
{
  for (size_t i = 0; i < 5; i++)
  {
    const size_t inputElements = math::RandInt(2, 1000);

    arma::mat input;
    input.set_size(inputElements, 1);

    HardTanH<> module;

    double error = JacobianTest(module, input);
    BOOST_REQUIRE_LE(error, 1e-5);
  }
}

/**
 * Simple select module test.
 */
BOOST_AUTO_TEST_CASE(SimpleSelectLayerTest)
{
  arma::mat outputA, outputB, input, delta;

  input = arma::ones(10, 5);
  for (size_t i = 0; i < input.n_cols; ++i)
  {
    input.col(i) *= i;
  }

  // Test the Forward function.
  Select<> moduleA(3);
  moduleA.Forward(std::move(input), std::move(outputA));
  BOOST_REQUIRE_EQUAL(30, arma::accu(outputA));

  // Test the Forward function.
  Select<> moduleB(3, 5);
  moduleB.Forward(std::move(input), std::move(outputB));
  BOOST_REQUIRE_EQUAL(15, arma::accu(outputB));

  // Test the Backward function.
  moduleA.Backward(std::move(input), std::move(outputA), std::move(delta));
  BOOST_REQUIRE_EQUAL(30, arma::accu(delta));

  // Test the Backward function.
  moduleB.Backward(std::move(input), std::move(outputA), std::move(delta));
  BOOST_REQUIRE_EQUAL(15, arma::accu(delta));
}

/**
 * Simple join module test.
 */
BOOST_AUTO_TEST_CASE(SimpleJoinLayerTest)
{
  arma::mat output, input, delta;
  input = arma::ones(10, 5);

  // Test the Forward function.
  Join<> module;
  module.Forward(std::move(input), std::move(output));
  BOOST_REQUIRE_EQUAL(50, arma::accu(output));

  bool b = output.n_rows == 1 || output.n_cols == 1;
  BOOST_REQUIRE_EQUAL(b, true);

  // Test the Backward function.
  module.Backward(std::move(input), std::move(output), std::move(delta));
  BOOST_REQUIRE_EQUAL(50, arma::accu(delta));

  b = delta.n_rows == input.n_rows && input.n_cols;
  BOOST_REQUIRE_EQUAL(b, true);
}

/**
 * Simple add merge module test.
 */
BOOST_AUTO_TEST_CASE(SimpleAddMergeLayerTest)
{
  arma::mat output, input, delta;
  input = arma::ones(10, 1);

  for (size_t i = 0; i < 5; ++i)
  {
    AddMerge<> module;
    const size_t numMergeModules = math::RandInt(2, 10);
    for (size_t m = 0; m < numMergeModules; ++m)
    {
      IdentityLayer<> identityLayer;
      identityLayer.Forward(std::move(input),
          std::move(identityLayer.OutputParameter()));

      module.Add(identityLayer);
    }

    // Test the Forward function.
    module.Forward(std::move(input), std::move(output));
    BOOST_REQUIRE_EQUAL(10 * numMergeModules, arma::accu(output));

    // Test the Backward function.
    module.Backward(std::move(input), std::move(output), std::move(delta));
    BOOST_REQUIRE_EQUAL(arma::accu(output), arma::accu(delta));
  }
}

/**
 * Test the LSTM layer with a user defined rho parameter and without.
 */
BOOST_AUTO_TEST_CASE(LSTMRrhoTest)
{
  const size_t rho = 5;
  arma::cube input = arma::randu(1, 1, 5);
  arma::cube target = arma::ones(1, 1, 5);
  RandomInitialization init(0.5, 0.5);

  // Create model with user defined rho parameter.
  RNN<NegativeLogLikelihood<>, RandomInitialization> modelA(
      input, target, rho, false, NegativeLogLikelihood<>(), init);
  modelA.Add<IdentityLayer<> >();
  modelA.Add<Linear<> >(1, 10);

  // Use LSTM layer with rho.
  modelA.Add<LSTM<> >(10, 3, rho);
  modelA.Add<LogSoftMax<> >();

  // Create model without user defined rho parameter.
  RNN<NegativeLogLikelihood<> > modelB(
      input, target, rho, false, NegativeLogLikelihood<>(), init);
  modelB.Add<IdentityLayer<> >();
  modelB.Add<Linear<> >(1, 10);

  // Use LSTM layer with rho = MAXSIZE.
  modelB.Add<LSTM<> >(10, 3);
  modelB.Add<LogSoftMax<> >();

  optimization::StandardSGD opt(0.1, 1, 5, -100, false);
  modelA.Train(input, target, opt);
  modelB.Train(input, target, opt);

  CheckMatrices(modelB.Parameters(), modelA.Parameters());
}

/**
 * LSTM layer numerical gradient test.
 */
BOOST_AUTO_TEST_CASE(GradientLSTMLayerTest)
{
  // LSTM function gradient instantiation.
  struct GradientFunction
  {
    GradientFunction()
    {
      input = arma::randu(1, 1, 5);
      target.ones(1, 1, 5);
      const size_t rho = 5;

      model = new RNN<NegativeLogLikelihood<> >(input, target, rho);
      model->Add<IdentityLayer<> >();
      model->Add<Linear<> >(1, 10);
      model->Add<LSTM<> >(10, 3, rho);
      model->Add<LogSoftMax<> >();
    }

    ~GradientFunction()
    {
      delete model;
    }

    double Gradient(arma::mat& gradient) const
    {
      arma::mat output;
      double error = model->Evaluate(model->Parameters(), 0, 1);
      model->Gradient(model->Parameters(), 0, gradient, 1);
      return error;
    }

    arma::mat& Parameters() { return model->Parameters(); }

    RNN<NegativeLogLikelihood<> >* model;
    arma::cube input, target;
  } function;

  BOOST_REQUIRE_LE(CheckGradient(function), 1e-4);
}

/**
 * Test the FastLSTM layer with a user defined rho parameter and without.
 */
BOOST_AUTO_TEST_CASE(FastLSTMRrhoTest)
{
  const size_t rho = 5;
  arma::cube input = arma::randu(1, 1, 5);
  arma::cube target = arma::ones(1, 1, 5);
  RandomInitialization init(0.5, 0.5);

  // Create model with user defined rho parameter.
  RNN<NegativeLogLikelihood<>, RandomInitialization> modelA(
      input, target, rho, false, NegativeLogLikelihood<>(), init);
  modelA.Add<IdentityLayer<> >();
  modelA.Add<Linear<> >(1, 10);

  // Use FastLSTM layer with rho.
  modelA.Add<FastLSTM<> >(10, 3, rho);
  modelA.Add<LogSoftMax<> >();

  // Create model without user defined rho parameter.
  RNN<NegativeLogLikelihood<> > modelB(
      input, target, rho, false, NegativeLogLikelihood<>(), init);
  modelB.Add<IdentityLayer<> >();
  modelB.Add<Linear<> >(1, 10);

  // Use FastLSTM layer with rho = MAXSIZE.
  modelB.Add<FastLSTM<> >(10, 3);
  modelB.Add<LogSoftMax<> >();

  optimization::StandardSGD opt(0.1, 1, 5, -100, false);
  modelA.Train(input, target, opt);
  modelB.Train(input, target, opt);

  CheckMatrices(modelB.Parameters(), modelA.Parameters());
}

/**
 * FastLSTM layer numerical gradient test.
 */
BOOST_AUTO_TEST_CASE(GradientFastLSTMLayerTest)
{
  // Fast LSTM function gradient instantiation.
  struct GradientFunction
  {
    GradientFunction()
    {
      input = arma::randu(1, 1, 5);
      target = arma::ones(1, 1, 5);
      const size_t rho = 5;

      model = new RNN<NegativeLogLikelihood<> >(input, target, rho);
      model->Add<IdentityLayer<> >();
      model->Add<Linear<> >(1, 10);
      model->Add<FastLSTM<> >(10, 3, rho);
      model->Add<LogSoftMax<> >();
    }

    ~GradientFunction()
    {
      delete model;
    }

    double Gradient(arma::mat& gradient) const
    {
      arma::mat output;
      double error = model->Evaluate(model->Parameters(), 0, 1);
      model->Gradient(model->Parameters(), 0, gradient, 1);
      return error;
    }

    arma::mat& Parameters() { return model->Parameters(); }

    RNN<NegativeLogLikelihood<> >* model;
    arma::cube input, target;
  } function;

  // The threshold should be << 0.1 but since the Fast LSTM layer uses an
  // approximation of the sigmoid function the estimated gradient is not
  // correct.
  BOOST_REQUIRE_LE(CheckGradient(function), 0.2);
}

/**
 * Check if the gradients computed by GRU cell are close enough to the
 * approximation of the gradients.
 */
BOOST_AUTO_TEST_CASE(GradientGRULayerTest)
{
  // GRU function gradient instantiation.
  struct GradientFunction
  {
    GradientFunction()
    {
      input = arma::randu(1, 1, 5);
      target = arma::ones(1, 1, 5);
      const size_t rho = 5;

      model = new RNN<NegativeLogLikelihood<> >(input, target, rho);
      model->Add<IdentityLayer<> >();
      model->Add<Linear<> >(1, 10);
      model->Add<GRU<> >(10, 3, rho);
      model->Add<LogSoftMax<> >();
    }

    ~GradientFunction()
    {
      delete model;
    }

    double Gradient(arma::mat& gradient) const
    {
      arma::mat output;
      double error = model->Evaluate(model->Parameters(), 0, 1);
      model->Gradient(model->Parameters(), 0, gradient, 1);
      return error;
    }

    arma::mat& Parameters() { return model->Parameters(); }

    RNN<NegativeLogLikelihood<> >* model;
    arma::cube input, target;
  } function;

  BOOST_REQUIRE_LE(CheckGradient(function), 1e-4);
}

/**
 * GRU layer manual forward test.
 */
BOOST_AUTO_TEST_CASE(ForwardGRULayerTest)
{
  GRU<> gru(3, 3, 5);

  // Initialize the weights to all ones.
  NetworkInitialization<ConstInitialization>
    networkInit(ConstInitialization(1));
  networkInit.Initialize(gru.Model(), gru.Parameters());

  // Provide input of all ones.
  arma::mat input = arma::ones(3, 1);
  arma::mat output;

  gru.Forward(std::move(input), std::move(output));

  // Compute the z_t gate output.
  arma::mat expectedOutput = arma::ones(3, 1);
  expectedOutput *= -4;
  expectedOutput = arma::exp(expectedOutput);
  expectedOutput = arma::ones(3, 1) / (arma::ones(3, 1) + expectedOutput);
  expectedOutput = (arma::ones(3, 1)  - expectedOutput) % expectedOutput;

  // For the first input the output should be equal to the output of
  // gate z_t as the previous output fed to the cell is all zeros.
  BOOST_REQUIRE_LE(arma::as_scalar(arma::trans(output) * expectedOutput), 1e-2);

  expectedOutput = output;

  gru.Forward(std::move(input), std::move(output));

  double s = arma::as_scalar(arma::sum(expectedOutput));

  // Compute the value of z_t gate for the second input.
  arma::mat z_t = arma::ones(3, 1);
  z_t *= -(s + 4);
  z_t = arma::exp(z_t);
  z_t = arma::ones(3, 1) / (arma::ones(3, 1) + z_t);

  // Compute the value of o_t gate for the second input.
  arma::mat o_t = arma::ones(3, 1);
  o_t *= -(arma::as_scalar(arma::sum(expectedOutput % z_t)) + 4);
  o_t = arma::exp(o_t);
  o_t = arma::ones(3, 1) / (arma::ones(3, 1) + o_t);

  // Expected output for the second input.
  expectedOutput = z_t % expectedOutput + (arma::ones(3, 1) - z_t) % o_t;

  BOOST_REQUIRE_LE(arma::as_scalar(arma::trans(output) * expectedOutput), 1e-2);
}

/**
 * Simple concat module test.
 */
BOOST_AUTO_TEST_CASE(SimpleConcatLayerTest)
{
  arma::mat output, input, delta, error;

  Linear<> moduleA(10, 10);
  moduleA.Parameters().randu();
  moduleA.Reset();

  Linear<> moduleB(10, 10);
  moduleB.Parameters().randu();
  moduleB.Reset();

  Concat<> module;
  module.Add(moduleA);
  module.Add(moduleB);

  // Test the Forward function.
  input = arma::zeros(10, 1);
  module.Forward(std::move(input), std::move(output));

  BOOST_REQUIRE_CLOSE(arma::accu(
      moduleA.Parameters().submat(100, 0, moduleA.Parameters().n_elem - 1, 0)),
      arma::accu(output.col(0)), 1e-3);

  BOOST_REQUIRE_CLOSE(arma::accu(
      moduleB.Parameters().submat(100, 0, moduleB.Parameters().n_elem - 1, 0)),
      arma::accu(output.col(1)), 1e-3);

  // Test the Backward function.
  error = arma::zeros(10, 2);
  module.Backward(std::move(input), std::move(error), std::move(delta));
  BOOST_REQUIRE_EQUAL(arma::accu(delta), 0);
}

/**
 * Concat layer numerically gradient test.
 */
BOOST_AUTO_TEST_CASE(GradientConcatLayerTest)
{
  // Concat function gradient instantiation.
  struct GradientFunction
  {
    GradientFunction()
    {
      input = arma::randu(10, 1);
      target = arma::mat("1");

      model = new FFN<NegativeLogLikelihood<>, NguyenWidrowInitialization>(
          input, target);
      model->Add<IdentityLayer<> >();

      concat = new Concat<>();
      concat->Add<Linear<> >(10, 2);
      model->Add(concat);

      model->Add<LogSoftMax<> >();
    }

    ~GradientFunction()
    {
      delete model;
    }

    double Gradient(arma::mat& gradient) const
    {
      arma::mat output;
      double error = model->Evaluate(model->Parameters(), 0, 1);
      model->Gradient(model->Parameters(), 0, gradient, 1);
      return error;
    }

    arma::mat& Parameters() { return model->Parameters(); }

    FFN<NegativeLogLikelihood<>, NguyenWidrowInitialization>* model;
    Concat<>* concat;
    arma::mat input, target;
  } function;

  BOOST_REQUIRE_LE(CheckGradient(function), 1e-4);
}

/**
 * Simple lookup module test.
 */
BOOST_AUTO_TEST_CASE(SimpleLookupLayerTest)
{
  arma::mat output, input, delta, gradient;
  Lookup<> module(10, 5);
  module.Parameters().randu();

  // Test the Forward function.
  input = arma::zeros(2, 1);
  input(0) = 1;
  input(1) = 3;

  module.Forward(std::move(input), std::move(output));

  // The Lookup module uses index - 1 for the cols.
  const double outputSum = arma::accu(module.Parameters().col(0)) +
      arma::accu(module.Parameters().col(2));

  BOOST_REQUIRE_CLOSE(outputSum, arma::accu(output), 1e-3);

  // Test the Backward function.
  module.Backward(std::move(input), std::move(input), std::move(delta));
  BOOST_REQUIRE_EQUAL(arma::accu(input), arma::accu(input));

  // Test the Gradient function.
  arma::mat error = arma::ones(2, 5);
  error = error.t();
  error.col(1) *= 0.5;

  module.Gradient(std::move(input), std::move(error), std::move(gradient));

  // The Lookup module uses index - 1 for the cols.
  const double gradientSum = arma::accu(gradient.col(0)) +
      arma::accu(gradient.col(2));

  BOOST_REQUIRE_CLOSE(gradientSum, arma::accu(error), 1e-3);
  BOOST_REQUIRE_CLOSE(arma::accu(gradient), arma::accu(error), 1e-3);
}

/**
 * Simple LogSoftMax module test.
 */
BOOST_AUTO_TEST_CASE(SimpleLogSoftmaxLayerTest)
{
  arma::mat output, input, error, delta;
  LogSoftMax<> module;

  // Test the Forward function.
  input = arma::mat("0.5; 0.5");
  module.Forward(std::move(input), std::move(output));
  BOOST_REQUIRE_SMALL(arma::accu(arma::abs(
    arma::mat("-0.6931; -0.6931") - output)), 1e-3);

  // Test the Backward function.
  error = arma::zeros(input.n_rows, input.n_cols);
  // Assume LogSoftmax layer is always associated with NLL output layer.
  error(1, 0) = -1;
  module.Backward(std::move(input), std::move(error), std::move(delta));
  BOOST_REQUIRE_SMALL(arma::accu(arma::abs(
      arma::mat("1.6487; 0.6487") - delta)), 1e-3);
}

/**
 * Simple test for the Sigmoid Cross Entropy Layer.
 */
BOOST_AUTO_TEST_CASE(SimpleSigmoidCrossEntropyLayerTest)
{
  arma::mat input1, input2, input3, output, target1,
            target2, target3, expectedOutput;
  SigmoidCrossEntropyError<> module;

  // Test the Forward function on a user generator input and compare it against
  // the manually calculated result.
  input1 = arma::mat("0.5 0.5 0.5 0.5 0.5 0.5 0.5 0.5");
  target1 = arma::zeros(1, 8);
  double error1 = module.Forward(std::move(input1), std::move(target1));
  double expected = 0.97407699;
  // Value computed using tensorflow.
  BOOST_REQUIRE_SMALL(error1 / input1.n_elem - expected, 1e-7);

  input2 = arma::mat("1 2 3 4 5");
  target2 = arma::mat("0 0 1 0 1");
  double error2 = module.Forward(std::move(input2), std::move(target2));
  expected = 1.5027283;
  BOOST_REQUIRE_SMALL(error2 / input2.n_elem - expected, 1e-6);

  input3 = arma::mat("0 -1 -1 0 -1 0 0 -1");
  target3 = arma::mat("0 -1 -1 0 -1 0 0 -1");
  double error3 = module.Forward(std::move(input3), std::move(target3));
  expected = 0.00320443;
  BOOST_REQUIRE_SMALL(error3 / input3.n_elem - expected, 1e-6);

  // Test the Backward function.
  module.Backward(std::move(input1), std::move(target1), std::move(output));
  expected = 0.62245929;
  for (size_t i = 0; i < output.n_elem; i++)
    BOOST_REQUIRE_SMALL(output(i) - expected, 1e-5);
  BOOST_REQUIRE_EQUAL(output.n_rows, input1.n_rows);
  BOOST_REQUIRE_EQUAL(output.n_cols, input1.n_cols);

  expectedOutput = arma::mat(
      "0.7310586 0.88079709 -0.04742587 0.98201376 -0.00669285");
  module.Backward(std::move(input2), std::move(target2), std::move(output));
  for (size_t i = 0; i < output.n_elem; i++)
    BOOST_REQUIRE_SMALL(output(i) - expectedOutput(i), 1e-5);
  BOOST_REQUIRE_EQUAL(output.n_rows, input2.n_rows);
  BOOST_REQUIRE_EQUAL(output.n_cols, input2.n_cols);

  module.Backward(std::move(input3), std::move(target3), std::move(output));
  expectedOutput = arma::mat("0.5 1.2689414");
  for (size_t i = 0; i < 8; ++i)
  {
    double el = output.at(0, i);
    if (std::abs(input3.at(i) - 0.0) < 1e-5)
      BOOST_REQUIRE_SMALL(el - expectedOutput[0], 2e-6);
    else
      BOOST_REQUIRE_SMALL(el - expectedOutput[1], 2e-6);
  }
  BOOST_REQUIRE_EQUAL(output.n_rows, input3.n_rows);
  BOOST_REQUIRE_EQUAL(output.n_cols, input3.n_cols);
}

/*
 * Simple test for the cross-entropy error performance function.
 */
BOOST_AUTO_TEST_CASE(SimpleCrossEntropyErrorLayerTest)
{
  arma::mat input1, input2, output, target1, target2;
  CrossEntropyError<> module(1e-6);

  // Test the Forward function on a user generator input and compare it against
  // the manually calculated result.
  input1 = arma::mat("0.5 0.5 0.5 0.5 0.5 0.5 0.5 0.5");
  target1 = arma::zeros(1, 8);
  double error1 = module.Forward(std::move(input1), std::move(target1));
  BOOST_REQUIRE_SMALL(error1 - 8 * std::log(2), 2e-5);

  input2 = arma::mat("0 1 1 0 1 0 0 1");
  target2 = arma::mat("0 1 1 0 1 0 0 1");
  double error2 = module.Forward(std::move(input2), std::move(target2));
  BOOST_REQUIRE_SMALL(error2, 1e-5);

  // Test the Backward function.
  module.Backward(std::move(input1), std::move(target1), std::move(output));
  for (double el : output)
  {
    // For the 0.5 constant vector we should get 1 / (1 - 0.5) = 2 everywhere.
    BOOST_REQUIRE_SMALL(el - 2, 5e-6);
  }
  BOOST_REQUIRE_EQUAL(output.n_rows, input1.n_rows);
  BOOST_REQUIRE_EQUAL(output.n_cols, input1.n_cols);

  module.Backward(std::move(input2), std::move(target2), std::move(output));
  for (size_t i = 0; i < 8; ++i)
  {
    double el = output.at(0, i);
    if (input2.at(i) == 0)
      BOOST_REQUIRE_SMALL(el - 1, 2e-6);
    else
      BOOST_REQUIRE_SMALL(el + 1, 2e-6);
  }
  BOOST_REQUIRE_EQUAL(output.n_rows, input2.n_rows);
  BOOST_REQUIRE_EQUAL(output.n_cols, input2.n_cols);
}

/*
 * Simple test for the mean squared error performance function.
 */
BOOST_AUTO_TEST_CASE(SimpleMeanSquaredErrorLayerTest)
{
  arma::mat input, output, target;
  MeanSquaredError<> module;

  // Test the Forward function on a user generator input and compare it against
  // the manually calculated result.
  input = arma::mat("1.0 0.0 1.0 0.0 -1.0 0.0 -1.0 0.0");
  target = arma::zeros(1, 8);
  double error = module.Forward(std::move(input), std::move(target));
  BOOST_REQUIRE_EQUAL(error, 0.5);

  // Test the Backward function.
  module.Backward(std::move(input), std::move(target), std::move(output));
  // We subtract a zero vector, so the output should be equal with the input.
  CheckMatrices(input, output);
  BOOST_REQUIRE_EQUAL(output.n_rows, input.n_rows);
  BOOST_REQUIRE_EQUAL(output.n_cols, input.n_cols);

  // Test the error function on a single input.
  input = arma::mat("2");
  target = arma::mat("3");
  error = module.Forward(std::move(input), std::move(target));
  BOOST_REQUIRE_EQUAL(error, 1.0);

  // Test the Backward function on a single input.
  module.Backward(std::move(input), std::move(target), std::move(output));
  // Test whether the output is negative.
  BOOST_REQUIRE_EQUAL(arma::accu(output), -1);
  BOOST_REQUIRE_EQUAL(output.n_elem, 1);
}

/*
 * Simple test for the BilinearInterpolation layer
 */
BOOST_AUTO_TEST_CASE(SimpleBilinearInterpolationLayerTest)
{
  // Tested output against tensorflow.image.resize_bilinear()
  arma::mat input, output, unzoomedOutput, expectedOutput;
  size_t inRowSize = 2;
  size_t inColSize = 2;
  size_t outRowSize = 5;
  size_t outColSize = 5;
  size_t depth = 1;
  input.zeros(inRowSize * inColSize * depth, 1);
  input[0] = 1.0;
  input[1] = input[2] = 2.0;
  input[3] = 3.0;
  BilinearInterpolation<> layer(inRowSize, inColSize, outRowSize, outColSize,
      depth);
  expectedOutput = arma::mat("1.0000 1.4000 1.8000 2.0000 2.0000 \
      1.4000 1.8000 2.2000 2.4000 2.4000 \
      1.8000 2.2000 2.6000 2.8000 2.8000 \
      2.0000 2.4000 2.8000 3.0000 3.0000 \
      2.0000 2.4000 2.8000 3.0000 3.0000");
  expectedOutput.reshape(25, 1);
  layer.Forward(std::move(input), std::move(output));
  CheckMatrices(output - expectedOutput, arma::zeros(output.n_rows), 1e-12);

  expectedOutput = arma::mat("1.0000 1.9000 1.9000 2.8000");
  expectedOutput.reshape(4, 1);
  layer.Backward(std::move(output), std::move(output),
      std::move(unzoomedOutput));
  CheckMatrices(unzoomedOutput - expectedOutput,
      arma::zeros(input.n_rows), 1e-12);
}

BOOST_AUTO_TEST_SUITE_END();


//-----------------------------------------------------------------------------
// Examples and tests of the constant expression table library.
//-----------------------------------------------------------------------------
//
// Copyright (c) 2013
// Joshua Napoli <jnapoli@alum.mit.edu>
//
// 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)

#define BOOST_TEST_MODULE array_cat_element_test
#include <boost/test/unit_test.hpp>
#include <array_cat_element.hpp>

BOOST_AUTO_TEST_CASE( array_cat_element_function_operator_should_generate_concatenated_sequence )
{
  typedef std::array<int,2> array2_type;
  array2_type a{{1,2}};
  array2_type b{{3,4}};
  array_cat_element<int,2,2> cat{a,b};
  BOOST_CHECK_EQUAL(cat(0),1);
  BOOST_CHECK_EQUAL(cat(1),2);
  BOOST_CHECK_EQUAL(cat(2),3);
  BOOST_CHECK_EQUAL(cat(3),4);
}


#ifndef MAVE_TEST_TOLERANCE_HPP
#define MAVE_TEST_TOLERANCE_HPP

#include <boost/test/included/unit_test.hpp>

namespace mave
{
namespace test
{

template<typename T>
T tolerance_val();

#ifdef MAVE_USE_APPROXIMATION
template<> double tolerance_val<double>(){return 2.0  / 16384.0;}
template<> float  tolerance_val<float >(){return 3.0f / 4096.0f;}
#else
template<> double tolerance_val<double>(){return 1e-8;}
template<> float  tolerance_val<float >(){return 1e-4f;}
#endif

template<typename T>
auto tolerance() -> decltype(boost::test_tools::tolerance(std::declval<T>()))
{
    return boost::test_tools::tolerance(tolerance_val<T>());
}

} // test
} // mave
#endif