#include "HelmertTransformation.h"
#include "Exception.h"
#include <boost/math/constants/constants.hpp>
#include <cstdio>
#include <sstream>
#include <stdexcept>

#ifdef _MSC_VER
#define __PRETTY_FUNCTION__ BOOST_CURRENT_FUNCTION
#endif

Fmi::HelmertTransformation::HelmertTransformation() : m(1), ex(0), ey(0), ez(0), tx(0), ty(0), tz(0)
{
}

boost::array<double, 3> Fmi::HelmertTransformation::operator()(
    const boost::array<double, 3>& x) const
{
  try
  {
    boost::array<double, 3> y = {m * (x[0] + (-ez) * x[1] + (ey)*x[2]) + tx,
                                 m * ((ez)*x[0] + x[1] + (-ex) * x[2]) + ty,
                                 m * ((-ey) * x[0] + (ex)*x[1] + x[2]) + tz};
    return y;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

void Fmi::HelmertTransformation::set_fmi_sphere_to_reference_ellipsoid_conv(
    double r,
    double lat,
    double lon,
    const ReferenceEllipsoid& ref,
    enum FmiSphereConvScalingType scaling_type)
{
  try
  {
    ex = 0;
    ey = 0;
    ez = 0;
    if (scaling_type == FMI_SPHERE_NO_SCALING)
    {
      m = 1;
      const boost::array<double, 3> x0 = ref.to_geocentric(lat, lon, -r);
      tx = x0[0];
      ty = x0[1];
      tz = x0[2];
    }
    else if (scaling_type == FMI_SPHERE_PRESERVE_EAST_WEST_SCALE)
    {
      const double a = ref.get_semimajor_axis();
      const double e = ref.get_eccentricity();
      const double v = sqrt(1 - e * e * sin(lat) * sin(lat));
      m = a / (r * v);
      tx = 0;
      ty = 0;
      tz = -a * e * e * sin(lat) / v;
    }
    else if (scaling_type == FMI_SPHERE_PRESERVE_SOUTH_NORTH_SCALE)
    {
      const double a = ref.get_semimajor_axis();
      const double e = ref.get_eccentricity();
      const double v = sqrt(1 - e * e * sin(lat) * sin(lat));
      const double sf = sin(lat);
      const double cf = cos(lat);
      m = (a * (1 - e * e)) / (r * v * v * v);
      tx = a * e * e * cos(lon) * cf * cf * cf / (v * v * v);
      ty = a * e * e * sin(lon) * cf * cf * cf / (v * v * v);
      tz = -a * e * e * (1 - e * e) * sf * sf * sf / (v * v * v);
    }
    else
    {
      std::ostringstream msg;
      msg << __PRETTY_FUNCTION__ << ": wrong scaling type " << scaling_type;
      throw Fmi::Exception(BCP, msg.str());
    }
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

void Fmi::HelmertTransformation::set_reference_ellipsoid_to_fmi_sphere_conv(
    double r,
    double lat,
    double lon,
    const ReferenceEllipsoid& ref,
    enum FmiSphereConvScalingType scaling_type)
{
  try
  {
    ex = 0;
    ey = 0;
    ez = 0;
    if (scaling_type == FMI_SPHERE_NO_SCALING)
    {
      m = 1;
      const boost::array<double, 3> x0 = ref.to_geocentric(lat, lon, -r);
      tx = -x0[0];
      ty = -x0[1];
      tz = -x0[2];
    }
    else if (scaling_type == FMI_SPHERE_PRESERVE_EAST_WEST_SCALE)
    {
      const double a = ref.get_semimajor_axis();
      const double e = ref.get_eccentricity();
      const double v = sqrt(1 - e * e * sin(lat) * sin(lat));
      m = r * v / a;
      tx = 0;
      ty = 0;
      tz = r * e * e * sin(lat);
    }
    else if (scaling_type == FMI_SPHERE_PRESERVE_SOUTH_NORTH_SCALE)
    {
      const double a = ref.get_semimajor_axis();
      const double e = ref.get_eccentricity();
      const double v = sqrt(1 - e * e * sin(lat) * sin(lat));
      const double sf = sin(lat);
      const double cf = cos(lat);
      m = r * v * v * v / (a * (1 - e * e));
      tx = -r * e * e * cos(lon) * cf * cf * cf / (1 - e * e);
      ty = -r * e * e * sin(lon) * cf * cf * cf / (1 - e * e);
      tz = r * e * e * sf * sf * sf;
    }
    else
    {
      std::ostringstream msg;
      msg << __PRETTY_FUNCTION__ << ": wrong scaling type " << scaling_type;
      throw Fmi::Exception(BCP, msg.str());
    }
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

std::string Fmi::get_fmi_sphere_towgs84_proj4_string(
    double r,
    double lat,
    double lon,
    enum Fmi::HelmertTransformation::FmiSphereConvScalingType scaling_type)
{
  try
  {
    const double AS = 180.0 * 3600.0 / boost::math::constants::pi<double>();
    Fmi::HelmertTransformation conv;
    conv.set_fmi_sphere_to_reference_ellipsoid_conv(
        r, lat, lon, Fmi::ReferenceEllipsoid::wgs84, scaling_type);
    char buffer[512];
#ifndef _MSC_VER
    snprintf(buffer,
             sizeof(buffer),
#else
    _snprintf(buffer,
              sizeof(buffer),
#endif
             "+towgs84=%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.0f ",
             conv.tx,
             conv.ty,
             conv.tz,
             AS * conv.ex,
             AS * conv.ey,
             AS * conv.ez,
             1e6 * (conv.m - 1));
    buffer[sizeof(buffer) - 1] = 0;
    return buffer;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}


#include <stdio.h>  // printf
#include <stdlib.h> // calloc, free
#include <assert.h>
#include <Eigen/Dense>
#include "psmove.h"
#include "psmove_tracker.h"
#include "OVR_CAPI.h"
#include "Extras/OVR_Math.h"

#ifndef OVR_OS_WIN32
//#define ovr_Initialize ovrHmd_Initialize
#define ovr_ConfigureTracking ovrHmd_ConfigureTracking
#define ovr_GetTrackingState ovrHmd_GetTrackingState
#define ovr_RecenterPose ovrHmd_RecenterPose
#define ovr_Destroy ovrHmd_Destroy
#endif

#define NPOSES 300

OVR::Matrix4f getDK2CameraInv44(ovrHmd HMD) {

    ovrTrackingState dk2state;

#ifdef _WIN32
	dk2state = ovr_GetTrackingState(HMD, 0.0, ovrFalse);
#else
    dk2state = ovr_GetTrackingState(HMD, 0.0);
#endif
    OVR::Posef campose(dk2state.CameraPose);
    campose.Rotation.Normalize();  // Probably does nothing as the SDK returns normalized quats anyway.
    campose.Translation *= 100.0;  // m -> cm

    // Print to file - for testing in Matlab
    char *fpath = psmove_util_get_file_path("output_camerapose.csv");
    FILE *fp = fopen(fpath, "w");
    free(fpath);
    fprintf(fp, "%f, %f, %f, %f, %f, %f, %f\n",
        campose.Translation.x, campose.Translation.y, campose.Translation.z,
        campose.Rotation.w, campose.Rotation.x, campose.Rotation.y, campose.Rotation.z);
    fclose(fp);

    OVR::Matrix4f camMat(campose);

    printf("Camera pose 4x4:\n");
    printf("%f, %f, %f, %f\n%f, %f, %f, %f\n%f, %f, %f, %f\n%f, %f, %f, %f\n",
        camMat.M[0][0], camMat.M[0][1], camMat.M[0][2], camMat.M[0][3],
        camMat.M[1][0], camMat.M[1][1], camMat.M[1][2], camMat.M[1][3],
        camMat.M[2][0], camMat.M[2][1], camMat.M[2][2], camMat.M[2][3],
        camMat.M[3][0], camMat.M[3][1], camMat.M[3][2], camMat.M[3][3]);

    camMat.InvertHomogeneousTransform();
    printf("Inverted camera pose 4x4:\n");
    printf("%f, %f, %f, %f\n%f, %f, %f, %f\n%f, %f, %f, %f\n%f, %f, %f, %f\n",
        camMat.M[0][0], camMat.M[0][1], camMat.M[0][2], camMat.M[0][3],
        camMat.M[1][0], camMat.M[1][1], camMat.M[1][2], camMat.M[1][3],
        camMat.M[2][0], camMat.M[2][1], camMat.M[2][2], camMat.M[2][3],
        camMat.M[3][0], camMat.M[3][1], camMat.M[3][2], camMat.M[3][3]);

    return camMat;
}

void ovrmat2eigmat(OVR::Matrix4f& in_ovr, Eigen::Matrix4f& in_eig)
{
    // The following can probably be done with a simple memcpy but oh well.
    int row, col;
    for (row = 0; row < 4; row++)
    {
        for (col = 0; col < 4; col++)
        {
            in_eig(row, col) = in_ovr.M[row][col];
        }
    }
}

int main(int arg, char** args) {

    if (!psmove_init(PSMOVE_CURRENT_VERSION))
    {
        printf("PS Move API init failed (wrong version?)");
        return -1;
    }

    // Setup DK2
    ovrBool ovrresult;
    ovrHmd HMD;
    ovrTrackingState dk2state;
    ovrresult = ovr_Initialize(0);
#if defined(OVR_OS_WIN32)
    ovrGraphicsLuid luid;
    ovr_Create(&HMD, &luid);
#elif defined(OVR_OS_MAC)
    HMD = ovrHmd_Create(0);
#endif
    ovrresult = ovr_ConfigureTracking(HMD,
        ovrTrackingCap_Orientation |
        ovrTrackingCap_MagYawCorrection |
        ovrTrackingCap_Position, 0);  //

    // Initialize variables for our loop.
    OVR::Posef dk2pose;               // The DK2 pose
    OVR::Matrix4f dk2mat;             // The DK2 HMD pose in 4x4
    OVR::Posef psmovepose;            // The psmove pose
    OVR::Matrix4f psmovemat;          // The PSMove pose in 4x4
    OVR::Matrix4f camera_invxform;    // The DK2 camera pose inverse in 4x4

    psmovepose.Rotation = OVR::Quatf::Identity();  // PSMove orientation not used by this algorithm.

    int p = 0;                          // NPOSES counter
    Eigen::MatrixXf A(NPOSES * 3, 15);  // X = A/b
    Eigen::VectorXf b(NPOSES * 3);
    Eigen::Matrix4f dk2eig;             // DK2 pose in Eigen 4x4 mat
    Eigen::Matrix3f RMi;                // Transpose of inner 3x3 of DK2 pose

    // Setup PSMove
    int count = psmove_count_connected();
    PSMove **controllers = (PSMove **)calloc(count, sizeof(PSMove *));

    PSMoveTrackerSettings settings;
    psmove_tracker_settings_set_default(&settings);
    settings.color_mapping_max_age = 0;
    settings.exposure_mode = Exposure_LOW;
    settings.camera_mirror = PSMove_True;
    settings.color_save_colormapping = PSMove_False;
    settings.use_fitEllipse = 1;
    settings.color_list_start_ind = 3;

    PSMoveTracker* tracker = psmove_tracker_new_with_settings(&settings);
    if (tracker == NULL) {
        fprintf(stderr, "No tracker available! (Missing camera?)\n");
        exit(1);
    }

    PSMoveTrackerSmoothingSettings smoothing_settings;
    psmove_tracker_get_smoothing_settings(tracker, &smoothing_settings);
    smoothing_settings.filter_do_2d_r = 0;
    smoothing_settings.filter_do_2d_xy = 0;
    smoothing_settings.filter_3d_type = Smoothing_LowPass;
    psmove_tracker_set_smoothing_settings(tracker, &smoothing_settings);

    enum PSMoveTracker_Status tracking_status = Tracker_TRACKING;
    int result;
    int i = 0;
    controllers[i] = psmove_connect_by_id(i);
    while (1) {
        if (i == 0 && arg >= 3) {
            result = psmove_tracker_enable_with_color(tracker, controllers[i],
                                                      atoi(args[1]), atoi(args[2]), atoi(args[3]));
            printf("Setting LEDS for controller 1 from command-line r: %i, g: %i, b: %i\n",
                   atoi(args[1]), atoi(args[2]), atoi(args[3]));
        }
        else {
            result = psmove_tracker_enable(tracker, controllers[i]);
        }
        if (result == Tracker_CALIBRATED) {
            break;
        } else {
            printf("ERROR - retrying\n");
        }
    }
    assert(psmove_has_calibration(controllers[i]));
    psmove_enable_orientation(controllers[i], PSMove_True);  // Though we don't actually use it.
    assert(psmove_has_orientation(controllers[i]));
    int buttons = psmove_get_buttons(controllers[i]);

    // Start with current camera pose inverse
    camera_invxform = getDK2CameraInv44(HMD);

    // Print the column headers
    char *output_fpath = psmove_util_get_file_path("output.txt");
    FILE *output_fp = fopen(output_fpath, "w");
    free(output_fpath);
    fprintf(output_fp, "psm_px,psm_py,psm_pz,psm_ow,psm_ox,psm_oy,psm_oz,dk2_px,dk2_py,dk2_pz,dk2_ow,dk2_ox,dk2_oy,dk2_oz\n");
    printf("Hold the PSMove controller firmly against the DK2.\n");
    printf("Move them together through the workspace and press the Move button to sample (%d samples required).\n", NPOSES);
    fflush(stdout);
    while (p < NPOSES)
    {
        // Get PSMove position
        psmove_tracker_update_image(tracker);               // Refresh camera
        psmove_tracker_get_location(tracker, controllers[i],  // Copy location to psmovepose
            &psmovepose.Translation.x, &psmovepose.Translation.y, &psmovepose.Translation.z);
        tracking_status = psmove_tracker_get_status(tracker, controllers[i]);

        if (tracking_status != Tracker_TRACKING) {
            printf("PSMove tracker failed.\n");
        }

        // Get PSMove buttons
        while (psmove_poll(controllers[i]));
        buttons = psmove_get_buttons(controllers[i]);

        // If circle button is pressed on PSMove then recenter HMD
        if (buttons & Btn_CIRCLE)
        {
            ovr_RecenterPose(HMD);
            camera_invxform = getDK2CameraInv44(HMD);
        }

        if (buttons & Btn_TRIANGLE)
        {
            psmove_tracker_cycle_color(tracker, controllers[i]);
        }

        // Get DK2 tracking state (contains pose)
#ifdef _WIN32
		dk2state = ovr_GetTrackingState(HMD, 0.0, ovrFalse);
#else
        dk2state = ovr_GetTrackingState(HMD, 0.0);
#endif
        dk2pose = dk2state.HeadPose.ThePose;
        dk2pose.Rotation.Normalize();
        dk2pose.Translation *= 100.0;

        // If MOVE button is pressed on PSMove, sample the position
        if (buttons & Btn_MOVE && tracking_status == Tracker_TRACKING)
        {
            fprintf(output_fp, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
                psmovepose.Translation.x, psmovepose.Translation.y, psmovepose.Translation.z,
                psmovepose.Rotation.w, psmovepose.Rotation.x, psmovepose.Rotation.y, psmovepose.Rotation.z,
                dk2pose.Translation.x, dk2pose.Translation.y, dk2pose.Translation.z,
                dk2pose.Rotation.w, dk2pose.Rotation.x, dk2pose.Rotation.y, dk2pose.Rotation.z);

            dk2mat = OVR::Matrix4f(dk2pose);
            dk2mat = camera_invxform * dk2mat;  // Make the camera pose the new origin, so dk2 is returned relative to that.
            psmovemat = OVR::Matrix4f(psmovepose);

            /*
            if (p == 0)
            {
                printf("PSMove pose V7:\n");
                printf("%f, %f, %f, %f, %f, %f, %f\n",
                    psmovepose.Translation.x, psmovepose.Translation.y, psmovepose.Translation.z,
                    psmovepose.Rotation.w, psmovepose.Rotation.x, psmovepose.Rotation.y, psmovepose.Rotation.z);
                printf("PSMove pose 4x4:\n");
                printf("%f, %f, %f, %f\n%f, %f, %f, %f\n%f, %f, %f, %f\n%f, %f, %f, %f\n",
                    psmovemat.M[0][0], psmovemat.M[0][1], psmovemat.M[0][2], psmovemat.M[0][3],
                    psmovemat.M[1][0], psmovemat.M[1][1], psmovemat.M[1][2], psmovemat.M[1][3],
                    psmovemat.M[2][0], psmovemat.M[2][1], psmovemat.M[2][2], psmovemat.M[2][3],
                    psmovemat.M[3][0], psmovemat.M[3][1], psmovemat.M[3][2], psmovemat.M[3][3]);
            }
            */

            ovrmat2eigmat(dk2mat, dk2eig);
            RMi = dk2eig.topLeftCorner(3, 3).transpose();           // inner 33 transposed

            /*
            int i, j;
            for (i = 0; i < 4; i++)
            {
                printf("\n");
                for (j = 0; j < 4; j++)
                {
                    printf("%5.2f,", psmovemat(i, j));
                }
                printf("\t\t");
                for (j = 0; j < 4; j++)
                {
                    printf("%5.2f,", dk2mat(i, j));
                }
            }
            */

            A.block<3, 3>(p * 3, 0) = RMi * psmovemat.M[0][3];
            A.block<3, 3>(p * 3, 3) = RMi * psmovemat.M[1][3];
            A.block<3, 3>(p * 3, 6) = RMi * psmovemat.M[2][3];
            A.block<3, 3>(p * 3, 9) = RMi;
            A.block<3, 3>(p * 3, 12) = -Eigen::Matrix3f::Identity();
            b.segment(p * 3, 3) = RMi * dk2eig.block<3, 1>(0, 3);
            p++;

            printf("\rSampled %d / %d poses.", p, NPOSES);
            fflush(stdout);
        }

        if (buttons & Btn_SELECT)
            break;
    }

    if (p == NPOSES)
    {
        // TODO: Remove outliers

        /*
        for (p = 0; p < NPOSES * 3; p++)
        {
            printf("%4d: %5.2f,%5.2f,%5.2f,%5.2f,%5.2f,%5.2f,%5.2f,%5.2f,%5.2f,%5.2f,%5.2f,%5.2f,%5.2f,%5.2f,%5.2f\t%5.2f\n", p,
                A(p, 0), A(p, 1), A(p, 2), A(p, 3), A(p, 4),
                A(p, 5), A(p, 6), A(p, 7), A(p, 8), A(p, 9),
                A(p, 10), A(p, 11), A(p, 12), A(p, 13), A(p, 14), b(p));
        }
        */
        Eigen::VectorXf x(15);
        x = A.colPivHouseholderQr().solve(b);
        //globalxfm = reshape(x(1:12), 3, 4);
        //localxfm = [1 0 0 x(12); 0 1 0 x(13); 0 0 1 x(14); 0 0 0 1];
        printf("\nglobalxfm:\n%f,%f,%f,%f\n%f,%f,%f,%f\n%f,%f,%f,%f\n",
            x(0), x(3), x(6), x(9),
            x(1), x(4), x(7), x(10),
            x(2), x(5), x(8), x(11));
        printf("\nlocalxfm:\n%f,%f,%f\n", x(12), x(13), x(14));

        // Save XML to home directory
        char *fpath = psmove_util_get_file_path("transform.csv");
        FILE *fp = fopen(fpath, "w");
        free(fpath);

        // Print XML
        /*
        fprintf(fp, "< ? xml version = \"1.0\" encoding = \"UTF - 8\" ? >\n");
        fprintf(fp, "<globalxform>\n");
        int i, j;
        for (i = 0; i < 3; i++)
        {
            for (j = 0; j < 4; j++)
            {
                fprintf(fp, "    <value row=%d column=%d>%f</value>\n", i, j, x(j * 3 + i));
            }
        }
        fprintf(fp, "</globalxform>\n");
        */

        // Print simple csv
        fprintf(fp, "%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n",
            x(0), x(1), x(2), x(3), x(4), x(5), x(6), x(7), x(8), x(9), x(10), x(11));

        fclose(fp);

    }

    // Cleanup psmove
    psmove_disconnect(controllers[i]);
    psmove_tracker_free(tracker);
    free(controllers);
    psmove_shutdown();

    // Cleanup OVR
    ovr_Destroy(HMD);
    ovr_Shutdown();

    return 0;
}


#include <igl/opengl/glfw/Viewer.h>
#include <igl/writePLY.h>
#include <igl/hausdorff.h>
#include "../../MeshConnectivity.h"
#include <iostream>
#include <random>
#include <igl/opengl/glfw/imgui/ImGuiMenu.h>
#include <igl/opengl/glfw/imgui/ImGuiHelpers.h>
#include <igl/signed_distance.h>
#include <igl/cotmatrix.h>
#include <igl/per_vertex_normals.h>

#include <Eigen/CholmodSupport>
#include <imgui/imgui.h>
#include <json/json.h>

#include "../../SecondFundamentalForm/SecondFundamentalFormDiscretization.h"
#include "../../SecondFundamentalForm/MidedgeAngleSinFormulation.h"
#include "../../SecondFundamentalForm/MidedgeAngleTanFormulation.h"
#include "../../SecondFundamentalForm/MidedgeAverageFormulation.h"
#include "../../ShellSolver.h"
#include "../../WTFShell/MeshUpsampling.h"
#include "../../Obstacle.h"
#include "../../WTFShell/PhiEstimate.h"

#include "../../WTFShell/WTFShell.h"

#include "../../CommonFunctions.h"

#include "../../ZuenkoFormula.h"
#include "../../WTFShell/WTFIO.h"

WTFSetup setup;
WTFState curState;
WTFState initialState;

int quadType;
WTFOptimizationParams WTFOptParams;

double thickness;
double pressure;
double PoissonRatio;
double YoungsModulus;

bool isVisualizeWrinkles; // visualize interior points along the wrinkles
bool isVisualizeObstacle; // visualize the obstacle
int upsamplingTimes = 2;
bool isVisualizeOriginalMesh;
bool clampedChosenVerts;
bool restFlat; // plot tension line
double scale;   // plot scale
bool isSPDProj;
bool isShowProblemFaces; // tension faces after smoothing
bool isUseV1Term;
bool isUseV2Term;

std::string filePathPrefix;
std::string defaultPath;
std::string WTFPath;

void reset()
{
	std::cout << std::endl << "Reset" << std::endl << std::endl;
	curState = initialState;
}

void setParameters()
{
	thickness = setup.thickness;
	YoungsModulus = setup.YoungsModulus;
	PoissonRatio = setup.PoissonsRatio;
	scale = 1;

	isVisualizeWrinkles = false;
	isVisualizeObstacle = false;

	isSPDProj = true;
	quadType = 1;	//

	isVisualizeOriginalMesh = true;
	upsamplingTimes = 4;
	initialState = curState;

	//clampedChosenVerts = !setup.clampedChosenVerts ? setup.clampedChosenVerts : true;
	clampedChosenVerts = setup.clampedChosenVerts;

	isShowProblemFaces = false;
	isUseV1Term = false;
	isUseV2Term = true;
}

void repaint(igl::opengl::glfw::Viewer& viewer)
{
	viewer.data().clear();
	Eigen::MatrixXd curV, obsV, wrinkledV;
	Eigen::MatrixXd curColor, obsColor, wrinkledColor;
	Eigen::MatrixXi curF, obsF, wrinkledF;

	// edges
	Eigen::MatrixXd PStart, PEnd;
	Eigen::RowVector3d edgeColor(0, 0, 0);

	curV.resize(0, 3);
	curF.resize(0, 3);
	curColor.resize(0, 3);

	obsV.resize(0, 3);
	obsF.resize(0, 3);
	obsColor.resize(0, 3);

	wrinkledV.resize(0, 3);
	wrinkledF.resize(0, 3);
	wrinkledColor.resize(0, 3);

	PStart.resize(0, 3);
	PEnd.resize(0, 3);

	bool isFaceBased = true;
	if (isVisualizeOriginalMesh)
	{
		curV = curState.basePos;
		curF = curState.baseMesh.faces();
		std::cout << "visualize base mesh." << std::endl;
		std::cout << "num of verts: " << curV.rows() << ", num of faces: " << curF.rows() << std::endl;

		isFaceBased = false;
		curColor.resize(curV.rows(), 3);
		curColor.col(0).setConstant(1.0);
		curColor.col(1).setConstant(1.0);
		curColor.col(2).setConstant(0.0);
		for (auto& it : setup.clampedDOFs)
		{
			int vid = it.first / 3;
			curColor(vid, 0) = 1.0;
			curColor(vid, 1) = 0.0;
			curColor(vid, 2) = 0.0;
		}
	}

	if (isVisualizeObstacle && setup.obs.size())
	{
		std::cout << "visualize obstacle: " << std::endl;
		obsV = setup.obs[0].V;
		obsF = setup.obs[0].F;

		int colorRows = isFaceBased ? obsF.rows() : obsV.rows();
		obsColor.resize(colorRows, 3);
		for (int i = 0; i < colorRows; i++)
		{
			obsColor.row(i) << 1, 1, 1;
		}
	}

	if (isVisualizeWrinkles)
	{
		curState.getWrinkleMesh(setup, upsamplingTimes, isUseV1Term, isUseV2Term);
		igl::writeOBJ(filePathPrefix + "_wrinkledMesh.obj", curState.wrinkledPos, curState.wrinkledF);

		wrinkledV = curState.wrinkledPos;
		wrinkledF = curState.wrinkledF;
		int colorRows = isFaceBased ? wrinkledF.rows() : wrinkledV.rows();
		wrinkledColor.resize(colorRows, 3);
		for (int i = 0; i < colorRows; i++)
		{
			wrinkledColor.row(i) << 1, 0, 0;
		}
	}

	Eigen::MatrixXd cutStartP, cutEndP;
	Eigen::Vector3d cutcolor;
	if (isShowProblemFaces)
	{
		Eigen::Vector3d vertcolor;
		vertcolor << 1, 1, 1;
		std::set<int> problemFaces = curState.tensionFaces;

		isFaceBased = true;

		curColor.resize(curState.baseMesh.nFaces(), 3);
		curColor.col(0).setConstant(1.0);
		curColor.col(1).setConstant(1.0);
		curColor.col(2).setConstant(0.0);

		for (auto& fid : problemFaces)
		{
			curColor.row(fid) << vertcolor;
		}

		std::cout << "num of problem faces: " << problemFaces.size() << std::endl;
	}

	std::cout << "Setting data (V, F): " << std::endl;
	Eigen::MatrixXd dataV, dataColors;
	Eigen::MatrixXi dataF;

	int totalNumV = curV.rows() + obsV.rows() + wrinkledV.rows();
	int totalNumF = curF.rows() + obsF.rows() + wrinkledF.rows();
	int totalNumColors = curColor.rows() + obsColor.rows() + wrinkledColor.rows();

	dataV.resize(totalNumV, 3);
	dataF.resize(totalNumF, 3);
	dataColors.resize(totalNumColors, 3);

	dataV.block(0, 0, curV.rows(), 3) = curV;
	dataV.block(curV.rows(), 0, obsV.rows(), 3) = obsV;
	dataV.block(curV.rows() + obsV.rows(), 0, wrinkledV.rows(), 3) = wrinkledV;

	dataF.block(0, 0, curF.rows(), 3) = curF;

	for (int i = 0; i < obsF.rows(); i++)
	{
		for(int j = 0; j < 3; j++)
			dataF(curF.rows() + i, j) = obsF(i, j) + curV.rows();
	}

	for (int i = 0; i < wrinkledF.rows(); i++)
	{
		for (int j = 0; j < 3; j++)
			dataF(curF.rows() + obsF.rows() + i, j) = wrinkledF(i, j) + curV.rows() + obsV.rows();
	}

	std::cout << "Setting Colors: " << std::endl;
	dataColors.block(0, 0, curColor.rows(), 3) = curColor;
	dataColors.block(curColor.rows(), 0, obsColor.rows(), 3) = obsColor;
	dataColors.block(curColor.rows() + obsColor.rows(), 0, wrinkledColor.rows(), 3) = wrinkledColor;

	// reset data
	std::cout << "Reset Viewer" << std::endl;
	viewer.data().clear();
	viewer.data().set_face_based(isFaceBased);

	viewer.data().set_mesh(dataV, dataF);
	viewer.data().set_colors(dataColors);

	std::cout << "adding edges: " << std::endl;
	if(PStart.rows())
		viewer.data().add_edges(PStart, PEnd, edgeColor);
	if(cutStartP.rows())
		viewer.data().add_edges(cutStartP, cutEndP, cutcolor);

	std::cout << "Reset Viewer done" << std::endl;
}

int main(int argc, char* argv[])
{
	defaultPath = "../../Sims/disc/disc_WTF.json";
	if (argc >= 2)
		defaultPath = argv[1];
	std::cout << defaultPath << std::endl;
	int index = defaultPath.rfind("_");
	filePathPrefix = defaultPath.substr(0, index);
	std::cout << "Model Name is: " << filePathPrefix << std::endl;
	// generateWTFJson(filePathPrefix);

	bool ok = loadWTF(defaultPath, setup, curState);
	int loadingTimes = 0;
	while (!ok)
	{
		defaultPath = "../" + defaultPath;
		ok = loadWTF(defaultPath, setup, curState);
		loadingTimes++;
		if (loadingTimes >= 5)
		{
			std::cout << "Failed to load Problem with the path" << defaultPath << std::endl;
			return -1;
		}
	}
	WTFPath = defaultPath;
	index = defaultPath.rfind("_");
	filePathPrefix = defaultPath.substr(0, index);
	std::cout << "Model Name is: " << filePathPrefix << std::endl;

	std::cout << "Set Parameters" << std::endl;
	setParameters();

	igl::opengl::glfw::Viewer viewer;

	// Attach a menu plugin
	igl::opengl::glfw::imgui::ImGuiMenu menu;
	viewer.plugins.push_back(&menu);

	// Add content to the default menu window
	menu.callback_draw_viewer_menu = [&]()
	{
		// Draw parent menu content
		//        menu.draw_viewer_menu();
		if (ImGui::CollapsingHeader("I/O", ImGuiTreeNodeFlags_DefaultOpen))
		{
			float w = ImGui::GetContentRegionAvailWidth();
			float p = ImGui::GetStyle().FramePadding.x;
			if (ImGui::Button("Load Problem", ImVec2(ImVec2((w - p) / 2.f, 0))))
			{
				const std::string path = igl::file_dialog_open();
				if (path == "")
				{
					std::cout << "Failed to load problem. Set to the defualt path: " << defaultPath << std::endl;
					WTFPath = defaultPath;
				}
				else
				{
					int index = path.rfind("_");
					filePathPrefix = path.substr(0, index);
					std::cout << "Model Name is: " << filePathPrefix << std::endl;
					WTFPath = path;
				}
				WTFSetup newSetup;
				WTFState newState;
				bool ok = loadWTF(WTFPath, newSetup, newState);
				if (!ok)
				{
					std::cout << "Failed to load files !!!!" << std::endl;
					return;
				}
				else
				{
					setup = newSetup;
					curState = newState;
					setParameters();
					repaint(viewer);
					viewer.core().align_camera_center(viewer.data().V, viewer.data().F);
				}
			}
			ImGui::SameLine(0, p);
			if (ImGui::Button("Save Problem", ImVec2((w - p) / 2.f, 0)))
			{
				const std::string path = igl::file_dialog_save();
				if (path == "")
				{
					std::cout << "Invalid saving path" << std::endl;
				}
				else
				{
					bool ok = saveWTF(path, setup, curState);
					if (!ok)
						std::cout << "Failed to save problem." << std::endl;
				}
			}

			if (ImGui::Button("Save Wrinkled Mesh", ImVec2(-1, 0)))
			{

				std::string path = igl::file_dialog_save();
				Eigen::MatrixXd NV, NVSeam;
				Eigen::MatrixXi NF, NFSeam;
				Eigen::MatrixXd upsampledTFTV, soupPhiV, soupProblemV;
				Eigen::MatrixXi upsampledTFTF, soupPhiF, soupProblemF;
				Eigen::VectorXd upsampledAmp, soupPhi;

				Eigen::MatrixXd curV = curState.basePos;
				Eigen::MatrixXi curF = curState.baseMesh.faces();
				Eigen::VectorXd curAmp = curState.amplitude;
				Eigen::VectorXd curPhi = curState.phi;
				std::set<int> clampedVerts;
				if (setup.clampedChosenVerts)
				{
					for (auto& it : setup.clampedDOFs)
					{
						int vid = it.first / 3;
						if (clampedVerts.count(vid) == 0)
							clampedVerts.insert(vid);
					}
				}
				std::set<int> problemFaces;
				roundPhiFromDphiCutbyTension(curState.basePos, curState.baseMesh.faces(), curV, curF, setup.abars, curState.amplitude, curState.dphi, GurobiRound, curState.phi, curPhi, curAmp, problemFaces);
				wrinkledMeshUpsamplingUncut(curState.basePos, curState.baseMesh.faces(), setup.restV, setup.restF, curV, curF, problemFaces, clampedVerts,
					&NV, &NF, &upsampledTFTV, &upsampledTFTF, &soupPhiV, &soupPhiF, &soupProblemV, &soupProblemF, &upsampledAmp, &soupPhi,
					curAmp, curPhi, *setup.sff, setup.YoungsModulus, setup.PoissonsRatio, upsamplingTimes, Loop, isUseV1Term, isUseV2Term);

				// stitchMeshesWithTol(NVSeam, NFSeam, NV, NF, 1e-5);
				if (!igl::writeOBJ(path, NV, NF))
				{
					path = filePathPrefix + "_wrinkledMesh.obj";
					std::cout << "Invalid path, use current path: " << path << std::endl;
				}
				size_t lastindex = path.find_last_of(".");
				std::string rawname = path.substr(0, lastindex);
				std::string upsampledTFTpath = rawname + "_upsampledTFT.obj";
				igl::writeOBJ(upsampledTFTpath, upsampledTFTV, upsampledTFTF);
				std::string soupPath = rawname + "_phiSoup.obj";
				igl::writeOBJ(soupPath, soupPhiV, soupPhiF);
				std::string ampPath = rawname + "_upsampledAmp.csv";
				std::ofstream ampFile(ampPath);
				for (int i = 0; i < upsampledAmp.size(); i++)
					ampFile << upsampledAmp[i] << ",\t" << 3.14159 << std::endl; // fix for buggy Houdini import
				std::string phiPath = rawname + "_phiSoup.csv";
				std::ofstream phiFile(phiPath);
				for (int i = 0; i < soupPhi.size(); i++)
					phiFile << soupPhi[i] << ",\t" << 3.14159 << std::endl;

				std::string problempath = rawname + "_problem.obj";
				igl::writeOBJ(problempath, soupProblemV, soupProblemF);
			}
		}
		if (ImGui::CollapsingHeader("Viewing Options", ImGuiTreeNodeFlags_DefaultOpen))
		{
			if (ImGui::Button("Center object", ImVec2(-1, 0)))
			{
				viewer.core().align_camera_center(viewer.data().V, viewer.data().F);
			}
			if (ImGui::Button("Snap canonical view", ImVec2(-1, 0)))
			{
				viewer.snap_to_canonical_quaternion();
			}

			// Select rotation type
			int rotation_type = static_cast<int>(viewer.core().rotation_type);
			//int rotation_type = 0;
			static Eigen::Quaternionf trackball_angle = Eigen::Quaternionf::Identity();
			static bool orthographic = true;
			if (ImGui::Combo("Camera Type", &rotation_type, "Trackball\0Two Axes\02D Mode\0\0"))
			{
				using RT = igl::opengl::ViewerCore::RotationType;
				auto new_type = static_cast<RT>(rotation_type);
				if (new_type != viewer.core().rotation_type)
				{
					if (new_type == RT::ROTATION_TYPE_NO_ROTATION)
					{
						trackball_angle = viewer.core().trackball_angle;
						orthographic = viewer.core().orthographic;
						viewer.core().trackball_angle = Eigen::Quaternionf::Identity();
						viewer.core().orthographic = true;
					}
					else if (viewer.core().rotation_type == RT::ROTATION_TYPE_NO_ROTATION)
					{
						viewer.core().trackball_angle = trackball_angle;
						viewer.core().orthographic = orthographic;
					}
					viewer.core().set_rotation_type(new_type);
				}
			}

			// Orthographic view
			ImGui::Checkbox("Orthographic view", &(viewer.core().orthographic));
		}
		 // Draw options
		if (ImGui::CollapsingHeader("Draw Options", ImGuiTreeNodeFlags_DefaultOpen))
		{
			if (ImGui::InputDouble("plot scale", &scale))
			{
				repaint(viewer);
			}
			if (ImGui::Checkbox("Visualize Wrinkles", &isVisualizeWrinkles))
			{
				repaint(viewer);
			}

			if (ImGui::Checkbox("v1 in plane correction", &isUseV1Term))
			{
				repaint(viewer);
			}
			if (ImGui::Checkbox("v2 in plane correction", &isUseV2Term))
			{
				repaint(viewer);
			}
			if (ImGui::InputInt("upsampling times", &upsamplingTimes))
			{
				if (upsamplingTimes >= 0)
					repaint(viewer);
			}
			if (ImGui::Checkbox("Visualize Original Mesh", &isVisualizeOriginalMesh))
			{
				repaint(viewer);
			}
			if (ImGui::Checkbox("Visualize Obstacle", &isVisualizeObstacle))
			{
				repaint(viewer);
			}

			if (ImGui::Checkbox("show problem faces", &isShowProblemFaces))
			{
				repaint(viewer);
			}

			if (ImGui::Checkbox("Invert normals", &(viewer.data().invert_normals)))
			{
				viewer.data().dirty |= igl::opengl::MeshGL::DIRTY_NORMAL;
			}
			ImGui::ColorEdit4("Background", viewer.core().background_color.data(),
				ImGuiColorEditFlags_NoInputs | ImGuiColorEditFlags_PickerHueWheel);
			ImGui::ColorEdit4("Line color", viewer.data().line_color.data(),
				ImGuiColorEditFlags_NoInputs | ImGuiColorEditFlags_PickerHueWheel);
			ImGui::PushItemWidth(ImGui::GetWindowWidth() * 0.3f);
			ImGui::DragFloat("Shininess", &(viewer.data().shininess), 0.05f, 0.0f, 100.0f);
			//ImGui::PopItemWidth();
		}

		// Overlays
		auto make_checkbox = [&](const char* label, unsigned int& option)
		{
			return ImGui::Checkbox(label,
				[&]() { return viewer.core().is_set(option); },
				[&](bool value) { return viewer.core().set(option, value); }
			);
		};

		if (ImGui::CollapsingHeader("Overlays", ImGuiTreeNodeFlags_DefaultOpen))
		{
			make_checkbox("Wireframe", viewer.data().show_lines);
			make_checkbox("Fill", viewer.data().show_faces);
			bool showvertid = viewer.data().show_vertex_labels != 0;
			if (ImGui::Checkbox("Show vertex labels", &showvertid))
			{
				viewer.data().show_vertex_labels = (showvertid ? 1 : 0);
			}
			bool showfaceid = viewer.data().show_face_labels != 0;
			if (ImGui::Checkbox("Show faces labels", &showfaceid))
			{
				viewer.data().show_face_labels = showfaceid;
			}
		}
	};
	menu.callback_draw_custom_window = [&]()
	{
		// Define next window position + size
		ImGui::SetNextWindowPos(ImVec2(180.f * menu.menu_scaling(), 10), ImGuiCond_FirstUseEver);
		ImGui::SetNextWindowSize(ImVec2(0.0, 0.0), ImGuiCond_FirstUseEver);
		ImGui::Begin(
			"Optimization", nullptr,
			ImGuiWindowFlags_NoSavedSettings
		);

		if (ImGui::CollapsingHeader("Material Parameters", ImGuiTreeNodeFlags_DefaultOpen))
		{
			if (ImGui::InputDouble("Thickness", &thickness))
			{
				if (thickness > 0)
				{
					setup.thickness = thickness;
				}
			}
			if (ImGui::InputDouble("Young's Modulus", &YoungsModulus))
			{
				if (YoungsModulus > 0)
				{
					setup.YoungsModulus = YoungsModulus;
				}
			}
			if (ImGui::InputDouble("Poisson's Ratio", &PoissonRatio))
			{
				if (PoissonRatio >= 0)
				{
					setup.PoissonsRatio = PoissonRatio;
				}
			}
		}

		if (ImGui::CollapsingHeader("Optimzation Parameters", ImGuiTreeNodeFlags_DefaultOpen))
		{
			if (ImGui::InputInt("Max iterations", &WTFOptParams.iterations))
			{
				if (!(WTFOptParams.iterations > 0))
				{
					WTFOptParams.iterations = 1e3;
				}
			}
			if (ImGui::InputDouble("Grad Tol", &WTFOptParams.gradNorm))
			{
				if (!(WTFOptParams.gradNorm >= 0))
				{
					WTFOptParams.gradNorm = 1e-10;
				}
			}
			if (ImGui::InputDouble("delta_f", &WTFOptParams.fDelta))
			{
				if (!(WTFOptParams.fDelta >= 0))
				{
					WTFOptParams.fDelta = 1e-10;
				}
			}
			if (ImGui::InputDouble("delta_x", &WTFOptParams.xDelta))
			{
				if (!(WTFOptParams.xDelta >= 0))
				{
					WTFOptParams.xDelta = 1e-10;
				}
			}
			ImGui::Checkbox("WTF TBB Parallel", &WTFOptParams.isParallel);
		}
		if (ImGui::CollapsingHeader("WTF Type", ImGuiTreeNodeFlags_DefaultOpen))
		{
			if (ImGui::Combo("Quadrature number", &quadType, "one\0three\0six\0sixteen\0\0"))
			{
				switch (quadType)
				{
				case 0:
					setup.quadNum = 1;
					break;
				case 1:
					setup.quadNum = 3;
					break;
				case 2:
					setup.quadNum = 6;
					break;
				case 3:
					setup.quadNum = 16;
					break;
				default:
					setup.quadNum = 3;
					break;
				}
				setup.buildQuadraturePoints();
				std::cout << "number of quadrature points used : " << setup.quadPoints.size() << std::endl;
			}

			ImGui::Checkbox("clamp chosen(red) verts", &setup.clampedChosenVerts);
			if (ImGui::Button("Reinitalization", ImVec2(-1, 0)))
			{
				curState.reinitializeWrinkleVaribles(setup);
				initialState = curState;
				repaint(viewer);
			}

		}

		if (ImGui::Button("Optimize WTF", ImVec2(-1, 0)))
		{
			std::cout << "Start with: " << std::endl;
			std::cout << "norm of phi : " << curState.phi.norm() << std::endl;
			std::cout << "norm of dphi : " << curState.dphi.norm() << std::endl;
			std::cout << "norm of amplitude : " << curState.amplitude.norm() << std::endl;

			std::cout << "file path prefix: " << filePathPrefix << std::endl;

			ShellSolver::WTFSQPSolver(setup, curState, filePathPrefix, WTFOptParams);
			saveWTF(WTFPath, setup, curState);

			std::cout << "end with: " << std::endl;
			std::cout << "norm of phi : " << curState.phi.lpNorm<Eigen::Infinity>() << std::endl;
			std::cout << "norm of dphi : " << curState.dphi.lpNorm<Eigen::Infinity>() << std::endl;
			std::cout << "norm of sqrt amplitude : " << curState.amplitude.lpNorm<Eigen::Infinity>() << std::endl;

			Eigen::MatrixXd NV, NVSeam;
			Eigen::MatrixXi NF, NFSeam;

			Eigen::MatrixXd curV = curState.basePos;
			Eigen::MatrixXi curF = curState.baseMesh.faces();
			Eigen::VectorXd curAmp = curState.amplitude;
			Eigen::VectorXd curPhi = curState.phi;

			std::string filename = filePathPrefix + "_wrinkledMesh.obj";
			igl::writeOBJ(filename.c_str(), curState.wrinkledPos, curState.wrinkledF);

			isVisualizeWrinkles = true;
			repaint(viewer);
		}
		ImGui::End();
	};
	viewer.data().set_face_based(false);
	std::cout << "set data done!" << std::endl;
	repaint(viewer);
	std::cout << "Repaint done!" << std::endl;
	viewer.launch();
	return 0;

}


#include <El.hpp>
#include <skylark.hpp>
#include <fstream>
#include <iostream>
#include <sstream>
#include <cstdlib>
#include <string>
#include <boost/program_options.hpp>
#include "hilbert.hpp"

#if SKYLARK_HAVE_OPENMP
#include <omp.h>
#endif

int main(int argc, char* argv[]) {

    El::Initialize (argc, argv);

    boost::mpi::environment env (argc, argv);
    boost::mpi::communicator comm;

    hilbert_options_t options (argc, argv, comm.size());
    skylark::base::context_t context (options.seed);

    if (options.exit_on_return) { return -1; }

    bool sparse = (options.fileformat == LIBSVM_SPARSE)
        || (options.fileformat == HDF5_SPARSE);

    SKYLARK_BEGIN_TRY()

    if (!options.trainfile.empty()) {
        // Training
        if (comm.rank() == 0) {
            std::cout << options.print();
            std::cout << "Mode: Training. Loading data..." << std::endl;
        }

        if (sparse) {
            skylark::base::sparse_matrix_t<double> X;
            El::Matrix<double> Y;
            read(comm, options.fileformat, options.trainfile, X, Y);
            skylark::ml::LargeScaleKernelLearning(comm, X, Y, context, options);
        } else {
            El::Matrix<double> X, Y;
            read(comm, options.fileformat, options.trainfile, X, Y);
            skylark::ml::LargeScaleKernelLearning(comm, X, Y, context, options);
        }
    } else if (!options.testfile.empty()) {
        // Testing from file
        if (comm.rank() == 0) {
            std::cout << "Mode: Predicting (from file). Loading data..."
                      << std::endl;
            std::cout << options.print();
        }

        El::Matrix<double> Y;
        El::DistMatrix<double, El::VC, El::STAR> DecisionValues;
        El::DistMatrix<El::Int, El::VC, El::STAR> PredictedLabels;
        El::Int n;
        skylark::ml::hilbert_model_t model(options.modelfile);

        if (sparse) {
            skylark::base::sparse_matrix_t<double> X;
            read(comm, options.fileformat, options.testfile, X, Y,
                model.get_input_size());

            boost::mpi::all_reduce(comm, Y.Height(), n, std::plus<El::Int>());
            PredictedLabels.Resize(n, 1);
            DecisionValues.Resize(n, model.get_output_size());

            model.predict(X, PredictedLabels.Matrix(), DecisionValues.Matrix(),
                options.numthreads);
        } else {
            El::Matrix<double> X;
            read(comm, options.fileformat, options.testfile, X, Y,
                model.get_input_size());

            boost::mpi::all_reduce(comm, Y.Height(), n, std::plus<El::Int>());
            PredictedLabels.Resize(n, 1);
            DecisionValues.Resize(n, model.get_output_size());

            model.predict(X, PredictedLabels.Matrix(), DecisionValues.Matrix(),
                options.numthreads);
        }

        if (model.is_regression()) {

            // TODO can be done better if Y and VC,STAR as well...
            El::Matrix<double> Ye = DecisionValues.Matrix();
            El::Axpy(-1.0, Y, Ye);
            double localerr = std::pow(El::Nrm2(Ye), 2);
            double localnrm = std::pow(El::Nrm2(Y), 2);
            double err, nrm;
            boost::mpi::reduce(comm, localerr, err,
                std::plus<double>(), 0);
            boost::mpi::reduce(comm, localnrm, nrm,
                std::plus<double>(), 0);

            if (!options.outputfile.empty())
                El::Write(DecisionValues, options.outputfile, El::ASCII);

	    if (comm.rank() == 0)
	      std::cout << "Error rate: "
                        << boost::format("%.4e") % std::sqrt(err / nrm)
                        << std::endl;

        } else {
            El::Int correct = skylark::ml::classification_accuracy(Y,
                DecisionValues.Matrix());
            El::Int totalcorrect, total;
            boost::mpi::reduce(comm, correct, totalcorrect,
                std::plus<El::Int>(), 0);

            if (!options.outputfile.empty()) {
                if (options.decisionvals)
                    El::Write(DecisionValues, options.outputfile, El::ASCII);
                else
                    El::Write(PredictedLabels, options.outputfile, El::ASCII);
            }

	    if(comm.rank() == 0)
	      std::cout << "Error rate: "
			<< boost::format("%.2f") % ((n - totalcorrect) * 100.0 / n)
			<< "%" << std::endl;

        }

        // TODO list?
        // fix logistic case
        // option for probabilities in predicition
        // clean up evaluate
    } else {
        // Preidicting from stdin mode
        skylark::ml::hilbert_model_t model(options.modelfile);

        El::Matrix<double> X, DecisionValues(1, model.get_output_size());
        El::Matrix<El::Int> PredictedLabel(1, 1);

        while(true) {
            std::string line, token;
            std::getline(std::cin, line);
            if (line.empty())
                break;

            El::Zeros(X, model.get_input_size(), 1);

            std::istringstream tokenstream(line);
            while (tokenstream >> token) {
                int delim  = token.find(':');
                std::string ind = token.substr(0, delim);
                std::string val = token.substr(delim+1);
                X.Set(atoi(ind.c_str()) - 1, 0, atof(val.c_str()));
            }

            model.predict(X, PredictedLabel, DecisionValues,
                options.numthreads);

            std::cout <<
                (model.is_regression() ?
                    DecisionValues.Get(0,0) : PredictedLabel.Get(0, 0))
                      << std::endl;
        }
    }

    comm.barrier();

    SKYLARK_END_TRY() SKYLARK_CATCH_AND_PRINT((comm.rank() == 0))

    El::Finalize();

    return 0;
}


#include "TROOT.h"
#include "TStyle.h"
#include "TFile.h"
#include "TH1.h"
#include "TGraphAsymmErrors.h"
#include "TCanvas.h"
#include <cmath>
#include <fstream>
#include <iostream>

#include <exception>
#include <iterator>
#include <string>
#include <vector>

#if (defined (STANDALONE) or defined (__CINT__) )
    #include "ClopperPearsonBinomialInterval.h"
#else
    #include "PhysicsTools/RooStatsCms/interface/ClopperPearsonBinomialInterval.h"
#endif

#include <boost/program_options.hpp>
using namespace boost;
namespace po = boost::program_options;
using namespace std;

// A helper function to simplify the main part.
template<class T>
ostream& operator<<(ostream& os, const vector<T>& v) {
  copy(v.begin(), v.end(), ostream_iterator<T>(cout, " "));
  return os;
}

int main(int ac, char *av[]) {
  gROOT->SetBatch(kTRUE);
  gROOT->SetStyle("Plain");

  try{
    string file;
    string num;
    string den;
    string ext;
    string pname;
    unsigned int rebin=1;
    po::options_description desc("Allowed options");
    desc.add_options()
      ("help,h", "produce help message")
      ("input-file,i", po::value<string >(&file), "input file")
      ("num,n", po::value<string > (&num), "numHisto")
      ("den,d", po::value<string > (&den), "denHisto")
      ("plotname,o", po::value<string > (&pname), "plot name")
      ("rebin,r", po::value<unsigned int > (&rebin)->default_value(1), "rebin")
      ("plot-format,p", po::value<string>(&ext)->default_value("gif"),
       "output plot format");

    po::positional_options_description p;
    po::variables_map vm;
    po::store(po::command_line_parser(ac, av).
	      options(desc).positional(p).run(), vm);
    po::notify(vm);

    if (vm.count("help")) {
      cout << "Usage: options_description [options]\n";
      cout << desc;
      return 0;
    }

   	TFile * root_file = new TFile(file.c_str(),"read");
	//TFile * root_file = new TFile("MuTrigger_133874_133828_768ub.root","read");
	//string d_string = *itd;
	string  dirDen =   den;
	string  dirNum =   num;

	TH1D * denh = (TH1D*) root_file->Get( dirDen.c_str()  );
	TH1D * numh = (TH1D*) root_file->Get( dirNum.c_str() );

         int bins = denh->GetXaxis()->GetNbins();
        bins = bins/rebin;
	const double xMax = denh->GetXaxis()->GetXmax();
  	const double xMin = denh->GetXaxis()->GetXmin();
	//std::cout << "xMax " << xMax << endl;
	//std::cout << "xMin " << xMax << endl;
	//std::cout << "bins " << bins << endl;
	double * x = new double[bins];
	double *eff = new double[bins];
	double * exl= new double[bins];
	double *exh = new double[bins];
	double *  eefflCP= new double[bins];
	double * eeffhCP = new double[bins];

	ClopperPearsonBinomialInterval cp;
	//  alpha = 1 - CL
	const double alpha = (1-0.682);
	cp.init(alpha);
	TH1D histo("histo", "Efficiency", bins, xMin, xMax);

	for(int i = 1; i <= bins; i++) {
          int j = i-1;
	  x[j] = ((double(i-0.5)) * (xMax - xMin) / (bins )) + xMin;
	    int n0 = denh->GetBinContent(i);
	  //	  std::cout << " n0 " << n0 << endl;
	   int n1 = numh->GetBinContent(i);
	  // std::cout << " n1 " << n1 << endl;
	  if ( n0!=0) {
	    eff[j] = double(n1)/double(n0);
	    histo.SetBinContent(i,eff[j]);
	    exl[j] = exh[j] = 0;
	    cp.calculate(n1, n0);
	    eefflCP[j] = eff[j] - cp.lower();
	    eeffhCP[j] = cp.upper() - eff[j];
	  } else {
	    eff[j]=0;
	    histo.SetBinContent(i,eff[j]);
	    exl[j] = exh[j] = 0;
	    //cp.calculate(n1, n0);
	    eefflCP[i] = 0;
	    eeffhCP[i] = 0;

	  }
	  //std::cout<< "x[j] " <<x[j]<<std::endl;
	  //std::cout<< "n0 " <<n0<<std::endl;
	  //std::cout<< "n1 " <<n1<<std::endl;
	  //histo.SetBinContent(i+1,eff[i]);
	      //exl[i] = exh[i] = 0;
	      //cp.calculate(n1, n0);
	      //eefflCP[i] = eff[i] - cp.lower();
	      //eeffhCP[i] = cp.upper() - eff[i];
	}
	TGraphAsymmErrors graphCP(bins, x, eff, exl, exh, eefflCP, eeffhCP);
	graphCP.SetTitle("trigger (HLT_Mu9 path) efficiency");
	graphCP.SetMarkerColor(kRed);
	graphCP.SetMarkerStyle(21);
	graphCP.SetLineWidth(1);
	graphCP.SetLineColor(kRed);
        string cname = pname;
	TCanvas * c = new TCanvas(cname.c_str());
	gStyle->SetOptStat(0);
	histo.SetTitle("MC trigger (HLT_Mu9 path) efficiency");
	histo.Draw();
	histo.SetLineColor(kWhite);
        histo.SetMinimum(0.0)
;	histo.GetXaxis()->SetTitle("p_{T} (GeV/c)");
	histo.GetYaxis()->SetTitle("efficiency");
        graphCP.Draw("P");
    	string plot= "HLTMu_9" +  pname + "." +  ext ;
	c->SaveAs(plot.c_str());
        string outfile= "Effhisto" +   pname + ".root";
	TFile * output_file = TFile::Open(outfile.c_str(), "recreate");
        string outdir =  pname;
	TDirectory * dir = output_file->mkdir(outdir.c_str());
	dir->cd();
        c->Write();
        histo.Write();
        graphCP.Write();
	output_file->Close();
        delete c;
      }

  catch(std::exception& e) {
    cerr << "error: " << e.what() << "\n";
    return 1;
  }
  catch(...) {
    cerr << "Exception of unknown type!\n";
  }

  return 0;
}