#include <boost/test/unit_test.hpp>

#include "logs.h"
#include "route.h"
#include "track.h"

using namespace GPS;

BOOST_AUTO_TEST_SUITE(Route_Max_Latitude)

const bool isFileName = true;

// Testing whether the function works with the regular pre-made GPX log named NorthYorkMoors.gpx.
BOOST_AUTO_TEST_CASE( Route_NorthYorkMoors_Test )
{
   Route route = Route(LogFiles::GPXRoutesDir + "NorthYorkMoors.gpx", isFileName);
   BOOST_CHECK_EQUAL(route.maxLatitude(), 54.50918734073639);
}

//Testing to see how the function deals with a log file where all of the latitude positions are set to zero
BOOST_AUTO_TEST_CASE ( Route_Latitude_At_Zero_Test )
{
    Route route = Route(LogFiles::GPXRoutesDir + "Route_Latitude_At_Zero_Log.gpx", isFileName);
    BOOST_CHECK_EQUAL( route.maxLatitude(), 0 );
}

//Testing to see whether the function correctly gives a correct value when there is only one position within the log
BOOST_AUTO_TEST_CASE ( Route_One_Position_Test )
{
    Route route = Route(LogFiles::GPXRoutesDir + "Route_One_Position_Log.gpx", isFileName);
    BOOST_CHECK_EQUAL( route.maxLatitude(), 63.47589375938599 );
}

//Testing to see whether the function can tell the difference between positions with a difference of less than 1
BOOST_AUTO_TEST_CASE ( Route_Minute_Differences_Test )
{
    Route route = Route(LogFiles::GPXRoutesDir + "Route_Minute_Differences_Log.gpx", isFileName);
    BOOST_CHECK_EQUAL( route.maxLatitude(), 23.42204773426059);
}

// Testing whether the function can find the correct latitude within a log of unchanging points.
BOOST_AUTO_TEST_CASE( Route_Unchanging_Points_Test )
{
   Route route = Route(LogFiles::GPXRoutesDir + "Route_Unchanging_Points_Log.gpx", isFileName);
   BOOST_CHECK_EQUAL(route.maxLatitude(), 54.42204773426058);
}

//Testing whether the function can still produce the correct latitude when the only part that changes is longitude
BOOST_AUTO_TEST_CASE( Route_Different_Longitude_Test )
{
   Route route = Route(LogFiles::GPXRoutesDir + "Route_Different_Longitudes_Log.gpx", isFileName);
   BOOST_CHECK_EQUAL( route.maxLatitude(), 54.42204773426058);
}

//Testing to see if the function can find the correct value with a log of negative latitudes
BOOST_AUTO_TEST_CASE( Route_Negative_Latitude_Test )
{
   Route route = Route(LogFiles::GPXRoutesDir + "Route_Negative_Latitudes_Log.gpx", isFileName);
   BOOST_CHECK_EQUAL( route.maxLatitude(), -34.42204773426058);
}

//Testing whether the function can correctly give the correct value when the highest lat is at the edge of its positive boundary
BOOST_AUTO_TEST_CASE ( Route_Positive_Edge_Test)
{
    Route route = Route(LogFiles::GPXRoutesDir + "Route_Positive_Edge_Log.gpx", isFileName);
    BOOST_CHECK_EQUAL( route.maxLatitude(), 89.99999999999999);
}

//Testing whether the function can correctly give the correct value when the highest lat is at the edge of its negative boundary
BOOST_AUTO_TEST_CASE ( Route_Negative_Edge_Test )
{
    Route route = Route(LogFiles::GPXRoutesDir + "Route_Negative_Edge_Log.gpx", isFileName);
    BOOST_CHECK_EQUAL( route.maxLatitude(), -89.99999999999999 );
}

//Testing whether the function can return the correct value when the latitudes are in ascending order
BOOST_AUTO_TEST_CASE ( Route_Ascending_Order_Test )
{
    Route route = Route(LogFiles::GPXRoutesDir + "Route_Ascending_Order_Log.gpx", isFileName);
    BOOST_CHECK_EQUAL( route.maxLatitude(), 89.10856639294758);
}

//Testing whether the function can return the correct value when the latitudes are in descending order
BOOST_AUTO_TEST_CASE ( Route_Descending_Order_Test )
{
    Route route = Route(LogFiles::GPXRoutesDir + "Route_Descending_Order_Log.gpx", isFileName);
    BOOST_CHECK_EQUAL( route.maxLatitude(), 88.10856639294758);
}

//Testing to see how the function deals with a latitude position that is over the positive boundary of 90
BOOST_AUTO_TEST_CASE ( Route_Over_Positive_Boundary_Test )
{
    Route route = Route(LogFiles::GPXRoutesDir + "Route_Over_Positive_Boundary_Log.gpx", isFileName);
    BOOST_CHECK_THROW( route.maxLatitude(), std::out_of_range);
}

//Testing to see how the function deals with a latitude position that is over the negative boundary of -90
BOOST_AUTO_TEST_CASE ( Route_Over_Negative_Boundary_Test )
{
    Route route = Route(LogFiles::GPXRoutesDir + "Route_Over_Negative_Boundary_Log.gpx", isFileName);
    BOOST_CHECK_THROW( route.maxLatitude(), std::out_of_range);
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/comparing.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/group.hpp>
#include <boost/hana/length.hpp>
#include <boost/hana/range.hpp>
#include <boost/hana/tuple.hpp>
namespace hana = boost::hana;

int main() {
    constexpr auto sequences = hana::make_tuple(
        hana::make_tuple(1, 2, 3),
        hana::make_tuple('x', 'y', 'z'),
        hana::range_c<long, 0, 1>,
        hana::tuple_t<char, int>,
        hana::range_c<int, 0, 2>,
        hana::make_tuple(123.4, nullptr)
    );

    constexpr auto grouped = hana::group.by(hana::comparing(hana::length), sequences);

    static_assert(grouped == hana::make_tuple(
        hana::make_tuple(
            hana::make_tuple(1, 2, 3),
            hana::make_tuple('x', 'y', 'z')
        ),
        hana::make_tuple(
            hana::range_c<long, 0, 1>
        ),
        hana::make_tuple(
            hana::tuple_t<char, int>,
            hana::range_c<int, 0, 2>,
            hana::make_tuple(123.4, nullptr)
        )
    ), "");
}


#include <nobody/glut_app.hpp>

#include <GL/glut.h>
#include <Eigen/Dense>
#include <algorithm>
#include <chrono>
#include <cmath>
#include <fstream>
#include <functional>
#include <iostream>
#include <list>
#include <random>
#include <vector>

#include <nobody/camera.hpp>
#include <nobody/energy.hpp>
#include <nobody/euler_integrator.hpp>
#include <nobody/particle_loader.hpp>
#include <nobody/runge_kutta_integrator.hpp>

namespace {

// const data
constexpr float rad_to_degree_scale = 180.0f / M_PI;
constexpr unsigned char glut_key_esc = 27;
constexpr unsigned char glut_key_space = 32;
constexpr unsigned char glut_key_p = 112;
constexpr unsigned char glut_key_l = 108;
constexpr unsigned char glut_key_a = 97;
constexpr unsigned char glut_key_e = 101;
constexpr unsigned char glut_key_s = 115;
constexpr unsigned char glut_key_r = 114;

// static data
std::vector<nobody::particle> particle_vector;
std::vector<nobody::particle> particle_start_data;
float time_step = 0.0005f;
float camera_altitude = 0.0f;
float camera_azimuth = 0.0f;
float camera_distance = 10.0f;
float world_cube_size = 1.0f;
int key_modifiers;
int fps_frame_count = 0;
float fps_time_bound = 1.0f;
std::chrono::time_point<std::chrono::system_clock> fps_current_time =
    std::chrono::system_clock::now();
std::chrono::time_point<std::chrono::system_clock> fps_last_time =
    std::chrono::system_clock::now();
int old_mouse_x{};
int old_mouse_y{};
bool mouse_left_pressed = false;
bool mouse_right_pressed = false;
bool paused = true;
bool render_particles = true;
int render_particle_paths = 1;
nobody::Camera camera(400, 400, M_PI_2);
nobody::Orthonormal_frame world =
    nobody::blender_orthonormal_frame(Eigen::Vector3f(0, 0, 0));
float time = 0.0f;
float time_tmp = 0.0f;
std::vector<std::list<Eigen::Vector3f>> particle_path_data;

nobody::integrator_type integrator = nobody::leapfrog_integrator;
bool use_adaptive_integrator = false;
int selection = 0;
std::fstream file;
int iteration = 0;
bool track_results = false;

// helper-function declarations and definitions
void initialize(int argc, char** argv);
void execute();
void free();
void render();
void resize(int width, int height);
void idle();
void process_normal_keys(unsigned char key, int x, int y);
void process_special_keys(int key, int x, int y);
void process_mouse_buttons(int button, int button_state, int x, int y);
void process_mouse_move(int x, int y);
void process_passive_mouse_move(int x, int y);
void process_mouse_wheel(int wheel, int direction, int x, int y);
void compute_camera_frame();

void initialize(int argc, char** argv) {
  glutInit(&argc, argv);
  glutInitDisplayMode(GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGBA | GLUT_MULTISAMPLE);
  // glutInitWindowPosition(100, 100);
  glutInitWindowSize(400, 400);
  glutCreateWindow("nobody: n-body simulator");

  // glutCloseFunc(free);
  std::atexit(free);
  glutDisplayFunc(render);
  glutReshapeFunc(resize);
  glutIdleFunc(idle);
  glutKeyboardFunc(process_normal_keys);
  glutSpecialFunc(process_special_keys);
  glutMouseFunc(process_mouse_buttons);
  glutMotionFunc(process_mouse_move);
  glutPassiveMotionFunc(process_passive_mouse_move);
  // glutMouseWheelFunc(process_mouse_wheel);

  // glutSetOption(GLUT_ACTION_ON_WINDOW_CLOSE,
  // GLUT_ACTION_GLUTMAINLOOP_RETURNS);

  glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
  glEnable(GL_MULTISAMPLE);
  glEnable(GL_POINT_SMOOTH);
  // glEnable(GL_DEPTH_TEST);
  glPointSize(8.0f);
  // glEnable(GL_DEPTH_TEST);

  if (2 != argc) {
    // std::cout << "GEBE EINEN Dateipfad AN DU ARSCHLOCH!" << std::endl;
    // exit(-1);
    // initialize random particle data
    particle_vector.resize(2000);

    std::mt19937 rng(std::random_device{}());
    std::uniform_real_distribution<float> dist(-1.0f, 1.0f);

    for (int i = 0; i < static_cast<int>(particle_vector.size()); ++i) {
      particle_vector[i].position =
          1.5f * Eigen::Vector3f(dist(rng), dist(rng), dist(rng));
      particle_vector[i].velocity = Eigen::Vector3f(0, -5, 0);
      // particle_vector[i].velocity = Eigen::Vector3f(0, 0, 0);
      // particle_vector[i].velocity =
      //     particle_vector[i].position.cross(Eigen::Vector3f(0, 0, 1));
      // particle_vector[i].velocity.normalize();
      // particle_vector[i].velocity *= 0.5f;
      // 0.1f * Eigen::Vector3f(dist(rng), dist(rng), dist(rng));
      particle_vector[i].mass = 100.0f * (0.5 * dist(rng) + 1.1f);
    }

    particle_vector[0].mass = 1e6f;
    particle_vector[0].position = Eigen::Vector3f(-4, 0, 0);
    particle_vector[0].velocity = Eigen::Vector3f(0, 1.1, 0);

    // particle_vector[1].mass = 1e6f;
    // particle_vector[1].position = Eigen::Vector3f(10, 0, 0);
    // particle_vector[1].velocity = Eigen::Vector3f(0, -1, 0);
  } else {
    particle_vector = nobody::particle_system(std::string(argv[1]));
  }

  particle_start_data = particle_vector;

  particle_path_data.resize(particle_vector.size());
  for (std::size_t i = 0; i < particle_path_data.size(); ++i) {
    particle_path_data[i].clear();
    particle_path_data[i].push_back(particle_vector[i].position);
  }

  file.open("results.txt", std::ios::out | std::ios::trunc);
}

void execute() { glutMainLoop(); }

void free() { std::cout << "Glut_app cleaned up." << std::endl; }

void resize(int width, int height) {
  camera.set_screen_resolution(width, height);
  compute_camera_frame();

  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  glViewport(0, 0, camera.screen_width(), camera.screen_height());
  gluPerspective(camera.opengl_field_of_view(), camera.aspect_ratio(), 1e-5f,
                 1e5f);
  glMatrixMode(GL_MODELVIEW);
}

void render() {
  // compute camera position
  // camera_direction = Eigen::Vector3f(
  //     cosf(camera_azimuth) * cosf(camera_altitude), sinf(camera_altitude),
  //     sinf(camera_azimuth) * cosf(camera_altitude));
  // camera_position = world_center + camera_distance * camera_direction;

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

  // glLoadIdentity();
  // gluLookAt(camera_position(0), camera_position(1), camera_position(2),
  //           world_center(0), world_center(1), world_center(2), world_up(0),
  //           world_up(1), world_up(2));

  glLineWidth(1.0f);
  glColor3f(0, 0, 0);
  glutWireCube(world_cube_size);

  // we have to order the points

  if (1 == render_particle_paths) {
    glLineWidth(1.5f);
    for (std::size_t i = 0; i < particle_path_data.size(); ++i) {
      const auto& particle_path = particle_path_data[i];
      glBegin(GL_LINE_STRIP);
      for (const auto& point : particle_path) glVertex3fv(point.data());
      glVertex3fv(particle_vector[i].position.data());
      glEnd();
    }
  } else if (2 == render_particle_paths) {
    glLineWidth(1.5f);
    const auto& particle_path = particle_path_data[selection];
    glBegin(GL_LINE_STRIP);
    for (const auto& point : particle_path) glVertex3fv(point.data());
    glVertex3fv(particle_vector[selection].position.data());
    glEnd();
  }

  if (render_particles) {
    for (int i = 0; i < static_cast<int>(particle_vector.size()); ++i) {
      const float distance =
          (particle_vector[i].position - camera.position()).norm();

      const float brightness = std::min(0.7f, 1.0f / (distance * distance));

      glPointSize(std::max(0.007f / camera.pixel_size() *
                               (cbrtf(particle_vector[i].mass) / (distance)),
                           5.0f));

      glBegin(GL_POINTS);
      glColor3f(brightness, brightness, brightness);
      glVertex3f(particle_vector[i].position(0), particle_vector[i].position(1),
                 particle_vector[i].position(2));
      glEnd();
    }
  }

  glutSwapBuffers();
}

void idle() {
  if (paused) return;

  // compute fps
  fps_current_time = std::chrono::system_clock::now();
  fps_frame_count++;
  const float delta_time =
      std::chrono::duration<float>(fps_current_time - fps_last_time).count();

  const float kinetic = nobody::kinetic_energy(particle_vector);
  const float potential = nobody::potential_energy(particle_vector);
  const Eigen::Vector3f angular_momentum =
      nobody::angular_momentum(particle_vector);

  if (delta_time >= 1.0f) {
    std::cout << "frame time: "
              << delta_time / static_cast<float>(fps_frame_count)
              << "s\tfps: " << static_cast<float>(fps_frame_count) / delta_time
              << std::endl;
    std::cout << "simulation time = " << time << " years" << std::endl;
    std::cout << "time step = " << time_step << " years" << std::endl;
    std::cout << "kinetic energy = " << kinetic << std::endl
              << "potential energy = " << potential << std::endl
              << "total energy = " << kinetic + potential << std::endl;
    std::cout << "angular momentum = (" << angular_momentum.transpose() << ")"
              << std::endl
              << "angular momentum norm = " << angular_momentum.norm()
              << std::endl;
    std::cout << std::endl;
    fps_frame_count = 0;
    fps_last_time = fps_current_time;
  }

  if (use_adaptive_integrator)
    adaptive_integrator(&particle_vector, time_step, integrator);
  else {
    integrator(&particle_vector, time_step);

    // time_tmp += time_step;
    // if (time_tmp >= 5.0f) {
    //   time += time_tmp;
    //   time_tmp = 0.0f;
    //   // paused = true;
    //   // std::cout << "time = " << time << " years" << std::endl;
    // }
    iteration++;
    time += time_step;

    if (track_results) {
      file << iteration << "\t" << time << "\t" << kinetic << "\t" << potential
           << "\t" << kinetic + potential << "\t"
           << angular_momentum.transpose() << "\t" << angular_momentum.norm()
           << "\n";
    }
  }

  // euler_integrator(particle_vector.data(),
  //                  static_cast<int>(particle_vector.size()), time_step);
  // rk4_integrator(particle_vector.data(),
  // static_cast<int>(particle_vector.size()), time_step);
  // rk4_integrator(&particle_vector, time_step);
  // leapfrog_integrator(&particle_vector, time_step);
  // leapfrog_adaptive_integrator(&particle_vector, time_step);
  // euler_integrator(&particle_vector, time_step);

  // world.set_origin(particle_vector[0].position);

  for (std::size_t i = 0; i < particle_path_data.size(); ++i) {
    auto& particle_path = particle_path_data[i];

    if (particle_path.back() != particle_vector[i].position) {
      if (particle_path.size() < 2) {
        particle_path.push_back(particle_vector[i].position);
      } else {
        auto p = particle_path.end();
        --p;
        --p;
        const Eigen::Vector3f last = particle_path.back() - *p;
        const Eigen::Vector3f next =
            particle_vector[i].position - particle_path.back();
        const float distance = next.norm();
        const float cos_angle = next.dot(last) / (distance * last.norm());
        if (cos_angle < 0.9999f) {
          particle_path.push_back(particle_vector[i].position);
          // std::cout << "added position to path: " << particle_path.size()
          // << std::endl;
        }

        if (particle_path.size() > 512) particle_path.pop_front();
      }
    }
  }

  glutPostRedisplay();
}

void process_normal_keys(unsigned char key, int x, int y) {
  key_modifiers = glutGetModifiers();

  switch (key) {
    case '0':
      selection = 0;
      break;
    case '1':
      selection = 1;
      break;
    case '2':
      selection = 2;
      break;
    case '3':
      selection = 3;
      break;
    case '4':
      selection = 4;
      break;
    case '5':
      selection = 5;
      break;
    case '6':
      selection = 6;
      break;
    case '7':
      selection = 7;
      break;
    case '8':
      selection = 8;
      break;
    case '9':
      selection = 9;
      break;

    case 'x' - 0x60:
      particle_vector[selection].mass = 0.0f;
      std::cout << "Mass of particle " << selection << " set to 0 EM."
                << std::endl;
      break;

    case 'm' - 0x60:
      particle_vector[selection].mass *= 1.5f;
      std::cout << "Mass of particle " << selection << " set to "
                << particle_vector[selection].mass << " EM." << std::endl;
      break;

    case 'L':
      std::cout << "Integrator set to leapfrog_integrator." << std::endl;
      integrator = nobody::leapfrog_integrator;
      break;

    case 'A':
      use_adaptive_integrator = !use_adaptive_integrator;
      if (use_adaptive_integrator)
        std::cout << "Integration is set to adaptive time step mode."
                  << std::endl;
      else
        std::cout << "Integrator is set to non-adaptive time step mode."
                  << std::endl;
      break;

    case 'R':
      std::cout << "Integrator set to rk4_integrator." << std::endl;
      integrator = nobody::rk4_integrator;
      break;

    case 'E':
      integrator = nobody::euler_integrator;
      std::cout << "Integrator set to euler_integrator." << std::endl;
      break;

    case 'S':
      integrator = nobody::symplectic_euler_integrator;
      std::cout << "Integrator set to symplectic_euler_integrator."
                << std::endl;
      break;

    case glut_key_esc:
      exit(0);
      break;

    case glut_key_space:
      paused = !paused;
      break;

    case 'p':
      render_particles = !render_particles;
      break;

    case 'l':
      render_particle_paths = (render_particle_paths + 1) % 3;
      break;

    case 'r':
      particle_vector = particle_start_data;
      for (std::size_t i = 0; i < particle_path_data.size(); ++i) {
        particle_path_data[i].clear();
        particle_path_data[i].push_back(particle_vector[i].position);
      }
      time = 0.0f;
      file.close();
      file.open("results.txt", std::ios::out | std::ios::trunc);
      break;

    case 't':
      track_results = !track_results;
      std::cout << "results tracking set to: " << std::boolalpha
                << track_results << std::endl;
      break;

    case 'v':
      world = nobody::opengl_orthonormal_frame(Eigen::Vector3f::Zero());
      compute_camera_frame();
      break;

    case 'w':
      world = nobody::blender_orthonormal_frame(Eigen::Vector3f::Zero());
      compute_camera_frame();
      break;

    case 'e':
      const float kinetic = nobody::kinetic_energy(particle_vector);
      const float potential = nobody::potential_energy(particle_vector);
      std::cout << "kinetic energy = " << kinetic << std::endl
                << "potential energy = " << potential << std::endl
                << "total energy = " << kinetic + potential << std::endl
                << "time = " << time << std::endl;

      const Eigen::Vector3f relative_position =
          particle_vector[selection].position - particle_vector[0].position;
      const float distance = relative_position.norm();
      const float potential_selection =
          -nobody::gravitation_const * particle_vector[0].mass *
          particle_vector[selection].mass / distance;
      const float kinetic_selection =
          0.5f * particle_vector[selection].mass *
          particle_vector[selection].velocity.squaredNorm();
      const float energy_selection = kinetic_selection + potential_selection;
      const Eigen::Vector3f angular_momentum_selection =
          particle_vector[selection].mass *
          relative_position.cross(particle_vector[selection].velocity);
      const float angular_momentum_norm_selection =
          angular_momentum_selection.norm();
      const float eccentricity_selection = std::sqrt(
          1 + (2.0 * energy_selection * angular_momentum_norm_selection *
               angular_momentum_norm_selection) /
                  (nobody::gravitation_const * nobody::gravitation_const *
                   particle_vector[0].mass * particle_vector[0].mass *
                   particle_vector[selection].mass *
                   particle_vector[selection].mass *
                   particle_vector[selection].mass));
      const float semi_major_axis_selection =
          -0.5f * nobody::gravitation_const * particle_vector[0].mass *
          particle_vector[selection].mass / energy_selection;

      std::cout << "selection data: " << std::endl
                << "kinetic energy = " << kinetic_selection << std::endl
                << "potential energy = " << potential_selection << std::endl
                << "total energy = " << energy_selection << std::endl
                << "angular momentum = "
                << angular_momentum_selection.transpose() << std::endl
                << "angular_momentum_norm_selection = "
                << angular_momentum_norm_selection << std::endl
                << "semi-major axis = " << semi_major_axis_selection << " AE"
                << std::endl
                << "eccentricity = " << eccentricity_selection << std::endl
                << std::endl;

      break;
  }

  selection = std::min(static_cast<int>(particle_vector.size()) - 1, selection);
  std::cout << "selection = " << selection << std::endl;
  compute_camera_frame();
}

void process_special_keys(int key, int x, int y) {
  key_modifiers = glutGetModifiers();

  switch (key_modifiers) {
    case GLUT_ACTIVE_CTRL:
      switch (key) {
        case GLUT_KEY_UP:
          camera_distance -= 0.5f;
          break;
        case GLUT_KEY_DOWN:
          camera_distance += 0.5f;
          break;
      }
      break;

    default:
      switch (key) {
        case GLUT_KEY_UP:
          camera_altitude += 0.1f;
          break;
        case GLUT_KEY_DOWN:
          camera_altitude -= 0.1f;
          break;
        case GLUT_KEY_LEFT:
          camera_azimuth += 0.1f;
          break;
        case GLUT_KEY_RIGHT:
          camera_azimuth -= 0.1f;
      }
      break;
  }

  compute_camera_frame();
}

void process_mouse_buttons(int button, int button_state, int x, int y) {
  key_modifiers = glutGetModifiers();

  if ((button == GLUT_LEFT_BUTTON) && (button_state == GLUT_DOWN)) {
    mouse_left_pressed = true;
  }
  if ((button == GLUT_LEFT_BUTTON) && (button_state == GLUT_UP)) {
    mouse_left_pressed = false;
  }
  if ((button == GLUT_RIGHT_BUTTON) && (button_state == GLUT_DOWN)) {
    mouse_right_pressed = true;
  }
  if ((button == GLUT_RIGHT_BUTTON) && (button_state == GLUT_UP)) {
    mouse_right_pressed = false;
  }
}

void process_mouse_move(int x, int y) {
  const float x_difference = static_cast<float>(x - old_mouse_x);
  const float y_difference = static_cast<float>(y - old_mouse_y);

  switch (key_modifiers) {
    case GLUT_ACTIVE_CTRL:
      if (mouse_left_pressed) {
        camera_distance += 0.02f * y_difference * camera_distance;
        if (camera_distance < 1e-5f) camera_distance = 1e-5f;
      } else if (mouse_right_pressed) {
        time_step += 0.02f * y_difference * time_step;
        if (time_step < 1e-6f) time_step = 1e-6f;
        std::cout << "time step = " << time_step << " years" << std::endl;
      }
      break;

    // moving
    case GLUT_ACTIVE_SHIFT:
      world.set_origin(world.origin() -
                       camera.frame().right() * x_difference *
                           camera.pixel_size() * camera_distance +
                       camera.frame().up() * y_difference *
                           camera.pixel_size() * camera_distance);
      break;

    default:
      camera_azimuth -= static_cast<float>(x - old_mouse_x) * 0.01f;
      camera_altitude += static_cast<float>(y - old_mouse_y) * 0.01f;
      if (camera_altitude > M_PI_2 - 0.0001f)
        camera_altitude = M_PI_2 - 0.0001f;
      if (camera_altitude < -M_PI_2 + 0.0001f)
        camera_altitude = -M_PI_2 + 0.0001f;
  }

  compute_camera_frame();

  old_mouse_x = x;
  old_mouse_y = y;
}

void process_passive_mouse_move(int x, int y) {
  old_mouse_x = x;
  old_mouse_y = y;
}

void process_mouse_wheel(int wheel, int direction, int x, int y) {}

void compute_camera_frame() {
  camera.look_at(horizontal_coordinates(world, camera_distance, camera_altitude,
                                        camera_azimuth),
                 world.origin(), world.up());
  camera.set_horizontal_field_of_view(M_PI_2);

  glLoadIdentity();
  gluLookAt(camera.position()(0), camera.position()(1), camera.position()(2),
            world.origin()(0), world.origin()(1), world.origin()(2),
            world.up()(0), world.up()(1), world.up()(2));

  glutPostRedisplay();
}

}  // namespace

Glut_app::Glut_app(int argc, char** argv) {
  static bool glut_initialized = false;
  if (glut_initialized) return;
  initialize(argc, argv);
  glut_initialized = true;
}

Glut_app::Glut_app() : Glut_app(0, nullptr) {}

void Glut_app::execute() { ::execute(); }


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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
//==============================================================================
#include <boost/simd/ieee/include/functions/ldexp.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/constant/constant.hpp>
#include <boost/simd/sdk/meta/cardinal_of.hpp>
#include <boost/simd/include/functions/simd/dec.hpp>
#include <boost/simd/include/functions/simd/divides.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/simd/sdk/simd/io.hpp>

#include <boost/simd/include/constants/valmax.hpp>
#include <boost/simd/include/constants/mindenormal.hpp>
#include <boost/simd/include/constants/minexponent.hpp>
#include <boost/simd/include/constants/halfeps.hpp>
#include <boost/simd/include/constants/smallestposval.hpp>
#include <boost/simd/include/constants/maxexponent.hpp>
#include <boost/simd/include/constants/limitexponent.hpp>

#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/two.hpp>
#include <boost/simd/include/constants/four.hpp>
#include <boost/simd/include/constants/inf.hpp>
#include <boost/simd/include/constants/minf.hpp>
#include <boost/simd/include/constants/nan.hpp>

NT2_TEST_CASE_TPL ( ldexp_real,  BOOST_SIMD_SIMD_REAL_TYPES)
{
  using boost::simd::ldexp;
  using boost::simd::tag::ldexp_;
  using boost::simd::native;
  using boost::simd::meta::cardinal_of;
  using boost::simd::Two;
  typedef typename boost::dispatch::meta::as_integer<T>::type iT;
  typedef BOOST_SIMD_DEFAULT_EXTENSION                     ext_t;
  typedef native<T,ext_t>                                     vT;
  typedef typename boost::dispatch::meta::as_integer<T>::type iT;
  typedef native<iT,ext_t>                                   ivT;
  typedef typename boost::dispatch::meta::call<ldexp_(vT,ivT)>::type r_t;

#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_EQUAL(ldexp(boost::simd::Inf<vT>(),  Two<ivT>()), boost::simd::Inf<r_t>());
  NT2_TEST_EQUAL(ldexp(boost::simd::Minf<vT>(), Two<ivT>()), boost::simd::Minf<r_t>());
  NT2_TEST_EQUAL(ldexp(boost::simd::Nan<vT>(),  Two<ivT>()), boost::simd::Nan<r_t>());
#endif
  NT2_TEST_EQUAL(ldexp(boost::simd::Mone<vT>(), Two<ivT>()), -boost::simd::Four<r_t>());
  NT2_TEST_EQUAL(ldexp(boost::simd::One<vT>(),  Two<ivT>()), boost::simd::Four<r_t>());
  NT2_TEST_EQUAL(ldexp(boost::simd::Zero<vT>(), Two<ivT>()), boost::simd::Zero<r_t>());
  NT2_TEST_EQUAL(ldexp(boost::simd::One <vT>(), boost::simd::Minexponent<vT>()), boost::simd::Smallestposval<r_t>());
  NT2_TEST_EQUAL(ldexp(boost::simd::One<vT>()-boost::simd::Halfeps<vT>(),  boost::simd::Maxexponent<vT>()), boost::simd::Valmax<vT>()/Two<vT>());
  NT2_TEST_EQUAL(ldexp(boost::simd::One<vT>()-boost::simd::Halfeps<vT>(),  boost::simd::Limitexponent<vT>()), boost::simd::Valmax<vT>());
#ifndef BOOST_SIMD_NO_DENORMALS
  NT2_TEST_EQUAL(ldexp(boost::simd::One <vT>(), dec(boost::simd::Minexponent<vT>())), boost::simd::Smallestposval<vT>()/Two<vT>());
  NT2_TEST_EQUAL(ldexp(boost::simd::Two <vT>(), dec(boost::simd::Minexponent<vT>())), boost::simd::Smallestposval<vT>());
  NT2_TEST_EQUAL(ldexp(boost::simd::Two <vT>(), dec(dec(boost::simd::Minexponent<vT>()))), boost::simd::Smallestposval<vT>()/Two<vT>());
#endif
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2011 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*
 * geometric_objects.h
 *
 *  Created on: 17.12.2011
 *      Author: Lars Lubkoll
 */

#ifndef GEOMETRIC_OBJECTS_HH_
#define GEOMETRIC_OBJECTS_HH_

#include <algorithm>
#include <cmath>
#include <iostream>
#include <utility>
#include <vector>
#include <boost/array.hpp>
#include <boost/utility/enable_if.hpp>
#include <dune/common/fvector.hh>
#include <dune/grid/common/genericreferenceelements.hh>
#include "utilities/geometry/geomtools.hh"
#include "tools/linalg/scalarproducts.hh"

namespace GeometricObject
{
  enum Direction{ X, Y, Z };

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

  template <class Scalar, int dim>
  class Point : public Dune::FieldVector<Scalar,dim>{
  public:
    typedef Dune::FieldVector<Scalar,dim> Base;
    Point() : Base(){}
    explicit Point(Dune::FieldVector<Scalar,dim> const& p) : Base(p) {}
  };

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

  template <class Scalar, int dim>
  struct Line{
    Line() : start(), end()
    {}

    Line(Point<Scalar,dim> const& s, Point<Scalar,dim> const& e) : start(s), end(e)
    {}

    Point<Scalar,dim> start, end;
  };

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

  // TODO implement 2D-Polygon
  template <class Scalar, int dim>
  struct Triangle{
    typedef Point<Scalar,dim> Vertex;
    Triangle()
    {
      init(Vertex(), Vertex(), Vertex());
    }

    Triangle(Vertex const& v1, Vertex const& v2, Vertex const& v3)
    {
      init(v1,v2,v3);
    }

    boost::array<Vertex,3> corners;

  private:
    void init(Vertex const& v1, Vertex const& v2, Vertex const& v3)
    {
      corners[0] = v1; corners[1] = v2; corners[2] = v3;
    }
  };

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

  template <class Scalar, int dim>
  struct Rectangle{
    Rectangle()
    {
      init(Point<Scalar,dim>(), Point<Scalar,dim>(), Point<Scalar,dim>(), Point<Scalar,dim>());
    }

    explicit Rectangle(boost::array<Point<Scalar,dim>, 4> const& e)
    {
      init(e[0], e[1], e[2], e[3]);
    }

    Rectangle(Point<Scalar,dim> const& p0, Point<Scalar,dim> const& p1, Point<Scalar,dim> const& p2, Point<Scalar,dim> const& p3)
    {
      init(p0, p1, p2, p3);
    }

    boost::array<Point<Scalar,dim>,4> corners;

  private:
    void init(Point<Scalar,dim> const& p0, Point<Scalar,dim> const& p1, Point<Scalar,dim> const& p2, Point<Scalar,dim> const& p3)
    {
      corners[0] = p0;  corners[1] = p1;  corners[2] = p2;  corners[3] = p3;
    }
  };

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

  namespace {
    template <int dim>
    struct BoundingBoxContainsImpl{
      template <class Array, class Coordinate>
      static bool apply(Array const& array, Coordinate const& x)
      {
        return (BoundingBoxContainsImpl<dim-1>::apply(array, x) && array[dim-1].first<=x[dim-1] && array[dim-1].second>=x[dim-1]);
      }
    };

    template <>
    struct BoundingBoxContainsImpl<1>{
      template <class Array, class Coordinate>
      static bool apply(Array const& array, Coordinate const& x)
      {
        return (array[0].first <= x[0] && array[0].second >= x[0]);
      }
    };

    template <class Scalar, int dim, int id>
    struct BoundingBoxGetCornersImpl
    {
      template <class Array>
      static void apply(std::vector<Point<Scalar,dim> > &corners, Array const& array)
      {
        size_t offset = (size_t)(corners.size()/pow(2,id+1));
        bool first = true;
        size_t changes = 0;
        for(size_t i=0; i<corners.size(); ++i)
        {
          if( (i-changes*offset)==offset )
          {
            ++changes;
            first = !first;
          }

          if(first) corners[i][id] = array[id].first;
          else corners[i][id] = array[id].second;
        }
        BoundingBoxGetCornersImpl<Scalar,dim,id+1>::apply(corners, array);
      }
    };

    template <class Scalar, int dim>
    struct BoundingBoxGetCornersImpl<Scalar,dim,dim>
    {
      template <class Array>
      static void apply(std::vector<Point<Scalar,dim> > &corners, Array const& array){}
    };

    template <class Scalar, int dim, int id>
    struct BoundingBoxGetEdgesImpl
    {
      template <class Array>
      static void apply(std::vector<Line<Scalar,dim> > &edges, Array const& array, int indexOffset)
      {
        int localNumberOfEdges = pow(2,dim-1);
        for(int edge=0; edge<localNumberOfEdges; ++edge)
        {
          Point<Scalar,dim> start, end;
          start[id] = array[id].first;
          end[id] = array[id].second;

          for(int i=0; i<dim; ++i)
          {
            if(i==id) continue;
            if(edge==0){
              insert(start, end, array[i].first, i);
              continue;
            }
            if(edge==1)
            {
              if(i==1) insert(start, end, array[i].second, i);
              if(i==2) insert(start, end, array[i].first, i);
              continue;
            }
            if(edge==2)
            {
              if(i==1) insert(start, end, array[i].first, i);
              if(i==2) insert(start, end, array[i].second, i);
              continue;
            }
            if(edge==3)
            {
              if(i==1) insert(start, end, array[i].second, i);
              if(i==2) insert(start, end, array[i].second, i);
              continue;
            }
          }
          edges[edge] = Line<Scalar,dim>(start,end);
        }
        BoundingBoxGetEdgesImpl<Scalar,dim,id+1>::apply(edges, array, localNumberOfEdges);
      } // end apply

    private:
      static void insert(Point<Scalar,dim>& start, Point<Scalar,dim>& end, Scalar value, int index)
      {
        start[index] = value, end[index] = value;
      }
    };

    template <class Scalar, int dim>
    struct BoundingBoxGetEdgesImpl<Scalar,dim,dim>
    {
      template <class Array>
      static void apply(std::vector<Line<Scalar,dim> >&, Array const&, int){};
    };

    template <int v1, int v2>
    struct MyLess{
      static bool const value=v1<v2;
    };

    template <int v1, int v2>
    struct Minus{
      static int const value = v1-v2;
    };

    template <int val>
    struct Decrement{
      static int const value = val-1;
    };

    template <class Scalar, int dim, int dimMinusCodim>
    struct GetCodimTypeImpl;

    template <class Scalar, int dim>
    struct GetCodimTypeImpl<Scalar,dim,0>{
      typedef typename boost::enable_if<MyLess<0,dim>, Point<Scalar,dim> >::type type;
    };

    template <class Scalar, int dim>
    struct GetCodimTypeImpl<Scalar,dim,1>{
      typedef typename boost::enable_if<MyLess<1,dim>, Line<Scalar,dim> >::type type;
    };

    template <class Scalar, int dim>
    struct GetCodimTypeImpl<Scalar,dim,2>{
      typedef typename boost::enable_if<MyLess<2,dim>, Rectangle<Scalar,dim> >::type type;
    };

    template <class Scalar, int dim, int codim>
    struct GetCodimType{
      typedef typename GetCodimTypeImpl<Scalar,dim,dim-codim>::type type;
    };

    template <class Scalar, int dim, int dimMinusCodim>
    struct CodimBaseImpl{};

    template <class Scalar, int dim>
    struct CodimBaseImpl<Scalar,dim,2>
    {
      template <class Array>
      std::vector<typename GetCodimType<Scalar,dim,dim-2>::type > getFaces(Array const& array) const
      {
        std::vector<Rectangle<Scalar,3> > faces(6);
        std::vector<Point<Scalar,3> > corners=CodimBaseImpl<Scalar,dim,0>::getCorners(array);
        faces[0] = Rectangle<Scalar,3>(corners[0], corners[2], corners[3], corners[1]);
        faces[1] = Rectangle<Scalar,3>(corners[4], corners[6], corners[7], corners[5]);
        faces[2] = Rectangle<Scalar,3>(corners[0], corners[4], corners[6], corners[2]);
        faces[3] = Rectangle<Scalar,3>(corners[1], corners[5], corners[7], corners[3]);
        faces[4] = Rectangle<Scalar,3>(corners[0], corners[4], corners[5], corners[1]);
        faces[5] = Rectangle<Scalar,3>(corners[2], corners[6], corners[7], corners[3]);
        return faces;
      }
    };

    template <class Scalar, int dim>
    struct CodimBaseImpl<Scalar,dim,1>
    {
      template <class Array>
      std::vector<Line<Scalar,dim> > getEdges(Array const& array) const
      {
        if(dim==0) return std::vector<Line<Scalar,dim> >();

        size_t numEdges = 1;
        for(int i=1; i<dim; ++i) numEdges = 2*numEdges + pow(2,i);

        std::vector<Line<Scalar,dim> > edges(numEdges);
        BoundingBoxGetEdgesImpl<Scalar,dim, 0>::apply(edges, array, 0);
        return edges;
      }
    };

    template <class Scalar, int dim>
    struct CodimBaseImpl<Scalar, dim, 0>
    {
      template <class Array>
      static std::vector<Point<Scalar,dim> > getCorners(Array const& array)
      {
        std::vector<Point<Scalar,dim> > corners(pow(2,dim));
        BoundingBoxGetCornersImpl<Scalar,dim,0>::apply(corners, array);
        return corners;
      }
    };

    template <class Scalar, int dim, int codim>
    struct CodimBase : public CodimBaseImpl<Scalar,dim,dim-codim>, public CodimBase<Scalar,dim,codim-1>{};

    template <class Scalar, int dim>
    struct CodimBase<Scalar,dim,0>{};
  }

  /// A bounding box
  template <class Scalar, int dimension>
  struct BoundingBox : public CodimBase<Scalar,dimension,dimension>{
    static int const dim=dimension;
    BoundingBox()
    {
      for(int i=0; i<dim; ++i) coord[i].first = coord[i].second = 0;
    }

    BoundingBox(BoundingBox const& boundingBox)
    {
      coord = boundingBox.coord;
    }

    explicit BoundingBox(typename Point<Scalar,dim>::Base const& x)
    {
      for(int i=0; i<dim; ++i) coord[i].first = coord[i].second = x[i];
    }

    template <class Coordinate>
    void update(Coordinate const& x)
    {
      for(int i=0; i<dim; ++i)
      {
        if(coord[i].first > x[i]) coord[i].first = x[i];
        if(coord[i].second < x[i]) coord[i].second = x[i];
      }
    }

    std::ostream& print(std::ostream &stream) const
    {
      stream << "BoundingBox: ";
      for(int i=0; i<dim-1; ++i)
        stream << coord[i].first << " - " << coord[i].second << " | ";
      stream << coord[dim-1].first << " - " << coord[dim-1].second << std::endl;
      return stream;
    }

    template <class Coordinate>
    bool contains(Coordinate const& x) const
    {
      return BoundingBoxContainsImpl<dim>::apply(coord, x);
    }

    friend std::ostream& operator<<(std::ostream& stream, BoundingBox<Scalar,dim> const& boundingBox)
    {
      return boundingBox.print(stream);
    }

    Dune::FieldVector<std::pair<Scalar,Scalar>,dim> coord;
  };

  template <class Scalar, int dim>
  struct FastBoundingBox : public BoundingBox<Scalar,dim>
  {
    typedef BoundingBox<Scalar,dim> Base;

    FastBoundingBox() : Base(){}

    FastBoundingBox(FastBoundingBox const& boundingBox) : Base(boundingBox),
        corners(boundingBox.corners), edges(boundingBox.edges), faces(boundingBox.faces)
    {}

    template <class Coordinate>
    explicit FastBoundingBox(Coordinate const& x) : Base(x){}

    void initCorners()
    {
      corners = getCorners(this->coord);
    }

    void initEdges()
    {
      edges = getEdges(this->coord);
    }

    void initFaces()
    {
      faces = getFaces(this->coord);
    }

    void initAll()
    {
      initCorners();    initEdges();    initFaces();
    }

    std::vector<Point<Scalar,dim> > const& getStoredCorners() const
                    {
      return corners;
                    }

    std::vector<Line<Scalar,dim> > const& getStoredEdges() const
                    {
      return edges;
                    }

    std::vector<Rectangle<Scalar,dim> > const& getStoredFaces() const
                    {
      return faces;
                    }

    std::vector<Point<Scalar,dim> > corners;
    std::vector<Line<Scalar,dim> > edges;
    std::vector<Rectangle<Scalar,dim> > faces;
  };

  template <class BB>
  struct BoundingBoxWrapper;

  template <class Scalar, int dim>
  struct BoundingBoxWrapper<BoundingBox<Scalar,dim> >
  {
    typedef BoundingBox<Scalar,dim> BB;

    static void init(BB& boundingBox){}
    static void initCorners(BB& boundingBox){}
    static void initEdges(BB& boundingBox){}
    static void initFaces(BB& boundingBox){}
  };

  template <class Scalar, int dim>
  struct BoundingBoxWrapper<FastBoundingBox<Scalar,dim> >
  {
    typedef FastBoundingBox<Scalar,dim> BB;

    static void init(BB& boundingBox){ boundingBox.initAll(); }
    static void initCorners(BB& boundingBox){ boundingBox.initCorners(); }
    static void initEdges(BB& boundingBox){ boundingBox.initEdges(); }
    static void initFaces(BB& boundingBox){ boundingBox.initFaces(); }
  };

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

  /// A ball
  template <class Scalar, int dim>
  struct Ball{
    Ball(Dune::FieldVector<Scalar,dim> &c, Scalar r) : center(c), radius(r)
    {}

    template <class Position>
    bool contains(Position const& x) const
    {
      return (x-center).two_norm() <= radius;
    }

    template <class Position>
    Scalar distanceFromCenter(Position const& x) const
    {
      return (x-center).two_norm();
    }

    Dune::FieldVector<Scalar,dim> center;
    Scalar radius;
  };
  /*******************************************************************************************************************************/
  // function declarations, definitions follow after namespace ImplementationDetail
  template <class Scalar, int dim, class Metric>
  Scalar distance(Point<Scalar,dim> const& first, Point<Scalar,dim> const& second);

  template <class Scalar, int dim, class Metric>
  Scalar distance(Point<Scalar,dim> const& point, Line<Scalar,dim> const& line);

  template <class Scalar, int dim, class Metric>
  Scalar distance(Line<Scalar,dim> const& line, Point<Scalar,dim> const& point);

  template <class Scalar, int dim, class Metric>
  Scalar distance(Ball<Scalar,dim> const& ball, Point<Scalar,dim> const& point);

  template <class Scalar, int dim, class Metric>
  Scalar distance(Point<Scalar,dim> const& point, Ball<Scalar,dim> const& ball);

  template <class Scalar, int dim, class Metric>
  Scalar distance(Ball<Scalar,dim> const& ball, Line<Scalar,dim> const& line);

  template <class Scalar, int dim, class Metric>
  Scalar distance(Line<Scalar,dim> const& line, Ball<Scalar,dim> const& ball);

  template <class Scalar, int dim, class Metric>
  Scalar distance(Ball<Scalar,dim> const& ball, Rectangle<Scalar,dim> const& rectangle);

  template <class Scalar, int dim, class Metric>
  Scalar distance(Rectangle<Scalar,dim> const& rectangle, Ball<Scalar,dim> const& ball);

  template <class Scalar, int dim, class Metric>
  Scalar distance(Ball<Scalar,dim> const& ball, BoundingBox<Scalar,dim> const& boundingBox);

  namespace ImplementationDetail{

    template <class FirstObject, class SecondObject, class Scalar, int dim, class ScalarProduct>
    struct ProjectionImpl;

    template <class Scalar, int dim, class ScalarProduct>
    struct ProjectionImpl<Point<Scalar,dim>, Line<Scalar,dim>, Scalar, dim, ScalarProduct>
    {
      static Point<Scalar,dim> apply(Point<Scalar,dim> const& point, Line<Scalar,dim> const& line, Scalar& a, ScalarProduct const scalarProduct)
      {
        Point<Scalar,dim> const direction = GeomTools::normalize(line.end-line.start);
        return line.start + scalarProduct(point-line.start, direction)*direction;
      }
    };

    template <class Scalar, int dim, class ScalarProduct>
    struct ProjectionImpl<Point<Scalar,dim>, Rectangle<Scalar,dim>, Scalar, dim, ScalarProduct>
    {
      static Point<Scalar,dim> apply(Point<Scalar,dim> const& point, Rectangle<Scalar,dim> const& rectangle, Scalar& a, Scalar& b, ScalarProduct const scalarProduct)
      {
        Point<Scalar,dim> const dir1 = GeomTools::normalize(rectangle.corners[1]-rectangle.corners[0]),
            dir2 = GeomTools::normalize(rectangle.corners[3]-rectangle.corners[0]);
        Point<Scalar,dim> const vec = point-rectangle.corners[0];
        return rectangle.corners[0] + scalarProduct(vec, dir1)*dir1 + scalarProduct(vec, dir2)*dir2;
      }
    };

    template <class Metric, class Scalar, int dim, class FirstObject, class SecondObject>
    struct DistanceImpl;

    template <class Metric, class Scalar, int dim>
    struct DistanceImpl<Metric, Scalar, dim, Point<Scalar,dim>, Point<Scalar,dim> >
    {
      static Scalar apply(Point<Scalar,dim> const& firstPoint, Point<Scalar,dim> const& secondPoint, Metric metric)
      {
        return metric(firstPoint-secondPoint);
      }
    };

    template <class Metric, class Scalar, int dim>
    struct DistanceImpl<Metric, Scalar, dim, Point<Scalar,dim>, Line<Scalar,dim> >
    {
      static Scalar apply(Point<Scalar,dim> const& point, Line<Scalar,dim> const& line, Metric metric)
      {
        Scalar a;
        Point<Scalar,dim> projectedPoint = ProjectionImpl<Point<Scalar,dim>, Line<Scalar,dim>, Scalar,dim,typename Metric::ScalarProduct>::apply(point,line,a);
        Scalar length = distance<Metric>(line.start, line.end);
        if(0 < a &&  a < length) return distance<Metric>(point,projectedPoint,metric);
        if(a <= 0) return distance<Metric>(point, line.start,metric);
        return distance<Metric>(point, line.end,metric);
      }
    };

    // template <class Metric

    template <class Metric, class Scalar, int dim>
    struct DistanceImpl<Metric, Scalar, dim, Point<Scalar,dim>, Rectangle<Scalar,dim> >
    {
      typedef Line<Scalar,dim> Edge;
      static Scalar apply(Point<Scalar,dim> const& point, Rectangle<Scalar,dim> const& rectangle, Metric metric)
      {
        Scalar a,b;
        Point<Scalar,dim> projectedPoint = ProjectionImpl<Point<Scalar,dim>, Rectangle<Scalar,dim>, Scalar,dim,typename Metric::ScalarProduct>::apply(point, rectangle, a, b);
        Scalar l0 = distance<Metric>(rectangle.corners[0], rectangle.corners[1], metric),
            l1 = distance<Metric>(rectangle.corners[0], rectangle.corners[3], metric);
        if(0 < a && a < l0 && 0 < b && b < l1) return distance<Metric>(point, projectedPoint, metric);

        if(a <= 0)
        {
          if(b <= 0) return distance<Metric>(point, rectangle.corners[0], metric);
          if(b >= l1) return distance<Metric>(point, rectangle.corners[3], metric);
          return distance<Metric>(point, Edge(rectangle.corners[0], rectangle.corners[3]), metric);
        }
        if(a >= l0)
        {
          if(b <= 0) return distance<Metric>(point, rectangle.corners[1], metric);
          if(b >= l1) return distance<Metric>(point, rectangle.corners[2], metric);
          return distance<Metric>(point, Edge(rectangle.corners[1], rectangle.corners[2]), metric);
        }
        if(b <= 0) return distance<Metric>(point, Edge(rectangle.corners[0], rectangle.corners[1]), metric);
        return distance<Metric>(point, Edge(rectangle.corners[2], rectangle.corners[3]), metric);
      }
    };

    template <class Metric, class Scalar, int dim>
    struct DistanceImpl<Metric, Scalar, dim, Ball<Scalar,dim>, Point<Scalar,dim> >
    {
      static Scalar apply(Ball<Scalar,dim> const& ball, Point<Scalar,dim> const& point, Metric metric)
      {
        Scalar distanceToCenter = distance<Metric>(point, ball.center, metric);
        return (distanceToCenter < ball.radius) ? 0 : distanceToCenter-ball.radius;
      }
    };

    template <class Metric, class Scalar, int dim>
    struct DistanceImpl<Metric, Scalar, dim, Ball<Scalar,dim>, Line<Scalar,dim> >
    {
      static Scalar apply(Ball<Scalar,dim> const& ball, Line<Scalar,dim> const& line, Metric metric)
      {

        int distanceToCenter = distance<Metric>(ball.center, line, metric);
        return (distanceToCenter < ball.radius) ? 0 : distanceToCenter - ball.radius;
      }
    };

    template <class Metric, class Scalar, int dim>
    struct DistanceImpl<Metric, Scalar, dim, Ball<Scalar,dim>, Rectangle<Scalar,dim> >
    {
      static Scalar apply(Ball<Scalar,dim> const& ball, Rectangle<Scalar,dim> const& rectangle, Metric metric)
      {
        Scalar distanceToCenter = distance<Metric>(ball.center, rectangle, metric);
        return (distanceToCenter < ball.radius) ? 0 : distanceToCenter - ball.radius;
      }
    };

    template <class Metric, class Scalar, int dim>
    struct DistanceImpl<Metric, Scalar, dim, Ball<Scalar,dim>, BoundingBox<Scalar,dim> >
    {
      static Scalar apply(Ball<Scalar,dim> const& ball, BoundingBox<Scalar,dim> const& boundingBox, Metric metric)
      {
        if(boundingBox.contains(ball.center)) return 0;
        Scalar result=9999;
        // check corners
        std::vector<Point<Scalar,dim> > const& corners = boundingBox.getCorners();
        for(size_t cornerId=0; cornerId<corners.size(); ++cornerId)
        {
          Scalar tmp = distance<Metric>(ball, corners[cornerId], metric);
          if(tmp > 0) result = std::min(tmp,result); else return 0;
        }
        // check edges
        std::vector<Line<Scalar,dim> > const edges = boundingBox.getEdges();
        for(size_t edgeId=0; edgeId<edges.size(); ++edgeId)
        {
          Scalar tmp = distance<Metric>(ball, edges[edgeId], metric);
          if(tmp > 0) result = std::min(tmp, result); else return 0;
        }
        // check faces
        std::vector<Rectangle<Scalar,dim> > const faces = boundingBox.getFaces();
        for(size_t faceId=0; faceId<faces.size(); ++faceId)
        {
          Scalar tmp = distance<Metric>(ball, faces[faceId], metric);
          if(tmp > 0) result = std::min(tmp, result); else return 0;
        }

        return result;
      } // end distance
    };

//    template <class Metric, class Scalar, int dim, class DuneSimplex>
//    struct DistanceImpl<Metric, Scalar, dim, Ball<Scalar,dim>, DuneSimplex>
//    {
//      static Scalar apply(Ball<Scalar,dim> const& ball, DuneSimplex const& duneEntity)
//      {
//        if(Dune::GenericReferenceElements<double,3>::simplex().checkInside(duneEntity.geometry().local(ball.center))) return 0;
//        Scalar result = 9999;
//        int const numberOfCorners = duneEntity.geometry().corners();
//        // check corners
//        for(int cornerId=0; cornerId<numberOfCorners; ++cornerId)
//        {
//          Scalar tmp = distance<Metric>(ball, duneEntity.geometry().corner(cornerId));
//          if(tmp > 0) result = std::min(tmp, result);
//          else return 0;
//        }
//        // check edges
//        for(int startCornerId=0; startCornerId<numberOfCorners; ++startCornerId)
//        {
//          for(int endCornerId=startCornerId+1; endCornerId<numberOfCorners; ++endCornerId)
//          {
//            Scalar tmp = distance<Metric>(ball, Line<Scalar,dim>(duneEntity.geometry().corner(startCornerId), duneEntity.geometry(),corner(endCornerId)));
//            if(tmp > 0) result = std::min(tmp, result);
//            else return 0;
//          }
//        }
//        // check faces
//        if(dim>2)
//        {
//
//        }
//
//        return result;
//      }
//    };

    /*----------------------------------------------------------------------------------*/

    template <class Metric, class Scalar, int dim, class FirstObject, class SecondObject>
    struct IntersectionCheckImpl;

    template <class Metric, class Scalar, int dim>
    struct IntersectionCheckImpl<Metric, Scalar, dim, Ball<Scalar,dim>, BoundingBox<Scalar,dim> >
    {
      static bool apply(Ball<Scalar,dim> const& ball, BoundingBox<Scalar,dim> const& boundingBox, Metric metric)
      {
        return DistanceImpl<Metric, Scalar, dim, Ball<Scalar,dim>, BoundingBox<Scalar,dim> >::apply(ball,boundingBox,metric) <= 0;
      }
    };

//    template <class Metric, class Scalar, int dim, class DuneSimplex>
//    struct IntersectionCheckImpl<Metric, Ball<Scalar,dim>, DuneSimplex, Scalar, dim>
//    {
//      static bool apply(Ball<Scalar,dim> const& ball, DuneSimplex const& duneEntity)
//      {
//        return DistanceImpl<Metric, Scalar, dim, Ball<Scalar,dim>, DuneSimplex >::apply(ball,duneEntity) <= 0;
//      }
//    };

  }

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

  // project first object on second
  template <class Scalar, int dim, class FirstObject, class SecondObject, class ScalarProduct>
  FirstObject projectFirstOnSecond(FirstObject const& first, SecondObject const& second, ScalarProduct const& scalarProduct = ScalarProduct())
  {
    return ImplementationDetail::ProjectionImpl<FirstObject, SecondObject, Scalar,dim,ScalarProduct>::apply(first,second,scalarProduct);
  }

  template <class Scalar, int dim, class Metric>
  Scalar distance(Point<Scalar,dim> const& first, Point<Scalar,dim> const& second, Metric const& metric = Metric())
  {
    return ImplementationDetail::DistanceImpl<Metric, Scalar, dim, Point<Scalar,dim>, Point<Scalar,dim> >::apply(first,second,metric);
  }

  template <class Scalar, int dim, class Metric>
  Scalar distance(Point<Scalar,dim> const& point, Line<Scalar,dim> const& line, Metric const& metric = Metric())
  {
    return ImplementationDetail::DistanceImpl<Metric, Scalar, dim, Point<Scalar,dim>, Line<Scalar,dim> >::apply(point,line,metric);
  }

  template <class Scalar, int dim, class Metric>
  Scalar distance(Line<Scalar,dim> const& line, Point<Scalar,dim> const& point, Metric const& metric = Metric())
  {
    return ImplementationDetail::DistanceImpl<Metric, Scalar, dim, Point<Scalar,dim>, Line<Scalar,dim> >::apply(point,line,metric);
  }

  template <class Scalar, int dim, class Metric>
  Scalar distance(Point<Scalar,dim> const& point, Triangle<Scalar,dim> const& triangle, Metric const& metric = Metric())
  {
    return ImplementationDetail::DistanceImpl<Metric, Scalar, dim, Point<Scalar,dim>, Triangle<Scalar,dim> >::apply(point,triangle,metric);
  }

  template <class Scalar, int dim, class Metric>
  Scalar distance(Triangle<Scalar,dim> const& triangle, Point<Scalar,dim> const& point, Metric const& metric = Metric())
  {
    return ImplementationDetail::DistanceImpl<Metric, Scalar, dim, Point<Scalar,dim>, Triangle<Scalar,dim> >::apply(point,triangle,metric);
  }

  template <class Scalar, int dim, class Metric>
  Scalar distance(Point<Scalar,dim> const& point, Rectangle<Scalar,dim> const& rectangle, Metric const& metric = Metric())
  {
    return ImplementationDetail::DistanceImpl<Metric, Scalar, dim, Point<Scalar,dim>, Rectangle<Scalar,dim> >::apply(point,rectangle,metric);
  }

  template <class Scalar, int dim, class Metric>
  Scalar distance(Rectangle<Scalar,dim> const& rectangle, Point<Scalar,dim> const& point, Metric const& metric = Metric())
  {
    return ImplementationDetail::DistanceImpl<Metric, Scalar, dim, Point<Scalar,dim>, Rectangle<Scalar,dim> >::apply(point,rectangle,metric);
  }

  template <class Scalar, int dim, class Metric>
  Scalar distance(Ball<Scalar,dim> const& ball, Point<Scalar,dim> const& point, Metric const& metric = Metric())
  {
    return ImplementationDetail::DistanceImpl<Metric, Scalar, dim, Ball<Scalar,dim>, Point<Scalar,dim> >::apply(ball,point,metric);
  }

  template <class Scalar, int dim, class Metric>
  Scalar distance(Point<Scalar,dim> const& point, Ball<Scalar,dim> const& ball, Metric const& metric = Metric())
  {
    return ImplementationDetail::DistanceImpl<Metric, Scalar, dim, Ball<Scalar,dim>, Point<Scalar,dim> >::apply(ball,point,metric);
  }

  template <class Scalar, int dim, class Metric>
  Scalar distance(Ball<Scalar,dim> const& ball, Line<Scalar,dim> const& line, Metric const& metric = Metric())
  {
    return ImplementationDetail::DistanceImpl<Metric, Scalar, dim, Ball<Scalar,dim>, Line<Scalar,dim> >::apply(ball,line,metric);
  }

  template <class Scalar, int dim, class Metric>
  Scalar distance(Line<Scalar,dim> const& line, Ball<Scalar,dim> const& ball, Metric const& metric = Metric())
  {
    return ImplementationDetail::DistanceImpl<Metric, Scalar, dim, Ball<Scalar,dim>, Line<Scalar,dim> >::apply(ball,line,metric);
  }

  template <class Scalar, int dim, class Metric>
  Scalar distance(Ball<Scalar,dim> const& ball, Triangle<Scalar,dim> const& triangle, Metric const& metric = Metric())
  {
    return ImplementationDetail::DistanceImpl<Metric, Scalar, dim, Ball<Scalar,dim>, Triangle<Scalar,dim> >::apply(ball,triangle,metric);
  }

  template <class Scalar, int dim, class Metric>
  Scalar distance(Triangle<Scalar,dim> const& triangle, Ball<Scalar,dim> const& ball, Metric const& metric = Metric())
  {
    return ImplementationDetail::DistanceImpl<Metric, Scalar, dim, Ball<Scalar,dim>, Triangle<Scalar,dim> >::apply(ball,triangle,metric);
  }

  template <class Scalar, int dim, class Metric>
  Scalar distance(Ball<Scalar,dim> const& ball, Rectangle<Scalar,dim> const& rectangle, Metric const& metric = Metric())
  {
    return ImplementationDetail::DistanceImpl<Metric, Scalar, dim, Ball<Scalar,dim>, Rectangle<Scalar,dim> >::apply(ball,rectangle,metric);
  }

  template <class Scalar, int dim, class Metric>
  Scalar distance(Rectangle<Scalar,dim> const& rectangle, Ball<Scalar,dim> const& ball, Metric const& metric = Metric())
  {
    return ImplementationDetail::DistanceImpl<Metric, Scalar, dim, Ball<Scalar,dim>, Rectangle<Scalar,dim> >::apply(ball,rectangle,metric);
  }

  template <class Scalar, int dim, class Metric>
  Scalar distance(Ball<Scalar,dim> const& ball, BoundingBox<Scalar,dim> const& boundingBox, Metric const& metric = Metric())
  {
    return ImplementationDetail::DistanceImpl<Metric, Scalar, dim, Ball<Scalar,dim>, BoundingBox<Scalar,dim> >::apply(ball,boundingBox,metric);
  }

//  template <class Metric, class Scalar, int dim, class DuneSimplex>
//  Scalar distance(Ball<Scalar,dim> const& ball, DuneSimplex const& duneEntity)
//  {
//    typedef typename DuneSimplex::Geometry Geom;
//    return ImplementationDetail::DistanceImpl<Metric, Scalar, dim, Ball<Scalar,dim>, DuneSimplex >::apply(ball,duneEntity);
//  }

  template <class Scalar, int dim, class Metric>
  Scalar intersects(Ball<Scalar,dim> const& ball, BoundingBox<Scalar,dim> const& boundingBox, Metric const& metric = Metric())
  {
    return ImplementationDetail::IntersectionCheckImpl<Metric, Scalar, dim, Ball<Scalar,dim>, BoundingBox<Scalar,dim> >::apply(ball,boundingBox,metric);
  }

//  template <class Metric, class Scalar, int dim, class DuneSimplex>
//  Scalar intersects(Ball<Scalar,dim> const& ball, DuneSimplex const& boundingBox)
//  {
//    typedef typename DuneSimplex::Geometry Geom;
//    return ImplementationDetail::IntersectionCheckImpl<Metric, Scalar, dim, Ball<Scalar,dim>, DuneSimplex>::apply(ball,duneEntity);
//  }
}

#endif /* GEOMETRIC_OBJECTS_HH_ */


#include <cmath>
#include <iostream>

#include <franka/exception.h>
#include <franka/robot.h>
#include <franka/model.h>
#include <atomic>
#include <thread>
#include <Eigen/Core>
#include <Eigen/Eigen>

int main(int argc, char** argv) {
  if (argc != 2) {
    std::cerr << "Usage: " << argv[0] << " <robot-hostname>" << std::endl;
    return -1;
  }

  // Initialize data fields for the print thread.
  struct {
    std::mutex mutex;
    bool has_data;
    franka::RobotState robot_state;
    double time;
  } print_data{};
  std::atomic_bool running{true};
  const double print_rate = 10.0; //number of prints per second
  // Start print thread.
  std::thread print_thread([print_rate, &print_data, &running]() {
    while (running) {
      // Sleep to achieve the desired print rate.
      std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>((1.0 / print_rate * 1000.0))));
      // Try to lock data to avoid read write collisions.
      if (print_data.mutex.try_lock()) {
        if (print_data.has_data) {
          std::cout << "time= " <<  print_data.time << std::endl;
          print_data.has_data = false;
        }
        print_data.mutex.unlock();
      }
    }
  });

  try {
    franka::Robot robot(argv[1]);
    robot.setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}}); //values taken from https://github.com/frankaemika/libfranka/blob/master/examples/examples_common.cpp#L18
    robot.setCartesianImpedance({{3000, 3000, 3000, 300, 300, 300}}); //values taken from https://github.com/frankaemika/libfranka/blob/master/examples/examples_common.cpp#L19
    robot.setCollisionBehavior( //values taken from https://github.com/frankaemika/libfranka/blob/master/examples/generate_joint_velocity_motion.cpp#L39
        {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
        {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
        {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}},
        {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}});
    franka::Model model = robot.loadModel();
    std::array<double, 7> initial_position = robot.readOnce().q_d;
    double time = 0.0;
    std::cout << "franka::kDefaultCutoffFrequency = " << franka::kDefaultCutoffFrequency << std::endl;
    double frequency = franka::kDefaultCutoffFrequency;
    frequency = 10000000000;
    frequency = 1;
    frequency = 1000; //required for velocity control
    std::cout << "using the frequency = " << frequency << std::endl;
    int counter=0;
    int numDelays=0;
    const bool usePrintThread=false;
    robot.control([&print_data,&initial_position, &time, &counter, &numDelays, &usePrintThread, &model](const franka::RobotState& robot_state, franka::Duration period) -> franka::JointPositions
    // robot.control([&print_data,&initial_position, &time, &counter, &numDelays, &usePrintThread, &model](const franka::RobotState& robot_state, franka::Duration period) -> franka::JointVelocities
    // robot.control([&print_data,&initial_position, &time, &counter, &numDelays, &usePrintThread, &model](const franka::RobotState& robot_state, franka::Duration period) -> franka::Torques
    {
      counter++;
      time += period.toSec(); //->no noise
      // time += 0.001; //->creates noise
      if(period.toSec() > 0.001 || period.toSec() < 0.001)
      {
        numDelays++;
        std::cout << counter << " => " << period.toSec() << std::endl;
      }

      double totalDuration = 5.0;
      // const double delta_q = M_PI / 30.0 * (1 - std::cos(M_PI / 0.25 * time)); //super fast motion
      const double delta_q = M_PI / 30.0 * (1 - std::cos(M_PI / 2.5 * time)); //slow motion
      const double delta_qd = M_PI / 15.0 * std::sin(2*M_PI * time / totalDuration); //slow motion
      const double delta_qdd = M_PI / 4.0 * std::sin(2*M_PI * time / totalDuration); //slow motion

      franka::JointPositions output_q = {{initial_position[0],
                                        initial_position[1] + delta_q,
                                        initial_position[2] + delta_q,
                                        initial_position[3] + delta_q, //+ 0.0001 * counter //adding increased offset in each iteration
                                        initial_position[4] + delta_q,
                                        initial_position[5] + delta_q,
                                        initial_position[6] + delta_q}};
      franka::JointVelocities output_qd = {{0.0,
                                          delta_qd,
                                          delta_qd,
                                          delta_qd, //+ 0.0001 * counter //adding increased offset in each iteration
                                          delta_qd,
                                          delta_qd,
                                          delta_qd}};
      const Eigen::Matrix<double, 7, 7> inertia(model.mass(robot_state).data());
      Eigen::Matrix<double, 7, 1> qdd =  Eigen::Matrix<double, 7, 1>();
      qdd<<0.0, delta_qdd, delta_qdd, delta_qdd, delta_qdd, delta_qdd, delta_qdd;
      Eigen::Matrix<double, 7, 1> tau = inertia*qdd;
      std::vector<double> foo(tau.data(),tau.data() + tau.rows() * tau.cols());
      std::array<double, 7> bar;
      std::copy(foo.begin(), foo.begin() + 6, bar.begin());
      franka::Torques output_tau = franka::Torques(bar);
      // franka::Torques output_tau = {{0.0,0.0,0.0,0.0,0.0,0.0,0.0}};

      if (time >= totalDuration) {
        std::cout << std::endl << "Finished motion, shutting down example" << std::endl;
        return franka::MotionFinished(output_q);
        // return franka::MotionFinished(output_qd);
        // return franka::MotionFinished(output_tau);
      }
      // Update data to print.
      if(usePrintThread)
      {
        if (print_data.mutex.try_lock()) {
          print_data.has_data = true;
          print_data.robot_state = robot_state;
          print_data.time = time;
          print_data.mutex.unlock();
        }
      }
      return output_q;
      // return output_qd;
      // return output_tau;
    }); //default params
    // }, franka::ControllerMode::kJointImpedance, true, franka::kDefaultCutoffFrequency); //default params
    // }, franka::ControllerMode::kJointImpedance, true, frequency); //user-defined frequency
    // }, franka::ControllerMode::kJointImpedance, false, frequency); //user-defined frequency & limit_rate=false;

    std::cout << "number of delays per second = " << numDelays / time << std::endl;
  } catch (const franka::Exception& e) {
    std::cout << e.what() << std::endl;
    return -1;
  }

  return 0;
}


//---------------------------------------------------------------------------//
//!
//! \file   Utility_GaussLegendreQuadratureSet.hpp
//! \author Luke Kersting
//! \brief Gauss - Legendre quadrature set declaration
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_GAUSS_LEGENDRE_QUADRATURE_SET_HPP
#define UTILITY_GAUSS_LEGENDRE_QUADRATURE_SET_HPP

// Boost Includes
#include <boost/multiprecision/cpp_dec_float.hpp>

// Trilinos Includes
#include <Teuchos_Array.hpp>
#include <Teuchos_TwoDArray.hpp>

namespace Utility{

typedef boost::multiprecision::cpp_dec_float_50 long_float;

//! Return the Gauss moments of the legendre expansion of a function, f(x)
void getGaussMoments( const Teuchos::Array<long_float>& legendre_expansion_moments,
                      Teuchos::Array<long_float>& gauss_moments );

//! Return the coefficients of the legendre expansion of x^n
void getLegendrePowerExpansionCoefficients(
                      Teuchos::TwoDArray<long_float>& coefficients,
                      const int power = 0 );
} // end Utility namespace

#endif // end UTILITY_GAUSS_LEGENDRE_QUADRATURE_SET_HPP

//---------------------------------------------------------------------------//
// end Utility_GaussLegendreQuadratureSet.hpp
//---------------------------------------------------------------------------//


/*
 Copyright (C) 2017 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file qle/termstructures/swaptionvolconstantspread.hpp
    \brief swaption cube that combines an ATM matrix and vol spreads from a cube
    \ingroup termstructures
*/

#ifndef quantext_swaptionvolcubeconstantspread_hpp
#define quantext_swaptionvolcubeconstantspread_hpp

#include <ql/termstructures/volatility/swaption/swaptionvolcube.hpp>

#include <boost/shared_ptr.hpp>

namespace QuantExt {
using namespace QuantLib;

namespace {
class ConstantSpreadSmileSection : public SmileSection {
public:
    ConstantSpreadSmileSection(const Handle<SwaptionVolatilityStructure>& atm,
                               const Handle<SwaptionVolatilityStructure>& cube, const Real optionTime,
                               const Real swapLength)
        : SmileSection(optionTime, DayCounter(), atm->volatilityType(),
                       atm->volatilityType() == ShiftedLognormal ? atm->shift(optionTime, swapLength) : 0.0),
          atm_(atm), cube_(cube), swapLength_(swapLength), section_(cube_->smileSection(optionTime, swapLength_)),
          atmStrike_(section_->atmLevel()) {
        registerWith(atm);
        registerWith(cube);
    }
    Rate minStrike() const { return cube_->minStrike(); }
    Rate maxStrike() const { return cube_->maxStrike(); }
    Rate atmLevel() const { return Null<Real>(); }

protected:
    Volatility volatilityImpl(Rate strike) const {
        Real t = exerciseTime();
        Real s = section_->volatility(strike) - section_->volatility(atmStrike_);
        Real v = atm_->volatility(t, swapLength_, strike);
        return v + s;
    }

private:
    const Handle<SwaptionVolatilityStructure> atm_, cube_;
    const Real swapLength_;
    const boost::shared_ptr<SmileSection> section_;
    const Real atmStrike_;
};
} // anonymous namespace

//! Swaption cube that combines an ATM matrix and vol spreads from a cube
/*! Notice that the TS has a floating reference date and accesses the source TS only via
 their time-based volatility methods.

 \warning the given atm vol structure should be strike independent, this is not checked
 \warning the given cube must provide smile sections that provide an ATM level

 \ingroup termstructures
*/
class SwaptionVolatilityConstantSpread : public SwaptionVolatilityStructure {
public:
    SwaptionVolatilityConstantSpread(const Handle<SwaptionVolatilityStructure>& atm,
                                     const Handle<SwaptionVolatilityStructure>& cube)
        : SwaptionVolatilityStructure(0, atm->calendar(), atm->businessDayConvention(), atm->dayCounter()), atm_(atm),
          cube_(cube) {
        enableExtrapolation(atm->allowsExtrapolation());
        registerWith(atm_);
        registerWith(cube_);
    }

    //! \name TermStructure interface
    //@{
    DayCounter dayCounter() const { return atm_->dayCounter(); }
    Date maxDate() const { return atm_->maxDate(); }
    Time maxTime() const { return atm_->maxTime(); }
    const Date& referenceDate() const { return atm_->referenceDate(); }
    Calendar calendar() const { return atm_->calendar(); }
    Natural settlementDays() const { return atm_->settlementDays(); }
    //! \name VolatilityTermStructure interface
    //@{
    Rate minStrike() const { return cube_->minStrike(); }
    Rate maxStrike() const { return cube_->maxStrike(); }
    //@}
    //! \name SwaptionVolatilityStructure interface
    //@{
    const Period& maxSwapTenor() const { return atm_->maxSwapTenor(); }
    VolatilityType volatilityType() const { return atm_->volatilityType(); }
    //@}
    const Handle<SwaptionVolatilityStructure>& atmVol() { return atm_; }
    const Handle<SwaptionVolatilityStructure>& cube() { return cube_; }

protected:
    boost::shared_ptr<SmileSection> smileSectionImpl(Time optionTime, Time swapLength) const {
        return boost::make_shared<ConstantSpreadSmileSection>(atm_, cube_, optionTime, swapLength);
    }

    Volatility volatilityImpl(Time optionTime, Time swapLength, Rate strike) const {
        if (strike == Null<Real>())
            return atm_->volatility(optionTime, swapLength, 0.0);
        else
            return smileSectionImpl(optionTime, swapLength)->volatility(strike);
    }

private:
    Handle<SwaptionVolatilityStructure> atm_, cube_;
};

} // namespace QuantExt

#endif


#define BOOST_TEST_MODULE SolutionTest

#include "solution.hpp"

//#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(SolutionSuite)

BOOST_AUTO_TEST_CASE(PlainTest1)
{
    vector<vector<int>> grid{
        {1,3,1},
        {1,5,1},
        {4,2,1}
    };
    int result = Solution().minPathSum(grid);

    int expected = 7;

    BOOST_CHECK_EQUAL(result, expected);
}

BOOST_AUTO_TEST_SUITE_END()


//==============================================================================
//         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 BOOST_SIMD_PREDICATES_FUNCTIONS_SIMD_COMMON_IS_NOT_EQUAL_WITH_EQUAL_NANS_HPP_INCLUDED
#define BOOST_SIMD_PREDICATES_FUNCTIONS_SIMD_COMMON_IS_NOT_EQUAL_WITH_EQUAL_NANS_HPP_INCLUDED
#include <boost/simd/predicates/functions/is_not_equal_with_equal_nans.hpp>
#include <boost/simd/sdk/simd/logical.hpp>
#include <boost/simd/include/functions/simd/logical_and.hpp>
#include <boost/simd/include/functions/simd/logical_not.hpp>
#include <boost/simd/include/functions/simd/is_equal_with_equal_nans.hpp>
#include <boost/simd/include/functions/simd/is_nan.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::is_not_equal_with_equal_nans_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<floating_<A0>,X>))((simd_<floating_<A0>,X>))
                            )
  {
    typedef typename meta::as_logical<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return logical_and(neq(a0,a1), logical_not(logical_and(boost::simd::is_nan(a0), boost::simd::is_nan(a1))));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::is_not_equal_with_equal_nans_, tag::cpu_,
                             (A0)(X),
                             ((simd_<integer_<A0>,X>))
                             ((simd_<integer_<A0>,X>))
                             )
  {
    typedef typename meta::as_logical<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return neq(a0,a1);
    }
  };
} } }

#endif


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file termstructures/dynamicswaptionvolmatrix.hpp
    \brief dynamic swaption volatility matrix
    \ingroup termstructures
*/

#ifndef quantext_dynamic_swaption_volatility_termstructure_hpp
#define quantext_dynamic_swaption_volatility_termstructure_hpp

#include <qle/termstructures/dynamicstype.hpp>

#include <ql/termstructures/volatility/swaption/swaptionvolmatrix.hpp>

#include <boost/make_shared.hpp>

namespace QuantExt {
using namespace QuantLib;
//! Takes a SwaptionVolatilityMatrix with fixed reference date and turns it into a floating reference date term
// structure.
/*! This class takes a SwaptionVolatilityMatrix with fixed reference date and turns it into a floating reference date
    term structure. There are different ways of reacting to time decay that can be specified.

    \warning the vols from the source ts are read using strike null (indicating atm)

    \ingroup termstructures
*/

class DynamicSwaptionVolatilityMatrix : public SwaptionVolatilityStructure {
public:
    DynamicSwaptionVolatilityMatrix(const boost::shared_ptr<SwaptionVolatilityStructure>& source,
                                    Natural settlementDays, const Calendar& calendar,
                                    ReactionToTimeDecay decayMode = ConstantVariance);

protected:
    /* SwaptionVolatilityStructure interface */
    const Period& maxSwapTenor() const;

    boost::shared_ptr<SmileSection> smileSectionImpl(Time optionTime, Time swapLength) const;

    Volatility volatilityImpl(Time optionTime, Time swapLength, Rate strike) const;

    Real shiftImpl(Time optionTime, Time swapLength) const;

    /* VolatilityTermStructure interface */
    Real minStrike() const;
    Real maxStrike() const;
    /* TermStructure interface */
    Date maxDate() const;
    /* Observer interface */
    void update();

    VolatilityType volatilityType() const;

private:
    const boost::shared_ptr<SwaptionVolatilityStructure> source_;
    ReactionToTimeDecay decayMode_;
    const Date originalReferenceDate_;
    VolatilityType volatilityType_;
};

inline VolatilityType DynamicSwaptionVolatilityMatrix::volatilityType() const { return volatilityType_; }

} // namespace QuantExt

#endif


/**
 * ODE solver - JMU REU 2021
 *
 * @author Mike Lam
 */

// standard headers
#include <cstdlib>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <sstream>
#include <string>
#include <cmath>
#include <functional>
using namespace std;

// Boost ODEint headers (individual files compile faster than the catch-all)
#include <boost/numeric/odeint.hpp>
//#include <boost/numeric/odeint/integrate/integrate_const.hpp>
//#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
using namespace boost::numeric::odeint;

// floating-point precision control (control via compiler parameter -Dreal_t)
//typedef double real_t;      // 64 bits
//typedef float real_t;       // 32 bits

// solver state
typedef std::vector<real_t> state_t;

// output stream
ofstream out;

// ODE system
void ode(const state_t &x, state_t &dxdt, const real_t /*t*/)
{
    // TODO: encode ODE for RK4 solver
}

/* Generic Parker-Sochacki solver function */
int ps_step(real_t *y[], real_t *co[],              /* in-out */
        real_t y1[], real_t ynew[],                 /* out */
        real_t fp[], real_t eta[],                  /* in */
        void (*first)(real_t *[], real_t *[], real_t []),
        void (*iter)(real_t *[], real_t *[], real_t [], int),
        int stop, int ps_limit, int nv, int err_nv)
{
    int i, p;
    real_t dt = fp[99], dt_pow;

    first(y, co, fp);                   /* Calculate first order terms */

    if (dt == 1) {
        for (i=0; i<nv; i++) {
            y1[i] = y[i][0] + y[i][1];
        }
    } else {
        for (i=0; i<nv; i++) {
            y1[i] = y[i][0] + dt*y[i][1];
        }
        dt_pow = dt*dt;
    }

    for (p=1; p<(ps_limit-1); p++) {    /* Iterations */

        iter(y, co, fp, p);

        /* Update solution */
        if (dt == 1) {
            for(i=0; i<nv; i++) {
                ynew[i] = y1[i] + y[i][p+1];
            }
        } else {
            for(i=0; i<nv; i++) {
                ynew[i] = y1[i] + y[i][p+1]*dt_pow;
            }
            dt_pow *= dt;
        }

        /* Check for solution divergence */
        if ((fabs(y[0][p+1]) > 10.0)) {
            p = -1;
            break;
        }

        /* Test error tolerance on variable value change */
        for (i=0; i<err_nv; i++) {
            if (fabs(ynew[i] - y1[i]) > eta[i]) {
                break;
            }
        }
        if (i == err_nv) break;

        for(i=0; i<nv; i++) {
            y1[i] = ynew[i];
        }
    }

    p++;
    if (stop == 1) {
        if (p == ps_limit) {
            cerr << "PS solve failed." << endl;
        }
    }
    return p;
}

/* PS - first term for Izhikevich model */
void iz_first(real_t *y[], real_t *co[], real_t fp[])
{
    real_t v, u, g_ampa, g_gaba, I, k, /*l,*/
           E_ampa, E_gaba, E, a, b, co_g_ampa, co_g_gaba, chi;

    v =      y[0][0];
    u =      y[1][0];
    g_ampa = y[2][0];
    g_gaba = y[3][0];
    chi =    y[4][0];

    I =         fp[0];
    k =         fp[1];
    E_ampa =    fp[3];
    E_gaba =    fp[4];
    E =         fp[5];
    a =         fp[6];
    b =         fp[7];
    co_g_ampa = fp[8];
    co_g_gaba = fp[9];

    y[0][1] = E*(v*chi - u + E_ampa*g_ampa + E_gaba*g_gaba + I);
    y[1][1] = a*(b*v - u);
    y[2][1] = co_g_ampa*g_ampa;
    y[3][1] = co_g_gaba*g_gaba;
    y[4][1] = k*y[0][1] - y[2][1] - y[3][1];
}

/* PS - iteration function for higher order terms */
void iz_iter(real_t *y[], real_t *co[], real_t fp[], int p)
{
    real_t /*v,*/ k, /*l,*/ E_ampa, E_gaba, b, /*chi,*/ vchi;
    int i;

    k =      fp[1];
    E_ampa = fp[3];
    E_gaba = fp[4];
    b =      fp[7];

    vchi = y[0][0]*y[4][p] + y[4][0]*y[0][p];
    for (i = 1; i < p; i++) {
        vchi += y[0][i] * y[4][p-i];
    }

    y[0][p+1] = co[0][p] * (vchi - y[1][p] + E_ampa*y[2][p] + E_gaba*y[3][p]);
    y[1][p+1] = co[1][p] * (b*y[0][p] - y[1][p]);
    y[2][p+1] = co[2][p] * y[2][p];
    y[3][p+1] = co[3][p] * y[3][p];
    y[4][p+1] = k*y[0][p+1] - y[2][p+1] - y[3][p+1];
}

// TODO: debug output helper
void observe(const state_t &x, const real_t t)
{
    //real_t sol = solution(a, b, c, x0, t);
    //real_t psm_sol = psm(0);

    // re-center power series
    //calc_coefficients(psm(dt));

    // write output
    //out << setw(8) << t;
    //out << setw(16) << sol;
    //out << setw(16) << x[0] << setw(16) << fabs(sol-x[0]);
    //out << setw(16) << psm_sol << setw(16) << fabs(sol-psm_sol);
    //out << endl;
}

int main(int argc, const char* argv[])
{
    // TODO: check parameters
    //if (argc != 7) {
        //cout << "Usage: " << argv[0] << " <a> <b> <c> <x0> <tn> <dt>" << endl;
        //return EXIT_FAILURE;
    //}

    // TODO: parse parameters

    int stop         = 1;
    int ps_limit     = 10;
    const int nv     = 5;
    const int err_nv = nv;

    real_t fp[100];      /* TODO: initialize indices 0-9 and 99 */

    real_t* y  [nv];     /* TODO: initialize */
    real_t* co [nv];     /* TODO: initialize */
    real_t  eta[nv];     /* TODO: initialize */

    for (int i=0; i<nv; i++) {
        y[i]  = (real_t*) calloc(nv, sizeof(real_t)*(ps_limit+1));
        co[i] = (real_t*) calloc(nv, sizeof(real_t)*(ps_limit+1));
    }

    real_t y1  [nv];
    real_t ynew[nv];

    // TODO: initialize RK4 state
    //state_t x(nv);

    // show floating-point width
    //cout << "sizeof(real_t)=" << sizeof(real_t) << endl;

    // stepper
    runge_kutta4<state_t> stp;

    out.open("out.dat");
    int steps = ps_step(y, co,
            y1, ynew, fp, eta, iz_first, iz_iter,
            stop, ps_limit, nv, err_nv);
    // TODO: integrate w/ debug output
    /*size_t steps =*/ //integrate_const(stp, ode, x, t0, tn, dt, observe);
    out.close();

    // show final output
    cout << "steps=" << steps /*<< " x=" << x[0]*/ << endl;

    for (int i=0; i<nv; i++) {
        free(y[i]);
        free(co[i]);
    }

    return EXIT_SUCCESS;
}