#include <iostream>
#include <string>
#include <stdexcept>

#include <yaml-cpp/yaml.h>
#include <Eigen/Dense>

#include <cannon/math/random_double.hpp>
#include <cannon/ray/ray.hpp>
#include <cannon/ray/write_ppm.hpp>
#include <cannon/ray/hittable_list.hpp>
#include <cannon/ray/sphere.hpp>
#include <cannon/ray/moving_sphere.hpp>
#include <cannon/ray/camera.hpp>
#include <cannon/ray/material.hpp>
#include <cannon/ray/raytracer.hpp>
#include <cannon/ray/bvh.hpp>
#include <cannon/ray/aa_rect.hpp>
#include <cannon/ray/constant_medium.hpp>
#include <cannon/ray/mesh.hpp>
#include <cannon/ray/texture.hpp>
#include <cannon/ray/filter.hpp>
#include <cannon/graphics/random_color.hpp>
#include <cannon/log/registry.hpp>
#include <cannon/utils/statistics.hpp>

using namespace Eigen;

using namespace cannon::ray;
using namespace cannon::math;
using namespace cannon::log;
using namespace cannon::utils;

std::shared_ptr<HittableList> random_scene() {
  auto world = std::make_shared<HittableList>();

  auto checker = std::make_shared<CheckerTexture>(Vector3d(0.2, 0.3, 0.1), Vector3d(0.9, 0.9, 0.9));
  world->add(std::make_shared<Sphere>(Vector3d(0, -1000, 0), 1000, std::make_shared<Lambertian>(checker)));

  for (int a = -11; a < 11; a++) {
    for (int b = -11; b < 11; b++) {
      auto choose_mat = random_double();

      Vector3d center(a + 0.9*random_double(), 0.2, b + 0.9*random_double());

      if ((center - Vector3d(4, 0.2, 0)).norm() > 0.9) {
        auto albedo = (Vector3d::Random().array() * Vector3d::Random().array()).matrix();

        if (choose_mat < 0.8) {
          // Diffuse
          auto sphere_material = std::make_shared<Lambertian>(albedo);
          Vector3d center2 = center + Vector3d(0, random_double(0, .5), 0);
          world->add(std::make_shared<MovingSphere>(center, center2, 0.0, 1.0, 0.2, sphere_material));
        } else if (choose_mat < 0.95) {
          // Metal
          auto fuzz = random_double(0, 0.5);
          auto sphere_material = std::make_shared<Metal>(albedo, fuzz);
          world->add(std::make_shared<Sphere>(center, 0.2, sphere_material));
        } else {
          // Glass
          auto sphere_material = std::make_shared<Dielectric>(1.5);
          world->add(std::make_shared<Sphere>(center, 0.2, sphere_material));
        }
      }

    }
  }

  auto material1 = std::make_shared<Dielectric>(1.5);
  world->add(std::make_shared<Sphere>(Vector3d(0, 1, 0), 1.0, material1));

  auto material2 = std::make_shared<Lambertian>(Vector3d(0.4, 0.2, 0.1));
  world->add(std::make_shared<Sphere>(Vector3d(-4, 1, 0), 1.0, material2));

  auto material3 = std::make_shared<Metal>(Vector3d(0.7, 0.6, 0.5), 0.0);
  world->add(std::make_shared<Sphere>(Vector3d(4, 1, 0), 1.0, material3));

  return world;
}

std::shared_ptr<HittableList> two_spheres_scene() {
  auto objects = std::make_shared<HittableList>();
  auto checker = std::make_shared<CheckerTexture>(Vector3d(0.2, 0.3, 0.1), Vector3d(0.9, 0.9, 0.9));

  objects->add(std::make_shared<Sphere>(Vector3d(0, -10, 0), 10, std::make_shared<Lambertian>(checker)));
  objects->add(std::make_shared<Sphere>(Vector3d(0, 10, 0), 10, std::make_shared<Lambertian>(checker)));

  return objects;
}

std::shared_ptr<HittableList> two_perlin_spheres_scene() {
  auto objects = std::make_shared<HittableList>();

  auto pertext = std::make_shared<NoiseTexture>(4);

  objects->add(std::make_shared<Sphere>(Vector3d(0, -1000, 0), 1000, std::make_shared<Lambertian>(pertext)));
  objects->add(std::make_shared<Sphere>(Vector3d(0, 2, 0), 2, std::make_shared<Lambertian>(pertext)));

  return objects;
}

std::shared_ptr<HittableList> earth_scene() {
  auto world = std::make_shared<HittableList>();

  auto earth_texture = std::make_shared<ImageTexture>("assets/earthmap.jpg");
  auto earth_surface = std::make_shared<Lambertian>(earth_texture);
  auto globe = std::make_shared<Sphere>(Vector3d(0, 2, 0), 2, earth_surface);

  world->add(globe);

  auto pertext = std::make_shared<NoiseTexture>(4);
  world->add(std::make_shared<Sphere>(Vector3d(0, -1000, 0), 1000, std::make_shared<Lambertian>(pertext)));

  return world;
}

std::shared_ptr<HittableList> simple_light_scene() {
  auto world = std::make_shared<HittableList>();

  auto pertext = std::make_shared<NoiseTexture>(4);
  world->add(std::make_shared<Sphere>(Vector3d(0, -1000, 0), 1000, std::make_shared<Lambertian>(pertext)));
  world->add(std::make_shared<Sphere>(Vector3d(0, 2, 0), 2, std::make_shared<Lambertian>(pertext)));

  // Light brighter than (1, 1, 1) for lighting
  auto difflight = std::make_shared<DiffuseLight>(Vector3d(4, 4, 4));
  world->add(std::make_shared<XYRect>(3, 5, 1, 3, -2, difflight));
  world->add(std::make_shared<Sphere>(Vector3d(0, 5, 0), 1, difflight));

  return world;
}

std::shared_ptr<HittableList> cornell_box() {
  auto world = std::make_shared<HittableList>();

  auto red = std::make_shared<Lambertian>(Vector3d(0.65, 0.05, 0.05));
  auto white = std::make_shared<Lambertian>(Vector3d(0.73, 0.73, 0.73));
  auto green = std::make_shared<Lambertian>(Vector3d(0.12, 0.45, 0.15));
  auto light = std::make_shared<DiffuseLight>(Vector3d(15, 15, 15));

  world->add(std::make_shared<YZRect>(0, 555, 0, 555, 555, green));
  world->add(std::make_shared<YZRect>(0, 555, 0, 555, 0, red));
  world->add(std::make_shared<XZRect>(213, 343, 227, 332, 554, light));
  world->add(std::make_shared<XZRect>(0, 555, 0, 555, 0, white));
  world->add(std::make_shared<XZRect>(0, 555, 0, 555, 555, white));
  world->add(std::make_shared<XYRect>(0, 555, 0, 555, 555, white));

  auto t = std::make_shared<Affine3d>(Affine3d::Identity());
  t->translate(Vector3d(265, 0, 295));
  t->rotate(AngleAxisd(0.2618, Vector3d::UnitY()));
  std::shared_ptr<Hittable> box1 = std::make_shared<Box>(t, Vector3d(0, 0, 0),
      Vector3d(165, 330, 165), white);
  world->add(box1);

  auto t2 = std::make_shared<Affine3d>(Affine3d::Identity());
  t2->translate(Vector3d(130, 0, 65));
  t2->rotate(AngleAxisd(-0.31415, Vector3d::UnitY()));
  std::shared_ptr<Hittable> box2 = std::make_shared<Box>(t2, Vector3d(0, 0, 0),
      Vector3d(165, 165, 165), white);
  world->add(box2);

  return world;
}

std::shared_ptr<HittableList> final_scene() {
  auto world = std::make_shared<HittableList>();
  auto ground = std::make_shared<Lambertian>(Vector3d(0.48, 0.83, 0.53));

  auto trans = std::make_shared<Affine3d>(Affine3d::Identity());

  const int boxes_per_side = 20;
  for (int i = 0; i < boxes_per_side; i++) {
    for (int j = 0; j < boxes_per_side; j++) {
      auto w = 100.0;
      auto x0 = -1000.0 + i*w;
      auto z0 = -1000.0 + j*w;
      auto y0 = 0.0;
      auto x1 = x0 + w;
      auto y1 = random_double(1, 101);
      auto z1 = z0 + w;

      world->add(std::make_shared<Box>(trans, Vector3d(x0, y0, z0), Vector3d(x1, y1, z1), ground));
    }
  }

  auto light = std::make_shared<DiffuseLight>(Vector3d(7, 7, 7));
  world->add(std::make_shared<XZRect>(123, 423, 147, 412, 554, light));

  Vector3d center1(400, 400, 200);
  Vector3d center2 = center1 + Vector3d(30, 0, 0);
  auto moving_sphere_material = std::make_shared<Lambertian>(Vector3d(0.7, 0.3, 0.1));
  world->add(std::make_shared<MovingSphere>(center1, center2, 0, 1, 50, moving_sphere_material));

  world->add(std::make_shared<Sphere>(Vector3d(260, 150, 45), 50, std::make_shared<Dielectric>(1.5)));
  world->add(std::make_shared<Sphere>(Vector3d(0, 150, 145), 50,
        std::make_shared<Metal>(Vector3d(0.8, 0.8, 0.9), 1.0)));

  auto boundary = std::make_shared<Sphere>(Vector3d(360, 150, 145), 70, std::make_shared<Dielectric>(1.5));
  world->add(boundary);
  world->add(std::make_shared<ConstantMedium>(boundary, 0.2, Vector3d(0.2, 0.4, 0.9)));
  boundary = std::make_shared<Sphere>(Vector3d(0, 0, 0), 5000, std::make_shared<Dielectric>(1.5));
  world->add(std::make_shared<ConstantMedium>(boundary, .0001, Vector3d(1, 1, 1)));

  auto emat = std::make_shared<Lambertian>(std::make_shared<ImageTexture>("assets/earthmap.jpg"));
  world->add(std::make_shared<Sphere>(Vector3d(400, 200, 400), 100, emat));
  auto pertext = std::make_shared<NoiseTexture>(0.1);
  world->add(std::make_shared<Sphere>(Vector3d(220, 280, 300), 80, std::make_shared<Lambertian>(pertext)));

  auto boxes = std::make_shared<HittableList>();
  auto white = std::make_shared<Lambertian>(Vector3d(0.73, 0.73, 0.73));
  int ns = 1000;
  for (int j = 0; j < ns; j++) {
    boxes->add(std::make_shared<Sphere>(random_vec(0, 165), 10, white));
  }

  auto t = std::make_shared<Affine3d>(Affine3d::Identity());
  t->translate(Vector3d(-100, 270, 395));
  t->rotate(AngleAxisd(0.2618, Vector3d::UnitY()));

  world->add(std::make_shared<BvhNode>(t, boxes, 0.0, 1.0));

  return world;
}

std::shared_ptr<HittableList> model_test() {
  auto world = std::make_shared<HittableList>();

  //auto c = std::make_shared<NormalDebug>();
  auto c = std::make_shared<Lambertian>(Vector3d(0.48, 0.83, 0.53));

  auto t = std::make_shared<Affine3d>(Affine3d::Identity());
  std::vector<std::shared_ptr<TriangleMesh>> model = load_model(t, c, "assets/backpack/backpack.obj");
  //std::vector<std::shared_ptr<TriangleMesh>> model = load_model(t, c, "assets/sphere/sphere.obj");
  for (auto& mesh : model) {
    world->add(std::make_shared<BvhNode>(std::make_shared<Affine3d>(Affine3d::Identity()), make_mesh_triangle_list(mesh), 0.0, 1.0));
  }

  auto pertext = std::make_shared<NoiseTexture>(4);
  world->add(std::make_shared<Sphere>(Vector3d(0, -1002, 0), 1000, std::make_shared<Lambertian>(pertext)));

  return world;
}

int main(int argc, char** argv) {
  if (argc <= 1) {
    log_error("Provide raytracer config file as argument");
    return 1;
  }

  // World
  //auto world = random_scene();
  //auto world = two_spheres_scene();
  //auto world = two_perlin_spheres_scene();
  //auto world = earth_scene();
  //auto world = simple_light_scene();
  auto world = cornell_box();
  //auto world = final_scene();
  //auto world = model_test();

  auto t = std::make_shared<Affine3d>(Affine3d::Identity());
  //log_info("Building bounding volume hierarchy");
  auto bvh = std::make_shared<BvhNode>(t, world, 0.0, 1.0);

  // Raytracer
  //log_info("Rendering");
  Raytracer raytracer(argv[1], bvh);
  raytracer.render(std::cout);
  //raytracer.render("test.ppm", std::make_unique<BoxFilter>(Vector2d::Ones() * 2.0));
  //raytracer.render("test.ppm", std::make_unique<GaussianFilter>(Vector2d::Ones() * 2.0, 1.0));
  //raytracer.render("test.ppm", std::make_unique<MitchellFilter>(Vector2d::Ones() * 2.0, 1.0/3.0, 1.0/3.0));
  //raytracer.render_interactive(std::make_unique<MitchellFilter>(Vector2d::Ones() * 2.0, 1.0/3.0, 1.0/3.0));

  // Report and write stats out
  report_thread_stats();
  print_stats("raytracer_stats.txt");
}


// Spear: Statistical Platform for Elucidating moleculAr Reactivity
// Copyright (C) Purdue University -- BSD license

#ifndef SPEAR_GRID_HPP
#define SPEAR_GRID_HPP

#include "spear/exports.hpp"

#include <unordered_map>
#include <unordered_set>

#include "spear/Molecule.hpp"

#include <boost/functional/hash.hpp>

namespace Spear {

using Eigen::Vector3d;

struct SPEAR_EXPORT GridPoint : public std::array<uint64_t, 3> {

    GridPoint(const Vector3d& point,
              const Vector3d& min,
              const Vector3d& max,
              const Vector3d& step,
              double dist = 0.0) {

        // Do not use auto, it causes a strange bug here as it attempts to
        // modify const memory?
        Vector3d shifted = point - min;
        shifted += Vector3d(dist, dist, dist);

        auto xcrd = shifted[0] < 0.0    ? 0.0 :
                    shifted[0] > max[0] ? max[0] :
                    std::floor(shifted[0] / step[0]);

        auto ycrd = shifted[1] < 0.0    ? 0.0 :
                    shifted[1] > max[1] ? max[1] :
                    std::floor(shifted[1] / step[1]);

        auto zcrd = shifted[2] < 0.0    ? 0.0 :
                    shifted[2] > max[2] ? max[2] :
                    std::floor(shifted[2] / step[2]);

        (*this)[0] = static_cast<uint64_t>(xcrd);
        (*this)[1] = static_cast<uint64_t>(ycrd);
        (*this)[2] = static_cast<uint64_t>(zcrd);
    }

	friend size_t hash_value(const GridPoint& p) {
		size_t seed = 0;
		boost::hash_combine(seed, p[0]);
		boost::hash_combine(seed, p[1]);
		boost::hash_combine(seed, p[2]);

		return seed;
	}

    GridPoint(uint64_t i, uint64_t j, uint64_t k)
        : std::array<uint64_t, 3>({i, j, k}) {}
};

inline bool operator==(const GridPoint& a, const GridPoint& b) {
	return a[0] == b[0] && a[1] == b[1] && a[2] == b[2];
}

inline bool operator<(const GridPoint& a, const GridPoint& b) {
    if (a[0] < b[0]) {
        return true;
    }

    if (a[0] > b[0]) {
        return false;
    }

    if (a[1] < b[1]) {
        return true;
    }

    if (a[1] > b[1]) {
        return false;
    }

    if (a[2] < b[2]) {
        return true;
    }

    if (a[2] > b[2]) {
        return false;
    }

    return false;
}

class SPEAR_EXPORT Grid {
    constexpr static auto eps = std::numeric_limits<double>::epsilon();
    constexpr static auto max = std::numeric_limits<double>::max();
    constexpr static auto min = std::numeric_limits<double>::min();
public:
    Grid(const std::vector<Vector3d>& points, const Vector3d& step = {1.0, 1.0, 1.0})
        : min_(max,max,max), max_(min,min,min), step_(step) {

        if (step_[0] <= eps || step_[1] <= eps || step_[2] <= eps) {
            throw std::invalid_argument("Step size too small");
        }

        for (auto point : points) {
            min_[0] = std::min(point[0], min_[0]);
            min_[1] = std::min(point[1], min_[1]);
            min_[2] = std::min(point[2], min_[2]);

            max_[0] = std::max(point[0], max_[0]);
            max_[1] = std::max(point[1], max_[1]);
            max_[2] = std::max(point[2], max_[2]);
        }

        // Compute the maximum range
        max_ -= min_;

        for (size_t i = 0; i < points.size(); ++i) {
            grid_.emplace(GridPoint(points[i], min_, max_, step_), i);
        }
    }

    std::unordered_set<size_t> neighbors(const Vector3d& point) const {
        auto gridpoint = GridPoint(point, min_, max_, step_);
        std::unordered_set<size_t> ret;

        auto range = grid_.equal_range(gridpoint);
        for (auto gp = range.first; gp != range.second; ++gp) {
            ret.insert(gp->second);
        }

        return ret;
    }

    std::vector<size_t> neighbors(const Vector3d& point,
                                         double dist,
                                         double lower_tol = 0.0,
                                         double upper_tol = 0.0) const {
        auto cmin = GridPoint(point, min_, max_, step_, -(dist + lower_tol));
        auto cmax = GridPoint(point, min_, max_, step_,   dist + upper_tol);
        std::vector<size_t> ret;

        auto lb = grid_.lower_bound(cmin);
        auto ub = grid_.upper_bound(cmax);

        std::for_each(lb, ub, [&](const std::pair<GridPoint, size_t>& i){
            ret.push_back(i.second);
        });

        return ret;
    }

    size_t size() const {
        return grid_.size();
    }

    std::unordered_set<GridPoint, boost::hash<GridPoint>> occupied() const {
        std::unordered_set<GridPoint, boost::hash<GridPoint>> ret;

        for (auto gp : grid_) {
            ret.insert(gp.first);
        }

        return ret;
    }

private:
    Vector3d min_;
    Vector3d max_;
    Vector3d step_;

    std::multimap<GridPoint, size_t> grid_;
};

}

#endif


// Copyright (C) 2014 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/filter_view_graph_cycles_by_rotation.h"

#include <ceres/rotation.h>
#include <Eigen/Core>
#include <glog/logging.h>
#include <unordered_map>
#include <vector>

#include "theia/math/util.h"
#include "theia/util/hash.h"
#include "theia/sfm/twoview_info.h"
#include "theia/sfm/types.h"
#include "theia/sfm/view_graph/triplet_extractor.h"
#include "theia/sfm/view_graph/view_graph.h"
#include "theia/sfm/view_triplet.h"

namespace theia {

namespace {

double ComputeLoopRotationError(const ViewTriplet& triplet) {
  // Get relative rotation matrices.
  Eigen::Matrix3d rotation1_2, rotation1_3, rotation2_3;
  ceres::AngleAxisToRotationMatrix(
      triplet.info_one_two.rotation_2.data(),
      ceres::ColumnMajorAdapter3x3(rotation1_2.data()));
  ceres::AngleAxisToRotationMatrix(
      triplet.info_one_three.rotation_2.data(),
      ceres::ColumnMajorAdapter3x3(rotation1_3.data()));
  ceres::AngleAxisToRotationMatrix(
      triplet.info_two_three.rotation_2.data(),
      ceres::ColumnMajorAdapter3x3(rotation2_3.data()));

  // Compute loop rotation.
  const Eigen::Matrix3d loop_rotation =
      rotation2_3 * rotation1_2 * rotation1_3.transpose();
  Eigen::Vector3d loop_rotation_angle_axis;
  ceres::RotationMatrixToAngleAxis(
      ceres::ColumnMajorAdapter3x3(loop_rotation.data()),
      loop_rotation_angle_axis.data());

  // Return the angle of the loop rotation which is the error of the
  // concatenated triplet rotation.
  return RadToDeg(loop_rotation_angle_axis.norm());
}

void RemoveTripletEdgesFromInvalidViewPairs(
    const ViewTriplet& triplet,
    std::unordered_set<ViewIdPair>* invalid_view_pairs) {
  invalid_view_pairs->erase(
      ViewIdPair(triplet.view_ids[0], triplet.view_ids[1]));
  invalid_view_pairs->erase(
      ViewIdPair(triplet.view_ids[0], triplet.view_ids[2]));
  invalid_view_pairs->erase(
      ViewIdPair(triplet.view_ids[1], triplet.view_ids[2]));
}

}  // namespace

void FilterViewGraphCyclesByRotation(const double max_loop_error_degrees,
                                     ViewGraph* view_graph) {
  const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs =
      view_graph->GetAllEdges();

  // Find all triplets.
  TripletExtractor triplet_extractor;
  std::vector<std::vector<ViewTriplet> > connected_triplets;
  CHECK(triplet_extractor.ExtractTripletsFromViewPairs(view_pairs,
                                                       &connected_triplets))
      << "Could not extract triplets from view pairs.";

  // Create a list of invalid view pairs. View pairs deemed valid will be
  // removed from this list.
  std::unordered_set<ViewIdPair> invalid_view_pairs;
  invalid_view_pairs.reserve(view_pairs.size());
  for (const auto& view_pair : view_pairs) {
    invalid_view_pairs.insert(view_pair.first);
  }

  // Examine the cycles of size 3 to determine invalid view pairs from the
  // rotations.
  for (const auto& triplets : connected_triplets) {
    for (const ViewTriplet& triplet : triplets) {
      // Compute loop rotation error.
      const double loop_rotation_error_degrees =
          ComputeLoopRotationError(triplet);
      LOG(INFO) << "Loop rotation error = " << loop_rotation_error_degrees;
      // Add the view pairs to the list of valid view pairs if the loop error is
      // within the designated tolerance.
      if (loop_rotation_error_degrees < max_loop_error_degrees) {
        RemoveTripletEdgesFromInvalidViewPairs(triplet, &invalid_view_pairs);
      }
    }
  }

  VLOG(1) << "Removing " << invalid_view_pairs.size() << " of "
          << view_pairs.size() << " view pairs from loop rotation filtering.";

  // Remove any view pairs not in the list of valid edges.
  for (const ViewIdPair& view_pair_id : invalid_view_pairs) {
    view_graph->RemoveEdge(view_pair_id.first, view_pair_id.second);
  }
}

}  // namespace theia


/*
 * patAccelMeasurementModel.cc
 *
 *  Created on: Nov 26, 2011
 *      Author: jchen
 */

#include "patAccelMeasurementModel.h"
#include "patNBParameters.h"
#include "patDisplay.h"
#include "patErrMiscError.h"
#include "patMixtureNormal.h"
#include<iostream>
#include<sstream>
#include<fstream>
#include "patError.h"
#include <boost/math/distributions/normal.hpp> // for normal_distribution
using boost::math::normal;
// typedef provides default type is double.
map<TransportMode, AccelModelParam> patAccelMeasurementModel::m_params;

void patAccelMeasurementModel::readParams(patError*& err) {

	vector<TransportMode> modes;
	modes.push_back(TransportMode(CAR));
	modes.push_back(TransportMode(BUS));
	modes.push_back(TransportMode(METRO));
	modes.push_back(TransportMode(TRAIN));
	modes.push_back(TransportMode(WALK));
	modes.push_back(TransportMode(BIKE));
	for (int i = 0; i < modes.size(); ++i) {
		string file_name = patNBParameters::the()->paramFolder + "accel/"
				+ getTransportModeString(modes[i]);
		//patAccelMeasurementModel::m_params[modes[i]];
		ifstream file_stream_handler;
		file_stream_handler.open(file_name.c_str(), ios::in);
		if (!file_stream_handler) {
			stringstream str;
			str << "Error while parsing " << file_name;
			err = new patErrMiscError(str.str());
			WARNING(err->describe());
			return;
		}
//		DEBUG_MESSAGE("Read file:" << file_name);
		int components = 0;
		vector<double> w, mu, sigma;
		double w1, mu1, sigma1;
		string line;
		while (getline(file_stream_handler, line)) {

			components++;
			istringstream linestream(line);

			string item;

			getline(linestream, item, ',');
			w1 = atof(item.c_str());

			getline(linestream, item, ',');
			mu1 = atof(item.c_str());

			getline(linestream, item, '\n');
			sigma1 = atof(item.c_str());

//			DEBUG_MESSAGE(w1<<","<<mu1<<","<<sigma1)
			w.push_back(w1);
			mu.push_back(mu1);
			sigma.push_back(sigma1);
		}
		file_stream_handler.close();
		patAccelMeasurementModel::m_params[modes[i]].components = components;
		patAccelMeasurementModel::m_params[modes[i]].w = w;
		patAccelMeasurementModel::m_params[modes[i]].mu = mu;
		patAccelMeasurementModel::m_params[modes[i]].sigma = sigma;

	}
}

patAccelMeasurementModel::patAccelMeasurementModel(patMeasurement* measurement,
		const patArc* arc, TransportMode mode, double d) :
		patMeasurementModel::patMeasurementModel(measurement, arc, mode) {
	initiate();
	m_distance_to_stop = d;

	m_mode = mode;
	m_const = arc->getLength();
	m_arc_length = arc->getLength();

	patError* err;
	patMixtureNormal mn(patAccelMeasurementModel::m_params[m_mode].components,
		patAccelMeasurementModel::m_params[m_mode].w,
		patAccelMeasurementModel::m_params[m_mode].mu,
		patAccelMeasurementModel::m_params[m_mode].sigma,
		err);

	m_proba = mn.pdf(measurement->getSimpleValue());
	// m_param = patAccelMeasurementModel::m_params[m_mode];
	// normal s;
	// m_proba = 0.0;
	// for (int i = 0; i < patAccelMeasurementModel::m_params[m_mode].components;
	// 		++i) {

	// 	m_proba += m_param.w[i]
	// 			* pdf(
	// 					s,
	// 					(measurement->getSimpleValue() - m_param.mu[i])
	// 							/ m_param.sigma[i]) ;
	// }

	m_stationary_proba = -1.0;
	/*
	if (patAccelMeasurementModel::m_params.find(TransportMode(STATIONARY))
			!= patAccelMeasurementModel::m_params.end())

			{
		m_stationary_proba = 0.0;
		for (int i = 0;
				i
						< patAccelMeasurementModel::m_params[TransportMode(
								STATIONARY)].components; ++i) {

			m_stationary_proba += m_param.w[i]
					* pdf(
							s,
							(measurement->getSimpleValue() - m_param.mu[i])
									/ m_param.sigma[i]) / m_param.sigma[i];
		}
	}
	*/
	//DEBUG_MESSAGE(getTransportMode(m_mode)<<","<<m_proba);
	//DEBUG_MESSAGE(m_proba);
}
/*
 double patAccelMeasurementModel::integral(double ell) {
 //DEBUG_MESSAGE(ell << ", "<< std_normal << ", " << rtn);
 return m_proba;

 //return m_proba;
 }
 */
double patAccelMeasurementModel::integral(double ell) {
	if (m_stationary_proba < 0.0) {
		return m_proba;
	} else {
		double distance = ell * (m_arc_length) + m_distance_to_stop;
		double stationary_prior_proba = patNBParameters::the()->stationaryPriorMu
				* exp(
						-patNBParameters::the()->stationaryPriorMu * distance
								/ 100.0);

		return stationary_prior_proba * m_stationary_proba
				+ (1.0 - m_stationary_proba) * m_proba;
	}
}
void patAccelMeasurementModel::initiate() {
}


#include <NTL/ZZ_p.h>
#include <NTL/xdouble.h>
#include <sstream>
#include <vector>
#include <tuple>
#include <complex>
#include <parallel/algorithm>
#include <fftw3.h>
#include "bleichenbacher.h"

using namespace std;
using namespace NTL;

xdouble average, stdDev_accum;

struct {

	bool operator()(tuple<ZZ_p, ZZ_p> a,
		tuple<ZZ_p, ZZ_p> b) const
	{
		ZZ a_ZZ, b_ZZ;

		a_ZZ = rep(get<1>(a));
		b_ZZ = rep(get<1>(b));

		return a_ZZ < b_ZZ;
	}

} compareHCtuple;

template<class To, class From>
To NTLtoOther(From *x)
{
	stringstream ss;
	ss << *x;
	return atof(ss.str().c_str());
}

double Z_exp(ZZ_p h);
vector<tuple<int, double>> internal_maxM(vector<tuple<ZZ_p, ZZ_p>> *hcPairs,
	int Z_index_divisor,
	int l);
void internal_hcFromRs(vector<tuple<ZZ_p, ZZ_p, ZZ_p>> *rsmTuples,
	vector<tuple<ZZ_p, ZZ_p>> *hcPairs,
	int numBits,
	ZZ knownBits,
	bool specialH);

void hcFromRs(vector<tuple<ZZ_p, ZZ_p, ZZ_p>> *rsmTuples,
	vector<tuple<ZZ_p, ZZ_p>> *hcPairs,
	int numBits,
	ZZ knownBits)
{
	internal_hcFromRs(rsmTuples,
		hcPairs,
		numBits,
		knownBits,
		true);
}

void hcFromRs(vector<tuple<ZZ_p, ZZ_p, ZZ_p>> *rsmTuples,
	vector<tuple<ZZ_p, ZZ_p>> *hcPairs)
{
	ZZ zero;
	zero = 0;
	internal_hcFromRs(rsmTuples,
		hcPairs,
		0,
		zero,
		false);
}

void internal_hcFromRs(vector<tuple<ZZ_p, ZZ_p, ZZ_p>> *rsmTuples,
	vector<tuple<ZZ_p, ZZ_p>> *hcPairs,
	int numBits,
	ZZ knownBits,
	bool specialH)
{
	for(vector<tuple<ZZ_p, ZZ_p, ZZ_p>>::iterator it = rsmTuples->begin();
		it != rsmTuples->end(); ++it)
	{
		ZZ_p h, c, r, s, m, sInverse;

		r = get<0>(*it);
		s = get<1>(*it);
		m = get<2>(*it);

		if(s == 0) continue;

		power(sInverse, s, -1);

		c = r * sInverse;

		if(specialH)
		{
			ZZ modulus, keyBits;

			/* Place known key bits in MSBs of modulus */
			keyBits = knownBits;
			keyBits >>= (NumBits(keyBits) - numBits);
			keyBits <<= (NumBits(r.modulus()) - numBits);

			h = m * sInverse + c * to_ZZ_p(keyBits);

		} else {
			h = m * sInverse;
		}

		hcPairs->push_back(make_tuple(h, c));
	}
}

void sortAndDiff(vector<tuple<ZZ_p, ZZ_p>> *hcPairs,
	int l,
	int t)
{
	ZZ comp;
	ZZ_p zero;
	int S;

	S = hcPairs->size();
	omp_set_nested(1);
	omp_set_num_threads(NUM_CPUs * THREADS_PER_CPU);

	for(int i = 0; i < t; i++)
	{
		ZZ_p hFirst, cFirst;

		__gnu_parallel::sort(hcPairs->begin(), hcPairs->end(),
			compareHCtuple);

		for(int j = 0; j <= S-t; j++)
		{
			ZZ_p new_h, new_c;

			sub(new_h, get<0>((*hcPairs)[j+1]),
				get<0>((*hcPairs)[j]));
			sub(new_c, get<1>((*hcPairs)[j+1]),
				get<1>((*hcPairs)[j]));

			hcPairs->at(j) = make_tuple(new_h, new_c);
		}
	}

	/* Remove (h,c) pairs with c < 2^l */

	comp = 2;
	zero = 0;
	power(comp, comp, l);

	for(int i = 0; i < S; i++)
	{
		if(rep(get<1>((*hcPairs)[i])) >= comp)
			hcPairs->at(i) = make_tuple(zero, zero);
	}

	__gnu_parallel::sort(hcPairs->begin(), hcPairs->end(),
			compareHCtuple);

	if(rep(get<1>((*hcPairs)[S-1])) == rep(zero))
	{
		hcPairs->clear();
		return;
	}

	for(int i = 0; i < S; i++)
	{
		if(rep(get<1>((*hcPairs)[i])) != rep(zero))
		{
			hcPairs->erase(hcPairs->begin(),
				(hcPairs->begin()) + i);
			break;
		}
	}
}

vector<tuple<int, double>> maxM(vector<tuple<ZZ_p, ZZ_p>> *hcPairs,
	int l)
{
	return internal_maxM(hcPairs, 1, l);
}

vector<tuple<int, double>> maxM(vector<tuple<ZZ_p, ZZ_p>> *hcPairs,
	int bits,
	int l)
{
	return internal_maxM(hcPairs, bits, l);
}

vector<tuple<int, double>> internal_maxM(vector<tuple<ZZ_p, ZZ_p>> *hcPairs,
	int Z_index_divisor,
	int l)
{
	int size;
	fftw_complex *in, *out;
    	fftw_plan p;
	vector<tuple<int, double>> indexValuePairs;

	size = pow(2,l)/Z_index_divisor;

	in = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * size);
    	out = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * size);
	p = fftw_plan_dft_1d(size, in, out, FFTW_BACKWARD, FFTW_ESTIMATE);

	/* Compute and store Z values using Euler's Formula */
	for(vector<tuple<ZZ_p, ZZ_p>>::iterator it = hcPairs->begin();
		it != hcPairs->end(); ++it)
	{
		int c;
		double z_exp;
		ZZ zz_c;

		zz_c = rep(get<1>(*it));
		c = NTLtoOther<int, ZZ>(&zz_c);
		z_exp = Z_exp(get<0>(*it));

		in[c/Z_index_divisor][0] += cos(z_exp); // Re
		in[c/Z_index_divisor][1] += sin(z_exp); // Im
	}

	/* Do FFT */
	fftw_execute(p);

	/* Get max |Z_m| */
	average = 0;
	for(int i = 0; i < size; i++)
	{
		double magnitude;

		magnitude = abs(complex<double>((out[i])[0], (out[i])[1]));
		average += magnitude;

		if(indexValuePairs.size() < 10)
		{
			indexValuePairs.push_back(make_tuple(i, magnitude));
		} else {
			for(int j = 0; j < 10; j++)
			{
				if(magnitude > get<1>(indexValuePairs[j]))
				{
					indexValuePairs.at(j) =
						make_tuple(i, magnitude);
					break;
				}
			}
		}
	}
	average /= size;

	/* Multiply the 1/L back in */
	average /= hcPairs->size();
	for(int i = 0; i < 10; i++)
	{
		int index;
		double B;

		index = get<0>(indexValuePairs[i]);
		B = get<1>(indexValuePairs[i]) / hcPairs->size();

		indexValuePairs.at(i) = make_tuple(index, B);
	}

	/* Compute square of standard deviation */
	stdDev_accum = 0;
	for(int i = 0; i < size; i++)
	{
		xdouble term;
		term = abs(complex<double>((out[i])[0], (out[i])[1]))
			/ hcPairs->size();
		term -= average;
		power(term, term, 2);
		stdDev_accum += term;
	}
	stdDev_accum /= size;

	fftw_destroy_plan(p);
	fftw_free(in);
	fftw_free(out);

	return indexValuePairs;
}

double avgBias()
{
	double ret;
	conv(ret, average);
	return ret;
}

double stdDevBias()
{
	double ret;
	conv(ret, stdDev_accum);
	ret = sqrt(ret);
	return ret;
}

double Z_exp(ZZ_p h)
{
	ZZ n;
	xdouble h_doub, n_doub, x, pi2;

	pi2 = 2 * M_PI;

	n = h.modulus();
	n_doub = to_xdouble(n);
	h_doub = to_xdouble(rep(h));
	x = pi2 * h_doub / n_doub;

	return NTLtoOther<double, xdouble>(&x);
}


#ifndef STAN_MATH_PRIM_SCAL_FUN_FALLING_FACTORIAL_HPP
#define STAN_MATH_PRIM_SCAL_FUN_FALLING_FACTORIAL_HPP

#include <stan/math/prim/meta.hpp>
#include <boost/math/special_functions/factorials.hpp>
#include <stan/math/prim/scal/fun/boost_policy.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <limits>

namespace stan {
namespace math {

/**
 * Return the falling factorial function evaluated
 * at the inputs.
 * Will throw for NaN x and for negative n
 *
 * @tparam T Type of x argument.
 * @param x Argument.
 * @param n Argument
 * @return Result of falling factorial function.
 * @throw std::domain_error if x is NaN
 * @throw std::domain_error if n is negative
 *
   \f[
   \mbox{falling\_factorial}(x, n) =
   \begin{cases}
     \textrm{error} & \mbox{if } x \leq 0\\
     (x)_n & \mbox{if } x > 0 \textrm{ and } -\infty \leq n \leq \infty \\[6pt]
     \textrm{NaN} & \mbox{if } x = \textrm{NaN or } n = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{falling\_factorial}(x, n)}{\partial x} =
   \begin{cases}
     \textrm{error} & \mbox{if } x \leq 0\\
     \frac{\partial\, (x)_n}{\partial x} & \mbox{if } x > 0 \textrm{ and }
 -\infty \leq n \leq \infty \\[6pt] \textrm{NaN} & \mbox{if } x = \textrm{NaN or
 } n = \textrm{NaN} \end{cases} \f]

   \f[
   \frac{\partial\, \mbox{falling\_factorial}(x, n)}{\partial n} =
   \begin{cases}
     \textrm{error} & \mbox{if } x \leq 0\\
     \frac{\partial\, (x)_n}{\partial n} & \mbox{if } x > 0 \textrm{ and }
 -\infty \leq n \leq \infty \\[6pt] \textrm{NaN} & \mbox{if } x = \textrm{NaN or
 } n = \textrm{NaN} \end{cases} \f]

   \f[
   (x)_n=\frac{\Gamma(x+1)}{\Gamma(x-n+1)}
   \f]

   \f[
   \frac{\partial \, (x)_n}{\partial x} = (x)_n\Psi(x+1)
   \f]

   \f[
   \frac{\partial \, (x)_n}{\partial n} = -(x)_n\Psi(n+1)
   \f]
 *
 */
template <typename T>
inline return_type_t<T> falling_factorial(const T& x, int n) {
  static const char* function = "falling_factorial";
  check_not_nan(function, "first argument", x);
  check_nonnegative(function, "second argument", n);
  return boost::math::falling_factorial(x, n, boost_policy_t());
}

}  // namespace math
}  // namespace stan

#endif