#include <sophus/se3.hpp>
#include <string>
#include <iostream>
#include <fstream>
#include <Eigen/Core>
#include <unistd.h>
// need pangolin for plotting trajectory
#include <pangolin/pangolin.h>

using namespace std;

// path to trajectory file
string trajectory_file = "../trajectory.txt";
string groundtruth_file = "../groundtruth.txt";
string estimated_file = "../estimated.txt";

typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;

// start point is red and end point is blue
void DrawTrajectory(TrajectoryType);
void DrawTwoTrajectory(TrajectoryType, TrajectoryType);
void CalculateError(TrajectoryType, TrajectoryType );

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

    TrajectoryType poses;
    TrajectoryType groundtruth;
    TrajectoryType estimated;

    /// implement pose reading code
    std::ifstream file_reader(trajectory_file);
    std::ifstream groundtruth_reader(groundtruth_file);
    std::ifstream estimated_reader(estimated_file);
    double time, tx, ty, tz, qx, qy, qz, qw;

    // Method 1
    // while (!file_reader.eof()){
    //     file_reader >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
    //     Eigen::Vector3d t(tx, ty, tz);
    //     Eigen::Quaterniond q(qw, qx, qy, qz);
    //     q.normalize();
    //     Eigen::Matrix3d Rotation_Matrix(q);
    //     Sophus::SE3d SE3_from_Eigen(Rotation_Matrix, t);
    //     poses.push_back(SE3_from_Eigen);
    // }
    // cout << "There are " << poses.size() << " timestamps." << endl;

    // Method 2
    while(file_reader >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw){
        poses.push_back(Sophus::SE3d(Eigen::Quaterniond(qw, qx, qy, qz), Eigen::Vector3d(tx, ty, tz)));
    }
    cout << "There are " << poses.size() << " timestamps." << endl;

    //    DrawTrajectory(poses);

    while(groundtruth_reader >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw){
        groundtruth.push_back(Sophus::SE3d(Eigen::Quaterniond(qw, qx, qy, qz), Eigen::Vector3d(tx, ty, tz)));
    }
    cout << "There are " << groundtruth.size() << " timestamps." << endl;

    while(estimated_reader >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw){
        estimated.push_back(Sophus::SE3d(Eigen::Quaterniond(qw, qx, qy, qz), Eigen::Vector3d(tx, ty, tz)));
    }
    cout << "There are " << estimated.size() << " timestamps." << endl;
    DrawTwoTrajectory(groundtruth, estimated);

    CalculateError(groundtruth, estimated);

    return 0;
}

void CalculateError(TrajectoryType groundtruth, TrajectoryType estimated){
    double error(0.0);
    double rmse(0.0);
    for (size_t i = 0; i < groundtruth.size(); i++){
        error = (groundtruth[i].inverse() * estimated[i]).log().norm();
        rmse += error * error;
    }
    rmse = sqrt(rmse / groundtruth.size());
    cout << "rmse: " << rmse << endl;
}

/*******************************************************************************************/
void DrawTrajectory(TrajectoryType poses) {

    if (poses.empty()) {
        cerr << "Trajectory is empty!" << endl;
        return;
    }

    // create pangolin window and plot the trajectory
    pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    pangolin::OpenGlRenderState s_cam(
            pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
            pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );

    pangolin::View &d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
            .SetHandler(new pangolin::Handler3D(s_cam));

    while (pangolin::ShouldQuit() == false) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

        glLineWidth(2);
        for (size_t i = 0; i < poses.size() - 1; i++) {
            glColor3f(1 - (float) i / poses.size(), 0.0f, (float) i / poses.size());
            glBegin(GL_LINES);
            auto p1 = poses[i], p2 = poses[i + 1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }

}

void DrawTwoTrajectory(TrajectoryType poses1, TrajectoryType poses2) {
    if (poses1.empty() || poses2.empty()) {
        cerr << "Trajectory is empty!" << endl;
        return;
    }

    // create pangolin window and plot the trajectory
    pangolin::CreateWindowAndBind("Groundtruth: red, Estimated: blue", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    pangolin::OpenGlRenderState s_cam(
            pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
            pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );

    pangolin::View &d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
            .SetHandler(new pangolin::Handler3D(s_cam));

    while (pangolin::ShouldQuit() == false) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

        glLineWidth(2);
        for (size_t i = 0; i < poses1.size() - 1; i++) {
            glColor3f(1.0f, 0.0f, 0.0f);
            glBegin(GL_LINES);
            auto p1 = poses1[i], p2 = poses1[i + 1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();

            glColor3f(0.0f, 0.0f, 1.0f);
            glBegin(GL_LINES);
            auto p3 = poses2[i], p4 = poses2[i + 1];
            glVertex3d(p3.translation()[0], p3.translation()[1], p3.translation()[2]);
            glVertex3d(p4.translation()[0], p4.translation()[1], p4.translation()[2]);
            glEnd();
        }
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }
}


#include "hsi_data_reader.h"
#include "spectral.h"
#include "settings.h"
#include "write_log_file.h"
#include <cmath>
#include <Eigen/Dense>
#include <chrono>
#include <filesystem>

#include "opencv2/highgui.hpp"
#include <vector>
#include <iostream>
#include <sstream>
#include <ctype.h>
#include <cstdlib>
#include <unordered_map>

using hsi::HSIData;
using hsi::HSIDataOptions;
using hsi::HSIDataReader;

float normalPDF(float s, float x, float m){
  return (1/(s*2.50663))*std::pow(2.71828,-0.5*std::pow((x-m)/s,2.0));
}

// convert a comma seperate sting list of the form
// [first, second, third, ... , last]
// to a vector of strings
std::vector<std::string> stringCS2Vector(std::string str){
  str.erase(std::remove(str.begin(), str.end(), '}'), str.end());
  str.erase(std::remove(str.begin(), str.end(), '{'), str.end());
  str.erase(std::remove(str.begin(), str.end(), ']'), str.end());
  str.erase(std::remove(str.begin(), str.end(), '['), str.end());
  std::vector<std::string> result;
  std::stringstream ss(str);
  while (ss.good()){
    std::string substr;
    std::getline(ss, substr, ',');
    result.push_back(substr);
  }
  return result;
}

// convert a comma seperate sting list of the form
// {first, second, third, ... , last}
// to a Eigen::VectorXf
Eigen::VectorXf stringCS2VectorFloats(std::string str, int len){
  Eigen::VectorXf result(len);
  str.erase(std::remove(str.begin(), str.end(), '}'), str.end());
  str.erase(std::remove(str.begin(), str.end(), '{'), str.end());
  str.erase(std::remove(str.begin(), str.end(), ']'), str.end());
  str.erase(std::remove(str.begin(), str.end(), '['), str.end());
  std::stringstream ss(str);
  int i = 0;
  while (ss.good()){
    std::string substr;
    std::getline(ss, substr, ',');
    float num_float = std::stof(substr);
    result(i) = num_float;
    i++;
  }
  return result;
}

RGBwavelengthsStruct computeRGBwavelengths(Eigen::VectorXf wl){
  RGBwavelengthsStruct RGBwavelengths;
  float redDiffWL = abs(wl(0)-650);
  float greenDiffWL = abs(wl(0)-550);
  float blueDiffWL = abs(wl(0)-450);
  for (int band_idx = 0; band_idx < wl.size(); ++band_idx){
    if (abs(wl(band_idx)-650) < redDiffWL){
      RGBwavelengths.bandRed = band_idx;
    }
    if (abs(wl(band_idx)-550) < redDiffWL){
      RGBwavelengths.bandGreen = band_idx;
    }
    if (abs(wl(band_idx)-450) < redDiffWL){
      RGBwavelengths.bandBlue = band_idx;
    }
  }
  return RGBwavelengths;
}

std::string getDataFname(int argc, char** argv){
  // Set paths to image files.
  if (argc == 1) {
    // if there are no input file names then use the default image
    // AVIRS which should be located in the same directory as the executable file.
    return std::filesystem::path(argv[0]).replace_filename("AVIRIS").string();
  }else {
    // input image and header file names
    return argv[1];
  }
}

std::string getHeaderFname(int argc, char** argv){
  // Set paths to image files.
  if (argc == 1) {
    // if there are no input file names then use the default
    return std::filesystem::path(argv[0]).replace_filename("AVIRIS.hdr").string();;
  }else if (argc == 2) {
    // case where file name is image and header is just the image with .hdr added
    return strcat(argv[1],".hdr");
  } else {
    // input image and header file names
    return argv[2];
  }
}

// Get the range of data (lines, columns, bands) to read
// Currently read the whole image, but subsets could be read here
hsi::HSIDataRange getDataRange(hsi::HSIDataOptions data_options){
  // Set range of data we want to read.
  hsi::HSIDataRange data_range;
  data_range.start_row = 0;
  data_range.end_row = data_options.num_data_rows;
  data_range.start_col = 0;
  data_range.end_col = data_options.num_data_cols;
  data_range.start_band = 0;
  data_range.end_band = data_options.num_data_bands;
  return data_range;
}

ImData HSIData2EigenData(const hsi::HSIData& hsi_data, hsi::HSIDataOptions data_options, settingsValues settings){
  // Start timing
  auto t_start_HSIData2EigenData = std::chrono::high_resolution_clock::now();

  // Create the structure to hold the image and metadata
  ImData Im;

  Im.rows = hsi_data.num_rows;
  Im.cols = hsi_data.num_cols;
  Im.bands = hsi_data.num_bands;
  Eigen::MatrixXf im2d(Im.rows * Im.cols, Im.bands);
  for (int band = 0; band < Im.bands; ++band) {
    for (int row = 0; row < Im.rows; ++row) {
      for (int col = 0; col < Im.cols; ++col) {
        im2d(row * Im.cols + col, band) = hsi_data.GetValue(row, col, band).value_as_float;
      }
    }
  }
  Im.im2d = im2d;

  // This creates the Eigen::VectorXf Im.wl from the wavelength string
  Im.wl = stringCS2VectorFloats(data_options.wavelength, Im.bands);

  // Im.wlScale is a scale factor on the wavelengths
  // multiplying by this factor puts the wavelengths
  // into units of nanometers
  Im.wlScale = 1;
  if (Im.wl.mean() < 10){
    Im.wlScale = 1000;
  }

  // Compute the indices for the wavelengths associated with
  // Reg, Green, Blue colors (650nm, 550nm, 450nm)
  RGBwavelengthsStruct RGBwavelengths = computeRGBwavelengths(Im.wl);
  int bandRed = RGBwavelengths.bandRed;
  int bandGreen = RGBwavelengths.bandGreen;
  int bandBlue = RGBwavelengths.bandBlue;

  // This creates the Eigen::VectorXf Im.fwhm from the fwhm string
  Im.fwhm = stringCS2VectorFloats(data_options.fwhm, Im.bands);

  auto t_end_HSIData2EigenData = std::chrono::high_resolution_clock::now();
  auto secondsElapsedWhite = std::chrono::duration_cast<std::chrono::milliseconds>(t_end_HSIData2EigenData - t_start_HSIData2EigenData);
  Logger("Converting HSIDATA to Eigen Matrices completed in : "+std::to_string(secondsElapsedWhite.count()/1000.)+"s", settings);

  std::cout << "some data from the image: \n" << im2d(Eigen::seq(0,5),Eigen::seq(0,5)) << std::endl;

  return Im;
}

void showRGBimage(ImData Im, settingsValues settings){
  //  create grayscale image
  Eigen::MatrixXf RX2d = Im.RX.reshaped(Im.rows, Im.cols);
  const cv::Size RX_image_size(RX2d.cols(), RX2d.rows());
  float min_value = 0;
  float max_value = 0;
  cv::Mat RX_image(RX_image_size, CV_64FC1);
  for (int row = 0; row < RX2d.rows(); ++row) {
    for (int col = 0; col < RX2d.cols(); ++col) {
      const float pixel_value = RX2d(row, col);
      RX_image.at<double>(row, col) = pixel_value;
      min_value = std::min(min_value, pixel_value);
      max_value = std::max(max_value, pixel_value);
    }
  }
  RX_image = 255*(RX_image-min_value)/(max_value-min_value);
  cv::namedWindow("test", cv::WINDOW_AUTOSIZE);
  cv::imshow("test", RX_image);
  cv::waitKey(0);
}

// Computes statistics for an image
ImStats computeImageStats(ImData Im, settingsValues settings) {
  // Create the structure to hold all the stats
  ImStats stats;

  // Compute the image mean
  stats.mean = Im.im2d.colwise().mean();

  // Compute the covariance matrix
  auto t_start_cov_Eigin = std::chrono::high_resolution_clock::now();
  Eigen::MatrixXd centered = (Im.im2d.rowwise() - stats.mean.transpose()).template cast<double>();
  Eigen::MatrixXd cov = (centered.transpose() * centered) / float(Im.im2d.rows() - 1);
  auto t_end_cov_Eigin = std::chrono::high_resolution_clock::now();
  auto secondsElapsed = std::chrono::duration_cast<std::chrono::milliseconds>(t_end_cov_Eigin - t_start_cov_Eigin);
  Logger("Covariance computation completed in "+std::to_string(secondsElapsed.count()/1000.)+"s",
    settings);

  // Compute the eigenvalues and eigenvectors
  auto t_start_evalsEvecs_Eigin = std::chrono::high_resolution_clock::now();
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigensolver(cov);
  auto t_end_evalsEvecs_Eigin = std::chrono::high_resolution_clock::now();
  Eigen::MatrixXd evals;
  Eigen::MatrixXd evecs;
  if (eigensolver.info() == Eigen::Success){
    auto secondsElapsed = std::chrono::duration_cast<std::chrono::milliseconds>(t_end_evalsEvecs_Eigin - t_start_evalsEvecs_Eigin);
    Logger("Eigenvalue and eignvector computation completed in "+std::to_string(secondsElapsed.count()/1000.)+"s",
      settings);
    evals = eigensolver.eigenvalues();
    evecs = eigensolver.eigenvectors();
  }
  else {
    Logger("WARNING: eigenvalues and eigenvectors failed to compute.", settings);
  };

  std::cout << "Eigenvalues:\n";
  for (int i = 0; i < 3; ++i) {
    std::cout << evals(i) << " | " << evals(i)/evals(evals.rows()-1) << std::endl;
  }
  std::cout << "...\n";
  for (int i = evals.rows()-3; i < evals.rows(); ++i) {
    std::cout << evals(i) << " | " << evals(i)/evals(evals.rows()-1) << std::endl;
  }

  // Computing the whitening matrix
  auto t_start_W = std::chrono::high_resolution_clock::now();
  Eigen::MatrixXd D = Eigen::MatrixXd::Identity(evals.rows(),evals.rows());
  double maxEval = evals.maxCoeff();
  for (int i = 0; i < evals.rows(); ++i) {
    // Optional regularization on eigenvalues
    //stats.evals(i) = std::max(stats.evals(i),maxEval*std::pow(10,-8));
    D(i,i) = 1/sqrt(evals(i));
  }
  Eigen::MatrixXd W =  (evecs*D);
  std::cout << "some data from the whitening matrix: \n" << W(Eigen::seq(0,5),Eigen::seq(0,5)) << std::endl;

  auto t_end_W = std::chrono::high_resolution_clock::now();
  auto secondsElapsedW = std::chrono::duration_cast<std::chrono::milliseconds>(t_end_W - t_start_W);
  Logger("Whitening matrix computation completed in "+std::to_string(secondsElapsedW.count()/1000.)+"s",
    settings);

  // Recast as float
  stats.cov = cov.template cast<float>();
  stats.evals = evals.template cast<float>();
  stats.evecs = evecs.template cast<float>();
  stats.W = W.template cast<float>();

  Logger("im2d Matrix is "+std::to_string(Im.im2d.rows())+"x"+std::to_string(Im.im2d.cols()),
    settings);
  Logger("Covariance is "+std::to_string(stats.cov.rows())+"x"+std::to_string(stats.cov.cols()),
    settings);
  Logger("Whitening Matrix is "+std::to_string(stats.W.rows())+"x"+std::to_string(stats.W.cols()),
    settings);

  // VALIDATION: If we want to validate the Whitening by comparison to the inverse
  //Eigen::MatrixXd Diff = cov.inverse() - (W)*(W.transpose());
  //std::cout << "some data from the Diff matrix: \n" << Diff(Eigen::seq(0,5),Eigen::seq(0,5)) << std::endl;

  return stats;
}

// Whitens an image using provided statistics
Eigen::MatrixXf computeWhitenedImage(ImData Im, ImStats stats, settingsValues settings) {

  auto t_start_White = std::chrono::high_resolution_clock::now();
  // VALIDATION: If we want to validat the data be viewing the image mean
  //std::cout << "image mean: \n" << stats.mean;
  Eigen::MatrixXf centered = Im.im2d.rowwise() - stats.mean.transpose();
  Eigen::MatrixXf whitened = centered*stats.W;
  auto t_end_White = std::chrono::high_resolution_clock::now();
  auto secondsElapsedWhite = std::chrono::duration_cast<std::chrono::milliseconds>(t_end_White - t_start_White);
  Logger("Whitening image completed in "+std::to_string(secondsElapsedWhite.count()/1000.)+"s",
    settings);
  Logger("Whitening image is "+std::to_string(whitened.rows())+"x"+std::to_string(whitened.cols()),
    settings);

  return whitened;
}

// Computes the RX anomaly detetion image from the whitened image
Eigen::MatrixXf computeRXimage(ImData Im, settingsValues settings){

  auto t_start_rx = std::chrono::high_resolution_clock::now();
  Eigen::MatrixXf  RX = Im.white2d.rowwise().norm();
  Logger("RX image has size: "+std::to_string(RX.size()), settings);
  auto t_end_rx = std::chrono::high_resolution_clock::now();
  auto secondsElapsedRX = std::chrono::duration_cast<std::chrono::milliseconds>(t_end_rx - t_start_rx);
  Logger("Creating RX image completed in "+std::to_string(secondsElapsedRX.count()/1000.)+"s", settings);

  return RX;
}

// Displays the RX anomaly detection image
void showRXimage(ImData Im, settingsValues settings){

  if (settings.saveRX || settings.displayRX){
    //  create grayscale image
    Eigen::MatrixXf RX2d = Im.RX.reshaped(Im.rows, Im.cols);
    const cv::Size RX_image_size(RX2d.cols(), RX2d.rows());
    float min_value = 0;
    float max_value = 0;
    cv::Mat RX_image(RX_image_size, CV_64FC1);
    for (int row = 0; row < RX2d.rows(); ++row) {
      for (int col = 0; col < RX2d.cols(); ++col) {
        const float pixel_value = RX2d(row, col);
        RX_image.at<double>(row, col) = pixel_value;
        min_value = std::min(min_value, pixel_value);
        max_value = std::max(max_value, pixel_value);
      }
    }
    RX_image = 5*(RX_image-min_value)/(max_value-min_value);

    if (settings.displayRX){
      cv::namedWindow("test", cv::WINDOW_AUTOSIZE);
      cv::imshow("test", RX_image);
      cv::waitKey(0);
    }

    if (settings.saveRX){
      bool check = cv::imwrite(settings.outDir+"\\RX.jpg", 255*RX_image);
      if (check == false) {
        Logger("WARNING Saving RX image failed.", settings);
      } else {
        Logger("RX Image saves as "+settings.outDir+"\\RX.jpg", settings);
      }
    }
  }
}

const std::string getTgtLibDataFname(const std::string hsi_data_path){
  std::filesystem::path p(hsi_data_path);
  return p.parent_path().string()+"\\lib_detect_fullresolution.sli";
}

const std::string getTgtLibHeaderFname(const std::string hsi_data_path){
  std::filesystem::path p(hsi_data_path);
  return p.parent_path().string()+"\\lib_detect_fullresolution.hdr";
}

// Converts data and metadata for a spectral library into
// a LibData structure using Eigen variable types.
LibData SpecLibData2EigenData(const hsi::HSIData& tgt_lib_data, hsi::HSIDataOptions tgt_lib_data_options){
  // Create the structure to hold the image and metadata
  LibData lib_tgt;
  lib_tgt.nSpectra = tgt_lib_data.num_cols;
  lib_tgt.bands = tgt_lib_data.num_rows;

  // This creates the vector Im.wl from the wavelength string
  lib_tgt.wl = stringCS2VectorFloats(tgt_lib_data_options.wavelength, lib_tgt.bands);

  // This creates the vector lib_tgt.specNames from the specNames string
  lib_tgt.specNames = stringCS2Vector(tgt_lib_data_options.specNames);

  // Make a matrix that will hold the spectra
  Eigen::MatrixXf spectra(lib_tgt.nSpectra, lib_tgt.bands);
  for (int i = 0; i < lib_tgt.nSpectra; ++i) {
    for (int j = 0; j < lib_tgt.bands; ++j) {
      spectra(i,j) = 10.0*tgt_lib_data.GetValueLib(j, i, 0).value_as_float;
    }
  }
  lib_tgt.spectra = spectra;
  std::cout << "some data from the library file spectra: \n" << spectra(Eigen::seq(0,5),Eigen::seq(0,5)) << std::endl;

  return lib_tgt;
}

LibData resampleLibrary(LibData lib_tgt_fullres, ImData Im) {
  // Create the structure to hold the image and metadata
  LibData lib_tgt;
  lib_tgt.specNames = lib_tgt_fullres.specNames;
  lib_tgt.nSpectra = lib_tgt_fullres.nSpectra;
  lib_tgt.bands = Im.bands;
  lib_tgt.wl = Im.wl;
  Eigen::MatrixXf spectra = Eigen::MatrixXf::Zero(lib_tgt_fullres.nSpectra, Im.bands);

  Eigen::VectorXf sigma(Im.bands);
  for (int i = 0; i < Im.bands; ++i) {
    sigma(i) = Im.fwhm(i)/2.355;
  }

  float sum = 0;
  float weight = 0;
  for (int im_bnd_idx = 0; im_bnd_idx < Im.bands; ++im_bnd_idx) {  //  looping over bands in the image
    for (int lb_bnd_idx = 0; lb_bnd_idx < lib_tgt_fullres.bands; ++lb_bnd_idx) { // looping over all bands in the library
      //  compute the difference between the image (target) wavelength and library wavelength so that we
      //  only compute contributions of library bands with wavelength that are withing 3 sigma of the image wavelength
      float diff = (lib_tgt_fullres.wl[lb_bnd_idx] - Im.wl[im_bnd_idx]);
      float standard_deviations_diff = diff/sigma(im_bnd_idx);

      /*  VALIDATION OUTPUT:  uncomment this to see how the resampling computation works
      std::cout << lib_tgt_fullres.wl[lb_bnd_idx] << " - " << Im.wl[im_bnd_idx] << std::endl;
      std::cout << diff << std::endl;
      std::cout << standard_deviations_diff << std::endl;
      std::cout << "Weight: " << weight << std::endl;
      std::cout << "Sum: " << sum << std::endl;
      */

      if (standard_deviations_diff > -3){
        if (standard_deviations_diff > 3){
          // stop the  iteration through library bands becasue the library band wavelength has
          // passed all wavelengths within 3 standard deviations of the image wavelength
          lb_bnd_idx = lib_tgt_fullres.bands;
        } else {
          weight = normalPDF(sigma(im_bnd_idx),
                            Im.wl[im_bnd_idx],
                            lib_tgt_fullres.wl[lb_bnd_idx]);
          for (int spec_idx = 0; spec_idx < lib_tgt.nSpectra; ++spec_idx) { // looping over all spectra in the library
            spectra(spec_idx, im_bnd_idx) = spectra(spec_idx, im_bnd_idx) +
                                        lib_tgt_fullres.spectra(spec_idx, lb_bnd_idx)*
                                        weight;
            sum = sum + weight;
          }
        }
      }
    }
    for (int spec_idx = 0; spec_idx < lib_tgt.nSpectra; ++spec_idx) { // looping over all spectra in the library
      spectra(spec_idx, im_bnd_idx) = spectra(spec_idx, im_bnd_idx)/sum;
    }
    sum = 0;
  }
  lib_tgt.spectra = spectra;

  /* VALIDATION: If we want to validate data by checking the first resampled spectrum from the library
  std::cout << "First spectrum:\n";
  for (int idx = 0; idx < Im.bands; ++idx) {
    std::cout << spectra(0,idx) << std::endl;
  }
  */

  std::cout << "some data from the resampled spectra: \n";
  std::cout << spectra(Eigen::seq(0,5),Eigen::seq(0,5)) << std::endl;

  return lib_tgt;
}

Eigen::MatrixXf computeWhitenedLibrary(LibData lib_tgt, ImStats stats, settingsValues settings) {

  auto t_start_White = std::chrono::high_resolution_clock::now();
  Eigen::MatrixXf centered = lib_tgt.spectra.rowwise() - stats.mean.transpose();
  std::cout << "some data from the centered spectra: \n" << centered(Eigen::seq(0,5),Eigen::seq(0,5)) << std::endl;
  Eigen::MatrixXf spectraWhite = centered*stats.W;
  std::cout << "some data from the whitened spectra: \n" << spectraWhite(Eigen::seq(0,5),Eigen::seq(0,5)) << std::endl;
  auto t_end_White = std::chrono::high_resolution_clock::now();
  auto secondsElapsedWhite = std::chrono::duration_cast<std::chrono::milliseconds>(t_end_White - t_start_White);
  Logger("Whitening library completed in "+std::to_string(secondsElapsedWhite.count()/1000.)+"s",
    settings);
  Logger("Whitening library is "+std::to_string(spectraWhite.rows())+"x"+std::to_string(spectraWhite.cols()),
    settings);

  return spectraWhite;
}

Eigen::MatrixXf computeACEimage(ImData Im, LibData lib_tgt, settingsValues settings){

  auto t_start_ACE = std::chrono::high_resolution_clock::now();
  std::cout << "some data from the whitened image: \n" << Im.white2d(Eigen::seq(0,5),Eigen::seq(0,5)) << std::endl;
  std::cout << "some data from the whitened library: \n" << lib_tgt.spectraWhite(Eigen::seq(0,5),Eigen::seq(0,5)) << std::endl;
  Eigen::MatrixXf  ACE_num = Im.white2d * lib_tgt.spectraWhite.transpose();
  std::cout << "some data from the ACE numerator: \n" << ACE_num(Eigen::seq(0,5),Eigen::seq(0,5)) << std::endl;
  Eigen::MatrixXf  ACE_denom = (Im.RX.replicate(1,lib_tgt.nSpectra).array() * lib_tgt.spectraWhite.transpose().colwise().norm().replicate(Im.rows*Im.cols, 1).array()).matrix();
  std::cout << "some data from the ACE denominator: \n" << ACE_denom(Eigen::seq(0,5),Eigen::seq(0,5)) << std::endl;
  Eigen::MatrixXf  ACE = (ACE_num.array() / ACE_denom.array()).matrix();
  std::cout << "some data from the ACE image: \n" << ACE(Eigen::seq(0,5),Eigen::seq(0,5)) << std::endl;
  Logger("ACE image is "+std::to_string(ACE_num.rows())+"x"+std::to_string(ACE_num.cols()),
    settings);
  auto t_end_ACE = std::chrono::high_resolution_clock::now();
  auto secondsElapsedACE = std::chrono::duration_cast<std::chrono::milliseconds>(t_end_ACE - t_start_ACE);
  Logger("Creating ACE image completed in "+std::to_string(secondsElapsedACE.count()/1000.)+"s", settings);

  return ACE;
}

void showACEResults(ImData Im, LibData lib_tgt, Eigen::MatrixXf ACE, settingsValues settings){

  if (settings.saveACE || settings.displayACE){
    // Create the openCV ACE image
    const cv::Size ACEimSize(Im.cols, Im.rows);
    cv::Mat ACEim(ACEimSize, CV_64FC1);

    for (int spec_idx = 0; spec_idx < lib_tgt.nSpectra; ++spec_idx) {
      Eigen::MatrixXf ACEslice2d = ACE(Eigen::all, spec_idx);
      Eigen::MatrixXf ACE2d = ACEslice2d.reshaped(Im.rows, Im.cols);
      //  create grayscale image
      float min_value = 100;
      float max_value = -100;
      for (int row = 0; row < Im.rows; ++row) {
        for (int col = 0; col < Im.cols; ++col) {
          float pixel_value = ACE2d(row, col);
          //pixel_value = ((pixel_value>0)-(pixel_value<0))*std::sqrt(std::abs(pixel_value));
          float min_thresh = 0.25;
          pixel_value = std::max(min_thresh,pixel_value);
          ACEim.at<double>(row, col) = pixel_value;
          min_value = std::min(min_value, pixel_value);
          max_value = std::max(max_value, pixel_value);
        }
      }
      ACEim = (ACEim-min_value)/(max_value-min_value);
      std::cout << lib_tgt.specNames[spec_idx] << std::endl;
      std::cout << "Min Value: " << min_value << std::endl;
      std::cout << "Max Value: " << max_value << std::endl;

      if (settings.displayACE){
        cv::namedWindow(lib_tgt.specNames[spec_idx], cv::WINDOW_AUTOSIZE);
        cv::imshow(lib_tgt.specNames[spec_idx], ACEim);
        cv::waitKey(0);
      }
      if (settings.saveACE){
        std::string specName = lib_tgt.specNames[spec_idx];
        specName.erase(std::remove(specName.begin(), specName.end(), ' '), specName.end());
        bool check = cv::imwrite(settings.outDir+"\\ACE_"+specName+".jpg", 255*ACEim);
        if (check == false) {
          Logger("WARNING Saving ACE image failed.", settings);
        } else {
          Logger("ACE Image saved as "+settings.outDir+"\\ACE_"+specName+".jpg", settings);
        }
      }
    }
  }

}

// Compute a covarnaince matrix and other stats for the input image
// using a 3 step process: compute stats and RX anomaly detection,
// remove the anomalies, and recompute stats from non-anomalous pixels.
// Input image matrix is pixels x bands
ImStats computeAnomalyCleanedImageStats(ImData Im, settingsValues settings) {

  // build a subseted compy of the image structure containing
  // only about 10,000 pixel spectra for quick approximate
  // first covariance computation
  ImData Im_sampled;
  Im_sampled.im2d = Im.im2d(Eigen::seq(0,Im.im2d.rows(),std::floor(Im.im2d.rows()/10000.)), Eigen::all);
  Im_sampled.rows = Im.rows;
  Im_sampled.cols = Im.cols;

  // compute the covariance for the subset of pixel spectra
  Logger("Computing statistics using Eigen (first pass)", settings);
  ImStats stats_sampled = computeImageStats(Im_sampled, settings);

  // Whiten the full image using the subset-computed covariance
  Logger("Whitening the image. (first pass)", settings);
  Im_sampled.white2d = computeWhitenedImage(Im, stats_sampled, settings);

  // Compute the RX anomaly image using the subset-computed covariance
  // This will be used to remove anomalies for the
  // primary covariance computation
  Logger("Compute the RX anomaly image. (first pass)", settings);
  Eigen::MatrixXf RX = computeRXimage(Im_sampled, settings);

  // compute a threshold for anomaly removeal
  // removing the most anomalous 5% of pixels
  auto t_start_rx_pctile = std::chrono::high_resolution_clock::now();
  std::vector<float> RXv;
  RXv.resize(RX.size());
  Eigen::VectorXf::Map(&RXv[0], RX.size()) = RX;
  std::sort(RXv.begin(), RXv.end());
  int idx_95pct = std::round(RX.size()*0.95);
  for (int i = 0; i < 100; ++i) {
    std::cout << RXv[i] << std::endl;
  }
  float thresh_95pct = RXv[idx_95pct];
  std::cout << "Thresh: " << thresh_95pct << std::endl;
  std::cout << "Max: " << RX.maxCoeff() << std::endl;
  auto t_end_rx_pctile = std::chrono::high_resolution_clock::now();
  auto secondsElapsedRX_pctile = std::chrono::duration_cast<std::chrono::milliseconds>(t_end_rx_pctile - t_start_rx_pctile);
  Logger("Computing RX threshold completed in "+std::to_string(secondsElapsedRX_pctile.count()/1000.)+"s", settings);

  auto t_subsample_start = std::chrono::high_resolution_clock::now();
  Eigen::VectorXi is_selected = (RX.array() < thresh_95pct).cast<int>();
  Eigen::VectorXi is_selectedindices(is_selected.sum());
  int idx = 0;
  for (int i = 0; i < Im.rows; ++i) {
    if (is_selected(i) == 1){
      is_selectedindices(idx) = i;
      idx++;
    }
  }

  Im_sampled.im2d = Im.im2d(is_selectedindices,Eigen::all);
  std::cout << Im_sampled.im2d.rows() << std::endl;
  std::cout << Im_sampled.im2d.cols() << std::endl;
  auto t_subsample_end = std::chrono::high_resolution_clock::now();
  auto secondsElapsed_subsample = std::chrono::duration_cast<std::chrono::milliseconds>(t_subsample_end - t_subsample_start);
  Logger("time for subset computation: "+std::to_string(secondsElapsed_subsample.count()/1000.)+"s", settings);

  // HAVING TROUBLE SUBSETTING THE IMAGE TO COMPUTE STATS FROM THE NON-ANOMALUS PIXELS
  // I AM MULTIPLYING ANOMALIES BY ZER FOR COV COMPUTATION BUT ITS NOT WORKING!

  // Now we compute the stats for the data with anomalies removed
  // Create the structure to hold all the stats
  ImStats stats;

  // Compute the image mean
  stats.mean = Im_sampled.im2d.colwise().mean();

  // Compute the covariance matrix
  auto t_start_cov_Eigin = std::chrono::high_resolution_clock::now();
  Eigen::MatrixXd centered = (Im_sampled.im2d.rowwise() - stats.mean.transpose()).template cast<double>();
  Eigen::MatrixXd cov = (centered.transpose() * centered) / float(is_selected.sum() - 1);
  auto t_end_cov_Eigin = std::chrono::high_resolution_clock::now();
  auto secondsElapsed = std::chrono::duration_cast<std::chrono::milliseconds>(t_end_cov_Eigin - t_start_cov_Eigin);
  Logger("Covariance computation completed in "+std::to_string(secondsElapsed.count()/1000.)+"s",
    settings);

  // Compute the eigenvalues and eigenvectors
  auto t_start_evalsEvecs_Eigin = std::chrono::high_resolution_clock::now();
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigensolver(cov);
  auto t_end_evalsEvecs_Eigin = std::chrono::high_resolution_clock::now();
  Eigen::MatrixXd evals;
  Eigen::MatrixXd evecs;
  if (eigensolver.info() == Eigen::Success){
    auto secondsElapsed = std::chrono::duration_cast<std::chrono::milliseconds>(t_end_evalsEvecs_Eigin - t_start_evalsEvecs_Eigin);
    Logger("Eigenvalue and eignvector computation completed in "+std::to_string(secondsElapsed.count()/1000.)+"s",
      settings);
    evals = eigensolver.eigenvalues();
    evecs = eigensolver.eigenvectors();
  }
  else {
    Logger("WARNING: eigenvalues and eigenvectors failed to compute.", settings);
  };

  std::cout << "Eigenvalues:\n";
  for (int i = 0; i < 3; ++i) {
    std::cout << evals(i) << " | " << evals(i)/evals(evals.rows()-1) << std::endl;
  }
  std::cout << "...\n";
  for (int i = evals.rows()-3; i < evals.rows(); ++i) {
    std::cout << evals(i) << " | " << evals(i)/evals(evals.rows()-1) << std::endl;
  }

  // Computing the whitening matrix
  auto t_start_W = std::chrono::high_resolution_clock::now();
  Eigen::MatrixXd D = Eigen::MatrixXd::Identity(evals.rows(),evals.rows());
  double maxEval = evals.maxCoeff();
  for (int i = 0; i < evals.rows(); ++i) {
    // Optional regularization on eigenvalues
    //evals(i) = std::max(evals(i),maxEval*std::pow(10,-6));
    D(i,i) = 1/sqrt(evals(i));
  }
  Eigen::MatrixXd W =  (evecs*D);
  std::cout << "some data from the whitening matrix: \n" << W(Eigen::seq(0,8),Eigen::seq(0,8)) << std::endl;

  auto t_end_W = std::chrono::high_resolution_clock::now();
  auto secondsElapsedW = std::chrono::duration_cast<std::chrono::milliseconds>(t_end_W - t_start_W);
  Logger("Whitening matrix computation completed in "+std::to_string(secondsElapsedW.count()/1000.)+"s",
    settings);

  // Recast as float
  stats.cov = cov.template cast<float>();
  stats.evals = evals.template cast<float>();
  stats.evecs = evecs.template cast<float>();
  stats.W = W.template cast<float>();

  Logger("im2d Matrix is "+std::to_string(Im.im2d.rows())+"x"+std::to_string(Im.im2d.cols()),
    settings);
  Logger("Covariance is "+std::to_string(stats.cov.rows())+"x"+std::to_string(stats.cov.cols()),
    settings);
  Logger("Whitening Matrix is "+std::to_string(stats.W.rows())+"x"+std::to_string(stats.W.cols()),
    settings);

  // VALIDATION: If we want to validate the Whitening by comparison to the inverse
  //Eigen::MatrixXd Diff = cov.inverse() - (W)*(W.transpose());
  //std::cout << "some data from the Diff matrix: \n" << Diff(Eigen::seq(0,5),Eigen::seq(0,5)) << std::endl;

  return stats;
}


///////////////////////////////////////////////////////////////////////////////
// statistics::model::algorithm::log_likelihood.hpp                          //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_MODEL_ALGORITHM_LOG_LIKELIHOOD_HPP_ER_2009
#define BOOST_STATISTICS_MODEL_ALGORITHM_LOG_LIKELIHOOD_HPP_ER_2009
#include <boost/binary_op/algorithm/for_each.hpp>
#include <boost/statistics/model/wrap/aggregate/model_parameter.hpp>
#include <boost/statistics/model/functional/log_likelihood_accumulator.hpp>

namespace boost{
namespace statistics{
namespace model{

    // Evaluates the likelihood at mp.parameter(), given model mp.model(),
    // by summing all contributions from a dataset i.e. a
    // sequence of covariates, [b_x,e_x), and reponses (starting at b_y)
    template<typename T,typename M,typename P,typename ItX,typename ItE>
    T
    log_likelihood(
        model_parameter_<M,P> mp,
        ItX b_x,
        ItX e_x,
        ItE b_y
    ){
        typedef log_likelihood_accumulator<T,M,P> acc_;
        acc_ acc(mp);
        return boost::binary_op::for_each<acc_>(
            b_x,
            e_x,
            b_y,
            acc
        ).value();
    }

}// model
}// statistics
}// boost

#endif


//
// Created by green on 04/12/17.
//
#include <iostream>     // std::cout, std::fixed
#include <iomanip>      // std::setprecision

#include "inc/Modules/LocationModule.h"
#include <boost/lexical_cast.hpp>
#include <lapacke.h>
#include "inc/Exceptions/UnsupportedOperationException.h"

using boost::lexical_cast;

// I think nodes should be the scanned nodes m_ScannedNodes
LocationModule::LocationModule(std::shared_ptr<node::NodeContainer>& nodes) : m_Nodes(nodes),
                                                                              m_dgelsLoc(std::make_shared<Location>()),
                                                                              m_dgetrsLoc(std::make_shared<Location>()),
                                                                              m_dgesvLoc(std::make_shared<Location>())
{

}

void LocationModule::CalculateLocations()
{
    boost::mutex::scoped_lock lock(g_i_mutex);
    try
    {
      // I think m_Nodes should be the nodes read from Settings.json by the FileNodeReaderModule
      calculateDgels(GetNodeTargets(m_Nodes)); // m_Nodes is a container of AP nodes, with known (x,y,z) coordinates

      calculateDgesvDgetrs(GetNodeTargets(m_Nodes));
      std::cerr << std::endl; // skip a line
    }
    catch (std::exception e)
    {
      //std::cout << "Not Enough Nodes in Range." << std::endl;
      // UnsupportedOperationException ex;
      // throw(ex);
    }

}

std::shared_ptr<Location> LocationModule::GetLocation()
{
    CalculateLocations();
    return m_dgelsLoc;
}

std::shared_ptr<node::NodeContainer> LocationModule::GetNodeTargets(std::shared_ptr<node::NodeContainer> tempNodes)
{
  // I think tempNodes should be the nodes read from Settings.json by the FileNodeReaderModule

    if (tempNodes == NULL)
    {
        UnsupportedOperationException ex;
        throw (ex);
    }
    std::shared_ptr<node::NodeContainer> targetNodes = std::make_shared<node::NodeContainer>();

    std::sort(tempNodes->GetNodes().begin(), tempNodes->GetNodes().end(),
              [](std::shared_ptr<INode> &lhs, std::shared_ptr<INode> &rhs)
              {
                  return lhs->getRSSI() > rhs->getRSSI();
              });
    for (auto &i : tempNodes->GetNodes())
    {
      // std::cerr << "LocationModule::GetNodeTargets: tempNode: " << i->getSSID() << std::endl;

      if (targetNodes->GetNodes().size() < 4 && i->getRSSI() != 0 && i->getRecentlyUpdated())
        {
	  // add node to use in LSQ location calculation
	  targetNodes->AddNode(i);
        }
    }
    for (auto &i : targetNodes->GetNodes())
    {
      // std::cout << "RSSI ORDERING (SSID,RSSI,MAC): " << i->getSSID() << ", " << i->getRSSI() << ", " << i->getMAC() << std::endl;
    }
    if (targetNodes->GetNodes().size() < 4)
    {
        std::exception e;
        throw (e);
    }
    return targetNodes;
}

web::json::value LocationModule::GetJson()
{
    CalculateLocations();
    web::json::value response;
    response["DGESV"] = m_dgesvLoc->ToJson();
    response["DGETRS"] = m_dgetrsLoc->ToJson();
    response["DGELS"] = m_dgelsLoc->ToJson();
    response["TargetNodes"] = GetNodeTargets(m_Nodes)->ToJson();
    return response;
}

web::json::value LocationModule::BasicJson()
{
    CalculateLocations();
    web::json::value response;
    response["DGESV"] = m_dgesvLoc->ToJson();
    response["DGETRS"] = m_dgetrsLoc->ToJson();
    response["DGELS"] = m_dgelsLoc->ToJson();
    response = m_dgelsLoc->ToJson();
    return response;
}

void LocationModule::calculateDgesvDgetrs(std::shared_ptr<node::NodeContainer> nodes)
{
    if (nodes->GetNodes().size() != 4)
    {
        throw ("Error::Too few Nodes!");
    }

    std::vector<std::shared_ptr<TargetNode>> Nodes;

    for (std::shared_ptr<INode> a: nodes->GetNodes())
    {
        Nodes.push_back(std::static_pointer_cast<TargetNode>(a));
    }

    double A[3][3] = {2 * (Nodes[1]->getXCoord() - Nodes[0]->getXCoord()),
                      2 * (Nodes[1]->getYCoord() - Nodes[0]->getYCoord()),
                      2 * (Nodes[1]->getZCoord() - Nodes[0]->getZCoord()),
                      2 * (Nodes[2]->getXCoord() - Nodes[0]->getXCoord()),
                      2 * (Nodes[2]->getYCoord() - Nodes[0]->getYCoord()),
                      2 * (Nodes[2]->getZCoord() - Nodes[0]->getZCoord()),
                      2 * (Nodes[3]->getXCoord() - Nodes[0]->getXCoord()),
                      2 * (Nodes[3]->getYCoord() - Nodes[0]->getYCoord()),
                      2 * (Nodes[3]->getZCoord() - Nodes[0]->getZCoord())};

    double B[3][1] = {(pow(Nodes[0]->GetDistance(), 2.0)) - (pow(Nodes[1]->GetDistance(), 2.0)) -
                      (pow(Nodes[0]->getXCoord(), 2.0)) + (pow(Nodes[1]->getXCoord(), 2.0)) -
                      (pow(Nodes[0]->getYCoord(), 2.0)) + (pow(Nodes[1]->getYCoord(), 2.0)) -
                      (pow(Nodes[0]->getZCoord(), 2.0)) + (pow(Nodes[1]->getZCoord(), 2.0)),
                      (pow(Nodes[0]->GetDistance(), 2.0)) - (pow(Nodes[2]->GetDistance(), 2.0)) -
                      (pow(Nodes[0]->getXCoord(), 2.0)) + (pow(Nodes[2]->getXCoord(), 2.0)) -
                      (pow(Nodes[0]->getYCoord(), 2.0)) + (pow(Nodes[2]->getYCoord(), 2.0)) -
                      (pow(Nodes[0]->getZCoord(), 2.0)) + (pow(Nodes[2]->getZCoord(), 2.0)),
                      (pow(Nodes[0]->GetDistance(), 2.0)) - (pow(Nodes[3]->GetDistance(), 2.0)) -
                      (pow(Nodes[0]->getXCoord(), 2.0)) + (pow(Nodes[3]->getXCoord(), 2.0)) -
                      (pow(Nodes[0]->getYCoord(), 2.0)) + (pow(Nodes[3]->getYCoord(), 2.0)) -
                      (pow(Nodes[0]->getZCoord(), 2.0)) + (pow(Nodes[3]->getZCoord(), 2.0))};

    lapack_int info, m, n, lda, ldb, nrhs;
    lapack_int *ipiv;
    m = 3;
    n = 3;
    nrhs = 1;
    lda = 3;
    ldb = 1;
    ipiv = (lapack_int *) malloc(n * sizeof(lapack_int));
    boost::timer::auto_cpu_timer t;
    info = LAPACKE_dgesv(LAPACK_ROW_MAJOR, n, nrhs, *A, lda, ipiv, *B, ldb);
    t.stop();

    if (info > 0)
    {
        printf("The diagonal element %i of the triangular factor ", info);
        printf("of A is zero, so that A does not have full rank;\n");
        printf("the least squares solution could not be computed.\n");
        return;
    }

    // printf("Calcuated values.");
    // fflush(stdout);
    m_dgesvLoc->updateCoords(B[0][0], B[1][0], B[2][0]);
    /*m_dgesvLoc->xCoord=(B[0][0]);
    m_dgesvLoc->yCoord=(B[1][0]);
    m_dgesvLoc->zCoord=(B[2][0]);*/
    m_dgesvLoc->m_calculationTime = t.elapsed().wall;

    std::cerr << "Calculated Location (dgesv): ";
    std::cerr << std::fixed << std::setprecision(4);
    std::cerr <<  "X:" << lexical_cast<std::string>(B[0][0]) << " Y:" << lexical_cast<std::string>(B[1][0]) << " Z:"
              << lexical_cast<std::string>(B[2][0]) << " Time:"
              << lexical_cast<std::string>(m_dgesvLoc->m_calculationTime) << std::endl;

    double B2[3][1] = {(pow(Nodes[0]->GetDistance(), 2.0)) - (pow(Nodes[1]->GetDistance(), 2.0)) -
                       (pow(Nodes[0]->getXCoord(), 2.0)) + (pow(Nodes[1]->getXCoord(), 2.0)) -
                       (pow(Nodes[0]->getYCoord(), 2.0)) + (pow(Nodes[1]->getYCoord(), 2.0)) -
                       (pow(Nodes[0]->getZCoord(), 2.0)) + (pow(Nodes[1]->getZCoord(), 2.0)),
                       (pow(Nodes[0]->GetDistance(), 2.0)) - (pow(Nodes[2]->GetDistance(), 2.0)) -
                       (pow(Nodes[0]->getXCoord(), 2.0)) + (pow(Nodes[2]->getXCoord(), 2.0)) -
                       (pow(Nodes[0]->getYCoord(), 2.0)) + (pow(Nodes[2]->getYCoord(), 2.0)) -
                       (pow(Nodes[0]->getZCoord(), 2.0)) + (pow(Nodes[2]->getZCoord(), 2.0)),
                       (pow(Nodes[0]->GetDistance(), 2.0)) - (pow(Nodes[3]->GetDistance(), 2.0)) -
                       (pow(Nodes[0]->getXCoord(), 2.0)) + (pow(Nodes[3]->getXCoord(), 2.0)) -
                       (pow(Nodes[0]->getYCoord(), 2.0)) + (pow(Nodes[3]->getYCoord(), 2.0)) -
                       (pow(Nodes[0]->getZCoord(), 2.0)) + (pow(Nodes[3]->getZCoord(), 2.0))};

    t.resume();
    LAPACKE_dgetrs(LAPACK_ROW_MAJOR, 'N', n, nrhs, *A, lda, ipiv, *B2, ldb);

    if (info > 0)
    {
        printf("The diagonal element %i of the triangular factor ", info);
        printf("of A is zero, so that A does not have full rank;\n");
        printf("the least squares solution could not be computed.\n");
        return;
    }

    t.stop();
    m_dgetrsLoc->updateCoords(B2[0][0], B2[1][0], B2[2][0]);
    /*m_dgetrsLoc->xCoord=(B2[0][0]);
    m_dgetrsLoc->yCoord=(B2[1][0]);
    m_dgetrsLoc->zCoord=(B2[2][0]);*/

    m_dgetrsLoc->m_calculationTime = t.elapsed().wall - m_dgesvLoc->m_calculationTime;

    std::cerr << "Calculated Location (dgetrs): ";
    std::cerr << std::setprecision(4) << "X:" << lexical_cast<std::string>(B2[0][0]) << " Y:" << lexical_cast<std::string>(B2[1][0]) << " Z:"
              << lexical_cast<std::string>(B2[2][0]) << " Time:"
              << lexical_cast<std::string>(m_dgetrsLoc->m_calculationTime) << std::endl;

    return;
}

/*
 * LocationModule::calculateDgels
 *
 * Input: nodes is a container of access point nodes, with known (x,y,z) coordinates
 *
 */
void LocationModule::calculateDgels(std::shared_ptr<node::NodeContainer> nodes)
{
    if (nodes->GetNodes().size() != 4)
    {
        throw ("Error::Too few Nodes!");
    }

    std::vector<std::shared_ptr<TargetNode>> Nodes; // TargetNodes with known (x,y,z) coordinates

    for (std::shared_ptr<INode> a: nodes->GetNodes())
    {
	// I think we need to get the corresponding node from the TargetNodes container
        Nodes.push_back(std::static_pointer_cast<TargetNode>(a));
    }

    double A[3][3] = {2 * (Nodes[1]->getXCoord() - Nodes[0]->getXCoord()),
                      2 * (Nodes[1]->getYCoord() - Nodes[0]->getYCoord()),
                      2 * (Nodes[1]->getZCoord() - Nodes[0]->getZCoord()),
                      2 * (Nodes[2]->getXCoord() - Nodes[0]->getXCoord()),
                      2 * (Nodes[2]->getYCoord() - Nodes[0]->getYCoord()),
                      2 * (Nodes[2]->getZCoord() - Nodes[0]->getZCoord()),
                      2 * (Nodes[3]->getXCoord() - Nodes[0]->getXCoord()),
                      2 * (Nodes[3]->getYCoord() - Nodes[0]->getYCoord()),
                      2 * (Nodes[3]->getZCoord() - Nodes[0]->getZCoord())};

    double B[3][1] = {(pow(Nodes[0]->GetDistance(), 2.0)) - (pow(Nodes[1]->GetDistance(), 2.0)) -
                      (pow(Nodes[0]->getXCoord(), 2.0)) + (pow(Nodes[1]->getXCoord(), 2.0)) -
                      (pow(Nodes[0]->getYCoord(), 2.0)) + (pow(Nodes[1]->getYCoord(), 2.0)) -
                      (pow(Nodes[0]->getZCoord(), 2.0)) + (pow(Nodes[1]->getZCoord(), 2.0)),
                      (pow(Nodes[0]->GetDistance(), 2.0)) - (pow(Nodes[2]->GetDistance(), 2.0)) -
                      (pow(Nodes[0]->getXCoord(), 2.0)) + (pow(Nodes[2]->getXCoord(), 2.0)) -
                      (pow(Nodes[0]->getYCoord(), 2.0)) + (pow(Nodes[2]->getYCoord(), 2.0)) -
                      (pow(Nodes[0]->getZCoord(), 2.0)) + (pow(Nodes[2]->getZCoord(), 2.0)),
                      (pow(Nodes[0]->GetDistance(), 2.0)) - (pow(Nodes[3]->GetDistance(), 2.0)) -
                      (pow(Nodes[0]->getXCoord(), 2.0)) + (pow(Nodes[3]->getXCoord(), 2.0)) -
                      (pow(Nodes[0]->getYCoord(), 2.0)) + (pow(Nodes[3]->getYCoord(), 2.0)) -
                      (pow(Nodes[0]->getZCoord(), 2.0)) + (pow(Nodes[3]->getZCoord(), 2.0))};

    lapack_int info, m, n, lda, ldb, nrhs;

    m = 3;
    n = 3;
    nrhs = 1;
    lda = 3;
    ldb = 1;
    boost::timer::auto_cpu_timer t;
    info = LAPACKE_dgels(LAPACK_ROW_MAJOR, 'N', m, n, nrhs, *A, lda, *B, ldb);
    t.stop();
    if (info > 0)
    {
        printf("The diagonal element %i of the triangular factor ", info);
        printf("of A is zero, so that A does not have full rank;\n");
        printf("the least squares solution could not be computed.\n");
        return;
    }

    m_dgelsLoc->updateCoords(B[0][0], B[1][0], B[2][0]);
    m_dgelsLoc->m_calculationTime = t.elapsed().wall;

    std::cerr << "Calculated Location (dgels): ";
    std::cerr << std::setprecision(5) << "X:" << lexical_cast<std::string>(B[0][0]) << " Y:" << lexical_cast<std::string>(B[1][0]) << " Z:"
              << lexical_cast<std::string>(B[2][0]) << " Time:"
              << lexical_cast<std::string>(m_dgelsLoc->m_calculationTime) << std::endl;

    return;

}

void LocationModule::initialize()
{
    m_isRunning = true;
}

void LocationModule::deInitialize()
{
    m_isRunning = false;
}

bool LocationModule::isRunning()
{
    return m_isRunning;
}


#pragma once
#include <type_traits>
#include <vector>
#include <string>
#include <array>
#include <exception>
#include <boost/multiprecision/cpp_int.hpp>
#include "keys.hpp"
#include "stream_handler.hpp"

namespace mp = boost::multiprecision;

namespace rsa {

    template<size_t E, size_t D>
    struct cipher {

        template<
                typename Handler,
                typename = std::enable_if_t<has_operation_v<Handler, read_number_padding_operation> && has_operation_v<Handler, eof_operation>>>
        static auto encrypt(Handler& handler, const keys<D * 8>& keys, unsigned int& n) {
            std::vector<typename num_utils<D * 8>::number> result;
            while (!handler.eof())
                result.push_back(mp::powm(
                        static_cast<typename num_utils<D * 8>::number>(handler.template read_number_padding<E * 8>(n)),
                        keys.get_e(), keys.get_n()));
            return result;
        }

        template<
                typename Handler,
                typename = std::enable_if_t<has_operation_v<Handler, read_number_operation> && has_operation_v<Handler, eof_operation>>>
        static auto decrypt(Handler& handler, const keys<D * 8>& keys) {
            std::vector<typename num_utils<E * 8>::number> result;
            while (!handler.eof())
                result.push_back(static_cast<typename num_utils<E * 8>::number>(mp::powm(
                        static_cast<typename num_utils<D * 8>::number>(handler.template read_number<D * 8>()),
                        keys.get_d(), keys.get_n())));
            return result;
        }

    };

}// namespace rsa


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EXPONENTIAL_FUNCTIONS_SCALAR_IMPL_LOGS_D_LOG_HPP_INCLUDED
#define NT2_EXPONENTIAL_FUNCTIONS_SCALAR_IMPL_LOGS_D_LOG_HPP_INCLUDED

#include <nt2/include/functions/scalar/sqr.hpp>
#include <nt2/include/functions/scalar/tofloat.hpp>
#include <nt2/include/functions/scalar/is_ltz.hpp>
#include <nt2/include/functions/scalar/is_eqz.hpp>
#include <nt2/include/functions/scalar/is_greater.hpp>
#include <nt2/include/functions/scalar/multiplies.hpp>
#include <nt2/include/functions/scalar/plus.hpp>
#include <nt2/include/functions/scalar/fast_frexp.hpp>
#include <nt2/include/functions/scalar/genmask.hpp>
#include <nt2/include/functions/scalar/minusone.hpp>
#include <nt2/include/functions/scalar/fma.hpp>
#include <nt2/include/functions/scalar/bitwise_and.hpp>
#include <nt2/polynomials/functions/scalar/impl/horner.hpp>
#include <nt2/include/constants/real_splat.hpp>
#include <nt2/include/constants/invlog_2.hpp>
#include <nt2/include/constants/invlog_10.hpp>
#include <nt2/include/constants/log_2olog_10.hpp>
#include <nt2/include/constants/sqrt_2o_2.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/nan.hpp>
#include <boost/simd/sdk/config.hpp>

#ifndef BOOST_SIMD_NO_NANS
#include <nt2/include/functions/scalar/is_nan.hpp>
#endif
#ifndef BOOST_SIMD_NO_INFINITIES
#include <nt2/include/constants/inf.hpp>
#endif

namespace nt2 { namespace details
{
  //////////////////////////////////////////////////////////////////////////////
  // math log functions
  //////////////////////////////////////////////////////////////////////////////

  template < class A0 >
  struct logarithm< A0, tag::not_simd_type, double>
  {
    static inline void kernel_log(const A0& a0,
                                  A0& dk,
                                  A0& hfsq,
                                  A0& s,
                                  A0& R,
                                  A0& f)
    {
      typedef typename meta::as_integer<A0, signed>::type int_type;
      typedef typename meta::scalar_of<A0>::type               sA0;
      A0 x;
      int_type k;
      nt2::fast_frexp(a0, x, k);
      const int_type x_lt_sqrthf = nt2::is_greater(nt2::Sqrt_2o_2<A0>(), x)?nt2::Mone<int_type>():nt2::Zero<int_type>();
      k += x_lt_sqrthf;
      f = nt2::minusone(x+nt2::b_and(x, nt2::genmask(x_lt_sqrthf)));
      dk = nt2::tofloat(k);
      s = f/nt2::add(nt2::Two<A0>(),f);
      A0 z = nt2::sqr(s);
      A0 w = nt2::sqr(z);
      A0 t1= w*nt2::horner<NT2_HORNER_COEFF_T(sA0, 3,
                                              (0x3fc39a09d078c69fll,
                                               0x3fcc71c51d8e78afll,
                                               0x3fd999999997fa04ll)
                                             )> (w);
      A0 t2= z*horner<NT2_HORNER_COEFF_T(sA0, 4,
                                         (0x3fc2f112df3e5244ll,
                                          0x3fc7466496cb03dell,
                                          0x3fd2492494229359ll,
                                          0x3fe5555555555593ll)
                                        )> (w);
      R = t2+t1;
      hfsq = nt2::mul(Half<A0>(), nt2::sqr(f));
    }

    static inline A0 log(const A0& a0)
    {
      // ln(2)hi  =  6.93147180369123816490e-01  or  0x3fe62e42fee00000
      // ln(2)lo  =  1.90821492927058770002e-10  or  0x3dea39ef35793c76
#ifndef BOOST_SIMD_NO_INFINITIES
      if (a0 == Inf<A0>()) return a0;
#endif
      if (is_eqz(a0)) return nt2::Minf<A0>();
#ifdef BOOST_SIMD_NO_NANS
      if (nt2::is_ltz(a0)) return nt2::Nan<A0>();
#else
      if (nt2::is_nan(a0)||nt2::is_ltz(a0)) return nt2::Nan<A0>();
#endif
      A0 dk, hfsq, s, R, f;
      kernel_log(a0, dk, hfsq, s, R, f);
      return  nt2::mul(dk, double_constant<A0, 0x3fe62e42fee00000ll>())-
        ((hfsq-(s*(hfsq+R)+nt2::mul(dk,double_constant<A0, 0x3dea39ef35793c76ll>())))-f);
    }

    static inline A0 log2(const A0& a0)
    {
#ifndef BOOST_SIMD_NO_INFINITIES
      if (a0 == nt2::Inf<A0>()) return a0;
#endif
      if (nt2::is_eqz(a0)) return nt2::Minf<A0>();
#ifdef BOOST_SIMD_NO_NANS
      if (nt2::is_ltz(a0)) return nt2::Nan<A0>();
#else
      if (nt2::is_nan(a0)||nt2::is_ltz(a0)) return nt2::Nan<A0>();
#endif
      A0 dk, hfsq, s, R, f;
      kernel_log(a0, dk, hfsq, s, R, f);
      return -(hfsq-(s*(hfsq+R))-f)*Invlog_2<A0>()+dk;
    }

    static inline A0 log10(const A0& a0)
    {
#ifndef BOOST_SIMD_NO_INFINITIES
      if (a0 == nt2::Inf<A0>()) return a0;
#endif
      if (nt2::is_eqz(a0)) return nt2::Minf<A0>();
#ifdef BOOST_SIMD_NO_NANS
      if (nt2::is_ltz(a0)) return nt2::Nan<A0>();
#else
      if (nt2::is_nan(a0)||nt2::is_ltz(a0)) return nt2::Nan<A0>();
#endif
      A0 dk, hfsq, s, R, f;
      kernel_log(a0, dk, hfsq, s, R, f);
      return -(hfsq-(s*(hfsq+R))-f)*nt2::Invlog_10<A0>()+dk*nt2::Log_2olog_10<A0>();
    }
  };
} }

#endif


// Copyright (c) Microsoft Corporation.
// Licensed under the MIT license.
//
// Generalized Pose-and-Scale Estimation using 4-Point Congruence Constraints
//
// Victor Fragoso and Sudipta Sinha.
// In Proc. of the IEEE International Conf. on 3D Vision (3DV) 2020.
//
// Please contact the author of this library if you have any questions.
// Author: Victor Fragoso (victor.fragoso@microsoft.com)

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/SVD>
#include <glog/logging.h>
#include <gp4pc/gp4pc.h>
#include <math/solve_gp4pc_polynomial.h>
#include <third_party/align_point_clouds.h>
#include <algorithm>
#include <utility>
#include <vector>

namespace msft {
namespace {

// Default coplanar threshold.
const double kDefaultCoplanarThresh = 1e-3;
const double kDefaultColinearThresh = 1e-2;
const double kRealNumberThreshold = 0.0;
const double kDuplicateSolnErrorThreshold = 1e-6;
const double kDuplicateCameraCenterErrorThreshold = 1e-6;
// Dimensions of the coefficient matrix: kNumConstraints x kNumMonomials.
const int kNumConstraints = 4;
const int kNumMonomials = 15;
// Max. num. of depth solutions.
const int kMaxNumDepthSolutions = 16;
// Size of the minimal sample.
const int kSizeOfMinimalSample = 4;

inline Eigen::Vector3d ComputeCameraPoint(const Eigen::Vector3d& camera_center,
                                          const Eigen::Vector3d& ray_direction,
                                          const double depth) {
  return camera_center + depth * ray_direction;
}

bool IsInputValid(const Gp4pc::Input& input) {
  // Check that the camera centers are different.
  int num_different_origins = 0;
  for (int i = 0; i < input.ray_origins.size(); ++i) {
    for (int j = i + 1; j < input.ray_origins.size(); ++j) {
      const double distance =
          (input.ray_origins[i] - input.ray_origins[j]).squaredNorm();
      if (distance > kDuplicateCameraCenterErrorThreshold) {
        num_different_origins += 1;
      }
    }
  }
  const bool has_different_origins = num_different_origins > 0;
  return has_different_origins;
}

// Build least-squares system Ax = b, where
// b = [p_1 p_2 p_3 p_4]^T
// and
//     | L(w_1)  I |
// A = | L(w_2)  I |
//     | L(w_3)  I |
//     | L(w_4)  I |.
void BuildLinearSystem(const std::vector<Eigen::Vector3d>& world_points,
                       const std::vector<Eigen::Vector3d>& camera_points,
                       Eigen::VectorXd* camera_points_vec_ptr,
                       Eigen::MatrixXd* world_points_mat_ptr) {
  // Dimensionality of the points.
  static const int kPointDimension = 3;

  const int num_points = world_points.size();
  Eigen::VectorXd& camera_points_vec = *camera_points_vec_ptr;
  Eigen::MatrixXd& world_points_mat = *world_points_mat_ptr;
  world_points_mat.setZero();

  int row_index = 0;
  for (int i = 0; i < num_points; ++i) {
    // Fill in camera points.
    camera_points_vec.segment(i * kPointDimension, kPointDimension) =
        camera_points[i];

    // Fill in world-point matrices.
    row_index = i * kPointDimension;
    world_points_mat.block(row_index, 0, 1, 3) = world_points[i].transpose();
    world_points_mat.block(row_index + 1, 3, 1, 3) =
        world_points[i].transpose();
    world_points_mat.block(row_index + 2, 6, 1, 3) =
        world_points[i].transpose();
    world_points_mat.block(row_index, 9, 3, 3) = Eigen::Matrix3d::Identity();
  }
}

bool IsInputDegenerateOrPlanar(const Gp4pc::Input& input,
                               const double coplanar_threshold,
                               const double colinear_threshold,
                               bool* is_planar) {
  *CHECK_NOTNULL(is_planar) = false;
  // Compute the SVD of the input 3D points.
  const Eigen::Vector3d mean_direction =
      (input.world_points[0] + input.world_points[1] + input.world_points[2] +
       input.world_points[3]) /
      4;
  Eigen::Matrix<double, 4, 3> normalized_points;
  normalized_points.row(0) = input.world_points[0] - mean_direction;
  normalized_points.row(1) = input.world_points[1] - mean_direction;
  normalized_points.row(2) = input.world_points[2] - mean_direction;
  normalized_points.row(3) = input.world_points[3] - mean_direction;
  const Eigen::Matrix3d covariance =
      normalized_points.transpose() * normalized_points;
  const Eigen::JacobiSVD<Eigen::Matrix3d> svd(
      covariance, Eigen::ComputeThinU | Eigen::ComputeThinV);
  const Eigen::Vector3d singular_values = svd.singularValues();

  // Is input degenerate, i.e., a line?
  double singular_value_ratio = singular_values[1] / singular_values[0];
  if (singular_value_ratio < colinear_threshold) {
    VLOG(2) << "Singular value ratio (colinear test): " << singular_value_ratio
            << " threshold: " << colinear_threshold;
    return true;
  }

  // Is the input planar?
  singular_value_ratio = singular_values[2] / singular_values[0];
  *is_planar = singular_value_ratio < coplanar_threshold;
  VLOG_IF(4, *is_planar) << "Singular value ratio (coplanar test): "
                         << singular_value_ratio
                         << " threshold: " << coplanar_threshold;

  return false;
}

// This function computes the values of t1 and t3 given the four points:
// (x1, x2, x3, x4).
//
// The point (x1 + t1 * v1) + t3 * v3 must be equal to the point (x3 + t2 * v2).
// This gives a linear system in t1, t2, t3. Solve it. This gives the following
// closest points on the lines.
//    x1 + t1 * v1;
//    x3 + t2 * v2;
Eigen::Vector2d Findt1t3(const Eigen::Vector3d& x1,
                         const Eigen::Vector3d& x2,
                         const Eigen::Vector3d& x3,
                         const Eigen::Vector3d& x4) {
  const Eigen::Vector3d v1 = x2 - x1;
  const Eigen::Vector3d v2 = x4 - x3;
  const Eigen::Vector3d v3 = v1.cross(v2);
  const Eigen::Vector3d b = x3 - x1;

  // A = [v1 -v2 v3]; (MATLAB code).
  Eigen::MatrixXd A(3, 3);
  A.col(0) = v1;
  A.col(1) = -v2;
  A.col(2) = v3;

  // t = A \ b; (MATLAB code).
  const Eigen::VectorXd x = A.lu().solve(b);

  // Solution
  return x;
}

// This function calculates (f1, f2, f3, f4) and (K1, K2) from the four points
// (x1, x2, x3, x4).
struct DerivedConstants {
  double f1;
  double f2;
  double f3;
  double f4;
  double k1;
  double k2;
};

DerivedConstants CalculateRatiosFromFourPoints(
    const Eigen::Vector3d& x1,
    const Eigen::Vector3d& x2,
    const Eigen::Vector3d& x3,
    const Eigen::Vector3d& x4) {
  DerivedConstants constants;
  double& f1 = constants.f1;
  double& f2 = constants.f2;
  double& f3 = constants.f3;
  double& f4 = constants.f4;
  double& K1 = constants.k1;
  double& K2 = constants.k2;

  // x1--x2 is the first line segment and x3--x4 is the second line segment.
  // z1 is on x1--x2, z3 is on x3--x4 and (z1--z3) is also perpendicular
  // to x1--x2 and x3--x4
  //
  // t1 is equal to dist(x1,z1) / dist(x1,x2)
  // t2 is equal to dist(x3,z3) / dist(x3,x4)
  //
  // [t1, t3, x1, x2, x3, x4] = findClosestPoints(X,1,2,3,4); (MATLAB code)

  const Eigen::Vector2d t_vec = Findt1t3(x1, x2, x3, x4);

  const double len12 = (x1 - x2).norm();
  const double len34 = (x3 - x4).norm();
  const double len13 = (x1 - x3).norm();

  const double s12 = len12 * len12;
  const double s34 = len34 * len34;
  const double s13 = len13 * len13;

  K1 = s12 / s34;
  K2 = s12 / s13;

  f1 = 1 - t_vec(0);
  f2 = t_vec(0);
  f3 = 1 - t_vec(1);
  f4 = t_vec(1);

  return constants;
}

Eigen::MatrixXd SolveForDepthsFromAPlanarInput(const Gp4pc::Input& input) {
  // Useful aliases.
  // Camera centers.
  const std::vector<Eigen::Vector3d>& ray_origins = input.ray_origins;
  // Unit rays originating from ray_origins.
  const std::vector<Eigen::Vector3d>& ray_directions = input.ray_directions;
  // World points.
  const std::vector<Eigen::Vector3d>& world_points = input.world_points;

  // Verify the input is consistent.
  CHECK_EQ(ray_origins.size(), ray_directions.size());
  CHECK_EQ(ray_origins.size(), world_points.size());

  // The mathematical relationship is the following:
  //
  // ray_origins[i] + depth[i] * ray_directions[i] =
  //         s * R * world_points[i] + t,
  //
  // where s is scale, R is a rotation matrix, and t is translation.

  // Input Constants.
  const Eigen::Vector3d& p1 = ray_origins[0];
  const Eigen::Vector3d& p2 = ray_origins[1];
  const Eigen::Vector3d& p3 = ray_origins[2];
  const Eigen::Vector3d& p4 = ray_origins[3];

  // Input Constants.
  const Eigen::Vector3d& u1 = ray_directions[0];
  const Eigen::Vector3d& u2 = ray_directions[1];
  const Eigen::Vector3d& u3 = ray_directions[2];
  const Eigen::Vector3d& u4 = ray_directions[3];

  // Input Constants.
  const Eigen::Vector3d& x1 = world_points[0];
  const Eigen::Vector3d& x2 = world_points[1];
  const Eigen::Vector3d& x3 = world_points[2];
  const Eigen::Vector3d& x4 = world_points[3];

  // Derived Constants.
  const DerivedConstants constants =
      CalculateRatiosFromFourPoints(x1, x2, x3, x4);

  // Derived Constants.
  const Eigen::Vector3d p5 =
      constants.f1 * p1 + constants.f2 * p2 -
      constants.f3 * p3 - constants.f4 * p4;
  const Eigen::Vector3d p6 = p1 - p2;
  const Eigen::Vector3d p8 = p1 - p3;

  // Derived Constants.
  const Eigen::Vector3d v1 = constants.f1 * u1;
  const Eigen::Vector3d v2 = constants.f2 * u2;
  const Eigen::Vector3d v3 = -constants.f3 * u3;
  const Eigen::Vector3d v4 = -constants.f4 * u4;

  // Think of the 3 x 3 system in s1, s2, s3
  // [a1 b1 c1] = [d1]
  // [a2 b2 c2] = [d2]
  // [a3 b3 c3] = [d3].
  const double& a1 = v1(0);
  const double& b1 = v2(0);
  const double& c1 = v3(0);
  const double& a2 = v1(1);
  const double& b2 = v2(1);
  const double& c2 = v3(1);
  const double& a3 = v1(2);
  const double& b3 = v2(2);
  const double& c3 = v3(2);

  const double denom =
      a1 * b2 * c3 + b1 * c2 * a3 + c1 * a2 * b3 -
      a3 * b2 * c1 - b3 * c2 * a1 - c3 * a2 * b1;

  const double coeff11 = b2 * c3 - b3 * c2;
  const double coeff12 = c1 * b3 - c3 * b1;
  const double coeff13 = b1 * c2 - b2 * c1;

  const double G1 =
      (-v4(0) * coeff11 - v4(1) * coeff12 - v4(2) * coeff13) / denom;
  const double H1 =
      (-p5(0) * coeff11 - p5(1) * coeff12 - p5(2) * coeff13) / denom;

  const double coeff21 = c2 * a3 - c3 * a2;
  const double coeff22 = a1 * c3 - a3 * c1;
  const double coeff23 = c1 * a2 - c2 * a1;

  const double G2 =
      (-v4(0) * coeff21 - v4(1) * coeff22 - v4(2) * coeff23) / denom;
  const double H2 =
      (-p5(0) * coeff21 - p5(1) * coeff22 - p5(2) * coeff23) / denom;

  const double coeff31 = b3 * a2 - b2 * a3;
  const double coeff32 = a3 * b1 - a1 * b3;
  const double coeff33 = b2 * a1 - b1 * a2;

  const double G3 =
      (-v4(0) * coeff31 - v4(1) * coeff32 - v4(2) * coeff33) / denom;
  const double H3 =
      (-p5(0) * coeff31 - p5(1) * coeff32 - p5(2) * coeff33) / denom;

  const double C1 = 1 - constants.k2;
  const double C2 = 1;
  const double C3 = -constants.k2;
  const double C4 = -2.0 * u1.dot(u2);
  const double C5 = 2.0 * constants.k2 * u1.dot(u3);
  const double C6 = 2.0 * (u1.dot(p6) - constants.k2 * u1.dot(p8));
  const double C7 = -2.0 * u2.dot(p6);
  const double C8 = 2.0 * constants.k2 * u3.dot(p8);
  const double C9 = p6.dot(p6) - constants.k2 * p8.dot(p8);

  // Derive the coefficients of the quadratic.
  const double A =
      C1 * G1 * G1 + C2 * G2 * G2 + C3 * G3 * G3 + C4 * G1 * G2 + C5 * G1 * G3;
  const double B =
      C1 * 2 * G1 * H1 + C2 * 2 * G2 * H2 + C3 * 2 * G3 * H3 +
      C4 * (G1 * H2 + G2 * H1) + C5 * (G1 * H3 + G3 * H1) + C6 * G1 +
      C7 * G2 + C8 * G3;
  const double C =
      C1 * H1 * H1 + C2 * H2 * H2 + C3 * H3 * H3 + C4 * H1 * H2 +
      C5 * H1 * H3 + C6 * H1 + C7 * H2 + C8 * H3 + C9;
  const double disc = (B * B - 4 * A * C);

  Eigen::MatrixXd depths;
  depths.resize(4, 2);
  depths.setZero();

  if (disc >= 0) {
    const double sqrtD = sqrt(disc);
    const double s4a = (-B - sqrtD) / (2 * A);
    const double s4b = (-B + sqrtD) / (2 * A);

    if (s4a > 0) {
      depths(0, 0) = G1 * s4a + H1;
      depths(1, 0) = G2 * s4a + H2;
      depths(2, 0) = G3 * s4a + H3;
      depths(3, 0) = s4a;
    }

    if (s4b > 0) {
      depths(0, 1) = G1 * s4b + H1;
      depths(1, 1) = G2 * s4b + H2;
      depths(2, 1) = G3 * s4b + H3;
      depths(3, 1) = s4b;
    }
  }

  return depths;
}

Eigen::MatrixXd BuildCoefficientMatrix(const Gp4pc::Input& input) {
  Eigen::MatrixXd coeff_mat(kNumConstraints, kNumMonomials);
  coeff_mat.setZero();

  // Useful aliases.
  // Camera centers.
  const std::vector<Eigen::Vector3d>& ray_origins = input.ray_origins;
  // Unit rays originating from ray_origins.
  const std::vector<Eigen::Vector3d>& ray_directions = input.ray_directions;
  // World points.
  const std::vector<Eigen::Vector3d>& world_points = input.world_points;

  // Verify the input is consistent.
  CHECK_EQ(ray_origins.size(), ray_directions.size());
  CHECK_EQ(ray_origins.size(), world_points.size());

  // The mathematical relationship is the following:
  //
  // ray_origins[i] + depth[i] * ray_directions[i] =
  //       s * R * world_points[i] + t,
  //
  // where s is scale, R is a rotation matrix, and t is translation.

  // Input Constants
  const Eigen::Vector3d& p1 = ray_origins[0];
  const Eigen::Vector3d& p2 = ray_origins[1];
  const Eigen::Vector3d& p3 = ray_origins[2];
  const Eigen::Vector3d& p4 = ray_origins[3];

  // Input Constants
  const Eigen::Vector3d& u1 = ray_directions[0];
  const Eigen::Vector3d& u2 = ray_directions[1];
  const Eigen::Vector3d& u3 = ray_directions[2];
  const Eigen::Vector3d& u4 = ray_directions[3];

  // Input Constants
  const Eigen::Vector3d& x1 = world_points[0];
  const Eigen::Vector3d& x2 = world_points[1];
  const Eigen::Vector3d& x3 = world_points[2];
  const Eigen::Vector3d& x4 = world_points[3];

  // Derived Constants
  const DerivedConstants constants =
      CalculateRatiosFromFourPoints(x1, x2, x3, x4);

  // Derived Constants
  const Eigen::Vector3d p5  =
      constants.f1 * p1 + constants.f2 * p2 -
      constants.f3 * p3 - constants.f4 * p4;
  const Eigen::Vector3d p6 = p1 - p2;
  const Eigen::Vector3d p7 = p3 - p4;
  const Eigen::Vector3d p8 = p1 - p3;
  const Eigen::Vector3d p9 = p2 - p4;
  const Eigen::Vector3d p10 = p1 - p4;
  const Eigen::Vector3d p11 = p2 - p3;

  // Derived Constants
  const Eigen::Vector3d v1 = constants.f1 * u1;
  const Eigen::Vector3d v2 = constants.f2 * u2;
  const Eigen::Vector3d v3 = -constants.f3 * u3;
  const Eigen::Vector3d v4 = -constants.f4 * u4;

  // Equation derived from orthogonality constraint 1.
  coeff_mat(0, 0) = v1.dot(u1);
  coeff_mat(0, 1) = -u2.dot(v2);
  coeff_mat(0, 2) = 0;
  coeff_mat(0, 3) = 0;
  coeff_mat(0, 4) = u1.dot(v2) - u2.dot(v1);
  coeff_mat(0, 5) = u1.dot(v3);
  coeff_mat(0, 6) = u1.dot(v4);
  coeff_mat(0, 7) = -u2.dot(v3);
  coeff_mat(0, 8) = -u2.dot(v4);
  coeff_mat(0, 9) = 0;
  coeff_mat(0, 10) = p6.dot(v1) + u1.dot(p5);
  coeff_mat(0, 11) = p6.dot(v2) - u2.dot(p5);
  coeff_mat(0, 12) = p6.dot(v3);
  coeff_mat(0, 13) = p6.dot(v4);
  coeff_mat(0, 14) = p6.dot(p5);

  // Equation derived from orthogonality constraint 2.
  coeff_mat(1, 0) =  0;
  coeff_mat(1, 1) =  0;
  coeff_mat(1, 2) =  u3.dot(v3);
  coeff_mat(1, 3) = -u4.dot(v4);
  coeff_mat(1, 4) =  0;
  coeff_mat(1, 5) =  u3.dot(v1);
  coeff_mat(1, 6) = -u4.dot(v1);
  coeff_mat(1, 7) =  u3.dot(v2);
  coeff_mat(1, 8) = -u4.dot(v2);
  coeff_mat(1, 9) =  u3.dot(v4) - u4.dot(v3);
  coeff_mat(1, 10) =  p7.dot(v1);
  coeff_mat(1, 11) =  p7.dot(v2);
  coeff_mat(1, 12) =  p7.dot(v3) + u3.dot(p5);
  coeff_mat(1, 13) =  p7.dot(v4) - u4.dot(p5);
  coeff_mat(1, 14) =  p7.dot(p5);

  // Coefficients of quadratic polynomial that corresponds to the squared
  // length for edges 1--2, 1--3 and 3--4.
  Eigen::VectorXd d12(kNumMonomials);
  Eigen::VectorXd d13(kNumMonomials);
  Eigen::VectorXd d34(kNumMonomials);
  d12.setZero();
  d13.setZero();
  d34.setZero();

  d12(0) = 1;
  d12(1) = 1;
  d12(4) = -2.0 * u1.dot(u2);
  d12(10) = 2.0 * u1.dot(p6);
  d12(11) = -2.0 * u2.dot(p6);
  d12(14) = p6.dot(p6);

  d13(0) = 1;
  d13(2) = 1;
  d13(5) = -2.0 * u1.dot(u3);
  d13(10) = 2.0 * u1.dot(p8);
  d13(12) = -2.0 * u3.dot(p8);
  d13(14) = p8.dot(p8);

  d34(2) = 1;
  d34(3) = 1;
  d34(9) = -2.0 * u3.dot(u4);
  d34(12) = 2.0 * u3.dot(p7);
  d34(13) = -2.0 * u4.dot(p7);
  d34(14) = p7.dot(p7);

  // Two equations derived from distance ratio constraints.
  coeff_mat.row(2) = d12 - constants.k1 * d34;
  coeff_mat.row(3) = d12 - constants.k2 * d13;

  return coeff_mat;
}

bool IsSimilarityTransformValid(const Eigen::Matrix3d& rotation,
                                const Eigen::Vector3d& translation,
                                const double scale) {
  // Make sure rotation is finite.
  const double* rotation_vals = rotation.data();
  bool is_rotation_finite = true;
  for (int i = 0; i < 9; ++i) {
    is_rotation_finite = is_rotation_finite && std::isfinite(rotation_vals[i]);
  }
  // Make sure translation is finite.
  const bool is_translation_finite =
      std::isfinite(translation(0)) &&
      std::isfinite(translation(1)) &&
      std::isfinite(translation(2));
  // Make scale is finite.
  const bool is_scale_valid = scale > 0.0;
  if (is_rotation_finite && is_translation_finite && is_scale_valid) {
    return true;
  }

  return false;
}

void SolveForRotationAndTranslation(
    const Gp4pc::Input& input,
    const std::vector<Eigen::Vector4d>& plausible_depths,
    Gp4pc::Solution* solution) {
  solution->rotations.reserve(plausible_depths.size());
  solution->translations.reserve(plausible_depths.size());
  solution->scales.reserve(plausible_depths.size());
  solution->depths.reserve(plausible_depths.size());

  // Solve for similarity transform using Umeyama's method. See
  // theia/sfm/transformation/align_point_clouds.h for more information.
  std::vector<Eigen::Vector3d> camera_points(input.world_points.size());
  Eigen::Matrix3d rotation;
  Eigen::Vector3d translation;
  double scale;
  for (const Eigen::Vector4d& depth : plausible_depths) {
    // Compute the camera points.
    camera_points[0] = ComputeCameraPoint(
        input.ray_origins[0], input.ray_directions[0], depth[0]);
    camera_points[1] = ComputeCameraPoint(
        input.ray_origins[1], input.ray_directions[1], depth[1]);
    camera_points[2] = ComputeCameraPoint(
        input.ray_origins[2], input.ray_directions[2], depth[2]);
    camera_points[3] = ComputeCameraPoint(
        input.ray_origins[3], input.ray_directions[3], depth[3]);
    theia::AlignPointCloudsUmeyama(
        input.world_points, camera_points, &rotation, &translation, &scale);

    if (IsSimilarityTransformValid(rotation, translation, scale)) {
      solution->rotations.emplace_back(rotation);
      solution->translations.emplace_back(translation);
      solution->scales.emplace_back(scale);
      solution->depths.emplace_back(depth);
    }
  }
}

}  // namespace

Gp4pc::Gp4pc(const Params& params) : params_(params) {}

std::vector<Eigen::Vector4d>
Gp4pc::KeepPlausibleSolutions(const Eigen::MatrixXcd& solutions) {
  std::vector<Eigen::Vector4d> plausible_solutions;
  plausible_solutions.reserve(kMaxNumDepthSolutions);

  // Iterate through the possible solutions.
  Eigen::Vector4d estimated_solution;
  double discriminant = 0.0;
  double max_imag_entry = 0.0;
  Eigen::Vector4d prev_solution;
  double min_depth = 0.0;
  prev_solution.setZero();
  for (int i = 0; i < solutions.cols(); ++i) {
    max_imag_entry = std::max(std::abs(solutions(0, i).imag()),
                              std::abs(solutions(1, i).imag()));
    max_imag_entry = std::max(max_imag_entry, std::abs(solutions(2, i).imag()));
    max_imag_entry = std::max(max_imag_entry, std::abs(solutions(3, i).imag()));
    // TODO(vfragoso): Threshold solutions with large imaginary parts. Set a
    // a good threshold for getting good solutions with small imaginary parts.
    const bool real_solution = (max_imag_entry <= kRealNumberThreshold);
    VLOG(4) << "Max imag entry: " << max_imag_entry
            << " => real_solution: " << real_solution;

    // Keep the solutions that correspond to positive depths.
    estimated_solution[0] = solutions(0, i).real();
    estimated_solution[1] = solutions(1, i).real();
    estimated_solution[2] = solutions(2, i).real();
    estimated_solution[3] = solutions(3, i).real();

    min_depth = estimated_solution.minCoeff();
    const bool all_positive_depths = (min_depth >= 0.0);
    VLOG(4) << "Min. depth: " << min_depth
            << " => " << all_positive_depths
            << " " << estimated_solution.transpose();

    // Avoid duplicated solutions coming from those with small imaginary parts.
    // Remove duplicate solutions which are contigous.
    const double error = (prev_solution - estimated_solution).squaredNorm();
    const bool is_unique_soln = (error >= kDuplicateSolnErrorThreshold);

    // Save previous solution, regardless.
    VLOG(4) << "Unique: " << is_unique_soln << " "
            << prev_solution.transpose() << " <-> "
            << estimated_solution.transpose();
    prev_solution = estimated_solution;
    if (real_solution && all_positive_depths && is_unique_soln) {
      // Save solution.
      plausible_solutions.emplace_back(std::move(estimated_solution));
    }
  }

  VLOG(4) << "Plausible depths: " << plausible_solutions.size()
          << " out of " << solutions.cols();
  return plausible_solutions;
}

std::vector<Eigen::Vector4d>
Gp4pc::SolveForPoseViaPlanarSolver(const Gp4pc::Input& input) {
  VLOG(4) << "Solving pose using planar solver...";
  // Solve for depths via the planar solver.
  const Eigen::MatrixXcd depths = SolveForDepthsFromAPlanarInput(input);

  // Discard bad solutions.
  const std::vector<Eigen::Vector4d> final_depths =
      KeepPlausibleSolutions(depths);

  return final_depths;
}

std::vector<Eigen::Vector4d>
Gp4pc::SolveForPoseViaGeneralSolver(const Input& input) {
  VLOG(4) << "Solving for pose!";
  // Build coefficient matrix.
  const Eigen::MatrixXd coefficients_matrix = BuildCoefficientMatrix(input);
  VLOG(4) << "Coeff matrix: \n" << coefficients_matrix;

  // Solve for depths.
  const Eigen::MatrixXcd depths = SolveGp4pcPolynomial(coefficients_matrix);

  // Discard bad solutions.
  const std::vector<Eigen::Vector4d> final_depths =
      KeepPlausibleSolutions(depths);

  return final_depths;
}

bool Gp4pc::EstimateSimilarityTransformation(const Input& input,
                                             Solution* solution) {
  using Eigen::MatrixXcd;
  using Eigen::Vector4d;
  CHECK_EQ(input.ray_origins.size(), kSizeOfMinimalSample);
  CHECK_EQ(input.ray_origins.size(), input.ray_directions.size());
  CHECK_EQ(input.ray_origins.size(), input.world_points.size());
  CHECK_NOTNULL(solution)->rotations.clear();
  solution->translations.clear();
  solution->scales.clear();
  solution->depths.clear();

  // Validate input.
  if (!IsInputValid(input)) {
    return false;
  }

  // Each entry is the depth wrt to a camera center.
  MatrixXcd depths;
  std::vector<Vector4d> plausible_depths;
  plausible_depths.reserve(kMaxNumDepthSolutions);

  // Check whether the input is planar.
  bool is_planar = false;
  const bool use_planar_solver = !params_.use_general_solver;
  if (IsInputDegenerateOrPlanar(input,
                                params_.coplanar_threshold,
                                params_.colinear_threshold,
                                &is_planar)) {
    VLOG(4) << "Degenerate case. Could not compute pose.";
    return false;
  } else if (use_planar_solver && is_planar) {
    VLOG(4) << "Coplanar case.";
    // Solve for the planar case.
    plausible_depths = SolveForPoseViaPlanarSolver(input);
  } else {
    VLOG(4) << "Using general solver.";
    plausible_depths = SolveForPoseViaGeneralSolver(input);
  }

  if (plausible_depths.empty()) {
    return false;
  }
  VLOG(4) << "Number of plausible depths: " << plausible_depths.size();

  // Solve for rotation and translation.
  SolveForRotationAndTranslation(input, plausible_depths, solution);
  VLOG(4) << "Number of final solutions: " << solution->rotations.size();

  return !solution->rotations.empty();
}

}  // namespace msft


#ifndef STAN_MATH_PRIM_SCAL_PROB_GAMMA_LPDF_HPP
#define STAN_MATH_PRIM_SCAL_PROB_GAMMA_LPDF_HPP

#include <stan/math/prim/scal/meta/is_constant_struct.hpp>
#include <stan/math/prim/scal/meta/partials_return_type.hpp>
#include <stan/math/prim/scal/meta/operands_and_partials.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_greater_or_equal.hpp>
#include <stan/math/prim/scal/err/check_less_or_equal.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/fun/multiply_log.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/fun/gamma_p.hpp>
#include <stan/math/prim/scal/fun/digamma.hpp>
#include <stan/math/prim/scal/meta/VectorBuilder.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <stan/math/prim/scal/meta/scalar_seq_view.hpp>
#include <stan/math/prim/scal/fun/grad_reg_inc_gamma.hpp>
#include <boost/random/gamma_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <cmath>

namespace stan {
  namespace math {

    /**
     * The log of a gamma density for y with the specified
     * shape and inverse scale parameters.
     * Shape and inverse scale parameters must be greater than 0.
     * y must be greater than or equal to 0.
     *
     \f{eqnarray*}{
     y &\sim& \mbox{\sf{Gamma}}(\alpha, \beta) \\
     \log (p (y \, |\, \alpha, \beta) ) &=& \log \left( \frac{\beta^\alpha}{\Gamma(\alpha)} y^{\alpha - 1} \exp^{- \beta y} \right) \\
     &=& \alpha \log(\beta) - \log(\Gamma(\alpha)) + (\alpha - 1) \log(y) - \beta y\\
     & & \mathrm{where} \; y > 0
     \f}
     * @param y A scalar variable.
     * @param alpha Shape parameter.
     * @param beta Inverse scale parameter.
     * @throw std::domain_error if alpha is not greater than 0.
     * @throw std::domain_error if beta is not greater than 0.
     * @throw std::domain_error if y is not greater than or equal to 0.
     * @tparam T_y Type of scalar.
     * @tparam T_shape Type of shape.
     * @tparam T_inv_scale Type of inverse scale.
     */
    template <bool propto,
              typename T_y, typename T_shape, typename T_inv_scale>
    typename return_type<T_y, T_shape, T_inv_scale>::type
    gamma_lpdf(const T_y& y, const T_shape& alpha, const T_inv_scale& beta) {
      static const char* function("gamma_lpdf");
      typedef typename stan::partials_return_type<T_y, T_shape,
                                                  T_inv_scale>::type
        T_partials_return;

      using stan::is_constant_struct;

      if (!(stan::length(y) && stan::length(alpha) && stan::length(beta)))
        return 0.0;

      T_partials_return logp(0.0);

      check_not_nan(function, "Random variable", y);
      check_positive_finite(function, "Shape parameter", alpha);
      check_positive_finite(function, "Inverse scale parameter", beta);
      check_consistent_sizes(function,
                             "Random variable", y,
                             "Shape parameter", alpha,
                             "Inverse scale parameter", beta);

      if (!include_summand<propto, T_y, T_shape, T_inv_scale>::value)
        return 0.0;

      scalar_seq_view<T_y> y_vec(y);
      scalar_seq_view<T_shape> alpha_vec(alpha);
      scalar_seq_view<T_inv_scale> beta_vec(beta);

      for (size_t n = 0; n < length(y); n++) {
        const T_partials_return y_dbl = value_of(y_vec[n]);
        if (y_dbl < 0)
          return LOG_ZERO;
      }

      size_t N = max_size(y, alpha, beta);
      operands_and_partials<T_y, T_shape, T_inv_scale>
        ops_partials(y, alpha, beta);

      using boost::math::lgamma;
      using boost::math::digamma;
      using std::log;

      VectorBuilder<include_summand<propto, T_y, T_shape>::value,
                    T_partials_return, T_y> log_y(length(y));
      if (include_summand<propto, T_y, T_shape>::value) {
        for (size_t n = 0; n < length(y); n++) {
          if (value_of(y_vec[n]) > 0)
            log_y[n] = log(value_of(y_vec[n]));
        }
      }

      VectorBuilder<include_summand<propto, T_shape>::value,
                    T_partials_return, T_shape> lgamma_alpha(length(alpha));
      VectorBuilder<!is_constant_struct<T_shape>::value,
                    T_partials_return, T_shape> digamma_alpha(length(alpha));
      for (size_t n = 0; n < length(alpha); n++) {
        if (include_summand<propto, T_shape>::value)
          lgamma_alpha[n] = lgamma(value_of(alpha_vec[n]));
        if (!is_constant_struct<T_shape>::value)
          digamma_alpha[n] = digamma(value_of(alpha_vec[n]));
      }

      VectorBuilder<include_summand<propto, T_shape, T_inv_scale>::value,
                    T_partials_return, T_inv_scale> log_beta(length(beta));
      if (include_summand<propto, T_shape, T_inv_scale>::value) {
        for (size_t n = 0; n < length(beta); n++)
          log_beta[n] = log(value_of(beta_vec[n]));
      }

      for (size_t n = 0; n < N; n++) {
        const T_partials_return y_dbl = value_of(y_vec[n]);
        const T_partials_return alpha_dbl = value_of(alpha_vec[n]);
        const T_partials_return beta_dbl = value_of(beta_vec[n]);

        if (include_summand<propto, T_shape>::value)
          logp -= lgamma_alpha[n];
        if (include_summand<propto, T_shape, T_inv_scale>::value)
          logp += alpha_dbl * log_beta[n];
        if (include_summand<propto, T_y, T_shape>::value)
          logp += (alpha_dbl - 1.0) * log_y[n];
        if (include_summand<propto, T_y, T_inv_scale>::value)
          logp -= beta_dbl * y_dbl;

        if (!is_constant_struct<T_y>::value)
          ops_partials.edge1_.partials_[n] += (alpha_dbl - 1) / y_dbl
            - beta_dbl;
        if (!is_constant_struct<T_shape>::value)
          ops_partials.edge2_.partials_[n] += -digamma_alpha[n] + log_beta[n]
            + log_y[n];
        if (!is_constant_struct<T_inv_scale>::value)
          ops_partials.edge3_.partials_[n] += alpha_dbl / beta_dbl - y_dbl;
      }
      return ops_partials.build(logp);
    }

    template <typename T_y, typename T_shape, typename T_inv_scale>
    inline
    typename return_type<T_y, T_shape, T_inv_scale>::type
    gamma_lpdf(const T_y& y, const T_shape& alpha, const T_inv_scale& beta) {
      return gamma_lpdf<false>(y, alpha, beta);
    }

  }
}
#endif


#include <memory>
#include <chrono>
#include <random>
#include <gflags/gflags.h>

#include <Eigen/Sparse>

#include "drake/solvers/mosek_solver.h"
#include "drake/solvers/gurobi_solver.h"
#include "drake/solvers/mathematical_program.h"

#include "examples/PlanarWalker/sgd_iter.h"
#include "systems/goldilocks_models/file_utils.h"

using Eigen::MatrixXd;
using Eigen::VectorXd;
using std::string;
using drake::solvers::VectorXDecisionVariable;

namespace dairlib {
namespace goldilocks_models {

void runSGD() {
  std::random_device randgen;
  std::default_random_engine e1(randgen());
  std::uniform_real_distribution<> dist(0, 1);

  int n_batch = 5;

  // int n_weights = 43;
  int n_weights =  10;
  MatrixXd theta_0 = MatrixXd::Zero(2,n_weights);
  theta_0(0,0) = -0.1;
  theta_0(0,3) = 1.0;
  theta_0(1,0) = 0;
  theta_0(1,1) = 1;
  writeCSV("data/0_theta.csv", theta_0);

  double length = 0.3;
  double duration = 1;
  int snopt_iter = 200;
  string directory = "data/";
  string init_z = "z_save.csv";
  string weights = "0_theta.csv";
  string output_prefix = "0_0_";
  sgdIter(length, duration, snopt_iter, directory, init_z, weights, output_prefix);

  for (int iter = 1; iter <= 50; iter++) {
    int input_batch = iter == 1 ? 1 : n_batch;

    std::vector<MatrixXd> A_vec;
    std::vector<MatrixXd> B_vec;
    std::vector<MatrixXd> H_vec;
    std::vector<MatrixXd> A_active_vec;
    std::vector<MatrixXd> B_active_vec;
    std::vector<MatrixXd> lb_vec;
    std::vector<MatrixXd> ub_vec;
    std::vector<MatrixXd> y_vec;
    std::vector<MatrixXd> w_vec;
    std::vector<MatrixXd> z_vec;
    std::vector<MatrixXd> theta_vec;
    std::vector<double> nl_vec;
    std::vector<double> nz_vec;

    int nz=0,nt=0,nl=0;

    for (int batch = 0; batch < input_batch; batch++) {
      string batch_prefix = std::to_string(iter-1) + "_" + std::to_string(batch) + "_";
      string iter_prefix = std::to_string(iter-1) + "_";

      A_vec.push_back(readCSV(directory + batch_prefix + "A.csv"));
      B_vec.push_back(readCSV(directory + batch_prefix + "B.csv"));
      H_vec.push_back(readCSV(directory + batch_prefix + "H.csv"));
      lb_vec.push_back(readCSV(directory + batch_prefix + "lb.csv"));
      ub_vec.push_back(readCSV(directory + batch_prefix + "ub.csv"));
      y_vec.push_back(readCSV(directory + batch_prefix + "y.csv"));
      w_vec.push_back(readCSV(directory + batch_prefix + "w.csv"));
      z_vec.push_back(readCSV(directory + batch_prefix + "z.csv"));
      theta_vec.push_back(readCSV(directory + iter_prefix + "theta.csv"));

      DRAKE_ASSERT(w_vec[batch].cols() == 1);
      DRAKE_ASSERT(lb_vec[batch].cols() == 1);
      DRAKE_ASSERT(ub_vec[batch].cols() == 1);
      DRAKE_ASSERT(y_vec[batch].cols() == 1);
      DRAKE_ASSERT(w_vec[batch].cols() == 1);
      DRAKE_ASSERT(z_vec[batch].cols() == 1);

      int n_active = 0;
      double tol = 1e-4;
      for (int i = 0; i < y_vec[batch].rows(); i++) {
        if (y_vec[batch](i) >= ub_vec[batch](i) - tol || y_vec[batch](i) <= lb_vec[batch](i) + tol)
          n_active++;
      }

      int nz_i = A_vec[batch].cols();
      int nt_i = B_vec[batch].cols();

      MatrixXd A_active(n_active, nz_i);
      MatrixXd B_active(n_active, nt_i);
      MatrixXd AB_active(n_active, nz_i + nt_i);

      int nl_i = 0;
      for (int i = 0; i < y_vec[batch].rows(); i++) {
        if (y_vec[batch](i) >= ub_vec[batch](i) - tol || y_vec[batch](i) <= lb_vec[batch](i) + tol) {
          A_active.row(nl_i) = A_vec[batch].row(i);
          B_active.row(nl_i) = B_vec[batch].row(i);
          AB_active.row(nl_i) << A_vec[batch].row(i), B_vec[batch].row(i);
          nl_i++;
        }
      }

      A_active_vec.push_back(A_active);
      B_active_vec.push_back(B_active);
      nl_vec.push_back(nl_i);
      nz_vec.push_back(nz_i);

      nl += nl_i;
      nz += nz_i;
      if (batch == 0) {
        nt = nt_i;
      } else {
        DRAKE_ASSERT(nt == nt_i);
        DRAKE_ASSERT((theta_vec[0] - theta_vec[batch]).norm() == 0);
      }
    }

    //Join matricies
    // MatrixXd AB_active = MatrixXd::Zero(nl,nz+nt);
    // MatrixXd H_ext = MatrixXd::Zero(nz + nt, nz + nt);
    VectorXd w_ext = VectorXd::Zero(nz+nt,1);
    int nl_start = 0;
    int nz_start = 0;
    for (int batch = 0; batch < input_batch; batch++) {
      // AB_active.block(nl_start, nz_start, nl_vec[batch], nz_vec[batch]) = A_active_vec[batch];
      // AB_active.block(nl_start, nz, nl_vec[batch], nt) = B_active_vec[batch];

      // H_ext.block(nz_start,nz_start,nz_vec[batch],nz_vec[batch]) = H_vec[batch];
      w_ext.segment(nz_start,nz_vec[batch]) = w_vec[batch].col(0);

      nl_start += nl_vec[batch];
      nz_start += nz_vec[batch];
    }
    // H_ext.block(nz,nz,nt,nt) = 1e-2*MatrixXd::Identity(nt,nt);

    // Eigen::BDCSVD<MatrixXd> svd(AB_active,  Eigen::ComputeFullV);

    // MatrixXd N = svd.matrixV().rightCols(AB_active.cols() - svd.rank());

    // auto gradient = N*N.transpose()*w_ext;

    // double scale_num= gradient.dot(gradient);
    // double scale_den = gradient.dot(Ei_ext*gradient);

    // auto dtheta = -0.02*gradient.tail(nt)*scale_num/scale_den;

    nl_start = 0;
    nz_start = 0;
    std::vector<Eigen::Triplet<double>> tripletList;
    std::vector<Eigen::Triplet<double>> tripletList_H;
    for (int batch = 0; batch < input_batch; batch++) {
      for (int i = 0; i < nz_vec[batch]; i++) {
        for (int j = 0; j < nz_vec[batch]; j++) {
          tripletList.push_back(Eigen::Triplet<double>(nz_start + i, nz_start + j, H_vec[batch](i,j)));
          tripletList_H.push_back(Eigen::Triplet<double>(nz_start + i, nz_start + j, H_vec[batch](i,j)));
        }
      }
      for (int i = 0; i < nl_vec[batch]; i++) {
        for (int j = 0; j < nz_vec[batch]; j++) {
          int i_ind = nz + nt + nl_start + i;
          int j_ind = nz_start + j;
          tripletList.push_back(Eigen::Triplet<double>(i_ind, j_ind, A_active_vec[batch](i,j)));
          tripletList.push_back(Eigen::Triplet<double>(j_ind, i_ind, A_active_vec[batch](i,j)));
        }
        for (int j = 0; j < nt; j++) {
          int i_ind = nz + nt + nl_start + i;
          int j_ind = nz + j;
          tripletList.push_back(Eigen::Triplet<double>(i_ind, j_ind, B_active_vec[batch](i,j)));
          tripletList.push_back(Eigen::Triplet<double>(j_ind, i_ind, B_active_vec[batch](i,j)));
        }
      }
      nl_start += nl_vec[batch];
      nz_start += nz_vec[batch];
    }

    VectorXd b(nz + nt + nl);
    b << -w_ext, VectorXd::Zero(nl);

    Eigen::SparseMatrix<double> M(nz + nt + nl, nz + nt + nl);
    M.setFromTriplets(tripletList.begin(), tripletList.end());

    Eigen::SparseMatrix<double> H_ext(nz + nt, nz + nt);
    H_ext.setFromTriplets(tripletList_H.begin(), tripletList_H.end());
    Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>> qr(M);
    auto gradient = qr.solve(b);

    // Eigen::SparseLU<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>> solver;
    // // fill A and b;
    // // Compute the ordering permutation vector from the structural pattern of A
    // solver.analyzePattern(M);
    // // Compute the numerical factorization
    // solver.factorize(M);
    // std::cout << solver.info() << std::endl;
    // //Use the factors to solve the linear system
    // auto gradient = solver.solve(b);

    // Eigen::BiCGSTAB<Eigen::SparseMatrix<double> > solver;
    // solver.compute(M);
    // auto gradient = solver.solve(b);
    // std::cout << "#iterations:     " << solver.iterations() << std::endl;
    // std::cout << "estimated error: " << solver.error()      << std::endl;

    // std::cout << M*gradient - b << std::endl;

    // std::cout << gradient << std::endl;

    // //M = [H_ext A'; A 0]
    // MatrixXd M(nz + nt + nl, nz + nt + nl);
    // M.block(0, 0, nz + nt, nz + nt) = H_ext;
    // M.block(0, nz + nt, nz + nt, nl) = AB_active.transpose();
    // M.block(nz + nt, 0, nl, nz + nt) = AB_active;
    // M.block(nz + nt, nz + nt, nl, nl) = MatrixXd::Zero(nl,nl);
    // VectorXd b(nz + nt + nl);
    // b << -w_ext, VectorXd::Zero(nl);
    // auto gradient = M.colPivHouseholderQr().solve(b);

    auto zt = gradient.head(nz+nt);

    auto resid = M*gradient - b;

    std::cout << "residual-norm: "<< resid.tail(nl).norm() << std::endl;
    std::cout << "descent: "<< gradient.head(nz+nt).dot(w_ext) << std::endl;

    double scale = 1/sqrt(zt.dot(H_ext*zt));

    std::cout << "scale: "<< scale << std::endl;

    auto dtheta = -.01*scale*gradient.segment(nz, nt);

    std::cout << "found dtheta"<< std::endl;

    std::cout << std::endl<< "dtheta norm: " << dtheta.norm() << std::endl;
    std::cout << "***********Next iteration*************" << std::endl;

    // std::cout << "scale predict: " << scale_num/scale_den << std::endl;

    //reshape dtheta
    MatrixXd theta_mat(theta_vec[0].rows(), theta_vec[0].cols());
    for (int i = 0; i < theta_vec[0].rows(); i++) {
      theta_mat.row(i) = theta_vec[0].row(i) +
                         dtheta.segment(i*theta_vec[0].cols(),theta_vec[0].cols()).transpose();
    }
    if (iter == 1)
      writeCSV("data/" + std::to_string(iter) + "_theta.csv", theta_vec[0]);
    else
      writeCSV("data/" + std::to_string(iter) + "_theta.csv", theta_mat);

    // init_z = std::to_string(iter-1) + "_z.csv";
    init_z = "z_save.csv";
    weights = std::to_string(iter) +  "_theta.csv";
    output_prefix = std::to_string(iter) +  "_";

    for(int batch = 0; batch < n_batch; batch++) {
    //randomize distance on [0.3,0.5]
      // length = 0.2 + 0.3*dist(e1);
      length = 0.15 + 0.05*batch + 0.0*dist(e1);
      // duration =  length/0.5; //maintain constaint speed of 0.5 m/s
      duration = 1;

      int length_file_index = (int) ((0.5 - length) * 10);
      if (iter == 1)
        init_z = "init_length_" + std::to_string(length_file_index) + "_speed_0_z.csv";
      else
        init_z = std::to_string(iter-1) + "_" + std::to_string(batch) + "_z.csv";

      std::cout << std::endl << "Iter-Batch: " << iter << "-" << batch << std::endl;
      std::cout << "New length: " << length << std::endl;

      string batch_prefix = output_prefix + std::to_string(batch) + "_";

      sgdIter(length, duration, snopt_iter, directory, init_z, weights, batch_prefix);
    }
  }
}
}  // namespace goldilocks_models
}  // namespace dairlib

int main(int argc, char* argv[]) {
  gflags::ParseCommandLineFlags(&argc, &argv, true);
  std::srand(time(0));  // Initialize random number generator.

  dairlib::goldilocks_models::runSGD();
}