//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/include/functions/fast_divides.hpp>
#include <boost/simd/include/functions/splat.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <boost/simd/include/constants/nan.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

NT2_TEST_CASE_TPL ( fast_divides_real,  BOOST_SIMD_SIMD_REAL_TYPES)
{
  using nt2::fast_divides;
  using boost::simd::splat;
  using nt2::tag::fast_divides_;
  using boost::simd::native;

  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>               vT;

  // return type conformity test
  NT2_TEST_TYPE_IS( typename boost::dispatch::meta::call<fast_divides_(vT,vT)>::type
                  , vT
                  );

  NT2_TEST_EQUAL(fast_divides(splat<vT>(0),splat<vT>(0)), boost::simd::Nan<vT>());

  NT2_TEST_ULP_EQUAL(fast_divides(splat<vT>(1) , splat<vT>(2) ), splat<vT>(0.5)  , 16);
  NT2_TEST_ULP_EQUAL(fast_divides(splat<vT>(1) , splat<vT>(10)), splat<vT>(0.1)  , 16);
  NT2_TEST_ULP_EQUAL(fast_divides(splat<vT>(2) , splat<vT>(3) ), splat<vT>(2./3.), 16);
}

NT2_TEST_CASE_TPL ( fast_divides_ints,  BOOST_SIMD_SIMD_INTEGRAL_TYPES)
{
  using nt2::fast_divides;
  using boost::simd::splat;
  using nt2::tag::fast_divides_;
  using boost::simd::native;

  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>               vT;

  // return type conformity test
  NT2_TEST_TYPE_IS( typename boost::dispatch::meta::call<fast_divides_(vT,vT)>::type
                  , vT
                  );

  NT2_TEST_EQUAL(fast_divides(splat<vT>(1) , splat<vT>(2) ), splat<vT>(0) );
  NT2_TEST_EQUAL(fast_divides(splat<vT>(5) , splat<vT>(2) ), splat<vT>(2) );
  NT2_TEST_EQUAL(fast_divides(splat<vT>(9) , splat<vT>(3) ), splat<vT>(3) );
}


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 1, 4> test_int_1_4(const Eigen::Matrix<int, 1, 4>& M) { return M; }
void export_int_1_4() { boost::python::def("test_int_1_4", test_int_1_4); }


/*
 * Copyright 2015-2019 Autoware Foundation. All rights reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * Authors: Simon Thompson, Ryohsuke Mitsudome
 */

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include <Eigen/Eigen>

#include <string>
#include <vector>

#include <lanelet2_extension/utility/message_conversion.h>
#include <lanelet2_extension/utility/query.h>
#include <lanelet2_extension/visualization/visualization.h>

#include <amathutils_lib/amathutils.hpp>

namespace
{
void adjacentPoints(const int i, const int N, const geometry_msgs::Polygon poly, geometry_msgs::Point32* p0,
                    geometry_msgs::Point32* p1, geometry_msgs::Point32* p2)
{
  if (p0 == nullptr || p1 == nullptr || p2 == nullptr)
  {
    ROS_ERROR_STREAM(__FUNCTION__ << ": either p0, p1, or p2 is null pointer!");
    return;
  }

  *p1 = poly.points[i];
  if (i == 0)
    *p0 = poly.points[N - 1];
  else
    *p0 = poly.points[i - 1];

  if (i < N - 1)
    *p2 = poly.points[i + 1];
  else
    *p2 = poly.points[0];
}

double hypot(const geometry_msgs::Point32& p0, const geometry_msgs::Point32& p1)
{
  return (sqrt(pow((p1.x - p0.x), 2.0) + pow((p1.y - p0.y), 2.0)));
}

bool isAttributeValue(const lanelet::ConstPoint3d p, const std::string attr_str, const std::string value_str)
{
  lanelet::Attribute attr = p.attribute(attr_str);
  if (attr.value().compare(value_str) == 0)
    return true;
  return false;
}

bool isLaneletAttributeValue(const lanelet::ConstLanelet ll, const std::string attr_str, const std::string value_str)
{
  lanelet::Attribute attr = ll.attribute(attr_str);
  if (attr.value().compare(value_str) == 0)
    return true;
  return false;
}

void lightAsMarker(lanelet::ConstPoint3d p, visualization_msgs::Marker* marker, const std::string ns)
{
  if (marker == nullptr)
  {
    ROS_ERROR_STREAM(__FUNCTION__ << ": marker is null pointer!");
    return;
  }

  marker->header.frame_id = "map";
  marker->header.stamp = ros::Time();
  marker->ns = ns;
  marker->id = p.id();
  marker->type = visualization_msgs::Marker::SPHERE;
  marker->pose.position.x = p.x();
  marker->pose.position.y = p.y();
  marker->pose.position.z = p.z();

  float s = 0.3;

  marker->scale.x = s;
  marker->scale.y = s;
  marker->scale.z = s;

  marker->color.r = 0.0f;
  marker->color.g = 0.0f;
  marker->color.b = 0.0f;
  marker->color.a = 1.0f;

  if (isAttributeValue(p, "color", "red"))
    marker->color.r = 1.0f;
  else if (isAttributeValue(p, "color", "green"))
    marker->color.g = 1.0f;
  else if (isAttributeValue(p, "color", "yellow"))
  {
    marker->color.r = 1.0f;
    marker->color.g = 1.0f;
  }
  else
  {
    marker->color.r = 1.0f;
    marker->color.g = 1.0f;
    marker->color.b = 1.0f;
  }
  marker->lifetime = ros::Duration();
}

void laneletDirectionAsMarker(const lanelet::ConstLanelet ll, visualization_msgs::Marker* marker, const int id,
                              const std::string ns)
{
  if (marker == nullptr)
  {
    ROS_ERROR_STREAM(__FUNCTION__ << ": marker is null pointer!");
    return;
  }

  marker->header.frame_id = "map";
  marker->header.stamp = ros::Time();
  marker->ns = ns;
  marker->id = id;
  marker->type = visualization_msgs::Marker::TRIANGLE_LIST;

  lanelet::BasicPoint3d pt[3];
  pt[0].x() = 0.0;
  pt[0].y() = -0.3;
  pt[0].z() = 0.0;
  pt[1].x() = 0.0;
  pt[1].y() = 0.3;
  pt[1].z() = 0;
  pt[2].x() = 1.0;
  pt[2].y() = 0.0;
  pt[2].z() = 0;

  lanelet::BasicPoint3d pc, pc2;

  lanelet::ConstLineString3d center_ls = ll.centerline();
  float s = 1.0;

  marker->lifetime = ros::Duration();

  marker->pose.position.x = 0.0;  // p.x();
  marker->pose.position.y = 0.0;  // p.y();
  marker->pose.position.z = 0.0;  // p.z();
  marker->scale.x = s;
  marker->scale.y = s;
  marker->scale.z = s;
  marker->color.r = 1.0f;
  marker->color.g = 1.0f;
  marker->color.b = 1.0f;
  marker->color.a = 1.0f;

  lanelet::Attribute attr = ll.attribute("turn_direction");
  double turn_dir = 0;

  std_msgs::ColorRGBA c;
  c.r = 0.0;
  c.g = 0.0;
  c.b = 1.0;
  c.a = 0.6;

  if (isLaneletAttributeValue(ll, "turn_direction", "right"))
  {
    turn_dir = -M_PI / 2.0;
    c.r = 1.0;
    c.g = 0.0;
    c.b = 1.0;
  }
  else if (isLaneletAttributeValue(ll, "turn_direction", "left"))
  {
    turn_dir = M_PI / 2.0;
    c.r = 0.0;
    c.g = 1.0;
    c.b = 1.0;
  }

  for (int ci = 0; ci < center_ls.size() - 1;)
  {
    pc = center_ls[ci];
    if (center_ls.size() > 1)
      pc2 = center_ls[ci + 1];
    else
      return;

    double heading = atan2(pc2.y() - pc.y(), pc2.x() - pc.x());

    lanelet::BasicPoint3d pt_tf[3];

    Eigen::Vector3d axis(0, 0, 1);
    Eigen::Transform<double, 3, Eigen::Affine> t(Eigen::AngleAxis<double>(heading, axis));

    for (int i = 0; i < 3; i++)
      pt_tf[i] = t * pt[i] + pc;

    geometry_msgs::Point gp[3];

    for (int i = 0; i < 3; i++)
    {
      std_msgs::ColorRGBA cn = c;

      gp[i].x = pt_tf[i].x();
      gp[i].y = pt_tf[i].y();
      gp[i].z = pt_tf[i].z();
      marker->points.push_back(gp[i]);
      marker->colors.push_back(cn);
    }
    ci = ci + 1;
  }
}

bool isReflexAngle(const geometry_msgs::Point32& a, const geometry_msgs::Point32& o, const geometry_msgs::Point32& b)
{
  // angle aob is reflex in counterclock wise direction if point b is on the right side of vector oa
  // which can be found out by calculating cross product of oa and ob
  return (a.x - o.x) * (b.y - o.y) - (a.y - o.y) * (b.x - o.x) < 0;
}

bool isWithinTriangle(const geometry_msgs::Point32& a, const geometry_msgs::Point32& b, const geometry_msgs::Point32& c,
                      const geometry_msgs::Point32& p)
{
  // point p in within triangle if p is on the same side
  const double side1 = (b.x - a.x) * (p.y - a.y) - (b.y - a.y) * (p.x - a.x);  // a to b
  const double side2 = (c.x - b.x) * (p.y - b.y) - (c.y - b.y) * (p.x - b.x);  // b to c
  const double side3 = (a.x - c.x) * (p.y - c.y) - (a.y - c.y) * (p.x - c.x);  // c to a
  return (side1 > 0.0 && side2 > 0.0 && side3 > 0.0) || (side1 < 0.0 && side2 < 0.0 && side3 < 0.0);
}

}  // anonymous namespace

namespace lanelet
{
void visualization::lanelet2Triangle(const lanelet::ConstLanelet& ll, std::vector<geometry_msgs::Polygon>* triangles)
{
  if (triangles == nullptr)
  {
    ROS_ERROR_STREAM(__FUNCTION__ << ": triangles is null pointer!");
    return;
  }

  triangles->clear();
  geometry_msgs::Polygon ll_poly;
  lanelet2Polygon(ll, &ll_poly);
  polygon2Triangle(ll_poly, triangles);
}

void visualization::polygon2Triangle(const geometry_msgs::Polygon& polygon,
                                     std::vector<geometry_msgs::Polygon>* triangles)
{
  geometry_msgs::Polygon poly = polygon;
  // ear clipping: find smallest internal angle in polygon
  int N = poly.points.size();

  // array of angles for each vertex
  std::vector<bool> is_reflex_angle;
  is_reflex_angle.assign(N, false);
  for (int i = 0; i < N; i++)
  {
    geometry_msgs::Point32 p0, p1, p2;

    adjacentPoints(i, N, poly, &p0, &p1, &p2);
    is_reflex_angle.at(i) = isReflexAngle(p0, p1, p2);
  }

  // start ear clipping
  while (N >= 3)
  {
    int clipped_vertex = -1;

    for (int i = 0; i < N; i++)
    {
      bool is_reflex = is_reflex_angle.at(i);
      if (!is_reflex)
      {
        geometry_msgs::Point32 p0, p1, p2;
        adjacentPoints(i, N, poly, &p0, &p1, &p2);

        int j_begin = (i + 2) % N;
        int j_end = (i - 1 + N) % N;
        bool is_ear = true;
        for (int j = j_begin; j != j_end; j = (j + 1) % N)
        {
          if (isWithinTriangle(p0, p1, p2, poly.points.at(j)))
          {
            is_ear = false;
            break;
          }
        }

        if (is_ear)
        {
          clipped_vertex = i;
          break;
        }
      }
    }
    if (clipped_vertex < 0 || clipped_vertex >= N)
    {
      ROS_ERROR("Could not find valid vertex for ear clipping triangulation. Triangulation result might be invalid");
      clipped_vertex = 0;
    }

    // create triangle
    geometry_msgs::Point32 p0, p1, p2;
    adjacentPoints(clipped_vertex, N, poly, &p0, &p1, &p2);
    geometry_msgs::Polygon triangle;
    triangle.points.push_back(p0);
    triangle.points.push_back(p1);
    triangle.points.push_back(p2);
    triangles->push_back(triangle);

    // remove vertex of center of angle
    auto it = poly.points.begin();
    std::advance(it, clipped_vertex);
    poly.points.erase(it);

    // remove from angle list
    auto it_angle = is_reflex_angle.begin();
    std::advance(it_angle, clipped_vertex);
    is_reflex_angle.erase(it_angle);

    // update angle list
    N = poly.points.size();
    if (clipped_vertex == N)
    {
      clipped_vertex = 0;
    }
    adjacentPoints(clipped_vertex, N, poly, &p0, &p1, &p2);
    is_reflex_angle.at(clipped_vertex) = isReflexAngle(p0, p1, p2);

    int i_prev = (clipped_vertex == 0) ? N - 1 : clipped_vertex - 1;
    adjacentPoints(i_prev, N, poly, &p0, &p1, &p2);
    is_reflex_angle.at(i_prev) = isReflexAngle(p0, p1, p2);
  }
}

void visualization::lanelet2Polygon(const lanelet::ConstLanelet& ll, geometry_msgs::Polygon* polygon)
{
  if (polygon == nullptr)
  {
    ROS_ERROR_STREAM(__FUNCTION__ << ": polygon is null pointer!");
    return;
  }

  lanelet::CompoundPolygon3d ll_poly = ll.polygon3d();

  polygon->points.clear();
  polygon->points.reserve(ll_poly.size());

  for (const auto& pt : ll_poly)
  {
    geometry_msgs::Point32 pt32;
    utils::conversion::toGeomMsgPt32(pt.basicPoint(), &pt32);
    polygon->points.push_back(pt32);
  }
}

visualization_msgs::MarkerArray visualization::laneletDirectionAsMarkerArray(const lanelet::ConstLanelets lanelets)
{
  visualization_msgs::MarkerArray marker_array;
  int ll_dir_count = 0;
  for (auto lli = lanelets.begin(); lli != lanelets.end(); lli++)
  {
    lanelet::ConstLanelet ll = *lli;

    //      bool road_found = false;
    if (ll.hasAttribute(std::string("turn_direction")))
    {
      lanelet::Attribute attr = ll.attribute("turn_direction");
      visualization_msgs::Marker marker;

      laneletDirectionAsMarker(ll, &marker, ll_dir_count, "lanelet direction");

      marker_array.markers.push_back(marker);
      ll_dir_count++;
    }
  }

  return (marker_array);
}

visualization_msgs::MarkerArray visualization::autowareTrafficLightsAsMarkerArray(
    const std::vector<lanelet::AutowareTrafficLightConstPtr> tl_reg_elems, const std_msgs::ColorRGBA c,
    const ros::Duration duration, const double scale)
{
  visualization_msgs::MarkerArray tl_marker_array;

  int tl_count = 0;
  for (auto tli = tl_reg_elems.begin(); tli != tl_reg_elems.end(); tli++)
  {
    lanelet::ConstLineStrings3d light_bulbs;
    lanelet::AutowareTrafficLightConstPtr tl = *tli;

    const auto lights = tl->trafficLights();
    for (const auto& lsp : lights)
    {
      if (lsp.isLineString())  // traffic ligths can either polygons or
      {                        // linestrings
        lanelet::ConstLineString3d ls = static_cast<lanelet::ConstLineString3d>(lsp);

        visualization_msgs::Marker marker;
        visualization::trafficLight2TriangleMarker(ls, &marker, "traffic_light_triangle", c, duration, scale);
        tl_marker_array.markers.push_back(marker);
      }
    }

    light_bulbs = tl->lightBulbs();
    for (auto ls : light_bulbs)
    {
      lanelet::ConstLineString3d l = static_cast<lanelet::ConstLineString3d>(ls);

      for (auto pt : l)
      {
        if (pt.hasAttribute("color"))
        {
          visualization_msgs::Marker marker;
          lightAsMarker(pt, &marker, "traffic_light");
          tl_marker_array.markers.push_back(marker);

          tl_count++;
        }
      }
    }
  }

  return (tl_marker_array);
}

visualization_msgs::MarkerArray
visualization::lineStringsAsMarkerArray(const std::vector<lanelet::ConstLineString3d> line_strings,
                                        const std::string name_space, const std_msgs::ColorRGBA c, const double lss)
{
  visualization_msgs::MarkerArray ls_marker_array;
  for (auto i = line_strings.begin(); i != line_strings.end(); i++)
  {
    lanelet::ConstLineString3d ls = *i;
    visualization_msgs::Marker ls_marker;

    visualization::lineString2Marker(ls, &ls_marker, "map", name_space, c, 0.2);

    ls_marker_array.markers.push_back(ls_marker);
  }

  return (ls_marker_array);
}

visualization_msgs::MarkerArray visualization::laneletsBoundaryAsMarkerArray(const lanelet::ConstLanelets& lanelets,
                                                                             const std_msgs::ColorRGBA c,
                                                                             const bool viz_centerline)
{
  double lss = 0.2;  // line string size
  visualization_msgs::MarkerArray marker_array;
  for (auto li = lanelets.begin(); li != lanelets.end(); li++)
  {
    lanelet::ConstLanelet lll = *li;

    lanelet::ConstLineString3d left_ls = lll.leftBound();
    lanelet::ConstLineString3d right_ls = lll.rightBound();
    lanelet::ConstLineString3d center_ls = lll.centerline();

    visualization_msgs::Marker left_line_strip, right_line_strip, center_line_strip;

    visualization::lineString2Marker(left_ls, &left_line_strip, "map", "left_lane_bound", c, lss);
    visualization::lineString2Marker(right_ls, &right_line_strip, "map", "right_lane_bound", c, lss);
    marker_array.markers.push_back(left_line_strip);
    marker_array.markers.push_back(right_line_strip);
    if (viz_centerline)
    {
      visualization::lineString2Marker(center_ls, &center_line_strip, "map", "center_lane_line", c, lss * 0.5);
      marker_array.markers.push_back(center_line_strip);
    }
  }
  return marker_array;
}

visualization_msgs::MarkerArray visualization::trafficLightsAsTriangleMarkerArray(
    const std::vector<lanelet::TrafficLightConstPtr> tl_reg_elems, const std_msgs::ColorRGBA c,
    const ros::Duration duration, const double scale)
{
  // convert to to an array of linestrings and publish as marker array using
  // exisitng function

  int tl_count = 0;
  std::vector<lanelet::ConstLineString3d> line_strings;
  visualization_msgs::MarkerArray marker_array;

  for (auto tli = tl_reg_elems.begin(); tli != tl_reg_elems.end(); tli++)
  {
    lanelet::TrafficLightConstPtr tl = *tli;
    lanelet::LineString3d ls;

    auto lights = tl->trafficLights();
    for (auto lsp : lights)
    {
      if (lsp.isLineString())  // traffic ligths can either polygons or
      {                        // linestrings
        lanelet::ConstLineString3d ls = static_cast<lanelet::ConstLineString3d>(lsp);

        visualization_msgs::Marker marker;
        visualization::trafficLight2TriangleMarker(ls, &marker, "traffic_light_triangle", c, duration, scale);
        marker_array.markers.push_back(marker);
        tl_count++;
      }
    }
  }

  return (marker_array);
}

visualization_msgs::MarkerArray visualization::laneletsAsTriangleMarkerArray(const std::string ns,
                                                                             const lanelet::ConstLanelets& lanelets,
                                                                             const std_msgs::ColorRGBA c)
{
  visualization_msgs::MarkerArray marker_array;
  visualization_msgs::Marker marker;

  marker.header.frame_id = "map";
  marker.header.stamp = ros::Time();
  marker.ns = ns;
  marker.id = 0;
  marker.type = visualization_msgs::Marker::TRIANGLE_LIST;
  marker.lifetime = ros::Duration();
  marker.pose.position.x = 0.0;  // p.x();
  marker.pose.position.y = 0.0;  // p.y();
  marker.pose.position.z = 0.0;  // p.z();
  marker.scale.x = 1.0;
  marker.scale.y = 1.0;
  marker.scale.z = 1.0;
  marker.color.r = 1.0f;
  marker.color.g = 1.0f;
  marker.color.b = 1.0f;
  marker.color.a = 1.0f;

  for (auto ll : lanelets)
  {
    std::vector<geometry_msgs::Polygon> triangles;
    lanelet2Triangle(ll, &triangles);

    for (auto tri : triangles)
    {
      geometry_msgs::Point tri0[3];

      for (int i = 0; i < 3; i++)
      {
        utils::conversion::toGeomMsgPt(tri.points[i], &tri0[i]);

        marker.points.push_back(tri0[i]);
        marker.colors.push_back(c);
      }
    }
  }
  if (!marker.points.empty())
  {
    marker_array.markers.push_back(marker);
  }

  return (marker_array);
}

void visualization::trafficLight2TriangleMarker(const lanelet::ConstLineString3d ls, visualization_msgs::Marker* marker,
                                                const std::string ns, const std_msgs::ColorRGBA cl,
                                                const ros::Duration duration, const double scale)
{
  if (marker == nullptr)
  {
    ROS_ERROR_STREAM(__FUNCTION__ << ": marker is null pointer!");
    return;
  }
  marker->header.frame_id = "map";
  marker->header.stamp = ros::Time();
  marker->ns = ns;
  marker->id = ls.id();
  marker->type = visualization_msgs::Marker::TRIANGLE_LIST;

  marker->lifetime = duration;

  marker->pose.position.x = 0.0;  // p.x();
  marker->pose.position.y = 0.0;  // p.y();
  marker->pose.position.z = 0.0;  // p.z();
  marker->scale.x = 1.0;
  marker->scale.y = 1.0;
  marker->scale.z = 1.0;
  marker->color.r = 1.0f;
  marker->color.g = 1.0f;
  marker->color.b = 1.0f;
  marker->color.a = 1.0f;

  double h = 0.7;
  if (ls.hasAttribute("height"))
  {
    lanelet::Attribute attr = ls.attribute("height");
    h = std::stod(attr.value());
  }

  // construct triangles and add to marker

  // define polygon of traffic light border
  Eigen::Vector3d v[4];
  v[0] << ls.front().x(), ls.front().y(), ls.front().z();
  v[1] << ls.back().x(), ls.back().y(), ls.back().z();
  v[2] << ls.back().x(), ls.back().y(), ls.back().z() + h;
  v[3] << ls.front().x(), ls.front().y(), ls.front().z() + h;

  Eigen::Vector3d c = (v[0] + v[1] + v[2] + v[3]) / 4;

  if (scale > 0.0 && scale != 1.0)
  {
    for (int i = 0; i < 4; i++)
    {
      v[i] = (v[i] - c) * scale + c;
    }
  }
  geometry_msgs::Point tri0[3];
  utils::conversion::toGeomMsgPt(v[0], &tri0[0]);
  utils::conversion::toGeomMsgPt(v[1], &tri0[1]);
  utils::conversion::toGeomMsgPt(v[2], &tri0[2]);
  geometry_msgs::Point tri1[3];
  utils::conversion::toGeomMsgPt(v[0], &tri1[0]);
  utils::conversion::toGeomMsgPt(v[2], &tri1[1]);
  utils::conversion::toGeomMsgPt(v[3], &tri1[2]);

  for (int i = 0; i < 3; i++)
  {
    marker->points.push_back(tri0[i]);
    marker->colors.push_back(cl);
  }
  for (int i = 0; i < 3; i++)
  {
    marker->points.push_back(tri1[i]);
    marker->colors.push_back(cl);
  }
}

void visualization::lineString2Marker(const lanelet::ConstLineString3d ls, visualization_msgs::Marker* line_strip,
                                      const std::string frame_id, const std::string ns, const std_msgs::ColorRGBA c,
                                      const float lss)
{
  if (line_strip == nullptr)
  {
    ROS_ERROR_STREAM(__FUNCTION__ << ": line_strip is null pointer!");
    return;
  }

  line_strip->header.frame_id = frame_id;
  line_strip->header.stamp = ros::Time();
  line_strip->ns = ns;
  line_strip->action = visualization_msgs::Marker::ADD;

  line_strip->pose.orientation.w = 1.0;
  line_strip->id = ls.id();

  line_strip->type = visualization_msgs::Marker::LINE_STRIP;

  line_strip->scale.x = lss;

  line_strip->color = c;

  // fill out lane line
  for (auto i = ls.begin(); i != ls.end(); i++)
  {
    geometry_msgs::Point p;
    p.x = (*i).x();
    p.y = (*i).y();
    p.z = (*i).z();
    line_strip->points.push_back(p);
  }
}

}  // namespace lanelet


#include "Common/Common.h"
#include "GL/glew.h"
#include "Demos/Visualization/MiniGL.h"
#include "Demos/Visualization/Selection.h"
#include "GL/glut.h"
#include "Demos/Simulation/TimeManager.h"
#include <Eigen/Dense>
#include "FluidModel.h"
#include "TimeStepFluidModel.h"
#include <iostream>
#include "Demos/Utils/Logger.h"
#include "Demos/Utils/Timing.h"
#include "Demos/Utils/FileSystem.h"

#define _USE_MATH_DEFINES
#include "math.h"
#include "Demos/Simulation/Constraints.h"
#include "Demos/Visualization/Visualization.h"

// Enable memory leak detection
#if defined(_DEBUG) && !defined(EIGEN_ALIGN)
	#define new DEBUG_NEW
#endif

INIT_TIMING
INIT_LOGGING

using namespace PBD;
using namespace Eigen;
using namespace std;

void timeStep ();
void buildModel ();
void renderWorld();
void createBreakingDam();
void addWall(const Vector3r &minX, const Vector3r &maxX, std::vector<Vector3r> &boundaryParticles, std::vector<bool> &boundaryActives,bool active);
void initBoundaryData(std::vector<Vector3r> &boundaryParticles, std::vector<bool> &boundaryActives);
void render ();
void cleanup();
void reset();
void selection(const Eigen::Vector2i &start, const Eigen::Vector2i &end);
void createSphereBuffers(Real radius, int resolution);
void renderSphere(const Vector3r &x, const float color[]);
void releaseSphereBuffers();
void TW_CALL setTimeStep(const void *value, void *clientData);
void TW_CALL getTimeStep(void *value, void *clientData);
void TW_CALL setVelocityUpdateMethod(const void *value, void *clientData);
void TW_CALL getVelocityUpdateMethod(void *value, void *clientData);
void TW_CALL setTempUpdateMethod(const void *value, void *clientData);
void TW_CALL getTempUpdateMethod(void *value, void *clientData);
void TW_CALL setViscosity(const void *value, void *clientData);
void TW_CALL getViscosity(void *value, void *clientData);
void TW_CALL setStiffness(const void *value, void *clientData);
void TW_CALL getStiffness(void *value, void *clientData);
void TW_CALL setDilation(const void *value, void *clientData);
void TW_CALL getDilation(void *value, void *clientData);
void TW_CALL setMelting(const void *value, void *clientData);
void TW_CALL getMelting(void *value, void *clientData);
void TW_CALL setEvaporation(const void *value, void *clientData);
void TW_CALL getEvaporation(void *value, void *clientData);
void TW_CALL setDifusion(const void *value, void *clientData);
void TW_CALL getDifusion(void *value, void *clientData);
void TW_CALL getTempContact(void *value, void *clientData);
void TW_CALL setTempContact(const void *value, void *clientData);
void TW_CALL getContact(void *value, void *clientData);
void TW_CALL setContact(const void *value, void *clientData);
void TW_CALL getXPBD(void *value, void *clientData);
void TW_CALL setXPBD(const void *value, void *clientData);

void TW_CALL getRadioSol(void *value, void *clientData);
void TW_CALL setRadioSol(const void *value, void *clientData);
void TW_CALL getNumConstraint(void *value, void *clientData);
void TW_CALL setNumConstraint(const void *value, void *clientData);

void TW_CALL getDesWidth(void *value, void *clientData);
void TW_CALL setDesWidth(const void *value, void *clientData);

void TW_CALL getDesHeight(void *value, void *clientData);
void TW_CALL setDesHeight(const void *value, void *clientData);
//Core
FluidModel model;
TimeStepFluidModel simulation;

Real compact = 0.98;// 0.928;

//const Real particleRadius = 0.025;
const Real particleRadius = 0.025;//0.015625//;
int width = 30;
int depth = 30;
int height = 80;
bool doPause = true;
bool thermal = true;
bool record = false;
bool capture = false;
bool rendering = true;
bool renderingWall = false;
bool renderingAir = false;

std::vector<unsigned int> selectedParticles;
Vector3r oldMousePos;
// initiate buffers
GLuint elementbuffer;
GLuint normalbuffer;
GLuint vertexbuffer;
int vertexBufferSize = 0;
GLint context_major_version, context_minor_version;
string exePath;
string dataPath;

GLuint textureFloor;
ofstream file;

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

	std::string logPath = FileSystem::normalizePath(FileSystem::getProgramPath() + "/log");
	FileSystem::makeDirs(logPath);
	logger.addSink(unique_ptr<ConsoleSink>(new ConsoleSink(LogLevel::INFO)));
	logger.addSink(unique_ptr<FileSink>(new FileSink(LogLevel::DEBUG, logPath + "/PBD.log")));

	exePath = FileSystem::getProgramPath();
	dataPath = exePath + "/" + std::string(PBD_DATA_PATH);

	// OpenGL
	MiniGL::init (argc, argv, 1280+620, 780, 0, 0, "Fluid demo");

	MiniGL::setClientIdleFunc (60, timeStep);

	MiniGL::initLights ();
	MiniGL::loadTexture(textureFloor);

	MiniGL::setKeyFunc(0, 'r', reset);
	//MiniGL::setKeyFunc(1, '1', demo1);
	//MiniGL::setKeyFunc(2, '2', demo2);
	MiniGL::setSelectionFunc(selection);

	//MiniGL::desWidth = 350;
	//MiniGL::desHeight = 100;

	MiniGL::getOpenGLVersion(context_major_version, context_minor_version);
	MiniGL::setClientSceneFunc(render);

	//MiniGL::setViewport (40.0, 0.1f, 500.0, Vector3r (0.0, 3.0, 8.0), Vector3r (0.0, 0.0, 0.0));
	MiniGL::setViewport(40.0, 0.1f, 800.0, Vector3r(0.0, 2.1, 7.0), Vector3r(0.0, 0.0, 0.0));

	TwAddVarRW(MiniGL::getTweakBar(), "Pause", TW_TYPE_BOOLCPP, &doPause, " label='Pause' group=Simulation key=SPACE ");
	TwAddVarCB(MiniGL::getTweakBar(), "TimeStepSize", TW_TYPE_REAL, setTimeStep, getTimeStep, &model, " label='Time step size'  min=0.0 max = 0.1 step=0.0001 precision=4 group=Simulation ");
	TwType enumType = TwDefineEnum("VelocityUpdateMethodType", NULL, 0);
	TwAddVarCB(MiniGL::getTweakBar(), "VelocityUpdateMethod", enumType, setVelocityUpdateMethod, getVelocityUpdateMethod, &simulation, " label='Velocity update method' enum='0 {First Order Update}, 1 {Second Order Update}' group=Simulation");

	TwAddVarCB(MiniGL::getTweakBar(), "XPBD", TW_TYPE_BOOL32, setXPBD, getXPBD, &model, " label='XPBD'  group=Simulation ");
	//TwAddVarRW(MiniGL::getTweakBar(), "XPDB", TW_TYPE_BOOLCPP, &XPBD, " label='XPBD' group=Simulation key=x ");
	TwAddVarCB(MiniGL::getTweakBar(), "Viscosity", TW_TYPE_REAL, setViscosity, getViscosity, &model, " label='Viscosity'  min=0.0 max = 5 step=0.001 precision=4 group=Simulation ");

	TwAddVarCB(MiniGL::getTweakBar(), "Radio Solidification", TW_TYPE_REAL, setRadioSol, getRadioSol, &model, " label='Radio Solidification'  min=0.045 max = 0.1 step=0.01 precision=4 group=Simulation ");
	TwAddVarCB(MiniGL::getTweakBar(), "Num Constraints", TW_TYPE_REAL, setNumConstraint, getNumConstraint, &model, " label='Num Constraints'  min=1 max = 10 step=1 precision=4 group=Simulation ");
	TwAddVarCB(MiniGL::getTweakBar(), "Stiffness", TW_TYPE_REAL, setStiffness, getStiffness, &model, " label='Stiffness'  min=0.01 max = 1 step=0.01 precision=4 group=Simulation ");
	TwAddVarCB(MiniGL::getTweakBar(), "Coef Dilation", TW_TYPE_REAL, setDilation, getDilation, &model, " label='Coef Dilation'  min=0 max = 0.01 step=0.001 precision=4 group=Simulation ");

	TwAddVarCB(MiniGL::getTweakBar(), "Temp Melting", TW_TYPE_REAL, setMelting, getMelting, &model, " label='Temp Melting'  min=0.0 max = 300 step=1.0 precision=4 group=Simulation ");
	TwAddVarCB(MiniGL::getTweakBar(), "Temp Evaporation", TW_TYPE_REAL, setEvaporation, getEvaporation, &model, " label='Temp Evaporation'  min=1 max = 500.0 step=1.0 precision=4 group=Simulation ");
	TwAddVarCB(MiniGL::getTweakBar(), "Coef Difusion", TW_TYPE_REAL, setDifusion, getDifusion, &model, " label='Coef Difusion'  min=1 max = 1000 step=5 precision=4 group=Simulation ");

	TwType enumType2 = TwDefineEnum("TemperatureUpdateMethodType", NULL, 0);
	TwAddVarCB(MiniGL::getTweakBar(), "TemperatureUpdateMethod", enumType2, setTempUpdateMethod, getTempUpdateMethod, &model, " label='Heat Transfer method' enum='0 {Our Method}, 1 {Cleary Method}' group=Simulation");

	TwAddVarCB(MiniGL::getTweakBar(), "Contact", TW_TYPE_BOOL32, setContact, getContact, &model, " label='Contact'  group=Simulation ");
	TwAddVarCB(MiniGL::getTweakBar(), "Temp Contact", TW_TYPE_REAL, setTempContact, getTempContact, &model, " label='Temp Contact'  min=-50 max = 500 step=10 precision=4 group=Simulation ");

	TwAddVarRW(MiniGL::getTweakBar(), "Thermal", TW_TYPE_BOOLCPP, &thermal, " label='Thermal' group=Simulation key=t ");

	TwAddVarRW(MiniGL::getTweakBar(), "start", TW_TYPE_BOOLCPP, &record, " label='start' group=Record key=g ");
	TwAddVarRW(MiniGL::getTweakBar(), "capture", TW_TYPE_BOOLCPP, &capture, " label='capture' group=Record key=c ");
	TwAddVarCB(MiniGL::getTweakBar(), "Des Width", TW_TYPE_INT32, setDesWidth, getDesWidth, &model, " label='Des Width'  min=0.0 max = 900 step=1.0  group=Record ");
	TwAddVarCB(MiniGL::getTweakBar(), "Des Height", TW_TYPE_INT32, setDesHeight, getDesHeight, &model, " label='Des Height'  min=0.0 max = 900 step=1.0 group=Record ");
	TwAddVarRW(MiniGL::getTweakBar(), "rendering", TW_TYPE_BOOLCPP, &rendering, " label='rendering' group=Record key=n ");
	TwAddVarRW(MiniGL::getTweakBar(), "renderingWall", TW_TYPE_BOOLCPP, &renderingWall, " label='renderingWall' group=Record key=m ");
	TwAddVarRW(MiniGL::getTweakBar(), "renderingAir", TW_TYPE_BOOLCPP, &renderingAir, " label='renderingAir' group=Record key=i ");

	buildModel();

	if (context_major_version >= 3)
		createSphereBuffers((Real)particleRadius, 4);
		//createSphereBuffers((Real)particleRadius, 25);

	glutMainLoop ();

	cleanup ();
	file.close();

	//Timing::printAverageTimes();

	return 0;
}

void cleanup()
{
	delete TimeManager::getCurrent();
	if (context_major_version >= 3)
		releaseSphereBuffers();
}

int test = 0;
int index = 0;
int frameIndex = 0;

int numFrames = 0;
int totalFrames = 0;
int numFile = 0;
int seconds = 10;
int totalFrame = 64 * seconds;

int timeEmision = 0;
int numberFiles = 10;
int firtTimeAugmented = 0;
int tempAugmented = 0;
bool addWallTop = true;
bool temperatureFloor = true;

int numParticlesAir = 0;
void reset()
{
	Timing::printAverageTimes();
	Timing::reset();

	model.reset();
	simulation.reset();
	TimeManager::getCurrent()->setTime(0.0);

	test++;
	index = 0;
	numFile = 0;
	numFrames = 0;
	frameIndex=0;
	capture = false;
	record = false;
	doPause = true;
}

void mouseMove(int x, int y)
{
	Vector3r mousePos;
	MiniGL::unproject(x, y, mousePos);
	const Vector3r diff = mousePos - oldMousePos;

	TimeManager *tm = TimeManager::getCurrent();
	const Real h = tm->getTimeStepSize();

	ParticleData &pd = model.getParticles();
	for (unsigned int j = 0; j < selectedParticles.size(); j++)
	{
		pd.getVelocity(selectedParticles[j]) += 5.0*diff/h;
	}
	oldMousePos = mousePos;
}

void selection(const Eigen::Vector2i &start, const Eigen::Vector2i &end)
{
	std::vector<unsigned int> hits;
	selectedParticles.clear();
	ParticleData &pd = model.getParticles();
	Selection::selectRect(start, end, &pd.getPosition(0), &pd.getPosition(pd.size() - 1), selectedParticles);
	if (selectedParticles.size() > 0)
		MiniGL::setMouseMoveFunc(GLUT_MIDDLE_BUTTON, mouseMove);
	else
		MiniGL::setMouseMoveFunc(-1, NULL);

	MiniGL::unproject(end[0], end[1], oldMousePos);
}

string nameFile = "condensation";

//*
void initData(int numParticles) {

	char bufPath[255];
	sprintf(bufPath, "/%s%d.txt",nameFile ,numFile++);
	file = ofstream(dataPath + bufPath, ios::out | ios::trunc | ios::binary);
	file.seekp(0, ios::beg);
	file.write(reinterpret_cast<char *>(&numParticles), sizeof(int));
}
void saveData(ParticleData* p) {

	if (numFrames == totalFrame) {
		file.close();

		if (numFile >= numberFiles) {
			reset();
			return;
			//exit(0);
		}
		char bufPath[255];
		sprintf(bufPath, "/%s%d.txt", nameFile, numFile++);
		file = ofstream(dataPath + bufPath, ios::out | ios::trunc | ios::binary);
		file.seekp(0, ios::beg);
		numFrames = 0;
	}

	int numParticles = p->getNumberOfParticles();

	file.seekp(0, ios::end);
	float x, y, z, t;
	for (int i = numParticlesAir; i < numParticles; i++) {
		Vector3r position = p->getPosition(i);

		t = p->getTemp(i);
		x = position[0];
		y = position[1];
		z = position[2];

		file.write(reinterpret_cast<char *>(&t), sizeof(float));
		file.write(reinterpret_cast<char *>(&x), sizeof(float));
		file.write(reinterpret_cast<char *>(&y), sizeof(float));
		file.write(reinterpret_cast<char *>(&z), sizeof(float));
	}
	numFrames++;
	totalFrames++;
}
//*/
void timeStep()
{

	if (capture) {
		MiniGL::saveFrame("D:\\heat-video\\frame", test, TimeManager::getCurrent()->getTime(), 255, 4);
		capture = false;
	}
	if (doPause) {
		return;
	}

	if (timeEmision > 0 && totalFrames >= 64 * timeEmision)
		model.setContact(true);
	if (model.getContact() && firtTimeAugmented > 0 && totalFrames >= 64 * firtTimeAugmented) {
		model.setTempContact(model.getTempContact() + tempAugmented);
		firtTimeAugmented = 0;
	}

	// Simulation code
	for (unsigned int i = 0; i < 4; i++) //Iterations Per Frame
	{
		simulation.step(model);
		// Draw simulation model
		/*ParticleData &pd = model.getParticles();
		const unsigned int nParticles = pd.size();
		for (unsigned int i = 0; i < nParticles; i++)
		{
			const Real t = 6.0f;
			if (pd.getPosition(i)[1] > 3) {
				pd.setTemp(i,t);
			}
		}*/

	}
	/* Measure Temp
	ParticleData &pd = model.getParticles();
	LOG_INFO << TimeManager::getCurrent()->getTime() << "\t" << pd.getTemp(0) << "\t" << pd.getTemp(pd.getNumberOfParticles() - 1) << "\t" << pd.getTemp(0) + pd.getTemp(pd.getNumberOfParticles() - 1);
	//*/
	saveData(&model.getParticles());

	/*
	if (TimeManager::getCurrent()->getTime()>5.0f) {
		Timing::printAverageTimes();
		exit(0);
	}*/

	if ( record) {
		MiniGL::saveFrame("D:\\heat-video\\frame", test, TimeManager::getCurrent()->getTime(), 255, 4);
	}

}

void buildModel ()
{
	//TimeManager::getCurrent ()->setTimeStepSize (0.05025);
	TimeManager::getCurrent()->setTimeStepSize(0.00390625);
	//TimeManager::getCurrent()->setTimeStepSize(0.001953125);

	createBreakingDam();

}

void renderWorld()
{

	/*glDepthMask(GL_TRUE);
	glEnable(GL_DEPTH_TEST);*/

	glEnable(GL_TEXTURE_2D);
	glBindTexture(GL_TEXTURE_2D, textureFloor);

	//float color[4] = { 1.0f, 1.0f, 1.0f, 0.9f };
	float color[4] = {0.9f, 0.9f, 0.9f, 1.0f};
	//MiniGL::hsvToRgb(1.0f, 1.0f, 1.0f, surfaceColor);
	//*

	float c = 40.0f;

	glBegin(GL_QUADS);
	float speccolor[4] = { 1.0, 1.0, 1.0, 1.0 };
	glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, color);
	glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, color);
	glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, speccolor);
	glMaterialf(GL_FRONT_AND_BACK, GL_SHININESS, 100.0);

	glTexCoord2d(-2.5, -2.5); glVertex3d(-c, 0.0, -c);
	glTexCoord2d(2.5, -2.5); glVertex3d(c, 0.0, -c);
	glTexCoord2d(2.5, 2.5); glVertex3d(c, 0.0, c);
	glTexCoord2d(-2.5, 2.5); glVertex3d(-c, 0.0, c);
	glEnd();
	glBindTexture(GL_TEXTURE_2D, 0);
}

void render ()
{
	if (!rendering) {
		MiniGL::drawTime(TimeManager::getCurrent()->getTime());
		return;
	}
	//renderWorld();

	// Draw simulation model
	const ParticleData &pd = model.getParticles();
	const unsigned int nParticles = pd.size();

	int particlesIni = 0;

	if (renderingAir) {
		particlesIni = numParticlesAir;
	}

	//glPointSize(4.0);

	const Real supportRadius = model.getSupportRadius();
	Real vmax = 0.4*2.0*supportRadius / TimeManager::getCurrent()->getTimeStepSize();
	Real vmin = 0.0;

	if (context_major_version > 3)
	{
		if (thermal) {

			for (unsigned int i = particlesIni; i < nParticles; i++)
			{
				Real v = pd.getVelocity(i).norm();
				v = 0.5*((v - vmin) / (vmax - vmin));
				v = min(128.0*v*v, 0.2);
				float fluidColor[4] = { 1.0f, 1.0f, 1.0f, 1.0f  };
				Real t = min((pd.getTemp(i)) *0.0065, 0.651);

				t = max(t, -0.05);
				//Real t = min((pd.getTemp(i)) *0.0075, 0.61);

				/*Real t = min((pd.getTemp(i)) *0.0081, 0.648);
				MiniGL::hsvToRgb(0.648 - t, 1.0f, 0.8f + (float)v, fluidColor);*/

				//if (t < 0)
					//t = 0;
				MiniGL::hsvToRgb(0.65 - t, 1.0f, 0.8f + (float)v, fluidColor);

				renderSphere(pd.getPosition(i), fluidColor);
			}

		}
		else {
			const vector<newDistanceConstraint> c = model.getConstraints();
			const unsigned int nc = c.size();

			for (unsigned int i = particlesIni; i < nParticles; i++)
			{
				Real v = pd.getVelocity(i).norm();
				v = 0.5*((v - vmin) / (vmax - vmin));
				v = min(128.0*v*v, 0.2);
				float fluidColor[4] = { 0.2f, 0.2f, 0.2f, 1.0 };
				MiniGL::hsvToRgb((pd.getState(i) == 0 ? 1 : 0.55), 1.0f, 0.8f + (float)v, fluidColor);
				renderSphere(pd.getPosition(i), fluidColor);
			}

			float color[4] = { 0.2f, 0.2f, 0.2f, 1.0 };
			MiniGL::hsvToRgb(0.25f, 1.0f, 1.0f, color);

			glLineWidth(3);
			glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, color);
			glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, color);
			glBegin(GL_LINES);

			for (unsigned int i = 0; i < nc; i++)
			{
				Vector3r x1 = pd.getPosition(c[i].m_bodies[0]);
				Vector3r x2 = pd.getPosition(c[i].m_bodies[1]);

				glVertex3f(x1[0], x1[1], x1[2]);
				glVertex3f(x2[0], x2[1], x2[2]);

			}
			glEnd();
		}
		if (renderingWall) {
			float surfaceColor[4] = { 0.2f, 0.2f, 0.2f, 1.0 };
			for (unsigned int i = 0; i < model.numBoundaryParticles(); i++)
				renderSphere(model.getBoundaryX(i), surfaceColor);
		}
	}
	else
	{
		glDisable(GL_LIGHTING);
		glBegin(GL_POINTS);
		for (unsigned int i = particlesIni; i < nParticles; i++)
		{
			Real v = pd.getVelocity(i).norm();
			v = 0.5*((v - vmin) / (vmax - vmin));
			v = min(128.0*v*v, 0.5);
			float fluidColor[4] = { 0.2f, 0.2f, 0.2f, 1.0 };
			MiniGL::hsvToRgb(0.55f, 1.0f, 0.5f + (float)v, fluidColor);

			glColor3fv(fluidColor);
			glVertex3v(&pd.getPosition(i)[0]);
		}
		glEnd();

		// 	glBegin(GL_POINTS);
		// 	for (unsigned int i = 0; i < model.numBoundaryParticles(); i++)
		// 	{
		// 		glColor3fv(surfaceColor);
		// 		glVertex3fv(&model.getBoundaryX(i)[0]);
		// 	}
		// 	glEnd();*/

		//glEnable(GL_LIGHTING);
	}

	float red[4] = { 1.0f, 1.0f, 1.0f, 1 };
	for (unsigned int j = 0; j < selectedParticles.size(); j++)
	{
		MiniGL::drawSphere(pd.getPosition(selectedParticles[j]), 0.05f, red);
	}

	/*
	TetModel *tetModel = model.getTetModel();
	const IndexedFaceMesh &surfaceMesh = tetModel->getSurfaceMesh();
	Visualization::drawMesh(pd, surfaceMesh, tetModel->getIndexOffset(), surfaceColor);
	//*/
	MiniGL::drawTime(TimeManager::getCurrent()->getTime());

	auto str = "s = "+ std::to_string(TimeManager::getCurrent()->getTime());

	//MiniGL::drawBitmapText(-0.33,0.25, str.c_str(), str.size()-4, red);
	//frameIndex++;

	renderWorld();
}

/** Create a breaking dam scenario
*/
void createBreakingDam()
{
	const Real diam = 2.0*particleRadius*compact;
	const Real startX = particleRadius;
	const Real startY = particleRadius;
	const Real startZ = particleRadius;

	model.setParticleRadius(particleRadius);

	/* Test for simulation complete (check in)
	width = 80;
	height = 70;
	depth = 60;
	float temperatureContact = 50;
	float temperatureSolid = -5;

	timeEmision = 2;//2s
	numberFiles = 7;

	firtTimeAugmented = 25;//20s
	tempAugmented = 80;

	model.addModel(dataPath + "/bunny4k.txt", diam, temperatureSolid, 0.55f, Vector3r(-1.5, startY, -1.2), false,0.09);
	//model.addModel(dataPath + "/bunny-6001.txt", diam, temperatureSolid, 0.55f, Vector3r(-0.6, startY, -0.5), false, 0.07);
	model.setDilation(0.0001);
	model.setDifusion(25);
	model.setTempContact(temperatureContact);
	nameFile = "simulation";
	//*/

	/* Test for simulation complete - inverse (check in)
	width = 40;
	height = 100;
	depth = 40;
	float temperatureGas = 105.0;
	float temperatureLiquid = 5.1;
	float temperatureContact = 0;
	float temperatureSolid = -50;

	numberFiles = 3;

	model.addGas(diam, temperatureGas, Vector3r(width, 4, depth), Vector3r(0, 45*diam, 0));

	//model.addModel(dataPath + "/bunny4k.txt", diam, temperatureSolid, 0.55f, Vector3r(-1.5, 2*startY, -1.2), false);
	//model.addModel(dataPath + "/bunny-14032.txt", diam, temperatureSolid, 0.55f, Vector3r(-1.2, startY, -0.9), false, 0.09);
	model.addModel(dataPath + "/bunny-6001.txt", diam, temperatureSolid, 0.55f, Vector3r(-0.75, startY, -0.6), false, 0.07);

	model.setContact(true);
	model.setTempContact(temperatureContact);
	model.setDifusion(50);
	model.setDilation(0.0001);
	model.setNumMaxConstraints(4);
	model.setRadioSolidification(0.055);

	temperatureFloor = false;

	nameFile = "simulationI";

	//*/

	/* Test for heat and cool wather -> compare Transfer Heat (check in)
	width = 60;
	height = 20;//80;
	depth = 30;

	numberFiles = 4;

	float temperatureLiquid = 1, temperatureLiquid2 = 99;
	model.addFluid(diam, temperatureLiquid, Vector3r(width*0.5, 10, depth), Vector3r(-width*diam*0.25, 0, 0));
	model.addFluid(diam, temperatureLiquid2, Vector3r(width*0.5, 10, depth), Vector3r(width*diam*0.25, 0, 0));

	nameFile = "heat";
	//*/

	/* Test for liquid evaporation - convection  (check in)
	height = 70;
	depth = 30;//30;
	width = 30;
	float temperatureLiquid = 20;//80;
	float temperatureContact = 140;

	timeEmision = 2;//2s
	numberFiles = 8;

	model.addFluid(diam, temperatureLiquid, Vector3r(width, 20, depth), Vector3r(0,0.0045,0));
	//model.setContact(true);
	model.setTempContact(temperatureContact);
	model.setDifusion(40);

	nameFile = "evaporation";
	//*/

	/* Test for condensation (check in)
	width = 120;
	height = 175;
	depth = 40;
	float temperatureGas = 120.0;
	//float temperatureLiquid = 15.1;
	float temperatureContact = 20;
	float temperatureSolid = 0.0;

	numberFiles = 2;

	/*float temperatureAir = 10;
	//float temperatureAir1 = 10;
	float temperatureAir2 = 7;
	float temperatureAir3 = 3;
	float temperatureAir4 = 0;
	//model.addAir(diam, temperatureAir, Vector3r(width, 3, depth), Vector3r(0, height*diam - 155 * 2 * startY, 0),14);
	//model.addAir(diam, temperatureAir2, Vector3r(width, 3, depth), Vector3r(0, height*diam - 115 * 2 * startY, 0),13);
	//model.addAir(diam, temperatureAir3, Vector3r(width, 4, depth), Vector3r(0, height*diam - 77 * 2 * startY, 0),11);
	//model.addAir(diam, temperatureAir4, Vector3r(width, 4, depth), Vector3r(0, height*diam - 35 * 2 * startY, 0),10);

	numParticlesAir = model.getParticles().size();* /

	model.addGas(diam, temperatureGas, Vector3r(width, 17, depth), Vector3r(0, diam , 0));

	model.setContact(true);
	model.setTempContact(temperatureContact);
	model.setDifusion(20);
	model.setViscosity(0.001);

	nameFile = "condensation";
	//*/

	/* Test for melting bunny with heat liquid (check in)

	// change time step = 0.001953125, comment code add border top
	width = 80;
	height = 100;
	depth = 60;
	float temperatureLiquid = 99.0f;
	float temperatureSolid = -5;

	numberFiles = 2;

	model.addModel(dataPath + "/bunny4k.txt", diam, temperatureSolid, 0.55f, Vector3r(-1.5, startY, -1.2), false, 0.09);
	model.addFluid(diam, temperatureLiquid, Vector3r(5, 300, 5), Vector3r(0.35, 3.0, 0.4));

	model.setDilation(0.0001);
	model.setDifusion(30);

	nameFile = "melting";
	addWallTop = false;
	//*/

	/* Test for solidify bunny (check) -
	width = 80;
	height = 100;
	depth = 60;
	float temperatureLiquid = 10.0f;
	float temperatureSolid = -40;

	numberFiles = 1;

	model.addFluid(diam, temperatureLiquid, Vector3r(8, 200,4), Vector3r(-0.25, 3.2, 0.1));
	//model.addFluid(diam, temperatureLiquid, Vector3r(5, 130, 2), Vector3r(-0.0, 3.0, 0.4));
	//model.addModel(dataPath + "/bunny-6001.txt", diam, temperatureSolid,0.55f, Vector3r(-0.75, startY, -0.6),false,0.07);
	model.addModel(dataPath + "/bunny4k.txt", diam, temperatureSolid,0.55f, Vector3r(-1.5, startY, -1.2),false,0.09);
	model.setDilation(0.0001);
	model.setNumMaxConstraints(6);
	model.setRadioSolidification(0.1);
	model.setDifusion(20);

	nameFile = "solidify";
	addWallTop = false;
	//*/

	/* Test for ( dilatation) --
	width = 60;
	height = 60;
	depth = 50;
	float temperatureSolid = -50;
	float temperarureContact = -1;
	//model.addSolid(diam, temperatureSolid,Vector3r(20,20,20),Vector3r(0, 0,0));
	model.addModel(dataPath + "/bunny4k.txt", diam, temperatureSolid, 0.55f, Vector3r(-1.5, startY, -1.2), false, 0.09);
	//model.setContact(true);
	model.setDilation(0.01);
	model.setTempContact(temperarureContact);
	model.setDifusion(50);
	//*/

	/* Test for armadillo on liquid - caendo -- (check in)
	width = 60;
	height = 80;
	depth = 60;
	float temperatureLiquid = 50;
	float temperatureSolid = -40;

	numberFiles = 15;

	model.addFluid(diam, temperatureLiquid, Vector3r(width, 40, depth), Vector3r(0, 0, 0));
	model.addModel(dataPath + "/armadillo4k.txt", diam, temperatureSolid, 0.55f, Vector3r(-1.25, 4.0, -depth*diam * 0.25), false,0.07);
	model.setContact(false);
	model.setDilation(0.0001);
	model.setTempContact(temperatureLiquid);
	model.setDifusion(20);

	nameFile = "armadillo";
	addWallTop = false;
	//*/

	/////////////////////////////////////////////////

	/* Test for solidification del liquido
	width = 100;
	height = 50;
	depth = 30;
	float temperatureLiquid = 10;
	float temperatureSolid = 0;
	model.addFluid(diam, temperatureLiquid, Vector3r(width*0.25, 20, depth), Vector3r(-width*diam*3/8 + particleRadius, startY, 0));

	model.setContact(true);
	model.setTempContact(-5);
	//model.addSolid(diam, 0, Vector3r(20, 20, 20), Vector3r(0.24, 2 * startY, 0));

	nameFile = "simulationI";

	//*/

	/* Test for multiples ice on liquid - caendo --
	width = 80;
	height = 120;
	depth = 50;
	float temperatureLiquid = 10;
	float temperatureSolid = 0;
	model.addFluid(diam, temperatureLiquid, Vector3r(width, 20, depth), Vector3r(startX, 2*startY, startZ));
	model.addSolid(diam, temperatureSolid, Vector3r(14, 14, 14), Vector3r(-1.5, 1.7, -0.5));
	model.addSolid(diam, temperatureSolid, Vector3r(17, 17, 17), Vector3r(1.3, 1.5, 0.5));
	model.addSolid(diam, temperatureSolid, Vector3r(10, 10, 10), Vector3r(-0.5, 2.7, -0.5));
	model.addSolid(diam, temperatureSolid, Vector3r(5, 5, 5), Vector3r(0.0, 3, -0.0));
	model.addSolid(diam, temperatureSolid, Vector3r(5, 5, 5), Vector3r(1.0, 3.2, -0.2));
	model.addSolid(diam, temperatureSolid, Vector3r(10, 10, 10), Vector3r(-1.2, 2.5, 0.5));
	//model.addModel(dataPath + "/armadillo4k.txt", diam, temperatureSolid, 0.45f, Vector3r(-width*diam * 0.25, 2.0, -depth*diam * 0.25),false);
	model.setContact(true);
	model.setDilation(0.0001);
	model.setTempContact(temperatureLiquid);
	model.setDifusion(25);
	//*/

	/* Test for armadillo on liquid - caendo -- (check in)
	width = 100;
	height = 250;
	depth = 70;
	float temperatureLiquid = 50;
	float temperatureSolid = 0;
	model.addFluid(diam, temperatureLiquid, Vector3r(width, 15, depth), Vector3r(startX, 2 * startY, startZ));
	model.addModel(dataPath + "/armadillo4k.txt", diam, temperatureSolid, 0.55f, Vector3r(-width*diam * 0.25, 2.0, -depth*diam * 0.25), false);
	model.setContact(false);
	model.setDilation(0.0001);
	model.setTempContact(temperatureLiquid);
	model.setDifusion(20);
	//*/

	/* Test for meeltin ( derretimiento) --
	width = 50;
	height = 50;
	depth = 50;
	float temperatureSolid = 0;
	float temperarureContact = 10;
	model.addSolid(diam, temperatureSolid,Vector3r(30,40,20),Vector3r(particleRadius, 2 * startY,0));
	model.setContact(true);
	model.setDilation(0.0001);
	model.setTempContact(temperarureContact);
	model.setDifusion(10);
	//*/

	/* Test for heat and cool wather -> compare Transfer Heat cleary
	width = 80;
	height = 80;
	depth = 40;
	float temperatureLiquid = 6, temperatureLiquid2 = 80;
	model.heatTransferModel = 1;//model cleary
	model.addFluid(diam, temperatureLiquid, Vector3r(width*0.5, 15, depth), Vector3r(-width*diam*0.25 , 0, 0));
	model.addFluid(diam, temperatureLiquid2, Vector3r(width*0.5, 15, depth), Vector3r(width*diam*0.25 , 0, 0));

	model.setContact(false);
	model.setTempContact(temperatureLiquid2);
	//*/

	/* Test for liquidos caendo
	model.addFluid(diam, 21, Vector3r(3, 20, 3), Vector3r(-0.36, 2.0, 0.0));
	model.addFluid(diam, 55, Vector3r(3, 20, 3), Vector3r(0.16, 0.8, 0.18));
	model.addFluid(diam, 99, Vector3r(3, 20, 3), Vector3r(0.22, 2.8, -0.2));
	model.addSolid(diam, 0, Vector3r(25, 10, 15), Vector3r(0.025, 0, 0));
	//*/

	/* Test for simulation complete - inverse (check in)
	width = 100;
	height = 290;
	depth = 60;
	float temperatureGas = 109.0;
	//float temperatureLiquid = 15.1;
	float temperatureContact = 0;
	float temperatureSolid = 0.0;

	float temperatureAir = 10;
	//float temperatureAir1 = 10;
	float temperatureAir2 = 7;
	float temperatureAir3 = 3;
	float temperatureAir4 = 0;
	model.addAir(diam, temperatureAir, Vector3r(width, 3, depth), Vector3r(particleRadius, height*diam - 155 * 2 * startY, particleRadius), 14);
	model.addAir(diam, temperatureAir2, Vector3r(width, 3, depth), Vector3r(particleRadius, height*diam - 115 * 2 * startY, particleRadius), 13);
	model.addAir(diam, temperatureAir3, Vector3r(width, 4, depth), Vector3r(particleRadius, height*diam - 77 * 2 * startY, particleRadius), 11);
	model.addAir(diam, temperatureAir4, Vector3r(width, 4, depth), Vector3r(particleRadius, height*diam - 35 * 2 * startY, particleRadius), 10);

	numParticlesAir = model.getParticles().size();

	model.addGas(diam, temperatureGas, Vector3r(width, 20, depth), Vector3r(particleRadius, height*diam - 265 * 2 * startY, particleRadius));
	//model.addModel(dataPath + "/bunny.txt", diam, temperatureSolid,0.55f, Vector3r(-0.5, 2*startY, -0.3),false);
	//model.addModel(dataPath + "/bunny4k.txt", diam, temperatureSolid, 0.55f, Vector3r(-1.5, 2 * startY, -1.2), false);

	//printf("%f",);
	//model.addFluid(diam, temperatureLiquid, Vector3r(width, 10, depth), Vector3r(particleRadius, 2 * startY, particleRadius));

	model.setContact(true);
	model.setTempContact(temperatureContact);
	model.setDifusion(30);
	//*/

	/* Test for heat and cool wather -> measure temp (check in)
	width = 50;
	height = 20;
	depth = 1;
	float temperatureLiquid = 1, temperatureLiquid2 = 99;
	model.addFluid(diam, temperatureLiquid, Vector3r(width*0.5, 10, depth), Vector3r(-width*diam*0.25, 0, 0));
	model.addFluid(diam, temperatureLiquid2, Vector3r(width*0.5, 10, depth), Vector3r(width*diam*0.25, 0, 0));
	nameFile = "measure temp";
	//*/

	/* Test for melting bunny - number particles (check in)
	width = 80;
	height = 250;
	depth = 60;
	float temperatureSolid = -5;

	timeEmision = 2;
	numberFiles = 1;

	//model.addModel(dataPath + "/bunny-46607.txt", diam, temperatureSolid, 0.55f, Vector3r(-1.5, startY, -1.2), false,0.09);nameFile = "46607Particles";//0.09
	//model.addModel(dataPath + "/bunny-27152.txt", diam, temperatureSolid, 0.55f, Vector3r(-1.5, startY, -1.2), false, 0.09); nameFile = "27152Particles";//0.09
	//model.addModel(dataPath + "/bunny-14032.txt", diam, temperatureSolid, 0.55f, Vector3r(-1.2, startY, -0.9), false, 0.09);nameFile = "14032Particles";//0.09
	model.addModel(dataPath + "/bunny-6001.txt", diam, temperatureSolid, 0.55f, Vector3r(-0.8, startY, -0.7), false,0.08);nameFile = "6001Particles";//0.08
	//model.addModel(dataPath + "/bunny-1849.txt", diam, temperatureSolid, 0.55f, Vector3r(-0.6, startY, -0.5), false,0.07);nameFile = "1849Particles";//0.07

	model.setDilation(0.001);
	model.setDifusion(50);
	model.setTempContact(50);

	//*/
	/* Test for solidify bunny (check)
	width = 80;
	height = 300;
	depth = 60;
	float temperatureLiquid = 5.6f;
	float temperatureSolid = 0;

	model.addFluid(diam, temperatureLiquid, Vector3r(8, 200,4), Vector3r(-0.25, 3.2, 0.1));
	//model.addFluid(diam, temperatureLiquid, Vector3r(5, 130, 2), Vector3r(-0.0, 3.0, 0.4));
	//model.addModel(dataPath + "/bunny.txt", diam, temperatureSolid,0.55f, Vector3r(-0.3, 2*startY, 0.0),false);
	model.addModel(dataPath + "/bunny4k.txt", diam, temperatureSolid,0.55f, Vector3r(-1.5, 2*startY, -1.2),false);
	model.setDilation(0.0001);
	model.setNumMaxConstraints(6);
	model.setRadioSolidification(0.05);
	model.setDifusion(50);
	//*/

	//* Test for multiples demos - number particles (check in)
	width = 80;
	height = 90;
	depth = 60;
	float temperatureSolid = 50;

	//timeEmision = 1;
	numberFiles = 1;

	float scale = 0.6f;

	//model.addModel(dataPath + "/bunny-46607.txt", diam, temperatureSolid, scale, Vector3r(-1.65, startY, -1.3), false,0.09);//0.09
	//model.addModel(dataPath + "/bunny-27152.txt", diam, temperatureSolid, scale, Vector3r(-1.5, startY, -1.2), false, 0.09);//0.09
	//model.addModel(dataPath + "/bunny-14032.txt", diam, temperatureSolid, scale, Vector3r(-1.2, startY, -0.9), false, 0.09);//0.09
	//model.addModel(dataPath + "/bunny-6001.txt", diam, temperatureSolid, scale, Vector3r(-0.8, startY, -0.7), false,0.08);//0.08
	model.addModel(dataPath + "/bunny-1849.txt", diam, temperatureSolid, scale, Vector3r(-0.6, startY, -0.5), false,0.07);//0.07

	model.setDilation(0.001);
	model.setDifusion(50);
	model.setTempContact(-10);
	model.setContact(true);
	model.setRadioSolidification(0.05);
	model.setNumMaxConstraints(3);
	nameFile = "demos";

	//*/

	std::vector<Vector3r> boundaryParticles;
	std::vector<bool> boundaryActives;
	initBoundaryData(boundaryParticles, boundaryActives);

	model.initModel( (unsigned int)boundaryParticles.size(), boundaryParticles.data(), &boundaryActives);

	LOG_INFO << "Number particles:" << (model.getParticles().size() - numParticlesAir);
	//LOG_INFO << "Number of particles Total: " << model.getParticles().size();
	initData(model.getParticles().size() - numParticlesAir);
}

void addWall(const Vector3r &minX, const Vector3r &maxX, std::vector<Vector3r> &boundaryParticles, std::vector<bool> &boundaryActives,bool active)
{
	const Real particleDistance = 2*model.getParticleRadius()*compact;

	const Vector3r diff = maxX - minX;
	const unsigned int stepsX = (unsigned int)(round(diff[0] / particleDistance)) + 1u;
	const unsigned int stepsY = (unsigned int)(round(diff[1] / particleDistance)) + 1u;
	const unsigned int stepsZ = (unsigned int)(round(diff[2] / particleDistance)) + 1u;

	//printf("%d %d %d %f\n",stepsX, (unsigned int)(41.00),(unsigned int)(diff[0] / particleDistance), diff[0] / particleDistance);

	const unsigned int startIndex = (unsigned int) boundaryParticles.size();
	boundaryParticles.resize(startIndex + stepsX*stepsY*stepsZ);
	boundaryActives.resize(startIndex + stepsX*stepsY*stepsZ);

	#pragma omp parallel default(shared)
	{
		#pragma omp for schedule(static)
		for (int j = 0; j < (int)stepsX; j++)
		{
			for (unsigned int k = 0; k < stepsY; k++)
			{
				for (unsigned int l = 0; l < stepsZ; l++)
				{
					const Vector3r currPos = minX + Vector3r(j*particleDistance, k*particleDistance, l*particleDistance);
					boundaryParticles[startIndex + j*stepsY*stepsZ + k*stepsZ + l] = currPos;
					boundaryActives[startIndex + j*stepsY*stepsZ + k*stepsZ + l] = active;
				}
			}
		}
	}
}
void initBoundaryData(std::vector<Vector3r> &boundaryParticles, std::vector<bool> &boundaryActives)
{

	const Real containerWidth = (width)*particleRadius*compact;
	const Real containerDepth = (depth)*particleRadius*compact;
	const Real containerHeight = (height)*particleRadius*compact;

	const Real diameter = 2*particleRadius*compact;// compact;// *0.8;

	const Real x1 = -containerWidth  - particleRadius * compact;
	const Real x2 = containerWidth + particleRadius * compact;
	const Real y1 = -(sqrt(3) - 0.80)*particleRadius;
	const Real y2 = 2*containerHeight+ diameter - (sqrt(3) - 0.80)*particleRadius;
	const Real z1 = -containerDepth - particleRadius * compact;
	const Real z2 = containerDepth + particleRadius * compact;

	float rest = 0.001f;

	// Floor
	addWall(Vector3r(x1, y1, z1), Vector3r(x2, y1, z2), boundaryParticles, boundaryActives,temperatureFloor);
	// Top
	if(addWallTop)
		addWall(Vector3r(x1, y2, z1), Vector3r(x2, y2, z2), boundaryParticles, boundaryActives, true);
	// Left
	addWall(Vector3r(x1 - rest, y1+ diameter, z1+ diameter), Vector3r(x1 - rest, y2 - diameter, z2- diameter), boundaryParticles, boundaryActives, false);
	// Right
	addWall(Vector3r(x2 + rest, y1 + diameter, z1+ diameter), Vector3r(x2 + rest, y2 - diameter, z2- diameter), boundaryParticles, boundaryActives, false);
	// Back
	addWall(Vector3r(x1, y1 + diameter, z1), Vector3r(x2, y2 - diameter, z1), boundaryParticles, boundaryActives, false);
	// Front
	addWall(Vector3r(x1, y1 + diameter, z2), Vector3r(x2, y2- diameter, z2), boundaryParticles, boundaryActives, false);
}

void createSphereBuffers(Real radius, int resolution)
{
	Real PI = static_cast<Real>(M_PI);
	// vectors to hold our data
	// vertice positions
	std::vector<Vector3r> v;
	// normals
	std::vector<Vector3r> n;
	std::vector<unsigned short> indices;

	// initiate the variable we are going to use
	Real X1, Y1, X2, Y2, Z1, Z2;
	Real inc1, inc2, inc3, inc4, radius1, radius2;

	for (int w = 0; w < resolution; w++)
	{
		for (int h = (-resolution / 2); h < (resolution / 2); h++)
		{
			inc1 = (w / (Real)resolution) * 2 * PI;
			inc2 = ((w + 1) / (Real)resolution) * 2 * PI;
			inc3 = (h / (Real)resolution)*PI;
			inc4 = ((h + 1) / (Real)resolution)*PI;

			X1 = sin(inc1);
			Y1 = cos(inc1);
			X2 = sin(inc2);
			Y2 = cos(inc2);

			// store the upper and lower radius, remember everything is going to be drawn as triangles
			radius1 = radius*cos(inc3);
			radius2 = radius*cos(inc4);

			Z1 = radius*sin(inc3);
			Z2 = radius*sin(inc4);

			// insert the triangle coordinates
			v.push_back(Vector3r(radius1*X1, Z1, radius1*Y1));
			v.push_back(Vector3r(radius1*X2, Z1, radius1*Y2));
			v.push_back(Vector3r(radius2*X2, Z2, radius2*Y2));

			indices.push_back((unsigned short)v.size() - 3);
			indices.push_back((unsigned short)v.size() - 2);
			indices.push_back((unsigned short)v.size() - 1);

			v.push_back(Vector3r(radius1*X1, Z1, radius1*Y1));
			v.push_back(Vector3r(radius2*X2, Z2, radius2*Y2));
			v.push_back(Vector3r(radius2*X1, Z2, radius2*Y1));

			indices.push_back((unsigned short)v.size() - 3);
			indices.push_back((unsigned short)v.size() - 2);
			indices.push_back((unsigned short)v.size() - 1);

			// insert the normal data
			n.push_back(Vector3r(X1, Z1, Y1));
			n.push_back(Vector3r(X2, Z1, Y2));
			n.push_back(Vector3r(X2, Z2, Y2));
			n.push_back(Vector3r(X1, Z1, Y1));
			n.push_back(Vector3r(X2, Z2, Y2));
			n.push_back(Vector3r(X1, Z2, Y1));
		}
	}

	for (unsigned int i = 0; i < n.size(); i++)
		n[i].normalize();

	glGenBuffersARB(1, &vertexbuffer);
	glBindBufferARB(GL_ARRAY_BUFFER_ARB, vertexbuffer);
	glBufferDataARB(GL_ARRAY_BUFFER_ARB, v.size() * sizeof(Vector3r), &v[0], GL_STATIC_DRAW);

	glGenBuffersARB(1, &normalbuffer);
	glBindBufferARB(GL_ARRAY_BUFFER_ARB, normalbuffer);
	glBufferDataARB(GL_ARRAY_BUFFER_ARB, n.size() * sizeof(Vector3r), &n[0], GL_STATIC_DRAW);
	glBindBufferARB(GL_ARRAY_BUFFER_ARB, 0);

	// Generate a buffer for the indices as well
	glGenBuffersARB(1, &elementbuffer);
	glBindBufferARB(GL_ELEMENT_ARRAY_BUFFER_ARB, elementbuffer);
	glBufferDataARB(GL_ELEMENT_ARRAY_BUFFER_ARB, indices.size() * sizeof(unsigned short), &indices[0], GL_STATIC_DRAW);
	glBindBufferARB(GL_ELEMENT_ARRAY_BUFFER_ARB, 0);

	// store the number of indices for later use
	vertexBufferSize = (unsigned int)indices.size();

	// clean up after us
	indices.clear();
	n.clear();
	v.clear();
}

void renderSphere(const Vector3r &x, const float color[])
{
	glEnableClientState(GL_VERTEX_ARRAY);
	glEnableClientState(GL_NORMAL_ARRAY);

	glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, color);
	glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, color);

	glBindBufferARB(GL_ARRAY_BUFFER_ARB, vertexbuffer);
	glVertexPointer(3, GL_REAL, 0, 0);

	glBindBufferARB(GL_ARRAY_BUFFER_ARB, normalbuffer);
	glNormalPointer(GL_REAL, 0, 0);

	glBindBufferARB(GL_ELEMENT_ARRAY_BUFFER_ARB, elementbuffer);

	glPushMatrix();
	glTranslated(x[0], x[1], x[2]);
	glDrawElements(GL_TRIANGLES, (GLsizei)vertexBufferSize, GL_UNSIGNED_SHORT, 0);
	glPopMatrix();
	glBindBufferARB(GL_ELEMENT_ARRAY_BUFFER_ARB, 0);
	glBindBufferARB(GL_ARRAY_BUFFER_ARB, 0);

	glDisableClientState(GL_NORMAL_ARRAY);
	glDisableClientState(GL_VERTEX_ARRAY);
}

void releaseSphereBuffers()
{
	if (elementbuffer != 0)
	{
		glDeleteBuffersARB(1, &elementbuffer);
		elementbuffer = 0;
	}
	if (normalbuffer != 0)
	{
		glDeleteBuffersARB(1, &normalbuffer);
		normalbuffer = 0;
	}
	if (vertexbuffer != 0)
	{
		glDeleteBuffersARB(1, &vertexbuffer);
		vertexbuffer = 0;
	}
}

void TW_CALL setTimeStep(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	TimeManager::getCurrent()->setTimeStepSize(val);
}

void TW_CALL getTimeStep(void *value, void *clientData)
{
	*(Real *)(value) = TimeManager::getCurrent()->getTimeStepSize();
}

void TW_CALL setVelocityUpdateMethod(const void *value, void *clientData)
{
	const short val = *(const short *)(value);
	((TimeStepFluidModel*)clientData)->setVelocityUpdateMethod((unsigned int)val);
}

void TW_CALL getVelocityUpdateMethod(void *value, void *clientData)
{
	*(short *)(value) = (short)((TimeStepFluidModel*)clientData)->getVelocityUpdateMethod();
}
//////

void TW_CALL setTempUpdateMethod(const void *value, void *clientData)
{
	const short val = *(const short *)(value);
	((FluidModel*)clientData)->heatTransferModel  = (unsigned int)val;
}

void TW_CALL getTempUpdateMethod(void *value, void *clientData)
{
	*(short *)(value) = (short)((FluidModel*)clientData)->heatTransferModel ;
}

//////
void TW_CALL setViscosity(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((FluidModel*)clientData)->setViscosity(val);
}

void TW_CALL getViscosity(void *value, void *clientData)
{
	*(Real *)(value) = ((FluidModel*)clientData)->getViscosity();
}

void TW_CALL getRadioSol(void *value, void *clientData)
{
	*(Real *)(value) = ((FluidModel*)clientData)->getRadioSolidification();
}
void TW_CALL setRadioSol(const void *value, void *clientData) {
	const Real val = *(const Real *)(value);
	((FluidModel*)clientData)->setRadioSolidification(val);
}
void TW_CALL getNumConstraint(void *value, void *clientData)
{
	*(Real *)(value) = ((FluidModel*)clientData)->getNumMaxConstraints();
}
void TW_CALL setNumConstraint(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((FluidModel*)clientData)->setNumMaxConstraints(val);
}
void TW_CALL setStiffness(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((FluidModel*)clientData)->setClothStiffness(val);
}

void TW_CALL getStiffness(void *value, void *clientData)
{
	*(Real *)(value) = ((FluidModel*)clientData)->getClothStiffness();
}

void TW_CALL setDilation(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((FluidModel*)clientData)->setDilation(val);
}

void TW_CALL getDilation(void *value, void *clientData)
{
	*(Real *)(value) = ((FluidModel*)clientData)->getDilation();
}

void TW_CALL setMelting(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((FluidModel*)clientData)->setMelting(val);
}

void TW_CALL getMelting(void *value, void *clientData)
{
	*(Real *)(value) = ((FluidModel*)clientData)->getMelting();
}

void TW_CALL setEvaporation(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((FluidModel*)clientData)->setEvaporation(val);
}

void TW_CALL getEvaporation(void *value, void *clientData)
{
	*(Real *)(value) = ((FluidModel*)clientData)->getEvaporation();
}

void TW_CALL setDifusion(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((FluidModel*)clientData)->setDifusion(val);
}

void TW_CALL getDifusion(void *value, void *clientData)
{
	*(Real *)(value) = ((FluidModel*)clientData)->getDifusion();
}

void TW_CALL setTempContact(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((FluidModel*)clientData)->setTempContact(val);
}

void TW_CALL getTempContact(void *value, void *clientData)
{
	*(Real *)(value) = ((FluidModel*)clientData)->getTempContact();
}

void TW_CALL setContact(const void *value, void *clientData)
{
	const bool val = *(const bool *)(value);
	((FluidModel*)clientData)->setContact(val);
}

void TW_CALL getContact(void *value, void *clientData)
{
	*(bool *)(value) = ((FluidModel*)clientData)->getContact();
}

void TW_CALL setXPBD(const void *value, void *clientData)
{
	const bool val = *(const bool *)(value);
	((FluidModel*)clientData)->setXPBD(val);
}

void TW_CALL getXPBD(void *value, void *clientData)
{
	*(bool *)(value) = ((FluidModel*)clientData)->getXPBD();
}

void TW_CALL setDesWidth(const void *value, void *clientData)
{
	int val = *( int *)(value);
	MiniGL::desWidth = val;
	MiniGL::reziseDesWidth();
}

void TW_CALL getDesWidth(void *value, void *clientData)
{
	*(int*)(value) = MiniGL::desWidth;
}

void TW_CALL setDesHeight(const void *value, void *clientData)
{
	int val = *(int *)(value);
	MiniGL::desHeight = val;
	MiniGL::reziseDesHeight();
}

void TW_CALL getDesHeight(void *value, void *clientData)
{
	*(int*)(value) = MiniGL::desHeight;
}


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_IF_ALLBITS_ELSE_ZERO_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IF_ALLBITS_ELSE_ZERO_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-boolean
    This function object returns a mask of bits. All ones if the
    argument is non @ref Zero else all zeros.
    This is an alias of @ref genmask

    @par Header <boost/simd/function/if_allbits_else_zero.hpp>

    @par Example:

      @snippet if_allbits_else_zero.cpp if_allbits_else_zero

    @par Possible output:

      @snippet if_allbits_else_zero.txt if_allbits_else_zero

  **/

  ///@{
  Value if_allbits_else_zero(Value const& x);
  as_arithmetic_t<LogicalValue> if_allbits_else_zero(LogicalValue const& x);
  ///@}
} }
#endif

#include <boost/simd/function/scalar/genmask.hpp>
#include <boost/simd/function/simd/if_allbits_else_zero.hpp>

#endif


/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2015 Artem Pavlenko
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

#ifndef MAPNIK_COORD_HPP
#define MAPNIK_COORD_HPP

#pragma GCC diagnostic push
#include <mapnik/warning_ignore.hpp>
#include <boost/operators.hpp>
#pragma GCC diagnostic pop

namespace mapnik {
template <typename T,int dim>
struct coord {
    using type = T;
};

template <typename T>
struct coord<T,2>
    : boost::addable<coord<T,2>,
                     boost::addable2<coord<T,2>,T,
                                     boost::subtractable<coord<T,2>,
                                                         boost::subtractable2<coord<T,2>,T,
                                                                              boost::dividable2<coord<T,2>, T,
                                                                                boost::multipliable2<coord<T,2>, T > > > > > >

{
    using type = T;
    T x;
    T y;
public:
    coord()
        : x(),y() {}
    coord(T x_,T y_)
        : x(x_),y(y_) {}

    coord(coord<T,2> const& rhs)
        : x(rhs.x),
          y(rhs.y) {}

    template <typename T2>
    coord (coord<T2,2> const& rhs)
        : x(type(rhs.x)),
          y(type(rhs.y)) {}

    coord(coord<T,2> && rhs) noexcept
        : x(std::move(rhs.x)),
          y(std::move(rhs.y)) {}

    coord<T,2>& operator=(coord<T,2> rhs)
    {
        swap(rhs);
        return *this;
    }

    template <typename T2>
    coord<T,2>& operator=(const coord<T2,2>& rhs)
    {
        coord<T,2> tmp(rhs);
        swap(rhs);
        return *this;
    }

    template <typename T2>
    bool operator==(coord<T2,2> const& rhs)
    {
        return x == rhs.x && y == rhs.y;
    }

    coord<T,2>& operator+=(coord<T,2> const& rhs)
    {
        x+=rhs.x;
        y+=rhs.y;
        return *this;
    }

    coord<T,2>& operator+=(T rhs)
    {
        x+=rhs;
        y+=rhs;
        return *this;
    }

    coord<T,2>& operator-=(coord<T,2> const& rhs)
    {
        x-=rhs.x;
        y-=rhs.y;
        return *this;
    }

    coord<T,2>& operator-=(T rhs)
    {
        x-=rhs;
        y-=rhs;
        return *this;
    }

    coord<T,2>& operator*=(T t)
    {
        x*=t;
        y*=t;
        return *this;
    }
    coord<T,2>& operator/=(T t)
    {
        x/=t;
        y/=t;
        return *this;
    }
private:
    void swap(coord<T,2> & rhs)
    {
        std::swap(this->x, rhs.x);
        std::swap(this->y, rhs.y);
    }
};

template <typename T>
struct coord<T,3>
{
    using type = T;
    T x;
    T y;
    T z;
public:
    coord()
        : x(),y(),z() {}
    coord(T x_,T y_,T z_)
        : x(x_),y(y_),z(z_) {}

    template <typename T2>
    coord (coord<T2,3> const& rhs)
        : x(type(rhs.x)),
          y(type(rhs.y)),
          z(type(rhs.z)) {}

    coord(coord<T,3> && rhs) noexcept
        : x(std::move(rhs.x)),
          y(std::move(rhs.y)),
          z(std::move(rhs.z)) {}

    coord<T,3> operator=(coord<T,3> rhs)
    {
        swap(rhs);
        return *this;
    }

    template <typename T2>
    coord<T,3>& operator=(coord<T2,3> const& rhs)
    {
        coord<T,3> tmp(rhs);
        swap(tmp);
        return *this;
    }
private:
    void swap(coord<T,3> & rhs)
    {
        std::swap(this->x, rhs.x);
        std::swap(this->y, rhs.y);
        std::swap(this->z, rhs.z);
    }
};

using coord2d = coord<double,2>;
using coord2i = coord<int,2>;

}

#endif // MAPNIK_COORD_HPP


/* Boost interval/detail/c99sub_rounding_control.hpp file
 *
 * Copyright 2000 Jens Maurer
 * Copyright 2002 Herv� Br�nnimann, Guillaume Melquiond, Sylvain Pion
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_NUMERIC_INTERVAL_DETAIL_C99SUB_ROUNDING_CONTROL_HPP
#define BOOST_NUMERIC_INTERVAL_DETAIL_C99SUB_ROUNDING_CONTROL_HPP

#include <boost/detail/fenv.hpp>      // ISO C 99 rounding mode control

namespace boost {
namespace numeric {
namespace interval_lib {
namespace detail {

extern "C" { double rint(double); }

struct c99_rounding_control
{
  typedef int rounding_mode;

  static void set_rounding_mode(rounding_mode mode)  { fesetround(mode); }
  static void get_rounding_mode(rounding_mode &mode) { mode = fegetround(); }
  static void downward()    { set_rounding_mode(FE_DOWNWARD);   }
  static void upward()      { set_rounding_mode(FE_UPWARD);     }
  static void to_nearest()  { set_rounding_mode(FE_TONEAREST);  }
  static void toward_zero() { set_rounding_mode(FE_TOWARDZERO); }

  template<class T>
  static T to_int(const T& r) { return rint(r); }
};

} // namespace detail
} // namespace interval_lib
} // namespace numeric
} // namespace boost

#endif // BOOST_NUMERIC_INTERVAL_DETAIL_C99SUB_ROUBDING_CONTROL_HPP


#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/qi_parse.hpp>
#include <boost/spirit/include/phoenix_function.hpp>
#include <boost/spirit/include/phoenix_stl.hpp>
#include <boost/fusion/include/std_pair.hpp>

#include "parser.h"

Parser::Parser() {}

namespace qi = boost::spirit::qi;
namespace ascii = boost::spirit::ascii;
namespace phx = boost::phoenix;

// this function object is used for star'ing from within the parser
// a new equation with the given new variable is created and returned
struct star_impl
{
	template <typename T, typename SR> struct result { typedef std::pair<VarId, SR> type; }; // needed because this is how boost knows of what type the result will be

	template <typename SR>
	std::pair<VarId, SR> operator()(VarId var, SR& s) const // var → 1 + var×s
	{
		return std::pair<VarId, SR>(var, (SR::one() + SR(var)*s));
	}
};
const phx::function<star_impl> star_equation;

struct option_impl
{
	template <typename T> struct result { typedef T type; }; // needed because this is how boost knows of what type the result will be

	template <typename SR>
	SR operator()(SR& s) const // A → 1 + A
	{
		return (SR::one() + s);
	}
};
const phx::function<option_impl> option;

struct rexp_var_impl
{
        template <typename T>
        struct result { typedef CommutativeRExp type; }; // this tells Boost the return type

        const CommutativeRExp operator()(std::string& s) const
        {
                // create an element with the given var
                return CommutativeRExp(Var::GetVarId(s));
        }
};
const phx::function<rexp_var_impl> rexp_var;

struct free_var_impl
{
        template <typename T>
        struct result { typedef FreeSemiring type; }; // this tells Boost the return type

        const FreeSemiring operator()(std::string& s) const
        {
                // create an element with the given var
                return FreeSemiring(Var::GetVarId(s));
        }
};
const phx::function<free_var_impl> free_var;

struct lossy_fa_var_impl
{
        template <typename T>
        struct result { typedef LossyFiniteAutomaton type; }; // this tells Boost the return type

        const LossyFiniteAutomaton operator()(std::string& s) const
        {
                // create an element with the given var
                return LossyFiniteAutomaton(Var::GetVarId(s));
        }
};
const phx::function<lossy_fa_var_impl> lossy_fa_var;

struct prefix_impl
{
	template <typename T, typename U>
	struct result { typedef PrefixSemiring type; }; // this tells Boost the return type

	const PrefixSemiring operator()(std::vector<VarId>& p, unsigned int length) const
	{
		// create a prefix with the given vars
		return PrefixSemiring(p, length);
	}
};
const phx::function<prefix_impl> prefix;

struct slset_var_impl
{
	template <typename T, typename U>
	struct result { typedef SemilinSetExp type; }; // this tells Boost the return type

	const SemilinSetExp operator()(std::string& s, unsigned int& cnt) const
	{
		// create an element with the given var
		return SemilinSetExp(Var::GetVarId(s), cnt);
	}
};
const phx::function<slset_var_impl> slset_var;

struct var_impl
{
	template <typename T>
	struct result { typedef VarId type; }; // this tells Boost the return type

	const VarId operator()(std::string& var) const
	{
		return Var::GetVarId(var);
	}
};
const phx::function<var_impl> variable;

struct new_var_impl
{
	template <typename T>
	struct result { typedef VarId type; }; // this tells Boost the return type

	template <typename T>
	const VarId operator()(T t) const // TODO: why has there to be an argument??? compiling fails without dummy argument... overloading???
	{
		return Var::GetVarId(); // return a fresh anonymous variable
	}
};
const phx::function<new_var_impl> new_var;

// some boost::qi methods for pushing values through our parser
using qi::_val;
using qi::_1;
using qi::_2;
using qi::_a;
using qi::lit;
using qi::lexeme;
using qi::eps;
using phx::ref; // must be used as phx::ref, otherwise g++ is confused with std::ref...
using phx::insert;
using phx::end;
using phx::begin;
using phx::ref;

typedef std::string::const_iterator iterator_type;

// parser for a float semiring element
struct float_elem_parser : qi::grammar<iterator_type, FloatSemiring()>
{
	float_elem_parser() : float_elem_parser::base_type(elem)
	{
		elem %= qi::double_;
	}
	qi::rule<iterator_type, FloatSemiring()> elem;
};

// parser for a commutative regular expression semiring element
struct rexp_elem_parser : qi::grammar<iterator_type, CommutativeRExp()>
{
	rexp_elem_parser() : rexp_elem_parser::base_type(elem)
	{
		elem = '"' >> qi::as_string[lexeme[+(ascii::char_ -'"')]] [_val = rexp_var(_1)] >> '"';
	}
	qi::rule<iterator_type, CommutativeRExp()> elem;
};

// parser for a semilinear set expression semiring element
// e.g.: "<a:2, b:5, c:7>" or "<a:1, b:2>" or "<>" (for the one-element)
struct slset_elem_parser : qi::grammar<iterator_type, SemilinSetExp(), qi::space_type>
{
	slset_elem_parser() : slset_elem_parser::base_type(elem)
	{
		// create new sl-set elements for each entry e.g. "a:2" and aggregate them by using multiplication!
		elem = qi::lit("\"<") >> eps [_val = SemilinSetExp::one()] >>
				   var_cnt [_val = _val * _1] >>
				   *( (',' >> var_cnt) [_val = _val * _1] ) >> qi::lit(">\"")
		    |  qi::lit("\"<") >> eps [_val = SemilinSetExp::one()] >> qi::lit(">\"");
		var_cnt = (varidentifier >> ':' >> qi::uint_) [_val = slset_var(_1, _2)];
		varidentifier = qi::as_string[+(qi::alnum)];
	}
	qi::rule<iterator_type, SemilinSetExp(), qi::space_type> elem;
	qi::rule<iterator_type, SemilinSetExp(), qi::space_type> var_cnt;
	qi::rule<iterator_type, std::string()> varidentifier;
};

// parser for a free semiring element
struct free_elem_parser : qi::grammar<iterator_type, FreeSemiring()>
{
        free_elem_parser() : free_elem_parser::base_type(elem)
        {
                elem = '"' >> qi::as_string[lexeme[+(ascii::char_ -'"')]] [_val = free_var(_1)] >> '"';
        }
        qi::rule<iterator_type, FreeSemiring()> elem;
};

// parser for a lossy semiring element
struct lossy_fa_elem_parser : qi::grammar<iterator_type, LossyFiniteAutomaton()>
{
        lossy_fa_elem_parser() : lossy_fa_elem_parser::base_type(elem)
        {
                elem = '"' >> qi::as_string[lexeme[+(ascii::char_ -'"')]] [_val = lossy_fa_var(_1)] >> '"';
        }
        qi::rule<iterator_type, LossyFiniteAutomaton()> elem;
};

// parser for a prefix semiring element
struct prefix_elem_parser : qi::grammar<iterator_type, PrefixSemiring()>
{
  unsigned int max_length;
        prefix_elem_parser() : prefix_elem_parser::base_type(elem)
        {
                elem = '"' >> (*var)[_val = prefix(_1,phx::ref(max_length))] >> '"';
                var = qi::as_string[lexeme[+(ascii::char_ - '"' - ',')]] [_val = variable(_1)] >> (',');
        }
        qi::rule<iterator_type, PrefixSemiring()> elem;
        qi::rule<iterator_type, VarId()> var;
};

// use the given parser and return a list of equations of SR polynomials
// the second parameter is either Polynomial<SR> or NonCommutativePolynomial<SR>
template <typename SR_Parser, typename PSR>
struct equation_parser : qi::grammar<iterator_type, std::vector<std::pair<VarId, PSR>>(), qi::space_type>
{
	std::vector<std::pair<VarId, PSR>> new_rules; // accumulate generated rules in this vector
	equation_parser() : equation_parser::base_type(equations)
	{
		// the first rule combines the generated equations with the read equations and returns them in one vector
		equations = (*equation)[_val = _1] [insert(_val, end(_val), begin(phx::ref(new_rules)), end(phx::ref(new_rules)))];
		equation %= (var >> lexeme["::="] >> polynomial >> ';');
		polynomial = summand [_val = _1] >> *('|' >> summand [_val = _val + _1]); // addition
		summand = eps [_val = PSR::one()] >> // set _val to one-element of the semiring
				*(
					('(' >> polynomial >> ')') [_val = _val * _1] | // group term
					('{' >> kstar >> '}') [_val = _val * _1] | // | // kleene star: {A} → B && B = 1 + BA
					('[' >> polynomial >>']')  [_val = _val * option(_1)] | // optional term: [A] → 1 + A
					var [_val = _val * _1] | // variable factor
					sr_elem [_val = _val * _1] // sr-elem factor
				 );
		var = varidentifier[_val = variable(_1)];
		kstar = polynomial	[_a = new_var(0)] // create a new variable _a for the new equation
					[phx::push_back(phx::ref(new_rules),star_equation(_a,_1))] // create a new equation _a = <stared equation> and save it
					[_val = _a]; // continue with the new variable
		varidentifier = qi::as_string[lexeme['<' >> +(ascii::char_ - '>') >> '>']];
	}
	qi::rule<iterator_type, std::vector<std::pair<VarId, PSR>>(), qi::space_type> equations;
	qi::rule<iterator_type, std::pair<VarId, PSR>(), qi::space_type> equation;
	qi::rule<iterator_type, PSR(), qi::space_type> polynomial;
	qi::rule<iterator_type, PSR(), qi::space_type> summand;
	qi::rule<iterator_type, std::string()> varidentifier;
	qi::rule<iterator_type, VarId(), qi::locals<VarId>, qi::space_type> kstar;
	qi::rule<iterator_type, VarId()> var;
	SR_Parser sr_elem;
};

// non commutative function for using the parser with one int and the prefix semiring
std::vector<std::pair<VarId, NonCommutativePolynomial<PrefixSemiring>>> non_commutative_parser(std::string input, unsigned int length)
{
        typedef equation_parser<prefix_elem_parser, NonCommutativePolynomial<PrefixSemiring>> equation_parser;
        equation_parser equation;
        equation.sr_elem.max_length = length;

        iterator_type iter = input.begin();
        iterator_type end = input.end();

        std::vector<std::pair<VarId, NonCommutativePolynomial<PrefixSemiring>>> result;
        if(!(qi::phrase_parse(iter, end, equation, qi::space, result) && iter == end))
                std::cout << "bad input, failed at: " << std::string(iter, end) << std::endl;

        return result;
}

// non commutative generic function for using the parser
template <typename SR_Parser, typename SR>
std::vector<std::pair<VarId, NonCommutativePolynomial<SR>>> non_commutative_parser(std::string input)
{
        typedef equation_parser<SR_Parser, NonCommutativePolynomial<SR>> equation_parser;
        equation_parser equation;

        iterator_type iter = input.begin();
        iterator_type end = input.end();

        std::vector<std::pair<VarId, NonCommutativePolynomial<SR>>> result;
        if(!(qi::phrase_parse(iter, end, equation, qi::space, result) && iter == end))
                std::cout << "bad input, failed at: " << std::string(iter, end) << std::endl;

        return result;
}

// commutative generic function for using the parser
template <typename SR_Parser, typename SR>
std::vector<std::pair<VarId, Polynomial<SR>>> commutative_parser(std::string input)
{
	typedef equation_parser<SR_Parser, Polynomial<SR>> equation_parser;
	equation_parser equation;

	iterator_type iter = input.begin();
	iterator_type end = input.end();

	std::vector<std::pair<VarId, Polynomial<SR>>> result;
	if(!(qi::phrase_parse(iter, end, equation, qi::space, result) && iter == end))
		std::cout << "bad input, failed at: " << std::string(iter, end) << std::endl;

	return result;
}

// wrapper function for float equations
std::vector<std::pair<VarId, Polynomial<FloatSemiring>>> Parser::float_parser(std::string input)
{
	return commutative_parser<float_elem_parser, FloatSemiring>(input);
}

// wrapper function for regular expression equations
std::vector<std::pair<VarId, Polynomial<CommutativeRExp>>> Parser::rexp_parser(std::string input)
{
	return commutative_parser<rexp_elem_parser, CommutativeRExp>(input);
}

// wrapper function for explicit semilinear set equations
std::vector<std::pair<VarId, Polynomial<SemilinSetExp>>> Parser::slset_parser(std::string input)
{
        return commutative_parser<slset_elem_parser, SemilinSetExp>(input);
}

// wrapper function for free semiring equations
std::vector<std::pair<VarId, NonCommutativePolynomial<FreeSemiring>>> Parser::free_parser(std::string input)
{
        return non_commutative_parser<free_elem_parser, FreeSemiring>(input);
}

// wrapper function for lossy semiring equations
std::vector<std::pair<VarId, NonCommutativePolynomial<LossyFiniteAutomaton>>> Parser::lossy_fa_parser(std::string input)
{
        return non_commutative_parser<lossy_fa_elem_parser, LossyFiniteAutomaton>(input);
}

// wrapper function for prefix semiring equations
std::vector<std::pair<VarId, NonCommutativePolynomial<PrefixSemiring>>> Parser::prefix_parser(std::string input, unsigned int length)
{
	return non_commutative_parser(input, length);
}


#include <boost/units/physical_dimensions/angular_acceleration.hpp>