//
// Created by Nathan on 7/10/2015.
//

#include "world/PlainsCell.h"
#include "world/DesertCell.h"
#include "world/ForestCell.h"
#include "game/Civilian.h"
#include "game/generation/ClusterSeederV2.h"
#include <boost/math/constants/constants.hpp>
#include <numeric/Random.h>
#include <boost/generator_iterator.hpp>

namespace undocked {
    namespace game {
        namespace generation {
            numeric::Random random(10);

            using namespace world;

            ClusterSeederV2::ClusterSeederV2() {

            }

            ClusterSeederV2::~ClusterSeederV2() {

            }

            void ClusterSeederV2::seedTerrain(GameState *state) {
                std::cout << "Seeding terrain!" << std::endl;
                CellularBoard *board = state->getBoard();
                int width = board->getWidth(), height = board->getHeight();

                std::vector<AnchorPoint*> anchors;
                boost::multi_array<AnchorPoint*, 2> closestAnchors(boost::extents[height][width]);

                std::vector<AnchorPoint*> desertAnchors;
                boost::multi_array<AnchorPoint*, 2> closestDesertAnchors(boost::extents[height][width]);

                // fill in with the default: PlainsCell
                for (int v = 0; v < height; v++) {
                    for (int u = 0; u < width; u++) {
                        board->setCell(u, v, PlainsCell());
                    }
                }

                // seed clusters of desert
                int dcc = 1 + (random.get() > 0.5 ? 0 : 1);

                auto widthGen = random.createUniformInteger(3, 7);
                for (int i = 0; i < dcc; i++) {
                    int cw = widthGen();
                    int ch = widthGen();

                    random.setUniform(0, width - cw - 1);
                    int tu = random.getUniformInteger();

                    random.setUniform(0, width - ch - 1);
                    int tv = random.getUniformInteger();

                    int cx = tu + cw / 2;
                    int cy = tv + ch / 2;
                    AnchorPoint *current = new AnchorPoint(sf::Vector2f(cx, cy));

                    desertAnchors.push_back(current);

                    for (int u = tu; u < tu + cw; u++) {
                        for (int v = tv; v < tv + ch; v++) {
                            if (random.get() < 0.8)
                                board->setCell(u, v, DesertCell());
                        }
                    }
                }

                // seed clusters of forest
                random.setUniform(2, 6);
                int fcc = random.getUniformInteger();

                for (int i = 0; i < fcc; i++) {
                    int cw = widthGen();
                    int ch = widthGen();
                    random.setUniform(0, width - cw - 1);
                    int tu = random.getUniformInteger();

                    random.setUniform(0, width - ch - 1);
                    int tv = random.getUniformInteger();

                    int cx = tu + cw / 2;
                    int cy = tv + ch / 2;
                    AnchorPoint *current = new AnchorPoint(sf::Vector2f(cx, cy));

                    anchors.push_back(current);

                    for (int u = tu; u < tu + cw; u++) {
                        for (int v = tv; v < tv + ch; v++) {
                            if (random.get() < 0.8)
                                board->setCell(u, v, ForestCell());
                        }
                    }
                }

                std::cout << "Calculating Anchor points!" << std::endl;
                for (int v = 0; v < height; v++) {
                    for (int u = 0; u < width; u++) {
                        AnchorPoint *closest = nullptr;
                        double minDist = 999999;
                        for (auto anchor : anchors) {
                            double dist = std::abs(anchor->location.x - u) + std::abs(anchor->location.y - v);
                            if (dist < minDist) {
                                closest = anchor;
                                minDist = dist;
                            }
                        }

                        closestAnchors[v][u] = closest;
                    }
                }

                for (int v = 0; v < height; v++) {
                    for (int u = 0; u < width; u++) {
                        AnchorPoint *closest = nullptr;
                        double minDist = 999999;
                        for (auto anchor : desertAnchors) {
                            double dist = std::abs(anchor->location.x - u) + std::abs(anchor->location.y - v);
                            if (dist < minDist) {
                                closest = anchor;
                                minDist = dist;
                            }
                        }
                        closestDesertAnchors[v][u] = closest;
                    }
                }

                std::cout << "Propagating terrain..." << std::endl;
                // propagate the desert and forest terrains
                int iterations = (int) ceil((sqrt(width * height) / 40) * 6) * 3;
                int ruleSet = 0;
                for (int iteration = 0; iteration < iterations; iteration++) {
                    std::cout << "\tIteration " << iteration << " / " << iterations - 1 << std::endl;
                    boost::multi_array<std::pair<int, int>, 2> neighbors(boost::extents[height][width]);

                    for (int v = 0; v < height; v++) {
                        for (int u = 0; u < width; u++) {
                            int right = (u + 1);
                            int up = (v + 1);
                            int down = v - 1;
                            int left = u - 1;
                            int d_incident = 0, f_incident = 0;

                            auto in = [height, width](int a, bool isUp) {
                                if (isUp) {
                                    return a >= 0 && a < height;
                                } else {
                                    return a >= 0 && a < width;
                                }
                            };

                            if (in(right, 0) && board->getCell(right, v).getType() == BoardCell::TYPE::Desert) {
                                d_incident += 1;
                            }
                            else if (in(right, 0) && board->getCell(right, v).getType() == BoardCell::TYPE::Forest) {
                                f_incident += 1;
                            }

                            if (in(left, 0) && board->getCell(left, v).getType() == BoardCell::TYPE::Desert) {
                                d_incident += 1;
                            }
                            else if (in(left, 0) && board->getCell(left, v).getType() == BoardCell::TYPE::Forest) {
                                f_incident += 1;
                            }

                            if (in(up, 1) && board->getCell(u, up).getType() == BoardCell::TYPE::Desert) {
                                d_incident += 1;
                            }
                            else if (in(up, 1) && board->getCell(u, up).getType() == BoardCell::TYPE::Forest) {
                                f_incident += 1;
                            }

                            if (in(down, 1) && board->getCell(u, down).getType() == BoardCell::TYPE::Desert) {
                                d_incident += 1;
                            }
                            else if (in(down, 1) && board->getCell(u, down).getType() == BoardCell::TYPE::Forest) {
                                f_incident += 1;
                            }

                            neighbors[v][u] = std::make_pair(d_incident, f_incident);
                        }
                    }

                    if ((iteration % 8) == 0) ruleSet = !ruleSet;

                    if (ruleSet == 0) {
                        for (int v = 0; v < height; v++) {
                            for (int u = 0; u < width; u++) {
                                BoardCell cell = board->getCell(u, v);
                                std::pair<int, int> pair = (std::pair<int, int>) neighbors[v][u];
                                int d_count = pair.first;
                                int f_count = pair.second;

                                if (cell.getType() == BoardCell::TYPE::Desert) {
                                    if (d_count == 1) {
                                        if (random.get() < 0)
                                            board->setCell(u, v, PlainsCell());

                                    }
                                    else if (d_count == 0) {
                                        board->setCell(u, v, PlainsCell());
                                    }
                                }
                                else if (cell.getType() == BoardCell::TYPE::Plains) {
                                    AnchorPoint *p = closestAnchors[v][u];
                                    AnchorPoint *dp = closestDesertAnchors[v][u];
                                    if (d_count > 1 && dp->getComponent(sf::Vector2f(u, v)) > random.get() + 0.1)
                                        board->setCell(u, v, DesertCell());
                                    else if (d_count > 0 && dp->getComponent(sf::Vector2f(u, v)) > random.get() + 0.1 && random.get() > 0.5) {
                                        board->setCell(u, v, DesertCell());
                                    }
                                    else if (f_count > 1 && p->getComponent(sf::Vector2f(u, v)) > random.get() + 0.1) {
                                        board->setCell(u, v, ForestCell());
                                    }
                                    else if (f_count > 0 && p->getComponent(sf::Vector2f(u, v)) > random.get() + 0.1 && random.get() > 0.5) {
                                        board->setCell(u, v, ForestCell());
                                    }

                                }
                                else if (cell.getType() == BoardCell::TYPE::Forest) {
                                    if (d_count > 0)
                                        board->setCell(u, v, PlainsCell());
                                    else if (f_count == 1 && random.get() > 0.9)
                                        board->setCell(u, v, PlainsCell());
                                    else if (f_count == 0 && random.get() > 0.5)
                                        board->setCell(u, v, PlainsCell());
                                }
                            }
                        }
                    } else {
                        for (int v = 0; v < height; v++) {
                            for (int u = 0; u < width; u++) {
                                BoardCell cell = board->getCell(u, v);
                                std::pair<int, int> pair = (std::pair<int, int>) neighbors[v][u];
                                int d_count = pair.first;
                                int f_count = pair.second;

                                if (cell.getType() == BoardCell::TYPE::Desert) {
                                    if (d_count == 1) {
                                        if (random.get() < 0.5)
                                            board->setCell(u, v, PlainsCell());
                                    }
                                    else if (d_count == 0) {
                                        board->setCell(u, v, PlainsCell());
                                    }
                                }
                                else if (cell.getType() == BoardCell::TYPE::Plains) {
                                    AnchorPoint *p = closestAnchors[v][u];
                                    AnchorPoint *dp = closestDesertAnchors[v][u];
                                    if (d_count > 1 && dp->getComponent(sf::Vector2f(u, v)) > random.get() + 0.1)
                                        board->setCell(u, v, DesertCell());
                                    else if (d_count > 0 && dp->getComponent(sf::Vector2f(u, v)) > random.get() + 0.1 && random.get() > 0.5) {
                                        board->setCell(u, v, DesertCell());
                                    }
                                    else if (f_count > 1 && p->getComponent(sf::Vector2f(u, v)) > random.get() + 0.1) {
                                        board->setCell(u, v, ForestCell());
                                    }
                                    else if (f_count > 0 && p->getComponent(sf::Vector2f(u, v)) > random.get() + 0.1 && random.get() > 0.5) {
                                        board->setCell(u, v, ForestCell());
                                    }

                                }
                                else if (cell.getType() == BoardCell::TYPE::Forest) {
                                    if (d_count > 0)
                                        board->setCell(u, v, PlainsCell());
                                    else if (f_count == 1 && random.get() > 0.7)
                                        board->setCell(u, v, PlainsCell());
                                    else if (f_count == 0 && random.get() > 0.4)
                                        board->setCell(u, v, PlainsCell());
                                }
                            }
                        }
                    }
                }

                board->generateAdjacencyGraph();

                // clean up
                for (auto anchor : desertAnchors) {
                    delete anchor;
                }

                for (auto anchor : anchors) {
                    delete anchor;
                }
            }

            void ClusterSeederV2::seedInfrastructure(GameState *state) {

            }

            void ClusterSeederV2::seedUnits(GameState *state) {
                int width = state->getBoard()->getWidth(),
                        height = state->getBoard()->getHeight();

                auto widthGen = random.createUniformInteger(4, 8);
                int cw = widthGen();
                int ch = widthGen();

                int tu = random.createUniformInteger(0, width - cw - 1)();
                int tv = random.createUniformInteger(0, height - ch - 1)();

                std::vector<int> locations((unsigned) cw * ch);
                std::iota(locations.begin(), locations.end(), 0);

                // random_shuffle should not be used, as it utilized rand()
                std::random_shuffle(locations.begin(), locations.end());

                for (int i = 0; i < 10; i++) {
                    Civilian *civvie = new Civilian();
                    int u = tu + locations[i] % cw,
                            v = tv + locations[i] / cw;
                    civvie->setLocation(sf::Vector2i(u, v));
                    state->units.push_back(civvie);
                }

                std::sort(state->units.begin(), state->units.end(), [](Unit* a, Unit* b) {
                    return a->getLocation().y < b->getLocation().y;
                });
            }

            double ClusterSeederV2::AnchorPoint::getComponent(sf::Vector2f loc) {
                sf::Vector2f displacement = loc - this->location;
                double theta = std::atan2(displacement.y, displacement.x);
                double num = func(theta) / 3.5;
                return std::min<double>(num, 1);
            }

            double ClusterSeederV2::AnchorPoint::dSigmoid(double x) {
                return std::exp(x) / ((1 + std::exp(x)) * (1 + std::exp(x)));
            }

            double ClusterSeederV2::AnchorPoint::func(double theta) {
                double value1 = 1, value2 = 1;
                double PI = boost::math::constants::pi<double>();

                for (auto pair : sizesAndPhases) {
                    double size = pair.first;
                    double phase = pair.second;

                    // warning: some kind of wrapping will be needed for this!
                    // this might explain some of the harsh edges seen; however this is a minor problem
                    value1 += size * dSigmoid(10 * (theta - phase));
                    value2 += size * dSigmoid(10 * (theta - 2 * PI - phase));
                }

                // two values in case a sigmoid function borders PI or -PI
                return std::abs(value1) > std::abs(value2) ? value1 : value2;
            }

            ClusterSeederV2::AnchorPoint::AnchorPoint(sf::Vector2f location) : location(location) {
                double PI = boost::math::constants::pi<double>();

                auto sizeGen = random.createUniformDouble(-0.5, 12);
                auto phaseGen = random.createUniformDouble(-PI, PI);

                for (int i = 0; i < 10; i++) {
                    sizesAndPhases.push_back(std::make_pair(sizeGen(), phaseGen()));
                }
            }

        }
    }
}


/*
* TS Elements
* Copyright 2015-2018 M. Newhouse
* Released under the MIT license.
*/

#include "handling_v2.hpp"
#include "car.hpp"
#include "world.hpp"

#include "resources/terrain_definition.hpp"

#include "utility/transform.hpp"
#include "utility/interpolate.hpp"
#include "utility/math_utilities.hpp"

#include <boost/container/small_vector.hpp>

#include <array>
#include <algorithm>

namespace ts
{
  namespace world
  {
    HandlingState update_car_state(Car& car, const World& world, double frame_duration)
    {
      auto& handling = car.handling();

      using controls::Control;
      auto half_wheelbase = handling.wheelbase_length * 0.5;

      std::array<Vector2d, 2> front_wheel_positions =
      { {
        { 0.0, -half_wheelbase + handling.wheelbase_offset }
        } };

      std::array<Vector2d, 2> rear_wheel_positions =
      { {
        { 0.0, half_wheelbase + handling.wheelbase_offset }
        } };

      if (handling.num_front_wheels >= 2)
      {
        front_wheel_positions =
        { {
          { -handling.front_axle_width * 0.5, -half_wheelbase + handling.wheelbase_offset },
          { handling.front_axle_width * 0.5, -half_wheelbase + handling.wheelbase_offset },
          } };
      }

      if (handling.num_rear_wheels >= 2)
      {
        rear_wheel_positions =
        { {
          { -handling.rear_axle_width * 0.5, half_wheelbase + handling.wheelbase_offset },
          { handling.rear_axle_width * 0.5, half_wheelbase + handling.wheelbase_offset }
        } };
      }

      auto transform = make_transformation(car.rotation());
      auto inv_transform = make_transformation(-car.rotation());

      auto handling_state = car.handling_state();
      handling_state.wheel_states.clear();

      auto local_velocity = transform_point(car.velocity(), inv_transform);
      auto local_heading = normalize(local_velocity);
      auto speed = magnitude(local_velocity);
      auto angular_velocity = car.angular_velocity();
      auto mass = car.mass();
      auto center_of_mass = car.center_of_mass();

      auto inv_moment = 1.0 / car.moment_of_inertia();

      auto num_wheels = handling.num_front_wheels + handling.num_rear_wheels;
      auto inv_num_wheels = 1.0 / num_wheels;
      auto wheel_traction_limit = handling.traction_limit * inv_num_wheels;

      auto total_downforce = local_heading.y * local_heading.y * speed * speed * handling.downforce_coefficient;
      if (local_heading.y > 0.0) total_downforce = 0.0;

      auto weight_distribution = clamp(center_of_mass.y / (handling.wheelbase_length * 0.5) + 0.5, 0.25, 0.75);

      auto rear_downforce = total_downforce * (0.5 + handling.downforce_balance);
      auto front_downforce = total_downforce * (0.5 - handling.downforce_balance);

      auto inv_num_front_wheels = 1.0 / handling.num_front_wheels;
      auto inv_num_rear_wheels = 1.0 / handling.num_rear_wheels;

      auto front_base_load = wheel_traction_limit * 2.0 * (1.0 - weight_distribution);
      auto rear_base_load = wheel_traction_limit * 2.0 * weight_distribution;

      auto front_load_transfer = handling_state.net_force.y * handling.load_transfer * 0.5;
      auto rear_load_transfer = -handling_state.net_force.y * handling.load_transfer * 0.5;

      auto front_traction_limit = std::max(front_base_load + (front_load_transfer + front_downforce) * inv_num_front_wheels, 0.0);
      auto rear_traction_limit = std::max(rear_base_load + (rear_load_transfer + rear_downforce) * inv_num_rear_wheels, 0.0);

      auto throttle_rate = car.control_state(Control::Throttle) / 255.0;
      auto braking_rate = car.control_state(Control::Brake) / 255.0;
      auto turning_left_rate = car.control_state(Control::Left) / 255.0;
      auto turning_right_rate = car.control_state(Control::Right) / 255.0;
      auto turning_rate = -turning_left_rate + turning_right_rate;
      auto net_throttle = throttle_rate - braking_rate;

      auto inv_frame_duration = 1.0 / frame_duration;

      auto front_steering = 0.5 - handling.steering_balance;
      auto rear_steering = 0.5 + handling.steering_balance;
      auto steering_multiplier = 1.0 / std::max(front_steering, rear_steering);
      front_steering *= steering_multiplier;
      rear_steering *= steering_multiplier;

      auto front_braking = 0.5 - handling.brake_balance;
      auto rear_braking = 0.5 + handling.brake_balance;
      auto braking_multiplier = 1.0 / std::max(front_steering, rear_steering);
      front_braking *= braking_multiplier;
      rear_braking *= braking_multiplier;

      auto front_acceleration = 0.0;
      auto rear_acceleration = 0.0;
      if (handling.front_driven && handling.rear_driven)
      {
        front_acceleration = 0.5 * inv_num_front_wheels;
        rear_acceleration = 0.5 * inv_num_rear_wheels;
      }

      else if (handling.front_driven)
      {
        front_acceleration = inv_num_front_wheels;
      }

      else if (handling.rear_driven)
      {
        rear_acceleration = inv_num_rear_wheels;
      }

      auto num_gears = static_cast<int>(handling.gear_ratios.size());
      if (handling_state.current_gear >= 0 && (speed <= 0.0001 || local_heading.y > 0.7) && net_throttle < 0.0)
      {
        handling_state.current_gear = -1;
      }

      if (handling_state.current_gear <= 0 && (speed <= 0.0001 || local_heading.y < -0.7) && net_throttle > 0.0)
      {
        auto gear = 0;

        while (gear < num_gears && speed * handling.gear_ratios[gear] >= handling.max_engine_revs * 0.8)
        {
          ++gear;
        }

        handling_state.current_gear = gear + 1;
      }

      if (handling_state.gear_shift_state != 0)
      {
        auto new_gear = handling_state.current_gear;
        if (handling_state.gear_shift_state > 0) ++new_gear;
        else --new_gear;

        auto old_ratio = handling.gear_ratios[handling_state.current_gear - 1];
        auto new_ratio = handling.gear_ratios[new_gear - 1];

        auto frame_progress = 1.0 / handling.gear_shift_duration;
        auto progress = 1.0 - (std::abs(handling_state.gear_shift_state) * frame_progress);

        auto current_ratio = interpolate_linearly(old_ratio, new_ratio, progress);
        auto updated_ratio = interpolate_linearly(old_ratio, new_ratio, progress + frame_progress);

        handling_state.engine_rev_speed /= current_ratio;
        handling_state.engine_rev_speed *= updated_ratio;

        if (handling_state.gear_shift_state > 0) --handling_state.gear_shift_state;
        else ++handling_state.gear_shift_state;

        if (handling_state.gear_shift_state == 0)
        {
          handling_state.current_gear = new_gear;
        }
      }

      else if (handling_state.engine_rev_speed >= 0.99)
      {
        if (handling_state.current_gear >= 1 && handling_state.current_gear < num_gears)
        {
          handling_state.gear_shift_state = handling.gear_shift_duration;
        }
      }

      else if (handling_state.engine_rev_speed < 0.8 && handling_state.current_gear > 1)
      {
        auto prev_gear = handling_state.current_gear - 1;
        auto new_revs = handling_state.engine_rev_speed;
        new_revs /= handling.gear_ratios[handling_state.current_gear - 1];
        new_revs *= handling.gear_ratios[prev_gear - 1];
        if (new_revs < 0.8)
        {
          handling_state.gear_shift_state = -handling.gear_shift_duration;
        }
      }

      auto gear_ratio = 1.0;
      if (handling_state.current_gear < 0)
      {
        net_throttle = -net_throttle;
        gear_ratio = -handling.reverse_gear_ratio;
      }

      else if (handling_state.current_gear == 0)
      {
        gear_ratio = 0.0;
      }

      else if (handling_state.current_gear - 1 < num_gears)
      {
        gear_ratio = handling.gear_ratios[handling_state.current_gear - 1];
      }

      if (handling_state.gear_shift_state == 0)
      {
        handling_state.engine_rev_speed = std::min(speed * std::abs(gear_ratio) / handling.max_engine_revs, 1.05);
      }

      else
      {
        gear_ratio = 0.0;
      }

      auto throttle_factor = 1.0;
      if (handling_state.engine_rev_speed > 1.0)
      {
        throttle_factor = 0.0;
      }

      auto acceleration_force_1d = std::max(net_throttle, 0.0) * handling.max_acceleration_force * gear_ratio * throttle_factor;
      auto braking_force_1d = std::max(-net_throttle, 0.0) * handling.max_braking_force;

      struct WheelState
      {
        Vector2d pos;
        Vector2d velocity;
        Vector2d bias;
        Vector2d wheel_facing;
        double heading_angle;
        double traction_limit;
        double vertical_load;
        double acceleration;
        double braking;
        double cornering;
        double max_steering_angle;
        double acceleration_force;
        double braking_force;
        double cornering_force;
        double slide_ratio;

        resources::TerrainDefinition terrain;
      };

      auto throttle_bias = 1.0;
      auto brake_bias = 1.0;
      auto cornering_bias = 1.0;
      auto antislide_bias = 1.0;
      auto longitudinal_bias = net_throttle >= 0.0 ? throttle_bias : brake_bias;

      auto cornering_bias_2d = normalize(make_vector2(cornering_bias, longitudinal_bias));
      auto antislide_bias_2d = normalize(make_vector2(cornering_bias, longitudinal_bias));

      boost::container::small_vector<WheelState, 4> wheel_states;
      for (auto p : front_wheel_positions)
      {
        WheelState ws{};
        ws.pos = p;
        ws.traction_limit = front_traction_limit;
        ws.acceleration = front_acceleration;
        ws.braking = front_braking;
        ws.cornering = handling.cornering;
        ws.max_steering_angle = front_steering * degrees(handling.max_steering_angle).radians();
        ws.bias = cornering_bias_2d;
        wheel_states.push_back(ws);
      }

      for (auto p : rear_wheel_positions)
      {
        WheelState ws{};
        ws.pos = p;
        ws.traction_limit = rear_traction_limit;
        ws.acceleration = rear_acceleration;
        ws.braking = rear_braking;
        ws.cornering = handling.cornering;
        ws.max_steering_angle = -rear_steering * degrees(handling.max_steering_angle).radians();
        ws.bias = antislide_bias_2d;
        wheel_states.push_back(ws);
      }

      auto max_cornering_multiplier = speed * mass * inv_num_wheels * inv_frame_duration;
      auto is_turning = std::abs(turning_rate) >= 0.0001;

      auto adjusted_steering_rate = 0.0;

      for (auto& ws : wheel_states)
      {
        auto global_pos = car.position() + transform_point(ws.pos, transform);

        ws.vertical_load = ws.traction_limit;
        ws.terrain = world.terrain_at(global_pos, car.z_level());
        ws.velocity = local_velocity + angular_velocity * make_vector2(-ws.pos.y, ws.pos.x);
        ws.heading_angle = std::atan2(ws.velocity.x, -ws.velocity.y);

        if (is_turning && std::abs(ws.max_steering_angle) >= 0.00001)
        {
          auto max_angle = turning_rate < 0.0 ? -ws.max_steering_angle : ws.max_steering_angle;
          auto desired_angle = degrees(handling.non_slide_angle);
          if (max_angle < 0.0) desired_angle = -desired_angle;

          auto steering_angle = radians(ws.heading_angle) + desired_angle;
          if (std::abs(steering_angle.degrees()) >= 90.0)
          {
            steering_angle -= degrees(180.0);
            steering_angle.normalize();
          }

          auto steering_rate = std::min(steering_angle.radians() / max_angle, 1.0);
          if (steering_rate > adjusted_steering_rate)
          {
            adjusted_steering_rate = steering_rate;
          }
        }
      }

      turning_rate *= adjusted_steering_rate;

      auto pedal_adjustment = 1.0;
      for (auto& ws : wheel_states)
      {
        ws.wheel_facing = make_vector2(0.0, -1.0);
        auto steering_angle = radians(turning_rate * ws.max_steering_angle);
        if (std::abs(steering_angle.radians()) >= 0.00001)
        {
          ws.wheel_facing = transform_point(ws.wheel_facing, steering_angle);
        }

        auto slip_angle = radians(ws.heading_angle) - steering_angle;
        if (std::abs(slip_angle.degrees()) >= 90.0)
        {
          slip_angle -= degrees(180.0);
          slip_angle.normalize();
        }

        auto slip_degrees = std::abs(slip_angle.degrees());

        ws.slide_ratio = 1.0;
        auto cornering_ratio = 1.0;
        if (slip_degrees < handling.full_slide_angle)
        {
          if (slip_degrees < handling.non_slide_angle)
          {
            //cornering_ratio = slip_degrees / handling.non_slide_angle;
            ws.slide_ratio = 0.0;
          }

          else
          {
            ws.slide_ratio = (slip_degrees - handling.non_slide_angle) /
              (handling.full_slide_angle - handling.non_slide_angle);
            ws.slide_ratio *= ws.slide_ratio;
          }
        }

        ws.traction_limit *= interpolate_linearly(ws.terrain.traction,
                                                  handling.sliding_grip * ws.terrain.sliding_traction,
                                                  ws.slide_ratio);

        auto facing = ws.wheel_facing;
        if (ws.velocity.y > 0.0) facing = -facing;

        auto max_cornering = std::abs(slip_angle.radians()) * max_cornering_multiplier;
        ws.cornering_force = std::min(cornering_ratio * ws.cornering * ws.traction_limit, max_cornering);

        auto braking_factor = facing.y * facing.y;
        ws.acceleration_force = ws.acceleration * acceleration_force_1d;
        ws.braking_force = ws.braking * braking_force_1d * braking_factor;

        auto longitudinal_force = (std::abs(ws.acceleration_force) + ws.braking_force) * pedal_adjustment;
        auto applied_force = make_vector2(ws.cornering_force, longitudinal_force);

        if (magnitude_squared(applied_force) > ws.traction_limit * ws.traction_limit)
        {
          auto reduce_x = applied_force.x > ws.bias.x * ws.traction_limit;
          auto reduce_y = applied_force.y > ws.bias.y * ws.traction_limit;

          if (reduce_y)
          {
            if (reduce_x)
            {
              pedal_adjustment = (ws.traction_limit * ws.bias.y) /
                (std::abs(ws.acceleration_force) + ws.braking_force);
            }

            else
            {
              auto rem = ws.traction_limit * ws.traction_limit - ws.cornering_force * ws.cornering_force;
              pedal_adjustment = std::sqrt(rem) / (std::abs(ws.acceleration_force) + ws.braking_force);
            }
          }
        }
      }

      handling_state.wheel_states.clear();
      auto net_force = make_vector2(0.0, 0.0);
      for (auto& ws : wheel_states)
      {
        {
          auto longitudinal_force = (std::abs(ws.acceleration_force) + ws.braking_force) * pedal_adjustment;
          auto rem = std::max(ws.traction_limit * ws.traction_limit - longitudinal_force * longitudinal_force, 0.0);

          if (rem < ws.cornering_force * ws.cornering_force)
          {
            ws.cornering_force = std::sqrt(rem);
          }
        }

        auto force = make_vector2(0.0, 0.0);

        auto wheel_heading = normalize(ws.velocity);
        auto target_heading = ws.wheel_facing;
        if (ws.velocity.y > 0.0) target_heading = -target_heading;
        auto slide_direction = normalize(target_heading - wheel_heading);

        auto terrain_cornering_multiplier = ws.terrain.cornering;
        if (&ws - wheel_states.data() >= handling.num_front_wheels)
        {
          terrain_cornering_multiplier = ws.terrain.antislide;
        }

        force += ws.acceleration_force * pedal_adjustment * ws.wheel_facing * ws.terrain.acceleration;
        force += ws.braking_force * pedal_adjustment * -target_heading * ws.terrain.braking;
        force += ws.cornering_force * slide_direction * terrain_cornering_multiplier;

        auto rolling_resistance = (1.0 - ws.slide_ratio) * handling.rolling_drag_coefficient *
          ws.vertical_load * ws.terrain.rolling_resistance;

        force += rolling_resistance * -wheel_heading;

        force += ws.terrain.roughness * mass * -ws.velocity * inv_num_wheels;

        car.apply_force(force, ws.pos);

        net_force += force;

        HandlingState::WheelState stored_info;
        stored_info.pos = car.position() + transform_point(ws.pos, transform);
        stored_info.slide_ratio = ws.slide_ratio;
        stored_info.terrain_color = ws.terrain.color;
        stored_info.terrain_roughness = ws.terrain.roughness;
        stored_info.speed = dot_product(wheel_heading, ws.velocity);
        handling_state.wheel_states.push_back(stored_info);
      }

      auto drag = speed * -local_velocity * handling.drag_coefficient;
      car.apply_force(drag, center_of_mass);
      net_force += drag;

      auto torque_effect = car.applied_torque() * inv_moment * mass;
      auto min_lateral_force = net_force.x + -wheel_states.front().pos.y * torque_effect;
      auto max_lateral_force = min_lateral_force;
      for (std::uint32_t idx = 1; idx < wheel_states.size(); ++idx)
      {
        auto lateral_force = net_force.x + -wheel_states[idx].pos.y * torque_effect;
        if (lateral_force < min_lateral_force)
        {
          min_lateral_force = lateral_force;
        }

        if (lateral_force > max_lateral_force)
        {
          max_lateral_force = lateral_force;
        }
      }

      if (std::signbit(min_lateral_force) == std::signbit(max_lateral_force))
      {
        auto m = (max_lateral_force - min_lateral_force) * 0.5;
        auto f = clamp(min_lateral_force < 0.0 ? -max_lateral_force : -min_lateral_force, -m, m);
        car.apply_force(make_vector2(f, 0.0), center_of_mass);
        net_force.x += f;
      }

      angular_velocity -= angular_velocity * handling.angular_damping * frame_duration;
      car.set_angular_velocity(angular_velocity);
      handling_state.net_force = net_force;
      return handling_state;
    }
  }
}


// GMTL is (C) Copyright 2001-2010 by Allen Bierbaum
// Distributed under the GNU Lesser General Public License 2.1 with an
// addendum covering inlined code. (See accompanying files LICENSE and
// LICENSE.addendum or http://www.gnu.org/copyleft/lesser.txt)

// Includes ====================================================================
#include <boost/python.hpp>
#include <gmtl-VecOps.h>

// Using =======================================================================
using namespace boost::python;

// Declarations ================================================================

namespace  {

BOOST_PYTHON_FUNCTION_OVERLOADS(isNormalized_overloads_1_2, gmtl::isNormalized, 1, 2)

}// namespace

// Module ======================================================================
void _Export_gmtl_VecOps_h()
{
    def("cross", (gmtl::Vec<double,3> & (*)(gmtl::Vec<double,3> &, const gmtl::Vec<double,3> &, const gmtl::Vec<double,3> &))&gmtl::cross, return_internal_reference< 1 >());
    def("cross", (gmtl::Vec<float,3> & (*)(gmtl::Vec<float,3> &, const gmtl::Vec<float,3> &, const gmtl::Vec<float,3> &))&gmtl::cross, return_internal_reference< 1 >());
    def("cross", (gmtl::Vec<int,3> & (*)(gmtl::Vec<int,3> &, const gmtl::Vec<int,3> &, const gmtl::Vec<int,3> &))&gmtl::cross, return_internal_reference< 1 >());
    def("dot", (double (*)(const gmtl::VecBase<double,4> &, const gmtl::VecBase<double,4> &))&gmtl::dot);
    def("dot", (double (*)(const gmtl::VecBase<double,3> &, const gmtl::VecBase<double,3> &))&gmtl::dot);
    def("dot", (double (*)(const gmtl::VecBase<double,2> &, const gmtl::VecBase<double,2> &))&gmtl::dot);
    def("dot", (float (*)(const gmtl::VecBase<float,4> &, const gmtl::VecBase<float,4> &))&gmtl::dot);
    def("dot", (float (*)(const gmtl::VecBase<float,3> &, const gmtl::VecBase<float,3> &))&gmtl::dot);
    def("dot", (float (*)(const gmtl::VecBase<float,2> &, const gmtl::VecBase<float,2> &))&gmtl::dot);
    def("dot", (int (*)(const gmtl::VecBase<int,4> &, const gmtl::VecBase<int,4> &))&gmtl::dot);
    def("dot", (int (*)(const gmtl::VecBase<int,3> &, const gmtl::VecBase<int,3> &))&gmtl::dot);
    def("dot", (int (*)(const gmtl::VecBase<int,2> &, const gmtl::VecBase<int,2> &))&gmtl::dot);
    def("isNormalized", (bool (*)(const gmtl::Vec<double,4> &, const double))&gmtl::isNormalized, isNormalized_overloads_1_2());
    def("isNormalized", (bool (*)(const gmtl::Vec<double,3> &, const double))&gmtl::isNormalized, isNormalized_overloads_1_2());
    def("isNormalized", (bool (*)(const gmtl::Vec<double,2> &, const double))&gmtl::isNormalized, isNormalized_overloads_1_2());
    def("isNormalized", (bool (*)(const gmtl::Vec<float,4> &, const float))&gmtl::isNormalized, isNormalized_overloads_1_2());
    def("isNormalized", (bool (*)(const gmtl::Vec<float,3> &, const float))&gmtl::isNormalized, isNormalized_overloads_1_2());
    def("isNormalized", (bool (*)(const gmtl::Vec<float,2> &, const float))&gmtl::isNormalized, isNormalized_overloads_1_2());
    def("isNormalized", (bool (*)(const gmtl::Vec<int,4> &, const int))&gmtl::isNormalized, isNormalized_overloads_1_2());
    def("isNormalized", (bool (*)(const gmtl::Vec<int,3> &, const int))&gmtl::isNormalized, isNormalized_overloads_1_2());
    def("isNormalized", (bool (*)(const gmtl::Vec<int,2> &, const int))&gmtl::isNormalized, isNormalized_overloads_1_2());
    def("length", (double (*)(const gmtl::Vec<double,4> &))&gmtl::length);
    def("length", (double (*)(const gmtl::Vec<double,3> &))&gmtl::length);
    def("length", (double (*)(const gmtl::Vec<double,2> &))&gmtl::length);
    def("length", (float (*)(const gmtl::Vec<float,4> &))&gmtl::length);
    def("length", (float (*)(const gmtl::Vec<float,3> &))&gmtl::length);
    def("length", (float (*)(const gmtl::Vec<float,2> &))&gmtl::length);
    def("length", (int (*)(const gmtl::Vec<int,4> &))&gmtl::length);
    def("length", (int (*)(const gmtl::Vec<int,3> &))&gmtl::length);
    def("length", (int (*)(const gmtl::Vec<int,2> &))&gmtl::length);
    def("lengthSquared", (double (*)(const gmtl::Vec<double,4> &))&gmtl::lengthSquared);
    def("lengthSquared", (double (*)(const gmtl::Vec<double,3> &))&gmtl::lengthSquared);
    def("lengthSquared", (double (*)(const gmtl::Vec<double,2> &))&gmtl::lengthSquared);
    def("lengthSquared", (float (*)(const gmtl::Vec<float,4> &))&gmtl::lengthSquared);
    def("lengthSquared", (float (*)(const gmtl::Vec<float,3> &))&gmtl::lengthSquared);
    def("lengthSquared", (float (*)(const gmtl::Vec<float,2> &))&gmtl::lengthSquared);
    def("lengthSquared", (int (*)(const gmtl::Vec<int,4> &))&gmtl::lengthSquared);
    def("lengthSquared", (int (*)(const gmtl::Vec<int,3> &))&gmtl::lengthSquared);
    def("lengthSquared", (int (*)(const gmtl::Vec<int,2> &))&gmtl::lengthSquared);
    def("lerp", (gmtl::VecBase<double,4> & (*)(gmtl::VecBase<double,4> &, const double &, const gmtl::VecBase<double,4> &, const gmtl::VecBase<double,4> &))&gmtl::lerp, return_internal_reference< 1 >());
    def("lerp", (gmtl::VecBase<double,3> & (*)(gmtl::VecBase<double,3> &, const double &, const gmtl::VecBase<double,3> &, const gmtl::VecBase<double,3> &))&gmtl::lerp, return_internal_reference< 1 >());
    def("lerp", (gmtl::VecBase<double,2> & (*)(gmtl::VecBase<double,2> &, const double &, const gmtl::VecBase<double,2> &, const gmtl::VecBase<double,2> &))&gmtl::lerp, return_internal_reference< 1 >());
    def("lerp", (gmtl::VecBase<float,4> & (*)(gmtl::VecBase<float,4> &, const float &, const gmtl::VecBase<float,4> &, const gmtl::VecBase<float,4> &))&gmtl::lerp, return_internal_reference< 1 >());
    def("lerp", (gmtl::VecBase<float,3> & (*)(gmtl::VecBase<float,3> &, const float &, const gmtl::VecBase<float,3> &, const gmtl::VecBase<float,3> &))&gmtl::lerp, return_internal_reference< 1 >());
    def("lerp", (gmtl::VecBase<float,2> & (*)(gmtl::VecBase<float,2> &, const float &, const gmtl::VecBase<float,2> &, const gmtl::VecBase<float,2> &))&gmtl::lerp, return_internal_reference< 1 >());
    def("lerp", (gmtl::VecBase<int,4> & (*)(gmtl::VecBase<int,4> &, const int &, const gmtl::VecBase<int,4> &, const gmtl::VecBase<int,4> &))&gmtl::lerp, return_internal_reference< 1 >());
    def("lerp", (gmtl::VecBase<int,3> & (*)(gmtl::VecBase<int,3> &, const int &, const gmtl::VecBase<int,3> &, const gmtl::VecBase<int,3> &))&gmtl::lerp, return_internal_reference< 1 >());
    def("lerp", (gmtl::VecBase<int,2> & (*)(gmtl::VecBase<int,2> &, const int &, const gmtl::VecBase<int,2> &, const gmtl::VecBase<int,2> &))&gmtl::lerp, return_internal_reference< 1 >());
    def("normalize", (double (*)(gmtl::Vec<double,4> &))&gmtl::normalize);
    def("normalize", (double (*)(gmtl::Vec<double,3> &))&gmtl::normalize);
    def("normalize", (double (*)(gmtl::Vec<double,2> &))&gmtl::normalize);
    def("normalize", (float (*)(gmtl::Vec<float,4> &))&gmtl::normalize);
    def("normalize", (float (*)(gmtl::Vec<float,3> &))&gmtl::normalize);
    def("normalize", (float (*)(gmtl::Vec<float,2> &))&gmtl::normalize);
    def("normalize", (int (*)(gmtl::Vec<int,4> &))&gmtl::normalize);
    def("normalize", (int (*)(gmtl::Vec<int,3> &))&gmtl::normalize);
    def("normalize", (int (*)(gmtl::Vec<int,2> &))&gmtl::normalize);
    def("reflect", (gmtl::VecBase<double,4> & (*)(gmtl::VecBase<double,4> &, const gmtl::VecBase<double,4> &, const gmtl::Vec<double,4> &))&gmtl::reflect, return_internal_reference< 1 >());
    def("reflect", (gmtl::VecBase<double,3> & (*)(gmtl::VecBase<double,3> &, const gmtl::VecBase<double,3> &, const gmtl::Vec<double,3> &))&gmtl::reflect, return_internal_reference< 1 >());
    def("reflect", (gmtl::VecBase<double,2> & (*)(gmtl::VecBase<double,2> &, const gmtl::VecBase<double,2> &, const gmtl::Vec<double,2> &))&gmtl::reflect, return_internal_reference< 1 >());
    def("reflect", (gmtl::VecBase<float,4> & (*)(gmtl::VecBase<float,4> &, const gmtl::VecBase<float,4> &, const gmtl::Vec<float,4> &))&gmtl::reflect, return_internal_reference< 1 >());
    def("reflect", (gmtl::VecBase<float,3> & (*)(gmtl::VecBase<float,3> &, const gmtl::VecBase<float,3> &, const gmtl::Vec<float,3> &))&gmtl::reflect, return_internal_reference< 1 >());
    def("reflect", (gmtl::VecBase<float,2> & (*)(gmtl::VecBase<float,2> &, const gmtl::VecBase<float,2> &, const gmtl::Vec<float,2> &))&gmtl::reflect, return_internal_reference< 1 >());
    def("reflect", (gmtl::VecBase<int,4> & (*)(gmtl::VecBase<int,4> &, const gmtl::VecBase<int,4> &, const gmtl::Vec<int,4> &))&gmtl::reflect, return_internal_reference< 1 >());
    def("reflect", (gmtl::VecBase<int,3> & (*)(gmtl::VecBase<int,3> &, const gmtl::VecBase<int,3> &, const gmtl::Vec<int,3> &))&gmtl::reflect, return_internal_reference< 1 >());
    def("reflect", (gmtl::VecBase<int,2> & (*)(gmtl::VecBase<int,2> &, const gmtl::VecBase<int,2> &, const gmtl::Vec<int,2> &))&gmtl::reflect, return_internal_reference< 1 >());
    def("isEqual", (bool (*)(const gmtl::VecBase<double,4> &, const gmtl::VecBase<double,4> &, const double))&gmtl::isEqual);
    def("isEqual", (bool (*)(const gmtl::VecBase<double,3> &, const gmtl::VecBase<double,3> &, const double))&gmtl::isEqual);
    def("isEqual", (bool (*)(const gmtl::VecBase<double,2> &, const gmtl::VecBase<double,2> &, const double))&gmtl::isEqual);
    def("isEqual", (bool (*)(const gmtl::VecBase<float,4> &, const gmtl::VecBase<float,4> &, const float))&gmtl::isEqual);
    def("isEqual", (bool (*)(const gmtl::VecBase<float,3> &, const gmtl::VecBase<float,3> &, const float))&gmtl::isEqual);
    def("isEqual", (bool (*)(const gmtl::VecBase<float,2> &, const gmtl::VecBase<float,2> &, const float))&gmtl::isEqual);
    def("isEqual", (bool (*)(const gmtl::VecBase<int,4> &, const gmtl::VecBase<int,4> &, const int))&gmtl::isEqual);
    def("isEqual", (bool (*)(const gmtl::VecBase<int,3> &, const gmtl::VecBase<int,3> &, const int))&gmtl::isEqual);
    def("isEqual", (bool (*)(const gmtl::VecBase<int,2> &, const gmtl::VecBase<int,2> &, const int))&gmtl::isEqual);
}


#include "interpolate.h"
#include "geoutil.h"
#include "lambert_conformal_grid.h"
#include "latitude_longitude_grid.h"
#include "numerical_functions.h"
#include "point_list.h"
#include "reduced_gaussian_grid.h"
#include "stereographic_grid.h"
#include "util.h"

#include "plugin_factory.h"
#include <Eigen/Dense>

#define HIMAN_AUXILIARY_INCLUDE

#include "querydata.h"

#undef HIMAN_AUXILIARY_INCLUDE

using namespace himan;
using namespace Eigen;

namespace himan
{
namespace interpolate
{
// Function to return identifier for each supported datatype
// In the interpolation weight cache we store weights separately
// for each data type.

template <typename T>
int DataTypeId();

template <>
int DataTypeId<float>()
{
	return 1;
}

template <>
int DataTypeId<double>()
{
	return 2;
}

template <>
int DataTypeId<short>()
{
	return 3;
}
template <>
int DataTypeId<unsigned char>()
{
	return 4;
}

bool IsSupportedGridForRotation(HPGridType type)
{
	switch (type)
	{
		case kRotatedLatitudeLongitude:
		case kStereographic:
		case kLambertConformalConic:
			return true;
		default:
			return false;
	}
}

template <typename T>
bool InterpolateArea(const grid* baseGrid, std::shared_ptr<info<T>> source)
{
	if (!source)
	{
		return false;
	}

#ifdef HAVE_CUDA
	if (source->PackedData()->HasData())
	{
		util::Unpack<T>({source});
	}
#endif

	base<T> target;
	target.grid = std::shared_ptr<himan::grid>(baseGrid->Clone());

	if (baseGrid->Class() == kRegularGrid)
	{
		target.data.Resize(dynamic_cast<const regular_grid*>(baseGrid)->Ni(),
		                   dynamic_cast<const regular_grid*>(baseGrid)->Nj());
	}
	else if (baseGrid->Class() == kIrregularGrid)
	{
		target.data.Resize(baseGrid->Size(), 1);
	}

	auto method = InterpolationMethod(source->Param().Name(), source->Param().InterpolationMethod());

	logger logr("interpolation");
	logr.Trace(fmt::format("Grid interpolation with method '{}'", HPInterpolationMethodToString.at(method)));

	if (interpolate::interpolator<T>().Interpolate(*source->Base(), target, method))
	{
		auto interpGrid = std::shared_ptr<grid>(baseGrid->Clone());

		interpGrid->UVRelativeToGrid(source->Grid()->UVRelativeToGrid());

		source->Base()->grid = interpGrid;
		source->Base()->data = std::move(target.data);

		return true;
	}

	return false;
}

template bool InterpolateArea<double>(const grid*, std::shared_ptr<info<double>>);
template bool InterpolateArea<float>(const grid*, std::shared_ptr<info<float>>);
template bool InterpolateArea<short>(const grid*, std::shared_ptr<info<short>>);
template bool InterpolateArea<unsigned char>(const grid*, std::shared_ptr<info<unsigned char>>);

template <typename T>
bool ReorderPoints(const grid* baseGrid, std::shared_ptr<info<T>> info)
{
	if (!info)
	{
		return false;
	}

	// Worst case: cartesian product ie O(n^2)

	auto targetStations = dynamic_cast<const point_list*>(baseGrid)->Stations();
	auto sourceStations = std::dynamic_pointer_cast<point_list>(info->Grid())->Stations();
	const auto& sourceData = info->Data();
	matrix<T> newData(targetStations.size(), 1, 1, MissingValue<T>());

	if (targetStations.size() == 0 || sourceStations.size() == 0)
	{
		return false;
	}

	std::vector<station> newStations;

	for (size_t i = 0; i < targetStations.size(); i++)
	{
		station s1 = targetStations[i];

		bool found = false;

		for (size_t j = 0; j < sourceStations.size() && !found; j++)
		{
			station s2 = sourceStations[j];

			if (s1.Id() == s2.Id())
			{
				newStations.push_back(s1);
				newData.Set(i, sourceData.At(j));

				found = true;
			}
		}

		if (!found)
		{
			// throw std::runtime_error("Failed, source data does not contain all the same points as target");
			return false;
		}
	}

	std::dynamic_pointer_cast<point_list>(info->Grid())->Stations(newStations);

	auto b = info->Base();
	b->data = std::move(newData);

	return true;
}

template bool ReorderPoints<double>(const grid*, std::shared_ptr<info<double>>);
template bool ReorderPoints<float>(const grid*, std::shared_ptr<info<float>>);
template bool ReorderPoints<short>(const grid*, std::shared_ptr<info<short>>);
template bool ReorderPoints<unsigned char>(const grid*, std::shared_ptr<info<unsigned char>>);

template <typename T>
bool Interpolate(const grid* baseGrid, const std::vector<std::shared_ptr<info<T>>>& infos)
{
	for (const auto& info : infos)
	{
		bool needInterpolation = false;
		bool needPointReordering = false;

		/*
		 * Possible scenarios:
		 * 1. from regular to regular (basic area&grid interpolation)
		 * 2. from regular to irregular (area to point)
		 * 3. from irregular to irregular (limited functionality, basically just point reordering)
		 * 4. from irregular to regular, not supported, except if source is gaussian
		 */

		// 1.

		if (baseGrid->Class() == kRegularGrid && info->Grid()->Class() == kRegularGrid)
		{
			if (*baseGrid != *info->Grid())
			{
				needInterpolation = true;
			}

			// == operator does not test scanning mode !

			else if (dynamic_cast<const regular_grid*>(baseGrid)->ScanningMode() !=
			         std::dynamic_pointer_cast<regular_grid>(info->Grid())->ScanningMode())
			{
#ifdef HAVE_CUDA
				if (info->PackedData()->HasData())
				{
					// must unpack before swapping
					util::Unpack<T>({info});
				}
#endif
				util::Flip<T>(info->Data());
				auto base = info->Base();
				const bool uv = base->grid->UVRelativeToGrid();
				base->grid = std::shared_ptr<himan::grid>(baseGrid->Clone());
				base->grid->UVRelativeToGrid(uv);
			}
		}

		// 2.

		else if (baseGrid->Class() == kIrregularGrid && info->Grid()->Class() == kRegularGrid)
		{
			needInterpolation = true;
		}

		// 3.

		else if (baseGrid->Class() == kIrregularGrid && info->Grid()->Class() == kIrregularGrid)
		{
			if (*baseGrid != *info->Grid())
			{
				needPointReordering = true;
			}
		}

		// 4.

		else if (baseGrid->Class() == kRegularGrid && info->Grid()->Class() == kIrregularGrid)
		{
			if (info->Grid()->Type() == kReducedGaussian)
			{
				needInterpolation = true;
			}
			else
			{
				throw std::runtime_error("Unable to extrapolate from points to grid");
			}
		}

		if (needInterpolation && InterpolateArea<T>(baseGrid, info) == false)
		{
			return false;
		}
		else if (needPointReordering && ReorderPoints<T>(baseGrid, info) == false)
		{
			return false;
		}
		else if (needInterpolation == false && needPointReordering == false)
		{
			logger logr("interpolate");
			logr.Trace("Areas are equal, no need to interpolate");
		}
	}

	return true;
}

template bool Interpolate<double>(const grid*, const std::vector<std::shared_ptr<info<double>>>&);
template bool Interpolate<float>(const grid*, const std::vector<std::shared_ptr<info<float>>>&);
template bool Interpolate<short>(const grid*, const std::vector<std::shared_ptr<info<short>>>&);
template bool Interpolate<unsigned char>(const grid*, const std::vector<std::shared_ptr<info<unsigned char>>>&);

bool IsVectorComponent(const std::string& paramName)
{
	if (paramName == "U-MS" || paramName == "V-MS" || paramName == "WGU-MS" || paramName == "WGV-MS" ||
	    paramName == "IVELU-MS" || paramName == "IVELV-MS" || paramName == "WVELU-MS" || paramName == "WVELV-MS")
	{
		return true;
	}

	return false;
}

bool IsNumericCodeTable(const std::string& paramName)
{
	if (paramName.find("NWCSAF_CLDTYPE") != std::string::npos ||
	    paramName.find("NWCSAF_CLDMASK") != std::string::npos || paramName == "NWCSAF_CTTH_QC-N" ||
	    paramName == "CLDTYPE-N")
	{
		return true;
	}
	return false;
}

HPInterpolationMethod InterpolationMethod(const std::string& paramName, HPInterpolationMethod interpolationMethod)
{
	// Later we'll add this information to radon directly
	if (interpolationMethod == kBiLinear &&
	    (
	        // vector parameters
	        IsVectorComponent(paramName) || paramName == "DD-D" || paramName == "FF-MS" ||
	        // precipitation
	        paramName == "RR-KGM2" || paramName == "SNR-KGM2" || paramName == "GRI-KGM2" || paramName == "RRR-KGM2" ||
	        paramName == "RRRC-KGM2" || paramName == "RRRL-KGM2" || paramName == "SNRC-KGM2" ||
	        paramName == "SNRL-KGM2" || paramName == "RRRS-KGM2" || paramName == "RR-1-MM" || paramName == "RR-3-MM" ||
	        paramName == "RR-6-MM" || paramName == "RRI-KGM2" || paramName == "SNRI-KGM2" ||
	        paramName == "SNACC-KGM2" ||
	        // symbols
	        paramName == "CLDSYM-N" || paramName == "PRECFORM-N" || paramName == "PRECFORM2-N" ||
	        paramName == "FOGSYM-N" || paramName == "ICING-N" || paramName == "POTPRECT-N" ||
	        paramName == "POTPRECF-N" || paramName == "FOGINT-N" || paramName == "PRECTYPE-N" ||
	        // code tables
	        IsNumericCodeTable(paramName)))
	{
#ifdef DEBUG
		std::cout << "Debug::interpolation Switching interpolation method from bilinear to nearest point" << std::endl;
#endif
		return kNearestPoint;  // nearest point in himan and newbase
	}

	return interpolationMethod;
}

template <typename T>
void RotateVectorComponentsCPU(const grid* from, const grid* to, himan::matrix<T>& U, himan::matrix<T>& V)
{
	// First convert to earth relative

	logger log("interpolate");

	if (from->UVRelativeToGrid() && from->Type() != kLatitudeLongitude)
	{
		log.Trace("Rotating from " + HPGridTypeToString.at(from->Type()) + " to earth relative");

		switch (from->Type())  // source type
		{
			case kLatitudeLongitude:
				break;
			case kRotatedLatitudeLongitude:
			{
				const auto rll = dynamic_cast<const rotated_latitude_longitude_grid*>(from);
				point southPole = rll->SouthPole();

				if (southPole.Y() > 0)
				{
					southPole.Y(-southPole.Y());
					southPole.X(0);
				}

				for (size_t i = 0; i < U.Size(); i++)
				{
					T u = U[i];
					T v = V[i];

					const point rotPoint = rll->RotatedLatLon(i);
					const point regPoint = rll->LatLon(i);

					// Algorithm by J.E. HAUGEN (HIRLAM JUNE -92), modified by K. EEROLA
					// Algorithm originally defined in hilake/TURNDD.F

					const double southPoleY = constants::kDeg * (southPole.Y() + 90);

					double sinPoleY, cosPoleY;
					sincos(southPoleY, &sinPoleY, &cosPoleY);

					const double cosRegY = cos(constants::kDeg * regPoint.Y());  // zcyreg
					const double zxmxc = constants::kDeg * (regPoint.X() - southPole.X());

					double sinxmxc, cosxmxc;
					sincos(zxmxc, &sinxmxc, &cosxmxc);

					const double rotXRad = constants::kDeg * rotPoint.X();
					const double rotYRad = constants::kDeg * rotPoint.Y();

					double sinRotX, cosRotX;
					sincos(rotXRad, &sinRotX, &cosRotX);

					double sinRotY, cosRotY;
					sincos(rotYRad, &sinRotY, &cosRotY);

					const double PA = cosxmxc * cosRotX + cosPoleY * sinxmxc * sinRotX;
					const double PB = cosPoleY * sinxmxc * cosRotX * sinRotY + sinPoleY * sinxmxc * cosRotY -
					                  cosxmxc * sinRotX * sinRotY;
					const double PC = (-sinPoleY) * sinRotX / cosRegY;
					const double PD = (cosPoleY * cosRotY - sinPoleY * cosRotX * sinRotY) / cosRegY;

					U[i] = static_cast<T>(PA * u + PB * v);
					V[i] = static_cast<T>(PC * u + PD * v);
				}
			}
			break;

			case kLambertConformalConic:
			{
				auto lcc = dynamic_cast<const lambert_conformal_grid*>(from);
				const double cone = lcc->Cone();
				const double orientation = lcc->Orientation();

				for (size_t i = 0; i < U.Size(); i++)
				{
					T u = U[i];
					T v = V[i];

					// http://www.mcs.anl.gov/~emconsta/wind_conversion.txt

					double angle = from->LatLon(i).X() - orientation;
					ASSERT(angle >= -180 && angle <= 180);

					const double anglex = cone * angle * constants::kDeg;
					double sinx, cosx;
					sincos(anglex, &sinx, &cosx);

					U[i] = static_cast<T>(cosx * u + sinx * v);
					V[i] = static_cast<T>(-1 * sinx * u + cosx * v);
				}
			}
			break;

			case kStereographic:
			{
				// The same as lambert but with cone = 1

				const double orientation = dynamic_cast<const stereographic_grid*>(from)->Orientation();

				for (size_t i = 0; i < U.Size(); i++)
				{
					T u = U[i];
					T v = V[i];

					const double angle = (from->LatLon(i).X() - orientation) * constants::kDeg;
					double sinx, cosx;

					sincos(angle, &sinx, &cosx);

					U[i] = static_cast<T>(cosx * u + sinx * v);
					V[i] = static_cast<T>(-1 * sinx * u + cosx * v);
				}
			}
			break;
			default:
				throw std::runtime_error("Unable to rotate from " + HPGridTypeToString.at(from->Type()) + " to " +
				                         HPGridTypeToString.at(to->Type()));
		}
	}

	if (to->UVRelativeToGrid() == false)
	{
		log.Trace("Result grid has UVRelativeToGrid=false, no need for further rotation");
		return;
	}

	switch (to->Type())
	{
		case kLatitudeLongitude:
			break;

		case kRotatedLatitudeLongitude:
		{
			const auto rll = dynamic_cast<const rotated_latitude_longitude_grid*>(to);
			point southPole = rll->SouthPole();

			if (southPole.Y() > 0)
			{
				southPole.Y(-southPole.Y());
				southPole.X(0);
			}

			for (size_t i = 0; i < U.Size(); i++)
			{
				T u = U[i];
				T v = V[i];

				const point rotPoint = rll->RotatedLatLon(i);
				const point regPoint = rll->LatLon(i);

				// Algorithm by J.E. HAUGEN (HIRLAM JUNE -92), modified by K. EEROLA
				// Algorithm originally defined in hilake/TURNDD.F

				const double southPoleY = constants::kDeg * (southPole.Y() + 90);

				double sinPoleY, cosPoleY;
				sincos(southPoleY, &sinPoleY, &cosPoleY);

				const double sinRegY = sin(constants::kDeg * regPoint.Y());  // zsyreg
				const double cosRegY = cos(constants::kDeg * regPoint.Y());  // zcyreg

				double zxmxc = constants::kDeg * (regPoint.X() - southPole.X());

				double sinxmxc, cosxmxc;
				sincos(zxmxc, &sinxmxc, &cosxmxc);

				const double rotXRad = constants::kDeg * rotPoint.X();

				double sinRotX, cosRotX;
				sincos(rotXRad, &sinRotX, &cosRotX);

				const double cosRotY = cos(constants::kDeg * rotPoint.Y());  // zcyrot

				const double PA = cosPoleY * sinxmxc * sinRotX + cosxmxc * cosRotX;
				const double PB =
				    cosPoleY * cosxmxc * sinRegY * sinRotX - sinPoleY * cosRegY * sinRotX - sinxmxc * sinRegY * cosRotX;
				const double PC = sinPoleY * sinxmxc / cosRotY;
				const double PD = (sinPoleY * cosxmxc * sinRegY + cosPoleY * cosRegY) / cosRotY;

				U[i] = static_cast<T>(PA * u + PB * v);
				V[i] = static_cast<T>(PC * u + PD * v);
			}
		}
		break;

		case kLambertConformalConic:
		{
			auto lcc = dynamic_cast<const lambert_conformal_grid*>(to);
			const double cone = lcc->Cone();
			const double orientation = lcc->Orientation();

			for (size_t i = 0; i < U.Size(); i++)
			{
				T u = U[i];
				T v = V[i];

				// http://www.mcs.anl.gov/~emconsta/wind_conversion.txt

				const double angle = to->LatLon(i).X() - orientation;
				ASSERT(angle >= -180 && angle <= 180);

				const double anglex = cone * angle * constants::kDeg;
				double sinx, cosx;
				sincos(anglex, &sinx, &cosx);

				U[i] = static_cast<T>(cosx * u - sinx * v);
				V[i] = static_cast<T>(sinx * u + cosx * v);
			}
		}
		break;
		case kStereographic:
		{
			const double orientation = dynamic_cast<const stereographic_grid*>(to)->Orientation();

			for (size_t i = 0; i < U.Size(); i++)
			{
				T u = U[i];
				T v = V[i];

				const double angle = (to->LatLon(i).X() - orientation) * constants::kDeg;
				double sinx, cosx;

				sincos(angle, &sinx, &cosx);

				U[i] = static_cast<T>(cosx * u - sinx * v);
				V[i] = static_cast<T>(sinx * u + cosx * v);
			}
		}
		break;

		default:
			throw std::runtime_error("Unable to rotate from " + HPGridTypeToString.at(from->Type()) + " to " +
			                         HPGridTypeToString.at(to->Type()));
	}
}

template void RotateVectorComponentsCPU<double>(const grid*, const grid*, himan::matrix<double>&,
                                                himan::matrix<double>&);
template void RotateVectorComponentsCPU<float>(const grid*, const grid*, himan::matrix<float>&, himan::matrix<float>&);
template void RotateVectorComponentsCPU<short>(const grid*, const grid*, himan::matrix<short>&, himan::matrix<short>&);
template void RotateVectorComponentsCPU<unsigned char>(const grid*, const grid*, himan::matrix<unsigned char>&,
                                                       himan::matrix<unsigned char>&);

template <typename T>
void RotateVectorComponents(const grid* from, const grid* to, himan::info<T>& UInfo, himan::info<T>& VInfo,
                            bool useCuda)
{
	ASSERT(UInfo.Grid()->UVRelativeToGrid() == VInfo.Grid()->UVRelativeToGrid());

	logger log("interpolate");
	if (!UInfo.Grid()->UVRelativeToGrid())
	{
		log.Trace("Source data is not relative to grid -- skipping rotation");
		return;
	}

#ifdef HAVE_CUDA
	if (useCuda)
	{
		if (UInfo.PackedData()->HasData() || VInfo.PackedData()->HasData())
		{
			throw std::runtime_error("Packed data needs to be unpacked before rotation on GPU");
		}

		cudaStream_t stream;
		CUDA_CHECK(cudaStreamCreate(&stream));
		RotateVectorComponentsGPU<T>(from, to, UInfo.Data(), VInfo.Data(), stream, 0, 0, 0);
		CUDA_CHECK(cudaStreamSynchronize(stream));
	}
	else
#endif
	{
		RotateVectorComponentsCPU<T>(from, to, UInfo.Data(), VInfo.Data());
	}

	UInfo.Grid()->UVRelativeToGrid(false);
	VInfo.Grid()->UVRelativeToGrid(false);
}

template void RotateVectorComponents<double>(const grid*, const grid*, info<double>&, info<double>&, bool);
template void RotateVectorComponents<float>(const grid*, const grid*, info<float>&, info<float>&, bool);
template void RotateVectorComponents<short>(const grid*, const grid*, info<short>&, info<short>&, bool);
template void RotateVectorComponents<unsigned char>(const grid*, const grid*, info<unsigned char>&,
                                                    info<unsigned char>&, bool);

template <typename T>
std::pair<std::vector<size_t>, std::vector<T>> InterpolationWeights(reduced_gaussian_grid& source, point target)
{
	// target lon 0 <= lon < 360
	if (target.X() < 0.0)
		target.X(target.X() + 360.0);
	else if (target.X() >= 360.0)
		target.X(target.X() - 360.0);

	const auto lats = source.Latitudes();

	// check if point is inside domain
	if (target.Y() >= lats.front())
	{
		// point north of domain
		return std::make_pair(std::vector<size_t>{0}, std::vector<T>{MissingValue<T>()});
	}

	if (target.Y() <= lats.back())
	{
		// point south of domain
		return std::make_pair(std::vector<size_t>{0}, std::vector<T>{MissingValue<T>()});
	}

	// find y-indices
	auto south = std::lower_bound(lats.begin(), lats.end(), target.Y(), std::greater_equal<double>());
	auto north = south;
	north--;

	size_t y_north = static_cast<size_t>(std::distance(lats.begin(), north));
	size_t y_south = static_cast<size_t>(std::distance(lats.begin(), south));

	// find x-indices
	size_t x_north_west = static_cast<size_t>(
	    std::floor(static_cast<double>(source.NumberOfPointsAlongParallels()[y_north]) * target.X() / 360.));
	size_t x_north_east =
	    x_north_west < static_cast<size_t>(source.NumberOfPointsAlongParallels()[y_north] - 1) ? x_north_west + 1 : 0;
	size_t x_south_west = static_cast<size_t>(
	    std::floor(static_cast<double>(source.NumberOfPointsAlongParallels()[y_south]) * target.X() / 360.));
	size_t x_south_east =
	    x_south_west < static_cast<size_t>(source.NumberOfPointsAlongParallels()[y_south] - 1) ? x_south_west + 1 : 0;

	/*
	 *
	 *  a---------------b
	 *  |               |
	 *   \     p        |
	 *   |              |
	 *   c--------------d
	 *
	 *  Now we know the indices x|y of the four points a,b,c,d that surround p
	 */

	std::vector<size_t> idxs;
	idxs.reserve(4);
	idxs.push_back(source.LocationIndex(x_north_west, y_north));
	idxs.push_back(source.LocationIndex(x_north_east, y_north));
	idxs.push_back(source.LocationIndex(x_south_west, y_south));
	idxs.push_back(source.LocationIndex(x_south_east, y_south));

	// calculate weights by bilinear interpolation to point target (p) surrounded by points A|B|C|D
	const point& p = target;
	const point a = source.LatLon(idxs[0]);
	const point b = source.LatLon(idxs[1]);
	const point c = source.LatLon(idxs[2]);
	const point d = source.LatLon(idxs[3]);

	std::vector<T> weights(4);

	// Matrices and Vectors
	Eigen::Matrix4d A(4, 4);
	Eigen::MatrixXd xi(4, 2);
	Eigen::Vector4d x(4);
	Eigen::Vector2d phi(2);

	// Construct linear system of equations to be solved
	A(0, 0) = 1;
	A(1, 0) = 1;
	A(2, 0) = 1;
	A(3, 0) = 1;
	A(0, 1) = a.X();
	A(1, 1) = b.X() == 0.0 ? 360 : b.X();

	A(2, 1) = c.X();
	A(3, 1) = d.X() == 0.0 ? 360 : d.X();

	A(0, 2) = a.Y();
	A(1, 2) = b.Y();
	A(2, 2) = c.Y();
	A(3, 2) = d.Y();
	A(0, 3) = a.X() * a.Y();
	A(1, 3) = b.X() == 0.0 ? 360 * b.Y() : b.X() * b.Y();
	A(2, 3) = c.X() * c.Y();
	A(3, 3) = d.X() == 0.0 ? 360 * d.Y() : d.X() * d.Y();

	xi(0, 0) = 0;
	xi(1, 0) = 1;
	xi(2, 0) = 0;
	xi(3, 0) = 1;
	xi(0, 1) = 1;
	xi(1, 1) = 1;
	xi(2, 1) = 0;
	xi(3, 1) = 0;

	// Solve linear system
	xi = A.colPivHouseholderQr().solve(xi);

	x[0] = 1;
	x[1] = p.X();
	x[2] = p.Y();
	x[3] = p.X() * p.Y();

	xi.transposeInPlace();
	phi = xi * x;

	weights[0] = static_cast<T>((1 - phi[0]) * phi[1]);
	weights[1] = static_cast<T>(phi[0] * phi[1]);
	weights[2] = static_cast<T>((1 - phi[0]) * (1 - phi[1]));
	weights[3] = static_cast<T>(phi[0] * (1 - phi[1]));

	return std::make_pair(idxs, weights);
}

template <typename T>
std::pair<std::vector<size_t>, std::vector<T>> InterpolationWeights(const regular_grid& source, const point& xy)
{
	if (IsMissing(xy.X()) || IsMissing(xy.Y()))
		return std::make_pair(std::vector<size_t>{0}, std::vector<T>{MissingValue<T>()});

	std::vector<size_t> idxs{static_cast<size_t>(xy.X()) + source.Ni() * static_cast<size_t>(xy.Y()),
	                         static_cast<size_t>(xy.X()) + source.Ni() * static_cast<size_t>(xy.Y()) + 1 -
	                             (static_cast<size_t>(xy.X()) == source.Ni() - 1 ? source.Ni() : 0),
	                         static_cast<size_t>(xy.X()) + source.Ni() * static_cast<size_t>(xy.Y() + 1),
	                         static_cast<size_t>(xy.X()) + source.Ni() * static_cast<size_t>(xy.Y() + 1) + 1 -
	                             (static_cast<size_t>(xy.X()) == source.Ni() - 1 ? source.Ni() : 0)};

	std::vector<T> weights(4);

	weights[0] = static_cast<T>((1 - std::fmod(xy.X(), 1)) * (1 - std::fmod(xy.Y(), 1)));
	weights[1] = static_cast<T>(std::fmod(xy.X(), 1) * (1 - std::fmod(xy.Y(), 1)));
	weights[2] = static_cast<T>((1 - std::fmod(xy.X(), 1)) * std::fmod(xy.Y(), 1));
	weights[3] = static_cast<T>(std::fmod(xy.X(), 1) * std::fmod(xy.Y(), 1));

	// Index is outside grid. This happens when target point is located on bottom edge
	// Set index to first grid point
	for (auto& idx : idxs)
	{
		if (idx > source.Size() - 1)
		{
			idx = 0;
		}
	}

	return std::make_pair(idxs, weights);
}

template <typename T>
std::vector<std::pair<std::vector<size_t>, std::vector<T>>> InterpolationWeights(const regular_grid& source,
                                                                                const std::vector<point>& targets)
{
	std::vector<std::pair<std::vector<size_t>, std::vector<T>>> ret;
	ret.reserve(targets.size());

	for (const auto& xy : targets)
	{
		ret.push_back(InterpolationWeights<T>(source, xy));
	}

	return ret;
}

template <typename T>
std::pair<size_t, T> NearestPoint(reduced_gaussian_grid& source, point target)
{
	// target lon 0 <= lon < 360
	if (target.X() < 0.0)
		target.X(target.X() + 360.0);
	else if (target.X() >= 360.0)
		target.X(target.X() - 360.0);

	const auto lats = source.Latitudes();

	// find y-indices
	auto south = std::lower_bound(lats.begin(), lats.end(), target.Y(), std::greater_equal<double>());
	auto north = south;
	north--;

	size_t y_north = std::distance(lats.begin(), north);
	size_t y_south = std::distance(lats.begin(), south);

	// find x-indices
	size_t x_north_west = static_cast<size_t>(
	    std::floor(static_cast<double>(source.NumberOfPointsAlongParallels()[y_north]) * target.X() / 360.));
	size_t x_north_east =
	    x_north_west < static_cast<size_t>(source.NumberOfPointsAlongParallels()[y_north] - 1) ? x_north_west + 1 : 0;
	size_t x_south_west = static_cast<size_t>(
	    std::floor(static_cast<double>(source.NumberOfPointsAlongParallels()[y_south]) * target.X() / 360.));
	size_t x_south_east =
	    x_south_west < static_cast<size_t>(source.NumberOfPointsAlongParallels()[y_south] - 1) ? x_south_west + 1 : 0;

	size_t nearest = x_north_west;
	for (auto p : {x_north_east, x_south_west, x_south_east})
	{
		if (geoutil::Distance(source.LatLon(p), target) < geoutil::Distance(source.LatLon(nearest), target))
			nearest = p;
	}

	return std::make_pair(nearest, 1.0);
}

template <typename T>
std::pair<size_t, T> NearestPoint(const regular_grid& source, const point& xy)
{
	if (IsMissing(xy.X()) || IsMissing(xy.Y()))
	{
		return std::make_pair(0, MissingValue<T>());
	}
	// In case of point in wrap-around region on global grid
	if (static_cast<size_t>(std::round(xy.X())) == source.Ni())
	{
		return std::make_pair(source.Ni() * static_cast<size_t>(std::round(xy.Y())), 1.0);
	}
	return std::make_pair(
	    static_cast<size_t>(std::round(xy.X())) + source.Ni() * static_cast<size_t>(std::round(xy.Y())), 1.0);
}

template <typename T>
std::vector<std::pair<size_t, T>> NearestPoint(const regular_grid& source, const std::vector<point>& targets)
{
	std::vector<std::pair<size_t, T>> ret;
	ret.reserve(targets.size());

	for (const auto& xy : targets)
	{
		ret.push_back(NearestPoint<T>(source, xy));
	}

	return ret;
}

// area_interpolation class member functions definitions
template <typename T>
area_interpolation<T>::area_interpolation(grid& source, grid& target, HPInterpolationMethod method)
    : itsInterpolation(target.Size(), source.Size())
{
	std::vector<Triplet<T>> coefficients;

	std::string useOldMethod = "no";
	try
	{
		useOldMethod = util::GetEnv("HIMAN_USE_OLD_PROJECTION_METHOD");
	}
	catch (...)
	{
	}

	// default to using new 'bulk' method
	if (source.Class() == target.Class() && source.Class() == kRegularGrid && useOldMethod == "no")
	{
		const regular_grid& sg = dynamic_cast<regular_grid&>(source);
		const regular_grid& tg = dynamic_cast<regular_grid&>(target);

		std::vector<point> xy = sg.XY(tg);
		std::vector<std::pair<std::vector<size_t>, std::vector<T>>> ws;

		if (method == kBiLinear)
		{
			ws = InterpolationWeights<T>(sg, xy);
		}
		else if (method == kNearestPoint)
		{
			auto np = NearestPoint<T>(sg, xy);

			for (size_t i = 0; i < np.size(); i++)
			{
				std::pair<std::vector<size_t>, std::vector<T>> w;
				w.first.push_back(np[i].first);
				w.second.push_back(np[i].second);
				ws.push_back(w);
			}
		}
		else
		{
			throw std::bad_typeid();
		}

		for (size_t i = 0; i < ws.size(); i++)
		{
			const auto& w = ws[i];
			for (size_t j = 0; j < w.first.size(); ++j)
			{
				coefficients.push_back(Triplet<T>(static_cast<int>(i), static_cast<int>(w.first[j]), w.second[j]));
			}
		}
	}
	else
	{
		// compute weights in the interpolation matrix line by line, i.e. point by point on target grid
		for (size_t i = 0; i < target.Size(); ++i)
		{
			std::pair<std::vector<size_t>, std::vector<T>> w;
			switch (source.Type())
			{
				case kLatitudeLongitude:
				case kRotatedLatitudeLongitude:
				case kStereographic:
				case kLambertConformalConic:
				case kLambertEqualArea:
				case kTransverseMercator:
					if (method == kBiLinear)
					{
						w = InterpolationWeights<T>(dynamic_cast<regular_grid&>(source),
						                            dynamic_cast<regular_grid&>(source).XY(target.LatLon(i)));
					}
					else if (method == kNearestPoint)
					{
						auto np = NearestPoint<T>(dynamic_cast<regular_grid&>(source),
						                          dynamic_cast<regular_grid&>(source).XY(target.LatLon(i)));
						w.first.push_back(np.first);
						w.second.push_back(np.second);
					}
					else
					{
						throw std::bad_typeid();
					}
					break;
				case kReducedGaussian:
					if (method == kBiLinear)
					{
						w = InterpolationWeights<T>(dynamic_cast<reduced_gaussian_grid&>(source), target.LatLon(i));
					}
					else if (method == kNearestPoint)
					{
						auto np = NearestPoint<T>(dynamic_cast<reduced_gaussian_grid&>(source), target.LatLon(i));
						w.first.push_back(np.first);
						w.second.push_back(np.second);
					}
					else
					{
						throw std::bad_typeid();
					}
					break;
				default:
					// what to throw?
					throw std::bad_typeid();
					break;
			}

			for (size_t j = 0; j < w.first.size(); ++j)
			{
				coefficients.push_back(Triplet<T>(static_cast<int>(i), static_cast<int>(w.first[j]), w.second[j]));
			}
		}
	}

	itsInterpolation.setFromTriplets(coefficients.begin(), coefficients.end());
}

template <typename T>
void area_interpolation<T>::Interpolate(base<T>& source, base<T>& target)
{
	Map<Matrix<T, Dynamic, Dynamic>> srcValues(source.data.ValuesAsPOD(), source.data.Size(), 1);
	Map<Matrix<T, Dynamic, Dynamic>> trgValues(target.data.ValuesAsPOD(), target.data.Size(), 1);

	trgValues = itsInterpolation * srcValues;
}

template <typename T>
size_t area_interpolation<T>::SourceSize() const
{
	return itsInterpolation.cols();
}

template <typename T>
size_t area_interpolation<T>::TargetSize() const
{
	return itsInterpolation.rows();
}

// Interpolator member functions
template <typename T>
std::map<size_t, area_interpolation<T>> interpolator<T>::cache;

template <typename T>
std::mutex interpolator<T>::interpolatorAccessMutex;

template <typename T>
bool interpolator<T>::Insert(const base<T>& source, const base<T>& target, HPInterpolationMethod method)
{
	std::lock_guard<std::mutex> guard(interpolatorAccessMutex);

	std::pair<size_t, himan::interpolate::area_interpolation<T>> insertValue;

	try
	{
		std::vector<size_t> hashes{method, source.grid->Hash(), target.grid->Hash()};
		insertValue.first = boost::hash_range(hashes.begin(), hashes.end());

		// area_interpolation is already present in cache
		if (cache.count(insertValue.first) > 0)
		{
			return true;
		}

		insertValue.second = himan::interpolate::area_interpolation<T>(*source.grid, *target.grid, method);
	}
	catch (const std::exception& e)
	{
		std::cerr << "Creating interpolation coefficients failed with " << e.what() << "\n";
		himan::Abort();
	}

	return cache.insert(std::move(insertValue)).second;
}

// template bool interpolator::Insert<double>(const base<double>&, const base<double>&, HPInterpolationMethod);
// template bool interpolator::Insert<float>(const base<float>&, const base<float>&, HPInterpolationMethod);

template <typename T>
bool interpolator<T>::Interpolate(base<T>& source, base<T>& target, HPInterpolationMethod method)
{
	std::vector<size_t> hashes{method, source.grid->Hash(), target.grid->Hash()};
	auto it = cache.find(boost::hash_range(hashes.begin(), hashes.end()));

	if (it != cache.end())
	{
		try
		{
			it->second.Interpolate(source, target);
			return true;
		}
		catch (const boost::bad_get& e)
		{
			std::cout << e.what() << std::endl;
			return false;
		}
	}

	else
	{
		Insert(source, target, method);
		return Interpolate(source, target, method);
	}
}

// template bool interpolator::Interpolate<double>(base<double>&, base<double>&, HPInterpolationMethod);
// template bool interpolator::Interpolate<float>(base<float>&, base<float>&, HPInterpolationMethod);

}  // namespace interpolate
}  // namespace himan


/*
 * Copyright (c) 2019, 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:
 *
 *    1. Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *
 *    2. 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.
 *
 *    3. Neither the name of the copyright holder 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 HOLDER 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(s) of this library if you have any questions.
 * Authors: David Fridovich-Keil   ( dfk@eecs.berkeley.edu )
 */

///////////////////////////////////////////////////////////////////////////////
//
// 14D quadrotor dynamics.
//
///////////////////////////////////////////////////////////////////////////////

#include <quads/quadrotor14d.h>
#include <quads/types.h>

#include <ros/ros.h>
#include <Eigen/Dense>
#include <string>
#include <iostream>

namespace quads {

Vector14d Quadrotor14D::operator()(const Vector14d& x,
                                   const Vector4d& u) const {
  ROS_ASSERT(initialized_);

  // Precompute sines/consines.
  const double cpsi = std::cos(x(kPsiIdx));
  const double spsi = std::sin(x(kPsiIdx));
  const double cphi = std::cos(x(kPhiIdx));
  const double sphi = std::sin(x(kPhiIdx));
  const double ctheta = std::cos(x(kThetaIdx));
  const double stheta = std::sin(x(kThetaIdx));

  const double gx = (cpsi * stheta * cphi + spsi * sphi) / m_;
  const double gy = (spsi * stheta * cphi - cpsi * sphi) / m_;
  const double gz = cphi * ctheta / m_;

  Vector14d xdot;
  xdot << x(kDxIdx), x(kDyIdx), x(kDzIdx), x(kQIdx), x(kRIdx), x(kPIdx),
      gx * x(kZetaIdx), gy * x(kZetaIdx), gz * x(kZetaIdx) - 9.81, x(kXiIdx),
      u(0), u(2) / Iy_, u(1) / Ix_, u(3) / Iz_;

  return xdot;
}

Matrix14x14d Quadrotor14D::StateJacobian(const Vector14d& x,
                                         const Vector4d& u) const {
  const double theta = x(kThetaIdx);
  const double psi = x(kPsiIdx);
  const double phi = x(kPhiIdx);
  const double zeta = x(kZetaIdx);

  Matrix14x14d F;
  F << 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
      zeta * std::cos(phi) * std::cos(psi) * std::cos(theta),
      zeta * (std::cos(phi) * std::sin(psi) -
              std::cos(psi) * std::sin(phi) * std::sin(theta)),
      zeta * (std::cos(psi) * std::sin(phi) -
              std::cos(phi) * std::sin(psi) * std::sin(theta)),
      0, 0, 0, std::sin(phi) * std::sin(psi) +
                   std::cos(phi) * std::cos(psi) * std::sin(theta),
      0, 0, 0, 0, 0, 0, 0,
      zeta * std::cos(phi) * std::cos(theta) * std::sin(psi),
      -zeta * (std::cos(phi) * std::cos(psi) +
               std::sin(phi) * std::sin(psi) * std::sin(theta)),
      zeta * (std::sin(phi) * std::sin(psi) +
              std::cos(phi) * std::cos(psi) * std::sin(theta)),
      0, 0, 0, std::cos(phi) * std::sin(psi) * std::sin(theta) -
                   std::cos(psi) * std::sin(phi),
      0, 0, 0, 0, 0, 0, 0, -zeta * std::cos(phi) * std::sin(theta),
      -zeta * std::cos(theta) * std::sin(phi), 0, 0, 0, 0,
      std::cos(phi) * std::cos(theta), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0;

  return F;
}

Matrix6x14d Quadrotor14D::OutputJacobian(const Vector14d& x) const {
  Matrix6x14d H(Matrix6x14d::Zero());
  H(0, 0) = 1.0;
  H(1, 1) = 1.0;
  H(2, 2) = 1.0;
  H(3, 3) = 1.0;
  H(4, 4) = 1.0;
  H(5, 5) = 1.0;
  return H;
}

bool Quadrotor14D::Initialize(const ros::NodeHandle& n) {
  name_ = ros::names::append(n.getNamespace(), "quadrotor14d");

  if (!LoadParameters(n)) {
    ROS_ERROR("%s: Failed to load parameters.", name_.c_str());
    return false;
  }

  initialized_ = true;
  return true;
}

bool Quadrotor14D::LoadParameters(const ros::NodeHandle& n) {
  ros::NodeHandle nl(n);

  // Mass and inertia.
  if (!nl.getParam("dynamics/m", m_)) return false;
  if (!nl.getParam("dynamics/Ix", Ix_)) return false;
  if (!nl.getParam("dynamics/Iy", Iy_)) return false;
  if (!nl.getParam("dynamics/Iz", Iz_)) return false;

  return true;
}

}  // namespace quads


#include <CGAL/Three/Polyhedron_demo_plugin_interface.h>
#include <CGAL/Three/Scene_interface.h>
#include <QApplication>
#include <QObject>
#include <QAction>
#include <QMainWindow>
#include <QInputDialog>
#include <QMessageBox>
#include <QMap>
#include "Messages_interface.h"
#ifdef USE_SURFACE_MESH
#include "Kernel_type.h"
#include "Scene_surface_mesh_item.h"
#else
#include "Scene_polyhedron_item.h"
#include "Polyhedron_type.h"
#endif
#include "Color_ramp.h"
#include "triangulate_primitive.h"
#include <CGAL/Polygon_mesh_processing/bbox.h>
#include <CGAL/Polygon_mesh_processing/distance.h>
#include <CGAL/Polygon_mesh_processing/compute_normal.h>
#include <boost/iterator/counting_iterator.hpp>
#include <CGAL/Search_traits_3.h>
#include <CGAL/Spatial_sort_traits_adapter_3.h>
#include <CGAL/property_map.h>
#include <boost/container/flat_map.hpp>

using namespace CGAL::Three;
namespace PMP = CGAL::Polygon_mesh_processing;

#ifdef USE_SURFACE_MESH
typedef Scene_surface_mesh_item Scene_face_graph_item;
#else
typedef Scene_polyhedron_item Scene_face_graph_item;
#endif

typedef Scene_face_graph_item::Face_graph Face_graph;

#if defined(CGAL_LINKED_WITH_TBB)
template <class AABB_tree, class Point_3>
struct Distance_computation{
  const AABB_tree& tree;
  const std::vector<Point_3>& sample_points;
  Point_3 initial_hint;
  tbb::atomic<double>* distance;
  std::vector<double>& output;

  Distance_computation(const AABB_tree& tree,
                       const Point_3 p,
                       const std::vector<Point_3>& sample_points,
                       tbb::atomic<double>* d,
                       std::vector<double>& out )
    : tree(tree)
    , sample_points(sample_points)
    , initial_hint(p)
    , distance(d)
    , output(out)
  {
  }
  void
  operator()(const tbb::blocked_range<std::size_t>& range) const
  {
    Point_3 hint = initial_hint;
    double hdist = 0;
    for( std::size_t i = range.begin(); i != range.end(); ++i)
    {
      hint = tree.closest_point(sample_points[i], hint);
      Kernel::FT dist = squared_distance(hint,sample_points[i]);
      double d = CGAL::sqrt(dist);
      output[i] = d;
      if (d>hdist) hdist=d;
    }

    if (hdist > distance->load())
      distance->store(hdist);
  }
};
#endif

class Scene_distance_polyhedron_item: public Scene_item
{
  Q_OBJECT
public:
  Scene_distance_polyhedron_item(Face_graph* poly, Face_graph* polyB, QString other_name, int sampling_pts)
    :Scene_item(NbOfVbos,NbOfVaos),
      poly(poly),
      poly_B(polyB),
      are_buffers_filled(false),
      other_poly(other_name)
  {
    nb_pts_per_face = sampling_pts;
    this->setRenderingMode(FlatPlusEdges);
    thermal_ramp.build_thermal();
  }
  bool supportsRenderingMode(RenderingMode m) const {
    return (m == Flat || m == FlatPlusEdges);
  }
  Scene_item* clone() const {return 0;}
  QString toolTip() const {return QString("Item %1 with color indicating distance with %2").arg(this->name()).arg(other_poly);}
  void draw(Viewer_interface *viewer) const
  {
    if(!are_buffers_filled)
    {
      computeElements();
      initializeBuffers(viewer);
      compute_bbox();
    }
    vaos[Facets]->bind();
    attribBuffers(viewer, PROGRAM_WITH_LIGHT);
    program = getShaderProgram(PROGRAM_WITH_LIGHT);
    program->bind();
    program->setUniformValue("is_selected", false);
    viewer->glDrawArrays(GL_TRIANGLES, 0, static_cast<GLsizei>(nb_pos/3));
    program->release();
    vaos[Facets]->release();
  }
  void drawEdges(Viewer_interface* viewer) const
  {
    vaos[Edges]->bind();

    attribBuffers(viewer, PROGRAM_WITHOUT_LIGHT);
    program = getShaderProgram(PROGRAM_WITHOUT_LIGHT);
    program->bind();
    //draw the edges
    program->setAttributeValue("colors", QColor(Qt::black));
    program->setUniformValue("is_selected", false);
    viewer->glDrawArrays(GL_LINES, 0, static_cast<GLsizei>(nb_edge_pos/3));
    vaos[Edges]->release();
    program->release();
  }

  void compute_bbox() const {
    _bbox = PMP::bbox(*poly);
  }

private:
  Face_graph* poly;
  Face_graph* poly_B;
  mutable bool are_buffers_filled;
  QString other_poly;
  mutable std::vector<float> m_vertices;
  mutable std::vector<float> edge_vertices;
  mutable std::vector<float> normals;
  mutable std::vector<float> colors;
  Color_ramp thermal_ramp;
  int nb_pts_per_face;

  enum VAOs {
    Facets=0,
    Edges,
    NbOfVaos};

  enum VBOs {
    Vertices=0,
    Edge_vertices,
    Normals,
    Colors,
    NbOfVbos};

  mutable std::size_t nb_pos;
  mutable std::size_t nb_edge_pos;
  mutable QOpenGLShaderProgram *program;

  //fills 'out' and returns the hausdorff distance for calibration of the color_ramp.

  double compute_distances(const Face_graph& m, const std::vector<Kernel::Point_3>& sample_points,
                           std::vector<double>& out)const
  {
    typedef CGAL::AABB_face_graph_triangle_primitive<Face_graph> Primitive;
    typedef CGAL::AABB_traits<Kernel, Primitive> Traits;
    typedef CGAL::AABB_tree< Traits > Tree;

    Tree tree( faces(m).first, faces(m).second, m);
    tree.accelerate_distance_queries();
    tree.build();
    boost::graph_traits<Face_graph>::vertex_descriptor vd = *(vertices(m).first);
    Traits::Point_3 hint = get(CGAL::vertex_point,*poly, vd);

#if !defined(CGAL_LINKED_WITH_TBB)
    double hdist = 0;
    for(std::size_t i = 0; i<sample_points.size(); ++i)
    {
      hint = tree.closest_point(sample_points[i], hint);
      Kernel::FT dist = squared_distance(hint,sample_points[i]);
      double d = CGAL::sqrt(dist);
      out[i]= d;
      if (d>hdist) hdist=d;
    }
      return hdist;
#else
    tbb::atomic<double> distance;
    distance.store(0);
    Distance_computation<Tree, Kernel::Point_3> f(tree, hint, sample_points, &distance, out);
    tbb::parallel_for(tbb::blocked_range<std::size_t>(0, sample_points.size()), f);
    return distance;
#endif
  }

  void computeElements()const
  {
    QApplication::setOverrideCursor(Qt::WaitCursor);
    m_vertices.resize(0);
    edge_vertices.resize(0);
    normals.resize(0);
    colors.resize(0);

    typedef Kernel::Vector_3	    Vector;
    typedef boost::graph_traits<Face_graph>::face_descriptor   face_descriptor;
    typedef boost::graph_traits<Face_graph>::vertex_descriptor vertex_descriptor;

    typedef boost::property_map<Face_graph,CGAL::vertex_point_t>::type VPmap;
    VPmap vpmap = get(CGAL::vertex_point,*poly);

    //facets
    {
      boost::container::flat_map<face_descriptor, Vector> face_normals_map;
      boost::associative_property_map< boost::container::flat_map<face_descriptor, Vector> >
          nf_pmap(face_normals_map);
      boost::container::flat_map<vertex_descriptor, Vector> vertex_normals_map;
      boost::associative_property_map< boost::container::flat_map<vertex_descriptor, Vector> >
          nv_pmap(vertex_normals_map);

      PMP::compute_normals(*poly, nv_pmap, nf_pmap);
      std::vector<Kernel::Point_3> total_points(0);

      BOOST_FOREACH(boost::graph_traits<Face_graph>::face_descriptor f, faces(*poly)) {
        Vector nf = get(nf_pmap, f);
        typedef FacetTriangulator<Face_graph, Kernel, boost::graph_traits<Face_graph>::vertex_descriptor> FT;
        double diagonal;
        if(this->diagonalBbox() != std::numeric_limits<double>::infinity())
          diagonal = this->diagonalBbox();
        else
          diagonal = 0.0;

        //compute distance with other polyhedron
        //sample facet
        std::vector<Kernel::Point_3> sampled_points;
        std::size_t nb_points =  (std::max)((int)std::ceil(nb_pts_per_face * PMP::face_area(f,*poly,PMP::parameters::geom_traits(Kernel()))),
                                            1);
        Kernel::Point_3 &p = get(vpmap,target(halfedge(f,*poly),*poly));
        Kernel::Point_3 &q = get(vpmap,target(next(halfedge(f,*poly),*poly),*poly));
        Kernel::Point_3 &r = get(vpmap,target(next(next(halfedge(f,*poly),*poly),*poly),*poly));
        CGAL::Random_points_in_triangle_3<Kernel::Point_3> g(p, q, r);
        CGAL::cpp11::copy_n(g, nb_points, std::back_inserter(sampled_points));
        sampled_points.push_back(p);
        sampled_points.push_back(q);
        sampled_points.push_back(r);

        //triangle facets with sample points for color display
        FT triangulation(f,sampled_points,nf,poly,diagonal);

        if(triangulation.cdt->dimension() != 2 )
        {
          qDebug()<<"Error : cdt not right (dimension != 2). Facet not displayed";
          continue;
        }

        //iterates on the internal faces to add the vertices to the positions
        //and the normals to the appropriate vectors

        for(FT::CDT::Finite_faces_iterator
            ffit = triangulation.cdt->finite_faces_begin(),
            end = triangulation.cdt->finite_faces_end();
            ffit != end; ++ffit)
        {
          if(ffit->info().is_external)
            continue;

          for (int i = 0; i<3; ++i)
          {
            total_points.push_back(ffit->vertex(i)->point());
            m_vertices.push_back(ffit->vertex(i)->point().x());
            m_vertices.push_back(ffit->vertex(i)->point().y());
            m_vertices.push_back(ffit->vertex(i)->point().z());

            normals.push_back(nf.x());
            normals.push_back(nf.y());
            normals.push_back(nf.z());
          }
        }
      }
      //compute the distances
      typedef CGAL::Spatial_sort_traits_adapter_3<Kernel,
                CGAL::Pointer_property_map<Kernel::Point_3>::type > Search_traits_3;

      std::vector<double> distances(total_points.size());
      std::vector<std::size_t> indices;
      indices.reserve(total_points.size());
      std::copy(boost::counting_iterator<std::size_t>(0),
                boost::counting_iterator<std::size_t>(total_points.size()),
                std::back_inserter(indices));
      spatial_sort(indices.begin(),
                   indices.end(),
                  Search_traits_3(CGAL::make_property_map(total_points)));
      std::vector<Kernel::Point_3> sorted_points(total_points.size());
      for(std::size_t i = 0; i < sorted_points.size(); ++i)
      {
        sorted_points[i] = total_points[indices[i]];
      }

      double hausdorff = compute_distances(*poly_B,
                                           sorted_points,
                                           distances);
      //compute the colors
      colors.resize(sorted_points.size()*3);
      for(std::size_t i=0; i<sorted_points.size(); ++i)
      {
        std::size_t k = indices[i];
        double d = distances[i]/hausdorff;
        colors[3*k]=thermal_ramp.r(d);
        colors[3*k+1]=thermal_ramp.g(d);
        colors[3*k+2]=thermal_ramp.b(d);
      }
    }

    //edges
    {
      //Lines
      typedef Kernel::Point_3		Point;
      typedef boost::graph_traits<Face_graph>::edge_descriptor	edge_descriptor;

      BOOST_FOREACH(edge_descriptor he, edges(*poly)){
        const Point& a = get(vpmap,target(he,*poly));
        const Point& b = get(vpmap,source(he,*poly));
        {

          edge_vertices.push_back(a.x());
          edge_vertices.push_back(a.y());
          edge_vertices.push_back(a.z());

          edge_vertices.push_back(b.x());
          edge_vertices.push_back(b.y());
          edge_vertices.push_back(b.z());
        }
      }
    }
    QApplication::restoreOverrideCursor();
  }
  void initializeBuffers(Viewer_interface *viewer)const
  {

    program = getShaderProgram(PROGRAM_WITH_LIGHT, viewer);
    program->bind();
    vaos[Facets]->bind();
    buffers[Vertices].bind();
    buffers[Vertices].allocate(m_vertices.data(),
                               static_cast<GLsizei>(m_vertices.size()*sizeof(float)));
    program->enableAttributeArray("vertex");
    program->setAttributeBuffer("vertex",GL_FLOAT,0,3);
    buffers[Vertices].release();
    buffers[Normals].bind();
    buffers[Normals].allocate(normals.data(),
                              static_cast<GLsizei>(normals.size()*sizeof(float)));
    program->enableAttributeArray("normals");
    program->setAttributeBuffer("normals",GL_FLOAT,0,3);
    buffers[Normals].release();
    buffers[Colors].bind();
    buffers[Colors].allocate(colors.data(),
                             static_cast<GLsizei>(colors.size()*sizeof(float)));
    program->enableAttributeArray("colors");
    program->setAttributeBuffer("colors",GL_FLOAT,0,3);
    buffers[Colors].release();
    vaos[Facets]->release();
    program->release();

    program = getShaderProgram(PROGRAM_WITHOUT_LIGHT, viewer);
    program->bind();
    vaos[Edges]->bind();
    buffers[Edge_vertices].bind();
    buffers[Edge_vertices].allocate(edge_vertices.data(),
                                    static_cast<GLsizei>(edge_vertices.size()*sizeof(float)));
    program->enableAttributeArray("vertex");
    program->setAttributeBuffer("vertex",GL_FLOAT,0,3);
    buffers[Edge_vertices].release();
    vaos[Facets]->release();
    program->release();

    nb_pos = m_vertices.size();
    m_vertices.resize(0);
    //"Swap trick" insures that the memory is indeed freed and not kept available
    std::vector<float>(m_vertices).swap(m_vertices);
    nb_edge_pos = edge_vertices.size();
    edge_vertices.resize(0);
    std::vector<float>(edge_vertices).swap(edge_vertices);
    normals.resize(0);
    std::vector<float>(normals).swap(normals);
    colors.resize(0);
    std::vector<float>(colors).swap(colors);
    are_buffers_filled = true;
  }
};
class DistancePlugin :
    public QObject,
    public Polyhedron_demo_plugin_interface
{
  Q_OBJECT
  Q_INTERFACES(CGAL::Three::Polyhedron_demo_plugin_interface)
  Q_PLUGIN_METADATA(IID "com.geometryfactory.PolyhedronDemo.PluginInterface/1.0")

  typedef Kernel::Point_3 Point_3;
public:
  //decides if the plugin's actions will be displayed or not.
  bool applicable(QAction*) const
  {
    return scene->selectionIndices().size() == 2 &&
        qobject_cast<Scene_face_graph_item*>(scene->item(scene->selectionIndices().first())) &&
        qobject_cast<Scene_face_graph_item*>(scene->item(scene->selectionIndices().last()));
  }
  //the list of the actions of the plugin.
  QList<QAction*> actions() const
  {
    return _actions;
  }
  //this acts like a constructor for the plugin. It gets the references to the mainwindow and the scene, and connects the action.
  void init(QMainWindow* mw, Scene_interface* sc, Messages_interface* mi)
  {
    //gets the reference to the message interface, to display text in the console widget
    this->messageInterface = mi;
    //get the references
    this->scene = sc;
    this->mw = mw;
    //creates the action
    QAction *actionComputeDistance= new QAction(QString("Compute Distance Between Polyhedra"), mw);
    //specifies the subMenu
    actionComputeDistance->setProperty("subMenuName", "Polygon Mesh Processing");
    //links the action
    if(actionComputeDistance) {
      connect(actionComputeDistance, SIGNAL(triggered()),
              this, SLOT(createDistanceItems()));
      _actions << actionComputeDistance;
    }
  }
public Q_SLOTS:
  void createDistanceItems()
  {
    bool ok = false;
    nb_pts_per_face = QInputDialog::getInt(mw, tr("Sampling"),
                                               tr("Number of points per face:"),40, 1,2147483647,1, &ok);
    if (!ok)
      return;

    //check the initial conditions
    Scene_face_graph_item* itemA = qobject_cast<Scene_face_graph_item*>(scene->item(scene->selectionIndices().first()));
    Scene_face_graph_item* itemB = qobject_cast<Scene_face_graph_item*>(scene->item(scene->selectionIndices().last()));
    if(! CGAL::is_triangle_mesh(*itemA->polyhedron()) ||
       !CGAL::is_triangle_mesh(*itemB->polyhedron()) ){
      messageInterface->error(QString("Distance not computed. (Both polyhedra must be triangulated)"));
      return;
    }
    QApplication::setOverrideCursor(Qt::WaitCursor);
    Scene_distance_polyhedron_item* new_itemA = new Scene_distance_polyhedron_item(itemA->polyhedron(),itemB->polyhedron(), itemB->name(), nb_pts_per_face);
    Scene_distance_polyhedron_item* new_itemB = new Scene_distance_polyhedron_item(itemB->polyhedron(),itemA->polyhedron(), itemA->name(), nb_pts_per_face);
    itemA->setVisible(false);
    itemB->setVisible(false);
    new_itemA->setName(QString("%1 to %2").arg(itemA->name()).arg(itemB->name()));
    new_itemB->setName(QString("%1 to %2").arg(itemB->name()).arg(itemA->name()));
    scene->addItem(new_itemA);
    scene->addItem(new_itemB);
    QApplication::restoreOverrideCursor();
  }
private:
  int nb_pts_per_face;
  QList<QAction*> _actions;
  Messages_interface* messageInterface;
  //The reference to the scene
  Scene_interface* scene;
  //The reference to the main window
  QMainWindow* mw;
};
#include "Distance_plugin.moc"