/**********************************************************************
*  Copyright (c) 2008-2016, Alliance for Sustainable Energy.
*  All rights reserved.
*
*  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 Street, Fifth Floor, Boston, MA  02110-1301  USA
**********************************************************************/

#include <gtest/gtest.h>
#include "../Intersection.hpp"
#include "GeometryFixture.hpp"

#undef BOOST_UBLAS_TYPE_CHECK
#include <boost/geometry/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/ring.hpp>
#include <boost/geometry/multi/geometries/multi_polygon.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>

typedef boost::geometry::model::d2::point_xy<double> BoostPoint;
typedef boost::geometry::model::polygon<BoostPoint> BoostPolygon;
typedef boost::geometry::model::ring<BoostPoint> BoostRing;
typedef boost::geometry::model::multi_polygon<BoostPolygon> BoostMultiPolygon;

using namespace std;
using namespace boost;
using namespace openstudio;

std::string printPolygon(const BoostPolygon& polygon)
{
  std::stringstream ss;

  ss << boost::geometry::area(polygon);

  BoostRing outer = polygon.outer();
  if (outer.empty()){
    return ss.str();
  }

  // add point for each vertex except final vertex
  ss << " [";
  for(unsigned i = 0; i < outer.size(); ++i){
    ss << outer[i].x() << " " << outer[i].y();
    if (i < outer.size()-1){
      ss << ", ";
    }
  }

  for (const BoostRing& inner : polygon.inners()){
    if (!inner.empty()){
      ss << "], [";
      // inner loop already in reverse order
      for(unsigned i = 0; i < inner.size(); ++i){
        ss << inner[i].x() << " " << inner[i].y();
        if (i < inner.size()-1){
          ss << ", ";
        }
      }
    }
  }
  ss << "]";

  return ss.str();
}

std::vector<Point3d> makeRectangleUp(double xmin, double ymin, double width, double height)
{
  std::vector<Point3d> result;
  result.push_back(Point3d(xmin, ymin, 0));
  result.push_back(Point3d(xmin+width, ymin, 0));
  result.push_back(Point3d(xmin+width, ymin+height, 0));
  result.push_back(Point3d(xmin, ymin+height, 0));
  return result;
}

std::vector<Point3d> makeRectangleDown(double xmin, double ymin, double width, double height)
{
  std::vector<Point3d> result;
  result.push_back(Point3d(xmin+width, ymin+height, 0));
  result.push_back(Point3d(xmin+width, ymin, 0));
  result.push_back(Point3d(xmin, ymin, 0));
  result.push_back(Point3d(xmin, ymin+height, 0));
  return result;
}

TEST_F(GeometryFixture, BoostGeometry_Polygon1)
{
  BoostPolygon blue, yellow;

  // blue
  boost::geometry::read_wkt(
      "POLYGON((-11.379200508 -12.0396003048 , -11.379200508 12.0395996952 , 3.555999492 12.0395996952 , 3.555999492 -12.0396003048 , -11.379200508 -12.0396003048))", blue);
  //Surface Intersection Model 74
  //Surface 37,                             !- Name
  //-11.379200508, -12.0396003048, 3.3528,  !- X,Y,Z Vertex 1 {m}
  //-11.379200508, 12.0395996952, 3.3528,   !- X,Y,Z Vertex 2 {m}
  //3.555999492, 12.0395996952, 3.3528,     !- X,Y,Z Vertex 3 {m}
  //3.555999492, -12.0396003048, 3.3528;    !- X,Y,Z Vertex 4 {m}

  // yellow
  boost::geometry::read_wkt(
      "POLYGON((3.2512004064 -7.4676003048 , 3.2512004064 7.4675996952 , 3.555999492 7.7723987808 , 3.555999492 -7.7723993904 , 3.2512004064 -7.4676003048))", yellow);
  //Surface Intersection Model 74
  //Surface 44,                             !- Name
  //3.555999492, -7.7723993904, 3.3528,     !- X,Y,Z Vertex 1 {m}
  //3.555999492, 7.7723987808, 3.3528,      !- X,Y,Z Vertex 2 {m}
  //3.2512004064, 7.4675996952, 3.3528,     !- X,Y,Z Vertex 3 {m}
  //3.2512004064, -7.4676003048, 3.3528;    !- X,Y,Z Vertex 4 {m}

  std::vector<BoostPolygon> output;
  boost::geometry::difference(yellow, blue, output);

  int i = 0;
  std::cout << "yellow - blue:" << std::endl;
  for (BoostPolygon const& p : output)
  {
    std::cout << i++ << ": " << printPolygon(p) << std::endl;
  }

  output.clear();
  boost::geometry::difference(blue, yellow, output);

  i = 0;
  std::cout << "blue - yellow:" << std::endl;
  for (BoostPolygon const& p : output)
  {
    std::cout << i++ << ": " << printPolygon(p) << std::endl;
  }

}

TEST_F(GeometryFixture, PointInPolygon_Down)
{
  double tol = 0.01;
  double tol2 = tol/2.0;

  Point3dVector points = makeRectangleDown(0, 0, 1, 1);

  // center
  EXPECT_TRUE(pointInPolygon(Point3d(0.5,0.5,0), points, tol));

  // corners
  EXPECT_TRUE(pointInPolygon(Point3d(0,0,0), points, tol));
  EXPECT_TRUE(pointInPolygon(Point3d(1,0,0), points, tol));
  EXPECT_TRUE(pointInPolygon(Point3d(1,1,0), points, tol));
  EXPECT_TRUE(pointInPolygon(Point3d(0,1,0), points, tol));

  // edges
  EXPECT_TRUE(pointInPolygon(Point3d(0.5,0,0), points, tol));
  EXPECT_TRUE(pointInPolygon(Point3d(1,0.5,0), points, tol));
  EXPECT_TRUE(pointInPolygon(Point3d(0.5,1,0), points, tol));
  EXPECT_TRUE(pointInPolygon(Point3d(0,0.5,0), points, tol));

  EXPECT_TRUE(within(Point3d(0.5, tol2, 0), points, tol));
  EXPECT_TRUE(within(Point3d(1 - tol2, 0.5, 0), points, tol));
  EXPECT_TRUE(within(Point3d(0.5, 1 - tol2, 0), points, tol));
  EXPECT_TRUE(within(Point3d(tol2, 0.5, 0), points, tol));

  EXPECT_FALSE(within(Point3d(0.5, -tol2, 0), points, tol));
  EXPECT_FALSE(within(Point3d(1 + tol2, 0.5, 0), points, tol));
  EXPECT_FALSE(within(Point3d(0.5, 1 + tol2, 0), points, tol));
  EXPECT_FALSE(within(Point3d(-tol2, 0.5, 0), points, tol));

  // outside
  EXPECT_FALSE(pointInPolygon(Point3d(2,0,0), points, tol));
  EXPECT_FALSE(pointInPolygon(Point3d(1,2,0), points, tol));
  EXPECT_FALSE(pointInPolygon(Point3d(-1,0,0), points, tol));
  EXPECT_FALSE(pointInPolygon(Point3d(-1,-1,0), points, tol));

  // not on z = 0
  EXPECT_FALSE(pointInPolygon(Point3d(0.5,0.5,0.5), points, tol));
}

TEST_F(GeometryFixture, PointInPolygon_Up)
{
  double tol = 0.01;
  double tol2 = tol / 2.0;

  Point3dVector points = makeRectangleUp(0, 0, 1, 1);

  // center
  EXPECT_FALSE(pointInPolygon(Point3d(0.5, 0.5, 0), points, tol));

  // corners
  EXPECT_FALSE(pointInPolygon(Point3d(0, 0, 0), points, tol));
  EXPECT_FALSE(pointInPolygon(Point3d(1, 0, 0), points, tol));
  EXPECT_FALSE(pointInPolygon(Point3d(1, 1, 0), points, tol));
  EXPECT_FALSE(pointInPolygon(Point3d(0, 1, 0), points, tol));

  // edges
  EXPECT_FALSE(pointInPolygon(Point3d(0.5, 0, 0), points, tol));
  EXPECT_FALSE(pointInPolygon(Point3d(1, 0.5, 0), points, tol));
  EXPECT_FALSE(pointInPolygon(Point3d(0.5, 1, 0), points, tol));
  EXPECT_FALSE(pointInPolygon(Point3d(0, 0.5, 0), points, tol));

  EXPECT_FALSE(within(Point3d(0.5, tol2, 0), points, tol));
  EXPECT_FALSE(within(Point3d(1 - tol2, 0.5, 0), points, tol));
  EXPECT_FALSE(within(Point3d(0.5, 1 - tol2, 0), points, tol));
  EXPECT_FALSE(within(Point3d(tol2, 0.5, 0), points, tol));

  EXPECT_FALSE(within(Point3d(0.5, -tol2, 0), points, tol));
  EXPECT_FALSE(within(Point3d(1 + tol2, 0.5, 0), points, tol));
  EXPECT_FALSE(within(Point3d(0.5, 1 + tol2, 0), points, tol));
  EXPECT_FALSE(within(Point3d(-tol2, 0.5, 0), points, tol));

  // outside
  EXPECT_FALSE(pointInPolygon(Point3d(2, 0, 0), points, tol));
  EXPECT_FALSE(pointInPolygon(Point3d(1, 2, 0), points, tol));
  EXPECT_FALSE(pointInPolygon(Point3d(-1, 0, 0), points, tol));
  EXPECT_FALSE(pointInPolygon(Point3d(-1, -1, 0), points, tol));

  // not on z = 0
  EXPECT_FALSE(pointInPolygon(Point3d(0.5, 0.5, 0.5), points, tol));
}

TEST_F(GeometryFixture, Within_Down)
{
  double tol = 0.01;
  double tol2 = tol/2.0;

  Point3dVector points = makeRectangleDown(0, 0, 1, 1);

  // center
  EXPECT_TRUE(within(Point3d(0.5, 0.5, 0), points, tol));

  // corners
  EXPECT_FALSE(within(Point3d(0, 0, 0), points, tol));
  EXPECT_FALSE(within(Point3d(1, 0, 0), points, tol));
  EXPECT_FALSE(within(Point3d(1, 1, 0), points, tol));
  EXPECT_FALSE(within(Point3d(0, 1, 0), points, tol));

  // edges
  EXPECT_FALSE(within(Point3d(0.5, 0, 0), points, tol));
  EXPECT_FALSE(within(Point3d(1, 0.5, 0), points, tol));
  EXPECT_FALSE(within(Point3d(0.5, 1, 0), points, tol));
  EXPECT_FALSE(within(Point3d(0, 0.5, 0), points, tol));

  EXPECT_TRUE(within(Point3d(0.5, tol2, 0), points, tol));
  EXPECT_TRUE(within(Point3d(1 - tol2, 0.5, 0), points, tol));
  EXPECT_TRUE(within(Point3d(0.5, 1 - tol2, 0), points, tol));
  EXPECT_TRUE(within(Point3d(tol2, 0.5, 0), points, tol));

  EXPECT_FALSE(within(Point3d(0.5, -tol2, 0), points, tol));
  EXPECT_FALSE(within(Point3d(1 + tol2, 0.5, 0), points, tol));
  EXPECT_FALSE(within(Point3d(0.5, 1 + tol2, 0), points, tol));
  EXPECT_FALSE(within(Point3d(-tol2, 0.5, 0), points, tol));

  // outside
  EXPECT_FALSE(within(Point3d(2, 0, 0), points, tol));
  EXPECT_FALSE(within(Point3d(1, 2, 0), points, tol));
  EXPECT_FALSE(within(Point3d(-1, 0, 0), points, tol));
  EXPECT_FALSE(within(Point3d(-1, -1, 0), points, tol));

  // not on z = 0
  EXPECT_FALSE(within(Point3d(0.5, 0.5, 0.5), points, tol));
}

TEST_F(GeometryFixture, Within_Up)
{
  double tol = 0.01;
  double tol2 = tol / 2.0;

  Point3dVector points = makeRectangleUp(0, 0, 1, 1);

  // center
  EXPECT_FALSE(within(Point3d(0.5, 0.5, 0), points, tol));

  // corners
  EXPECT_FALSE(within(Point3d(0, 0, 0), points, tol));
  EXPECT_FALSE(within(Point3d(1, 0, 0), points, tol));
  EXPECT_FALSE(within(Point3d(1, 1, 0), points, tol));
  EXPECT_FALSE(within(Point3d(0, 1, 0), points, tol));

  // edges
  EXPECT_FALSE(within(Point3d(0.5, 0, 0), points, tol));
  EXPECT_FALSE(within(Point3d(1, 0.5, 0), points, tol));
  EXPECT_FALSE(within(Point3d(0.5, 1, 0), points, tol));
  EXPECT_FALSE(within(Point3d(0, 0.5, 0), points, tol));

  EXPECT_FALSE(within(Point3d(0.5, tol2, 0), points, tol));
  EXPECT_FALSE(within(Point3d(1 - tol2, 0.5, 0), points, tol));
  EXPECT_FALSE(within(Point3d(0.5, 1 - tol2, 0), points, tol));
  EXPECT_FALSE(within(Point3d(tol2, 0.5, 0), points, tol));

  EXPECT_FALSE(within(Point3d(0.5, -tol2, 0), points, tol));
  EXPECT_FALSE(within(Point3d(1 + tol2, 0.5, 0), points, tol));
  EXPECT_FALSE(within(Point3d(0.5, 1 + tol2, 0), points, tol));
  EXPECT_FALSE(within(Point3d(-tol2, 0.5, 0), points, tol));

  // outside
  EXPECT_FALSE(within(Point3d(2, 0, 0), points, tol));
  EXPECT_FALSE(within(Point3d(1, 2, 0), points, tol));
  EXPECT_FALSE(within(Point3d(-1, 0, 0), points, tol));
  EXPECT_FALSE(within(Point3d(-1, -1, 0), points, tol));

  // not on z = 0
  EXPECT_FALSE(within(Point3d(0.5, 0.5, 0.5), points, tol));
}

TEST_F(GeometryFixture, Intersect_False)
{
  double tol = 0.01;

  boost::optional<IntersectionResult> test;
  Point3dVector points1;
  Point3dVector points2;

  // non intersecting, sense is down
  points1 = makeRectangleDown(0, 0, 1, 1);
  points2 = makeRectangleDown(3, 0, 1, 1);

  test = intersect(points1, points2, tol);
  EXPECT_FALSE(test);

  test = intersect(points2, points1, tol);
  EXPECT_FALSE(test);

  // non intersecting, sense is up
  points1 = makeRectangleUp(0, 0, 1, 1);
  points2 = makeRectangleUp(3, 0, 1, 1);

  test = intersect(points1, points2, tol);
  EXPECT_FALSE(test);

  test = intersect(points2, points1, tol);
  EXPECT_FALSE(test);

  // sense is down, same points but not on z=0 plane
  points1.clear();
  points1.push_back(Point3d(1, 1, 3));
  points1.push_back(Point3d(1, 0, 3));
  points1.push_back(Point3d(0, 0, 3));
  points1.push_back(Point3d(0, 1, 3));

  points2.clear();
  points2.push_back(Point3d(1, 1, 3));
  points2.push_back(Point3d(1, 0, 3));
  points2.push_back(Point3d(0, 0, 3));
  points2.push_back(Point3d(0, 1, 3));

  test = intersect(points1, points2, tol);
  EXPECT_FALSE(test);

  test = intersect(points2, points1, tol);
  EXPECT_FALSE(test);

  // sense is up, same points but not on z=0 plane
  points1.clear();
  points1.push_back(Point3d(0, 0, 3));
  points1.push_back(Point3d(1, 0, 3));
  points1.push_back(Point3d(1, 1, 3));
  points1.push_back(Point3d(0, 1, 3));

  points2.clear();
  points2.push_back(Point3d(0, 0, 3));
  points2.push_back(Point3d(1, 0, 3));
  points2.push_back(Point3d(1, 1, 3));
  points2.push_back(Point3d(0, 1, 3));

  test = intersect(points1, points2, tol);
  EXPECT_FALSE(test);

  test = intersect(points2, points1, tol);
  EXPECT_FALSE(test);

  // sense is right, same points but not on z=0 plane
  points1.clear();
  points1.push_back(Point3d(0, 0, 0));
  points1.push_back(Point3d(0, 0, 1));
  points1.push_back(Point3d(0, 1, 1));
  points1.push_back(Point3d(0, 1, 0));

  points2.clear();
  points2.push_back(Point3d(0, 0, 0));
  points2.push_back(Point3d(0, 0, 1));
  points2.push_back(Point3d(0, 1, 1));
  points2.push_back(Point3d(0, 1, 0));

  test = intersect(points1, points2, tol);
  EXPECT_FALSE(test);

  test = intersect(points2, points1, tol);
  EXPECT_FALSE(test);
}

TEST_F(GeometryFixture, Intersect_SamePoints)
{
  double tol = 0.01;

  boost::optional<IntersectionResult> test;
  Point3dVector points1;
  Point3dVector points2;

  // sense is down
  points1 = makeRectangleDown(0, 0, 1, 1);
  points2 = makeRectangleDown(0, 0, 1, 1);

  test = intersect(points1, points2, tol);
  ASSERT_TRUE(test);
  EXPECT_TRUE(circularEqual(points1, test->polygon1())) << test->polygon1();
  EXPECT_TRUE(circularEqual(points2, test->polygon2())) << test->polygon2();
  EXPECT_EQ(0, test->newPolygons1().size());
  EXPECT_EQ(0, test->newPolygons2().size());

  test = intersect(points2, points1, tol);
  ASSERT_TRUE(test);
  EXPECT_TRUE(circularEqual(points2, test->polygon1())) << test->polygon1();
  EXPECT_TRUE(circularEqual(points1, test->polygon2())) << test->polygon2();
  EXPECT_EQ(0, test->newPolygons1().size());
  EXPECT_EQ(0, test->newPolygons2().size());

  // sense is up
  points1 = makeRectangleUp(0, 0, 1, 1);
  points2 = makeRectangleUp(0, 0, 1, 1);

  test = intersect(points1, points2, tol);
  EXPECT_FALSE(test);

  test = intersect(points2, points1, tol);
  EXPECT_FALSE(test);

  // opposite sense, points1 is up, points2 is down
  points1 = makeRectangleUp(0, 0, 1, 1);
  points2 = makeRectangleDown(0, 0, 1, 1);

  test = intersect(points1, points2, tol);
  EXPECT_FALSE(test);

  test = intersect(points2, points1, tol);
  EXPECT_FALSE(test);
}

TEST_F(GeometryFixture, Intersect_Adjacent)
{
  double tol = 0.01;

  boost::optional<IntersectionResult> test;
  Point3dVector points1;
  Point3dVector points2;

  // sense is down
  points1 = makeRectangleDown(0, 0, 1, 1);
  points2 = makeRectangleDown(1, 0, 1, 1);

  test = intersect(points1, points2, tol);
  EXPECT_FALSE(test);

  test = intersect(points2, points1, tol);
  EXPECT_FALSE(test);

  // sense is up
  points1 = makeRectangleUp(0, 0, 1, 1);
  points2 = makeRectangleUp(1, 0, 1, 1);

  test = intersect(points1, points2, tol);
  EXPECT_FALSE(test);

  test = intersect(points2, points1, tol);
  EXPECT_FALSE(test);
}

TEST_F(GeometryFixture, Intersect_Overlap_Down)
{
  double tol = 0.01;

  boost::optional<IntersectionResult> test;
  Point3dVector points1;
  Point3dVector points2;

  // sense is down
  points1 = makeRectangleDown(0, 0, 2, 1);
  points2 = makeRectangleDown(1, 0, 2, 1);

  test = intersect(points1, points2, tol);
  ASSERT_TRUE(test);
  EXPECT_TRUE(circularEqual(makeRectangleDown(1, 0, 1, 1), test->polygon1())) << test->polygon1();
  EXPECT_TRUE(circularEqual(makeRectangleDown(1, 0, 1, 1), test->polygon2())) << test->polygon2();
  ASSERT_EQ(1, test->newPolygons1().size());
  EXPECT_TRUE(circularEqual(makeRectangleDown(0, 0, 1, 1), test->newPolygons1()[0])) << test->newPolygons1()[0];
  ASSERT_EQ(1, test->newPolygons2().size());
  EXPECT_TRUE(circularEqual(makeRectangleDown(2, 0, 1, 1), test->newPolygons2()[0])) << test->newPolygons2()[0];

  test = intersect(points2, points1, tol);
  ASSERT_TRUE(test);
  EXPECT_TRUE(circularEqual(makeRectangleDown(1, 0, 1, 1), test->polygon1())) << test->polygon1();
  EXPECT_TRUE(circularEqual(makeRectangleDown(1, 0, 1, 1), test->polygon2())) << test->polygon2();
  ASSERT_EQ(1, test->newPolygons1().size());
  EXPECT_TRUE(circularEqual(makeRectangleDown(2, 0, 1, 1), test->newPolygons1()[0])) << test->newPolygons1()[0];
  ASSERT_EQ(1, test->newPolygons2().size());
  EXPECT_TRUE(circularEqual(makeRectangleDown(0, 0, 1, 1), test->newPolygons2()[0])) << test->newPolygons2()[0];
}

TEST_F(GeometryFixture, Intersect_Overlap_Up)
{
  double tol = 0.01;

  boost::optional<IntersectionResult> test;
  Point3dVector points1;
  Point3dVector points2;

  // sense is down
  points1 = makeRectangleUp(0, 0, 2, 1);
  points2 = makeRectangleUp(1, 0, 2, 1);

  test = intersect(points1, points2, tol);
  EXPECT_FALSE(test);

  test = intersect(points2, points1, tol);
  EXPECT_FALSE(test);
}

TEST_F(GeometryFixture, Intersect_Within_Down)
{
  double tol = 0.01;

  boost::optional<IntersectionResult> test;
  Point3dVector points1;
  Point3dVector points2;

  // sense is down
  points1 = makeRectangleDown(0, 0, 3, 3);
  points2 = makeRectangleDown(1, 1, 2, 2);

  test = intersect(points1, points2, tol);
  ASSERT_TRUE(test);
  //EXPECT_TRUE(circularEqual(makeRectangleDown(1, 0, 1, 1), test->polygon1())) << test->polygon1();
  //EXPECT_TRUE(circularEqual(makeRectangleDown(1, 0, 1, 1), test->polygon2())) << test->polygon2();
  //ASSERT_EQ(1, test->newPolygons1().size());
  //EXPECT_TRUE(circularEqual(makeRectangleDown(0, 0, 1, 1), test->newPolygons1()[0])) << test->newPolygons1()[0];
  //ASSERT_EQ(1, test->newPolygons2().size());
  //EXPECT_TRUE(circularEqual(makeRectangleDown(2, 0, 1, 1), test->newPolygons2()[0])) << test->newPolygons2()[0];

  test = intersect(points2, points1, tol);
  ASSERT_TRUE(test);
  //EXPECT_TRUE(circularEqual(makeRectangleDown(1, 0, 1, 1), test->polygon1())) << test->polygon1();
  //EXPECT_TRUE(circularEqual(makeRectangleDown(1, 0, 1, 1), test->polygon2())) << test->polygon2();
  //ASSERT_EQ(1, test->newPolygons1().size());
  //EXPECT_TRUE(circularEqual(makeRectangleDown(2, 0, 1, 1), test->newPolygons1()[0])) << test->newPolygons1()[0];
  //ASSERT_EQ(1, test->newPolygons2().size());
  //EXPECT_TRUE(circularEqual(makeRectangleDown(0, 0, 1, 1), test->newPolygons2()[0])) << test->newPolygons2()[0];
}

TEST_F(GeometryFixture, Intersect_Within_Up)
{
  double tol = 0.01;

  boost::optional<IntersectionResult> test;
  Point3dVector points1;
  Point3dVector points2;

  // sense is up
  points1 = makeRectangleUp(0, 0, 3, 3);
  points2 = makeRectangleUp(1, 1, 2, 2);

  test = intersect(points1, points2, tol);
  EXPECT_FALSE(test);

  test = intersect(points2, points1, tol);
  EXPECT_FALSE(test);
}

TEST_F(GeometryFixture, Join_False)
{
  double tol = 0.01;

  boost::optional<std::vector<Point3d> > test;
  Point3dVector points1;
  Point3dVector points2;

  // non intersecting, sense is down
  points1 = makeRectangleDown(0, 0, 1, 1);
  points2 = makeRectangleDown(3, 0, 1, 1);

  test = join(points1, points2, tol);
  EXPECT_FALSE(test);

  test = join(points2, points1, tol);
  EXPECT_FALSE(test);

  // non intersecting, sense is up
  points1 = makeRectangleUp(0, 0, 1, 1);
  points2 = makeRectangleUp(3, 0, 1, 1);

  test = join(points1, points2, tol);
  EXPECT_FALSE(test);

  test = join(points2, points1, tol);
  EXPECT_FALSE(test);

  // sense is down, same points but not on z=0 plane
  points1.clear();
  points1.push_back(Point3d(1, 1, 3));
  points1.push_back(Point3d(1, 0, 3));
  points1.push_back(Point3d(0, 0, 3));
  points1.push_back(Point3d(0, 1, 3));

  points2.clear();
  points2.push_back(Point3d(1, 1, 3));
  points2.push_back(Point3d(1, 0, 3));
  points2.push_back(Point3d(0, 0, 3));
  points2.push_back(Point3d(0, 1, 3));

  test = join(points1, points2, tol);
  EXPECT_FALSE(test);

  test = join(points2, points1, tol);
  EXPECT_FALSE(test);

  // sense is up, same points but not on z=0 plane
  points1.clear();
  points1.push_back(Point3d(0, 0, 3));
  points1.push_back(Point3d(1, 0, 3));
  points1.push_back(Point3d(1, 1, 3));
  points1.push_back(Point3d(0, 1, 3));

  points2.clear();
  points2.push_back(Point3d(0, 0, 3));
  points2.push_back(Point3d(1, 0, 3));
  points2.push_back(Point3d(1, 1, 3));
  points2.push_back(Point3d(0, 1, 3));

  test = join(points1, points2, tol);
  EXPECT_FALSE(test);

  test = join(points2, points1, tol);
  EXPECT_FALSE(test);

  // sense is right, same points but not on z=0 plane
  points1.clear();
  points1.push_back(Point3d(0, 0, 0));
  points1.push_back(Point3d(0, 0, 1));
  points1.push_back(Point3d(0, 1, 1));
  points1.push_back(Point3d(0, 1, 0));

  points2.clear();
  points2.push_back(Point3d(0, 0, 0));
  points2.push_back(Point3d(0, 0, 1));
  points2.push_back(Point3d(0, 1, 1));
  points2.push_back(Point3d(0, 1, 0));

  test = join(points1, points2, tol);
  EXPECT_FALSE(test);

  test = join(points2, points1, tol);
  EXPECT_FALSE(test);
}

TEST_F(GeometryFixture, Join_SamePoints)
{
  double tol = 0.01;

  boost::optional<std::vector<Point3d> > test;
  Point3dVector points1;
  Point3dVector points2;

  // sense is down
  points1 = makeRectangleDown(0, 0, 1, 1);
  points2 = makeRectangleDown(0, 0, 1, 1);

  test = join(points1, points2, tol);
  ASSERT_TRUE(test);
  EXPECT_TRUE(circularEqual(points1, *test)) << *test;

  test = join(points2, points1, tol);
  ASSERT_TRUE(test);
  EXPECT_TRUE(circularEqual(points1, *test)) << *test;

  // sense is up
  points1 = makeRectangleUp(0, 0, 1, 1);
  points2 = makeRectangleUp(0, 0, 1, 1);

  test = join(points1, points2, tol);
  EXPECT_FALSE(test);

  test = join(points2, points1, tol);
  EXPECT_FALSE(test);

  // opposite sense, points1 is up, points2 is down
  points1 = makeRectangleUp(0, 0, 1, 1);
  points2 = makeRectangleDown(0, 0, 1, 1);

  test = join(points1, points2, tol);
  EXPECT_FALSE(test);

  test = join(points2, points1, tol);
  EXPECT_FALSE(test);
}

TEST_F(GeometryFixture, Join_Adjacent)
{
  double tol = 0.01;

  boost::optional<std::vector<Point3d> > test;
  Point3dVector points1;
  Point3dVector points2;

  // sense is down
  points1 = makeRectangleDown(0, 0, 1, 1);
  points2 = makeRectangleDown(1, 0, 1, 1);

  test = join(points1, points2, tol);
  ASSERT_TRUE(test);
  EXPECT_TRUE(circularEqual(makeRectangleDown(0, 0, 2, 1), *test)) << *test;

  test = join(points2, points1, tol);
  ASSERT_TRUE(test);
  EXPECT_TRUE(circularEqual(makeRectangleDown(0, 0, 2, 1), *test)) << *test;

  // sense is up
  points1 = makeRectangleUp(0, 0, 1, 1);
  points2 = makeRectangleUp(1, 0, 1, 1);

  test = join(points1, points2, tol);
  EXPECT_FALSE(test);

  test = join(points2, points1, tol);
  EXPECT_FALSE(test);
}

TEST_F(GeometryFixture, Join_Overlap_Down)
{
  double tol = 0.01;

  boost::optional<std::vector<Point3d> > test;
  Point3dVector points1;
  Point3dVector points2;

  // sense is down
  points1 = makeRectangleDown(0, 0, 2, 1);
  points2 = makeRectangleDown(1, 0, 2, 1);

  test = join(points1, points2, tol);
  ASSERT_TRUE(test);
  EXPECT_TRUE(circularEqual(makeRectangleDown(0, 0, 3, 1), *test)) << *test;

  test = join(points2, points1, tol);
  ASSERT_TRUE(test);
  EXPECT_TRUE(circularEqual(makeRectangleDown(0, 0, 3, 1), *test)) << *test;
}

TEST_F(GeometryFixture, Join_Overlap_Up)
{
  double tol = 0.01;

  boost::optional<std::vector<Point3d> > test;
  Point3dVector points1;
  Point3dVector points2;

  // sense is up
  points1 = makeRectangleUp(0, 0, 2, 1);
  points2 = makeRectangleUp(1, 0, 2, 1);

  test = join(points1, points2, tol);
  EXPECT_FALSE(test);

  test = join(points2, points1, tol);
  EXPECT_FALSE(test);
}

TEST_F(GeometryFixture, Join_Overlap2)
{
  double tol = 0.01;

  boost::optional<std::vector<Point3d> > test;
  Point3dVector points1;
  Point3dVector points2;

  // sense is down
  points1 = makeRectangleDown(0, 0, 4, 1);
  points2 = makeRectangleDown(1, -1, 4, 1);

  test = join(points1, points2, tol);
  ASSERT_TRUE(test);

  test = join(points2, points1, tol);
  ASSERT_TRUE(test);
}

TEST_F(GeometryFixture, JoinAll)
{
  double tol = 0.01;

  std::vector<Point3dVector> test;
  std::vector<Point3dVector> polygons;

  // empty
  test = joinAll(polygons, tol);
  EXPECT_EQ(0, test.size());

  // two overlapping
  polygons.clear();
  polygons.push_back(makeRectangleDown(0, 0, 1, 1));
  polygons.push_back(makeRectangleDown(1, 0, 1, 1));
  test = joinAll(polygons, tol);
  ASSERT_EQ(1u, test.size());
  EXPECT_TRUE(circularEqual(makeRectangleDown(0, 0, 2, 1), test[0])) << test[0];

  // three overlapping
  polygons.clear();
  polygons.push_back(makeRectangleDown(0, 0, 1, 1));
  polygons.push_back(makeRectangleDown(1, 0, 1, 1));
  polygons.push_back(makeRectangleDown(2, 0, 1, 1));
  test = joinAll(polygons, tol);
  ASSERT_EQ(1u, test.size());
  EXPECT_TRUE(circularEqual(makeRectangleDown(0, 0, 3, 1), test[0])) << test[0];

  // three not overlapping
  polygons.clear();
  polygons.push_back(makeRectangleDown(0, 0, 1, 1));
  polygons.push_back(makeRectangleDown(2, 0, 1, 1));
  polygons.push_back(makeRectangleDown(4, 0, 1, 1));
  test = joinAll(polygons, tol);
  ASSERT_EQ(3u, test.size());

  // two overlapping, one not
  polygons.clear();
  polygons.push_back(makeRectangleDown(0, 0, 1, 1));
  polygons.push_back(makeRectangleDown(1, 0, 1, 1));
  polygons.push_back(makeRectangleDown(3, 0, 1, 1));
  test = joinAll(polygons, tol);
  ASSERT_EQ(2u, test.size());

  bool found1 = false;
  bool found2 = false;
  for (unsigned i = 0; i < 2; ++i){
    if (circularEqual(makeRectangleDown(0, 0, 2, 1), test[i])){
      found1 = true;
    }else if (circularEqual(makeRectangleDown(3, 0, 1, 1), test[i])){
      found2 = true;
    }
  }
  EXPECT_TRUE(found1);
  EXPECT_TRUE(found2);
}

TEST_F(GeometryFixture, RemoveSpikes_Down)
{
  double tol = 0.01;

  std::vector<Point3d> points;
  std::vector<Point3d> expected;
  std::vector<Point3d> result;

  // spike at beginning
  {
    // sense is down
    points.clear();
    points.push_back(Point3d(10, 10, 0)); // the spike
    points.push_back(Point3d(10, 0, 0));
    points.push_back(Point3d(0, 0, 0));
    points.push_back(Point3d(0, 5, 0));
    points.push_back(Point3d(10, 5, 0));

    expected.clear();
    expected.push_back(Point3d(10, 5, 0));
    expected.push_back(Point3d(10, 0, 0));
    expected.push_back(Point3d(0, 0, 0));
    expected.push_back(Point3d(0, 5, 0));

    result = removeSpikes(points, tol);
    EXPECT_TRUE(circularEqual(expected, result)) << result;
  }

  // spike at beginning 2
  {
    // sense is down
    points.clear();
    points.push_back(Point3d(10, 5, 0));
    points.push_back(Point3d(10, 10, 0)); // the spike
    points.push_back(Point3d(10, 0, 0));
    points.push_back(Point3d(0, 0, 0));
    points.push_back(Point3d(0, 5, 0));

    expected.clear();
    expected.push_back(Point3d(10, 5, 0));
    expected.push_back(Point3d(10, 0, 0));
    expected.push_back(Point3d(0, 0, 0));
    expected.push_back(Point3d(0, 5, 0));

    result = removeSpikes(points, tol);
    EXPECT_TRUE(circularEqual(expected, result)) << result;
  }

  // spike in middle
  {
    // sense is down
    points.clear();
    points.push_back(Point3d(10, 5, 0));
    points.push_back(Point3d(10, 0, 0));
    points.push_back(Point3d(-5, 0, 0)); // the spike
    points.push_back(Point3d(0, 0, 0));
    points.push_back(Point3d(0, 5, 0));

    expected.clear();
    expected.push_back(Point3d(10, 5, 0));
    expected.push_back(Point3d(10, 0, 0));
    expected.push_back(Point3d(0, 0, 0));
    expected.push_back(Point3d(0, 5, 0));

    result = removeSpikes(points, tol);
    EXPECT_TRUE(circularEqual(expected, result)) << result;
  }
}

TEST_F(GeometryFixture, RemoveSpikes_Up)
{
  double tol = 0.01;

  std::vector<Point3d> points;
  std::vector<Point3d> expected;
  std::vector<Point3d> result;

  // spike at end
  {
    // sense is up
    points.clear();
    points.push_back(Point3d(10, 5, 0));
    points.push_back(Point3d(0, 5, 0));
    points.push_back(Point3d(0, 0, 0));
    points.push_back(Point3d(10, 0, 0));
    points.push_back(Point3d(10, 10, 0)); // the spike

    expected.clear();

    result = removeSpikes(points, tol);
    EXPECT_TRUE(circularEqual(expected, result)) << result;
  }

  // spike at end 2
  {
    // sense is up
    points.clear();
    points.push_back(Point3d(0, 5, 0));
    points.push_back(Point3d(0, 0, 0));
    points.push_back(Point3d(10, 0, 0));
    points.push_back(Point3d(10, 10, 0)); // the spike
    points.push_back(Point3d(10, 5, 0));

    expected.clear();

    result = removeSpikes(points, tol);
    EXPECT_TRUE(circularEqual(expected, result)) << result;
  }

  // spike in middle
  {
    // sense is up
    points.clear();
    points.push_back(Point3d(0, 0, 0));
    points.push_back(Point3d(10, 0, 0));
    points.push_back(Point3d(-5, 0, 0)); // the spike
    points.push_back(Point3d(10, 5, 0));
    points.push_back(Point3d(0, 5, 0));

    expected.clear();

    result = removeSpikes(points, tol);
    EXPECT_TRUE(circularEqual(expected, result)) << result;
  }
}

TEST_F(GeometryFixture, Subtract_SamePoints)
{
  double tol = 0.01;

  std::vector<std::vector<Point3d> > test;
  std::vector<std::vector<Point3d> > holes;
  Point3dVector points1;
  Point3dVector points2;

  // sense is down
  points1 = makeRectangleDown(0, 0, 1, 1);
  points2 = makeRectangleDown(0, 0, 1, 1);

  holes.clear();
  holes.push_back(points2);
  test = subtract(points1, holes, tol);
  for (auto polygon : test){
    std::cout << polygon << std::endl;
  }
  EXPECT_DOUBLE_EQ(0.0, totalArea(test));
  //ASSERT_TRUE(test);
  //EXPECT_TRUE(circularEqual(points1, test->polygon1())) << test->polygon1();
  //EXPECT_TRUE(circularEqual(points2, test->polygon2())) << test->polygon2();
  //EXPECT_EQ(0, test->newPolygons1().size());
  //EXPECT_EQ(0, test->newPolygons2().size());

  holes.clear();
  holes.push_back(points1);
  test = subtract(points2, holes, tol);
  for (auto polygon : test){
    std::cout << polygon << std::endl;
  }
  EXPECT_DOUBLE_EQ(0.0, totalArea(test));
  //ASSERT_TRUE(test);
  //EXPECT_TRUE(circularEqual(points2, test->polygon1())) << test->polygon1();
  //EXPECT_TRUE(circularEqual(points1, test->polygon2())) << test->polygon2();
  //EXPECT_EQ(0, test->newPolygons1().size());
  //EXPECT_EQ(0, test->newPolygons2().size());

  // sense is up
  points1 = makeRectangleUp(0, 0, 1, 1);
  points2 = makeRectangleUp(0, 0, 1, 1);

  holes.clear();
  holes.push_back(points2);
  test = subtract(points1, holes, tol);
  EXPECT_EQ(0, test.size());

  holes.clear();
  holes.push_back(points1);
  test = subtract(points2, holes, tol);
  EXPECT_EQ(0, test.size());

  // opposite sense, points1 is up, points2 is down
  points1 = makeRectangleUp(0, 0, 1, 1);
  points2 = makeRectangleDown(0, 0, 1, 1);

  holes.clear();
  holes.push_back(points2);
  test = subtract(points1, holes, tol);
  EXPECT_EQ(0, test.size());

  holes.clear();
  holes.push_back(points1);
  test = subtract(points2, holes, tol);
  EXPECT_EQ(0, test.size());
}

TEST_F(GeometryFixture, Subtract_Empty_Down)
{
  double tol = 0.01;

  std::vector<std::vector<Point3d> > test;
  std::vector<std::vector<Point3d> > holes;
  Point3dVector points1;
  Point3dVector points2;

  // sense is down
  points1 = makeRectangleDown(0, 0, 1, 1);

  test = subtract(points1, holes, tol);
  EXPECT_TRUE(test.size() == 1);
  for (auto polygon : test){
    std::cout << polygon << std::endl;
  }
  EXPECT_DOUBLE_EQ(1.0, totalArea(test));

  // can't subtract single point
  points2.push_back(Point3d(1, 1, 1));

  holes.clear();
  holes.push_back(points2);
  test = subtract(points1, holes, tol);
  EXPECT_TRUE(test.empty());

  // can't subtract single point
  holes.clear();
  holes.push_back(points1);
  test = subtract(points2, holes, tol);
  EXPECT_TRUE(test.empty());
}

TEST_F(GeometryFixture, Subtract_Empty_Up)
{
  double tol = 0.01;

  std::vector<std::vector<Point3d> > test;
  std::vector<std::vector<Point3d> > holes;
  Point3dVector points1;
  Point3dVector points2;

  // sense is down
  points1 = makeRectangleUp(0, 0, 1, 1);

  test = subtract(points1, holes, tol);
  EXPECT_TRUE(test.empty());

  // can't subtract single point
  points2.push_back(Point3d(1, 1, 1));

  holes.clear();
  holes.push_back(points2);
  test = subtract(points1, holes, tol);
  EXPECT_TRUE(test.empty());

  // can't subtract single point
  holes.clear();
  holes.push_back(points1);
  test = subtract(points2, holes, tol);
  EXPECT_TRUE(test.empty());
}

TEST_F(GeometryFixture, Subtract_Adjacent)
{
  double tol = 0.01;

  std::vector<std::vector<Point3d> > test;
  std::vector<std::vector<Point3d> > holes;
  Point3dVector points1;
  Point3dVector points2;

  // sense is down
  points1 = makeRectangleDown(0, 0, 1, 1);
  points2 = makeRectangleDown(1, 0, 1, 1);

  holes.clear();
  holes.push_back(points2);
  test = subtract(points1, holes, tol);
  EXPECT_TRUE(test.size() == 1);
  for (auto polygon : test){
    std::cout << polygon << std::endl;
  }
  EXPECT_DOUBLE_EQ(1.0, totalArea(test));

  holes.clear();
  holes.push_back(points1);
  test = subtract(points2, holes, tol);
  EXPECT_TRUE(test.size() == 1);
  for (auto polygon : test){
    std::cout << polygon << std::endl;
  }
  EXPECT_DOUBLE_EQ(1.0, totalArea(test));

  // sense is up
  points1 = makeRectangleUp(0, 0, 1, 1);
  points2 = makeRectangleUp(1, 0, 1, 1);

  holes.clear();
  holes.push_back(points2);
  test = subtract(points1, holes, tol);
  EXPECT_EQ(0, test.size());

  holes.clear();
  holes.push_back(points1);
  test = subtract(points2, holes, tol);
  EXPECT_EQ(0, test.size());
}

TEST_F(GeometryFixture, Subtract_Overlap)
{
  double tol = 0.01;

  std::vector<std::vector<Point3d> > test;
  std::vector<std::vector<Point3d> > holes;
  Point3dVector points1;
  Point3dVector points2;

  // sense is down
  points1 = makeRectangleDown(0, 0, 2, 1);
  points2 = makeRectangleDown(1, 0, 2, 1);

  holes.clear();
  holes.push_back(points2);
  test = subtract(points1, holes, tol);
  EXPECT_TRUE(test.size() == 1);
  for (auto polygon : test){
    std::cout << polygon << std::endl;
  }
  EXPECT_DOUBLE_EQ(1.0, totalArea(test));
  //ASSERT_TRUE(test);
  //EXPECT_TRUE(circularEqual(makeRectangleDown(1, 0, 1, 1), test->polygon1())) << test->polygon1();
  //EXPECT_TRUE(circularEqual(makeRectangleDown(1, 0, 1, 1), test->polygon2())) << test->polygon2();
  //ASSERT_EQ(1, test->newPolygons1().size());
  //EXPECT_TRUE(circularEqual(makeRectangleDown(0, 0, 1, 1), test->newPolygons1()[0])) << test->newPolygons1()[0];
  //ASSERT_EQ(1, test->newPolygons2().size());
  //EXPECT_TRUE(circularEqual(makeRectangleDown(2, 0, 1, 1), test->newPolygons2()[0])) << test->newPolygons2()[0];

  holes.clear();
  holes.push_back(points1);
  test = subtract(points2, holes, tol);
  EXPECT_TRUE(test.size() == 1);
  for (auto polygon : test){
    std::cout << polygon << std::endl;
  }
  EXPECT_DOUBLE_EQ(1.0, totalArea(test));
  //ASSERT_TRUE(test);
  //EXPECT_TRUE(circularEqual(makeRectangleDown(1, 0, 1, 1), test->polygon1())) << test->polygon1();
  //EXPECT_TRUE(circularEqual(makeRectangleDown(1, 0, 1, 1), test->polygon2())) << test->polygon2();
  //ASSERT_EQ(1, test->newPolygons1().size());
  //EXPECT_TRUE(circularEqual(makeRectangleDown(2, 0, 1, 1), test->newPolygons1()[0])) << test->newPolygons1()[0];
  //ASSERT_EQ(1, test->newPolygons2().size());
  //EXPECT_TRUE(circularEqual(makeRectangleDown(0, 0, 1, 1), test->newPolygons2()[0])) << test->newPolygons2()[0];
}

TEST_F(GeometryFixture, Subtract_Within_Down)
{
  double tol = 0.01;

  std::vector<std::vector<Point3d> > test;
  std::vector<std::vector<Point3d> > holes;
  Point3dVector points1;
  Point3dVector points2;

  // sense is down
  points1 = makeRectangleDown(0, 0, 3, 3);
  points2 = makeRectangleDown(1, 1, 1, 1);

  holes.clear();
  holes.push_back(points2);
  test = subtract(points1, holes, tol);
  EXPECT_TRUE(test.size() > 1);
  for (auto polygon : test){
    std::cout << polygon << std::endl;
  }
  EXPECT_DOUBLE_EQ(8.0, totalArea(test));
  //ASSERT_TRUE(test);
  //EXPECT_TRUE(circularEqual(makeRectangleDown(1, 0, 1, 1), test->polygon1())) << test->polygon1();
  //EXPECT_TRUE(circularEqual(makeRectangleDown(1, 0, 1, 1), test->polygon2())) << test->polygon2();
  //ASSERT_EQ(1, test->newPolygons1().size());
  //EXPECT_TRUE(circularEqual(makeRectangleDown(0, 0, 1, 1), test->newPolygons1()[0])) << test->newPolygons1()[0];
  //ASSERT_EQ(1, test->newPolygons2().size());
  //EXPECT_TRUE(circularEqual(makeRectangleDown(2, 0, 1, 1), test->newPolygons2()[0])) << test->newPolygons2()[0];

  holes.clear();
  holes.push_back(points1);
  test = subtract(points2, holes, tol);
  EXPECT_TRUE(test.empty());
  for (auto polygon : test){
    std::cout << polygon << std::endl;
  }
  EXPECT_DOUBLE_EQ(0.0, totalArea(test));
  //ASSERT_TRUE(test);
  //EXPECT_TRUE(circularEqual(makeRectangleDown(1, 0, 1, 1), test->polygon1())) << test->polygon1();
  //EXPECT_TRUE(circularEqual(makeRectangleDown(1, 0, 1, 1), test->polygon2())) << test->polygon2();
  //ASSERT_EQ(1, test->newPolygons1().size());
  //EXPECT_TRUE(circularEqual(makeRectangleDown(2, 0, 1, 1), test->newPolygons1()[0])) << test->newPolygons1()[0];
  //ASSERT_EQ(1, test->newPolygons2().size());
  //EXPECT_TRUE(circularEqual(makeRectangleDown(0, 0, 1, 1), test->newPolygons2()[0])) << test->newPolygons2()[0];
}

TEST_F(GeometryFixture, Subtract_Within_Up)
{
  double tol = 0.01;

  std::vector<std::vector<Point3d> > test;
  std::vector<std::vector<Point3d> > holes;
  Point3dVector points1;
  Point3dVector points2;

  // sense is up
  points1 = makeRectangleUp(0, 0, 3, 3);
  points2 = makeRectangleUp(1, 1, 1, 1);

  holes.clear();
  holes.push_back(points2);
  test = subtract(points1, holes, tol);
  EXPECT_TRUE(test.empty());

  holes.clear();
  holes.push_back(points1);
  test = subtract(points2, holes, tol);
  EXPECT_TRUE(test.empty());
}


#include <iostream>

//--- SpatialOps includes ---//
#include <spatialops/structured/FVStaggeredFieldTypes.h>
#include <spatialops/structured/IntVec.h>
#include <spatialops/structured/MemoryWindow.h>
#include <spatialops/structured/stencil/FVStaggeredOperatorTypes.h>
#include <spatialops/OperatorDatabase.h>
#include <spatialops/structured/stencil/StencilBuilder.h>
#include <spatialops/structured/FVStaggeredFieldTypes.h>
#include <spatialops/Nebo.h>

//-- boost includes ---//
#include <boost/program_options.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

namespace po = boost::program_options;

using namespace SpatialOps;

int print_length = 10;

template<typename Field>
void print(Field const & given) {
  typename Field::const_iterator ig = given.begin();

  for(int i = 0; i < print_length; i++) {
    std::cout << *ig << std::endl;
    ++ig;
  };

  std::cout << std::endl;
};

template<typename Field>
void print_all(Field const & given) {
  typename Field::const_iterator ig = given.begin();

  MemoryWindow window = given.window_with_ghost();
  for(int kk = 0; kk < window.glob_dim(2); kk++) {
      for(int jj = 0; jj < window.glob_dim(1); jj++) {
          for(int ii = 0; ii < window.glob_dim(0); ii++, ++ig) {
              std::cout << *ig << " ";
          }
          std::cout <<std::endl;
      }
      std::cout <<std::endl;
  };
};

#define INT_EQU(FIELD, FIRST, SECOND, PRINT)                            \
    {                                                                   \
        FIELD::const_iterator fi = FIRST.interior_begin();              \
        FIELD::const_iterator si = SECOND.interior_begin();             \
        FIELD::const_iterator se = SECOND.interior_end();               \
        bool flag = true;                                               \
        int count = 0;                                                  \
                                                                        \
        while( si != se ) {                                             \
            if( *fi != *si ) {                                          \
                flag = false;                                           \
                if( PRINT ) {                                           \
                    std::cout << count                                  \
                              << ": " << *fi                            \
                              << " " << *si << std::endl;               \
                };                                                      \
            };                                                          \
            ++fi;                                                       \
            ++si;                                                       \
            ++count;                                                    \
        };                                                              \
                                                                        \
        if( flag ) std::cout << "Success!";                             \
        else std::cout << "Error!";                                     \
        std::cout << std::endl;                                         \
    }

#define RUN_TEST(TEST,							\
		 TYPE)							\
  boost::posix_time::ptime start( boost::posix_time::microsec_clock::universal_time() ); \
  boost::posix_time::ptime end( boost::posix_time::microsec_clock::universal_time() ); \
  int ii = 0;								\
  start = boost::posix_time::microsec_clock::universal_time();		\
									\
  for(; ii < number_of_runs; ii++) {					\
    TEST;								\
  };									\
									\
  end = boost::posix_time::microsec_clock::universal_time();		\
  std::cout << TYPE;							\
  std::cout << " runs: ";						\
  std::cout << number_of_runs;						\
  std::cout << " result: ";						\
  std::cout << (end - start).total_microseconds()*1e-6;			\
  std::cout << std::endl;

template<typename FieldType>
inline void evaluate_serial_example(FieldType & result,
				    FieldType const & phi,
				    FieldType const & dCoef,
				    IntVec const npts,
				    double const Lx,
				    double const Ly,
				    double const Lz,
				    int number_of_runs) {

    SpatialOps::OperatorDatabase opDB;
    SpatialOps::build_stencils(npts[0],
                                           npts[1],
                                           npts[2],
                                           Lx,
                                           Ly,
                                           Lz,
                                           opDB);

    typename BasicOpTypes<FieldType>::GradX* const gradXOp_ = opDB.retrieve_operator<typename BasicOpTypes<FieldType>::GradX>();
    typename BasicOpTypes<FieldType>::GradY* const gradYOp_ = opDB.retrieve_operator<typename BasicOpTypes<FieldType>::GradY>();
    typename BasicOpTypes<FieldType>::GradZ* const gradZOp_ = opDB.retrieve_operator<typename BasicOpTypes<FieldType>::GradZ>();
    //typename BasicOpTypes<FieldType>::InterpC2FX* const interpXOp_ = opDB.retrieve_operator<typename BasicOpTypes<FieldType>::InterpC2FX>();
    //typename BasicOpTypes<FieldType>::InterpC2FY* const interpYOp_ = opDB.retrieve_operator<typename BasicOpTypes<FieldType>::InterpC2FY>();
    //typename BasicOpTypes<FieldType>::InterpC2FZ* const interpZOp_ = opDB.retrieve_operator<typename BasicOpTypes<FieldType>::InterpC2FZ>();
    typename BasicOpTypes<FieldType>::DivX* const divXOp_ = opDB.retrieve_operator<typename BasicOpTypes<FieldType>::DivX>();
    typename BasicOpTypes<FieldType>::DivY* const divYOp_ = opDB.retrieve_operator<typename BasicOpTypes<FieldType>::DivY>();
    typename BasicOpTypes<FieldType>::DivZ* const divZOp_ = opDB.retrieve_operator<typename BasicOpTypes<FieldType>::DivZ>();

    const MemoryWindow& w = phi.window_with_ghost();
    const GhostData& g = phi.get_ghost_data();
    const BoundaryCellInfo& bc = phi.boundary_info();
    typename FaceTypes<FieldType>::XFace  tmpFaceX( w, bc, g, NULL );
    typename FaceTypes<FieldType>::XFace tmpFaceX2( w, bc, g, NULL );
    FieldType tmpX( w, bc, g, NULL );
    typename FaceTypes<FieldType>::YFace  tmpFaceY( w, bc, g, NULL );
    typename FaceTypes<FieldType>::YFace tmpFaceY2( w, bc, g, NULL );
    FieldType tmpY( w, bc, g, NULL );
    typename FaceTypes<FieldType>::ZFace  tmpFaceZ( w, bc, g, NULL );
    typename FaceTypes<FieldType>::ZFace tmpFaceZ2( w, bc, g, NULL );
    FieldType tmpZ( w, bc, g, NULL );

    RUN_TEST(// X - direction
	     gradXOp_->apply_to_field( phi,    tmpFaceX  );
	     divXOp_->apply_to_field( tmpFaceX, tmpX );

	     // Y - direction
	     gradYOp_->apply_to_field( tmpX,    tmpFaceY  );
	     divYOp_->apply_to_field( tmpFaceY, tmpY );

	     // Z - direction
	     gradZOp_->apply_to_field( tmpY,    tmpFaceZ  );
	     divZOp_->apply_to_field( tmpFaceZ, tmpZ );

	     result <<= - tmpZ,
	     "old");

};

template<typename FieldType>
inline void evaluate_chaining_example(FieldType & result,
				      FieldType const & phi,
				      FieldType const & dCoef,
				      IntVec const npts,
				      double const Lx,
				      double const Ly,
				      double const Lz,
				      int number_of_runs) {

    SpatialOps::OperatorDatabase opDB;
    SpatialOps::build_stencils(npts[0],
                                           npts[1],
                                           npts[2],
                                           Lx,
					   Ly,
					   Lz,
                                           opDB);

    typename BasicOpTypes<FieldType>::GradX neboGradX = *opDB.retrieve_operator<typename BasicOpTypes<FieldType>::GradX>();
    typename BasicOpTypes<FieldType>::GradY neboGradY = *opDB.retrieve_operator<typename BasicOpTypes<FieldType>::GradY>();
    typename BasicOpTypes<FieldType>::GradZ neboGradZ = *opDB.retrieve_operator<typename BasicOpTypes<FieldType>::GradZ>();
    typename BasicOpTypes<FieldType>::InterpC2FX neboInterpX = *opDB.retrieve_operator<typename BasicOpTypes<FieldType>::InterpC2FX>();
    typename BasicOpTypes<FieldType>::InterpC2FY neboInterpY = *opDB.retrieve_operator<typename BasicOpTypes<FieldType>::InterpC2FY>();
    typename BasicOpTypes<FieldType>::InterpC2FZ neboInterpZ = *opDB.retrieve_operator<typename BasicOpTypes<FieldType>::InterpC2FZ>();
    typename BasicOpTypes<FieldType>::DivX neboDivX = *opDB.retrieve_operator<typename BasicOpTypes<FieldType>::DivX>();
    typename BasicOpTypes<FieldType>::DivY neboDivY = *opDB.retrieve_operator<typename BasicOpTypes<FieldType>::DivY>();
    typename BasicOpTypes<FieldType>::DivZ neboDivZ = *opDB.retrieve_operator<typename BasicOpTypes<FieldType>::DivZ>();

    RUN_TEST(result <<= (- neboDivZ(neboGradZ(
                                              neboDivY(neboGradY(
                                                                 neboDivX(neboGradX(phi))))))),
             "new");

};

int main(int iarg, char* carg[]) {
    typedef SVolField Field;

    bool test;
    int nx, ny, nz;
    int number_of_runs;
    double Lx, Ly, Lz;
#ifdef ENABLE_THREADS
  int thread_count;
#endif

    // parse the command line options input describing the problem
    {
        po::options_description desc("Supported Options");
	desc.add_options()
	  ( "help", "print help message" )
	  ( "nx", po::value<int>(&nx)->default_value(10), "Grid in x" )
	  ( "ny", po::value<int>(&ny)->default_value(10), "Grid in y" )
	  ( "nz", po::value<int>(&nz)->default_value(10), "Grid in z" )
	  ( "Lx", po::value<double>(&Lx)->default_value(1.0),"Length in x")
	  ( "Ly", po::value<double>(&Ly)->default_value(1.0),"Length in y")
	  ( "Lz", po::value<double>(&Lz)->default_value(1.0),"Length in z")
          ( "check", po::value<bool>(&test)->default_value(true),"Compare results of old and new versions")
#ifdef ENABLE_THREADS
      ( "tc", po::value<int>(&thread_count)->default_value(NTHREADS), "Number of threads for Nebo")
#endif
	  ( "runs", po::value<int>(&number_of_runs)->default_value(1), "Number of iterations of each test");

	po::variables_map args;
	po::store( po::parse_command_line(iarg,carg,desc), args );
	po::notify(args);

	if (args.count("help")) {
	    std::cout << desc << "\n";
	    return 1;
	}

#ifdef ENABLE_THREADS
    set_hard_thread_count(thread_count);
#endif
    }

    const GhostData ghost(1);
    const BoundaryCellInfo bc = BoundaryCellInfo::build<Field>(false,false,false);
    const MemoryWindow window( get_window_with_ghost(IntVec(nx,ny,nz),ghost,bc) );

    Field  a( window, bc, ghost, NULL );
    Field  b( window, bc, ghost, NULL );
    Field cr( window, bc, ghost, NULL );
    Field sr( window, bc, ghost, NULL );

    Field::iterator ia = a.begin();
    Field::iterator ib = b.begin();
    for(size_t kk = 0; kk < window.glob_dim(2); kk++) {
        for(size_t jj = 0; jj < window.glob_dim(1); jj++) {
            for(size_t ii = 0; ii < window.glob_dim(0); ii++, ++ia, ++ib) {
	      *ia = ii + jj * 2 + kk * 4;
	      *ib = ii + jj * 3 + kk * 5;
            }
        }
    };

    evaluate_serial_example(sr,
			    a,
			    b,
			    IntVec(nx,ny,nz),
			    Lx,
			    Ly,
			    Lz,
			    number_of_runs);

    evaluate_chaining_example(cr,
			      a,
			      b,
			      IntVec(nx,ny,nz),
			      Lx,
			      Ly,
			      Lz,
			      number_of_runs);

    if(test) {
        INT_EQU(Field, cr, sr, false);
    };

    return 0;
};


#include <boost/math/special_functions/next.hpp>
#include <boost/random.hpp>

#include <limits>

#include "caffe/common.hpp"
#include "caffe/util/math_functions.hpp"
#include "caffe/util/rng.hpp"

namespace caffe {

template<>
void caffe_cpu_gemm<float>(const CBLAS_TRANSPOSE TransA,
                           const CBLAS_TRANSPOSE TransB, const int_tp M,
                           const int_tp N, const int_tp K, const float alpha,
                           const float* A, const float* B, const float beta,
                           float* C) {
  int_tp lda = (TransA == CblasNoTrans) ? K : M;
  int_tp ldb = (TransB == CblasNoTrans) ? N : K;
  cblas_sgemm(CblasRowMajor, TransA, TransB, M, N, K, alpha, A, lda, B, ldb,
              beta, C, N);
}

template<>
void caffe_cpu_gemm<double>(const CBLAS_TRANSPOSE TransA,
                            const CBLAS_TRANSPOSE TransB, const int_tp M,
                            const int_tp N, const int_tp K, const double alpha,
                            const double* A, const double* B, const double beta,
                            double* C) {
  int_tp lda = (TransA == CblasNoTrans) ? K : M;
  int_tp ldb = (TransB == CblasNoTrans) ? N : K;
  cblas_dgemm(CblasRowMajor, TransA, TransB, M, N, K, alpha, A, lda, B, ldb,
              beta, C, N);
}

template<>
void caffe_cpu_gemv<float>(const CBLAS_TRANSPOSE TransA, const int_tp M,
                           const int_tp N, const float alpha, const float* A,
                           const float* x, const float beta, float* y) {
  cblas_sgemv(CblasRowMajor, TransA, M, N, alpha, A, N, x, 1, beta, y, 1);
}

template<>
void caffe_cpu_gemv<double>(const CBLAS_TRANSPOSE TransA, const int_tp M,
                            const int_tp N, const double alpha, const double* A,
                            const double* x, const double beta, double* y) {
  cblas_dgemv(CblasRowMajor, TransA, M, N, alpha, A, N, x, 1, beta, y, 1);
}

template<>
void caffe_axpy<float>(const int_tp N, const float alpha, const float* X,
                       float* Y) {
  cblas_saxpy(N, alpha, X, 1, Y, 1);
}

template<>
void caffe_axpy<double>(const int_tp N, const double alpha, const double* X,
                        double* Y) {
  cblas_daxpy(N, alpha, X, 1, Y, 1);
}

template<typename Dtype>
void caffe_set(const int_tp N, const Dtype alpha, Dtype* Y) {
  if (alpha == 0) {
    memset(Y, 0, sizeof(Dtype) * N);  // NOLINT(caffe/alt_fn)
    return;
  }
  for (int_tp i = 0; i < N; ++i) {
    Y[i] = alpha;
  }
}

template void caffe_set<int32_t>(const int_tp N, const int alpha, int* Y);
template void caffe_set<uint32_t>(const int_tp N, const uint32_t alpha,
                                  uint32_t* Y);
template void caffe_set<int64_t>(const int_tp N, int64_t alpha, int64_t* Y);
template void caffe_set<uint64_t>(const int_tp N, const uint64_t alpha,
                                  uint64_t* Y);
template void caffe_set<float>(const int_tp N, const float alpha, float* Y);
template void caffe_set<double>(const int_tp N, const double alpha, double* Y);

template<>
void caffe_add_scalar(const int_tp N, const float alpha, float* Y) {
  for (int_tp i = 0; i < N; ++i) {
    Y[i] += alpha;
  }
}

template<>
void caffe_add_scalar(const int_tp N, const double alpha, double* Y) {
  for (int_tp i = 0; i < N; ++i) {
    Y[i] += alpha;
  }
}

template<typename Dtype>
void caffe_cpu_copy(const int_tp N, const Dtype* X, Dtype* Y) {
  if (X != Y) {
    memcpy(Y, X, sizeof(Dtype) * N);  // NOLINT(caffe/alt_fn)
  }
}

template void caffe_cpu_copy<int_tp>(const int_tp N, const int_tp* X,
                                     int_tp* Y);
template void caffe_cpu_copy<uint_tp>(const int_tp N, const uint_tp* X,
uint_tp* Y);
template void caffe_cpu_copy<float>(const int_tp N, const float* X, float* Y);
template void caffe_cpu_copy<double>(const int_tp N, const double* X,
                                     double* Y);

template<typename Dtype>
void caffe_copy(const int_tp N, const Dtype* X, Dtype* Y) {
  if (X != Y) {
    if (Caffe::mode() == Caffe::GPU) {
#ifndef CPU_ONLY
#ifdef USE_CUDA
      // NOLINT_NEXT_LINE(caffe/alt_fn)
      CUDA_CHECK(cudaMemcpy(Y, X, sizeof(Dtype) * N, cudaMemcpyDefault));
#endif  // USE_CUDA
#else
      NO_GPU;
#endif
    } else {
      memcpy(Y, X, sizeof(Dtype) * N);  // NOLINT(caffe/alt_fn)
    }
  }
}

template void caffe_copy<int_tp>(const int_tp N, const int_tp* X, int_tp* Y);
template void caffe_copy<uint_tp>(const int_tp N, const uint_tp* X,
uint_tp* Y);
template void caffe_copy<float>(const int_tp N, const float* X, float* Y);
template void caffe_copy<double>(const int_tp N, const double* X, double* Y);

template<>
void caffe_scal<float>(const int_tp N, const float alpha, float *X) {
  cblas_sscal(N, alpha, X, 1);
}

template<>
void caffe_scal<double>(const int_tp N, const double alpha, double *X) {
  cblas_dscal(N, alpha, X, 1);
}

template<>
void caffe_cpu_axpby<float>(const int_tp N, const float alpha, const float* X,
                            const float beta, float* Y) {
  cblas_saxpby(N, alpha, X, 1, beta, Y, 1);
}

template<>
void caffe_cpu_axpby<double>(const int_tp N, const double alpha,
                             const double* X, const double beta, double* Y) {
  cblas_daxpby(N, alpha, X, 1, beta, Y, 1);
}

template<>
void caffe_add<float>(const int_tp n, const float* a, const float* b,
                      float* y) {
  vsAdd(n, a, b, y);
}

template<>
void caffe_add<double>(const int_tp n, const double* a, const double* b,
                       double* y) {
  vdAdd(n, a, b, y);
}

template<>
void caffe_sub<float>(const int_tp n, const float* a, const float* b,
                      float* y) {
  vsSub(n, a, b, y);
}

template<>
void caffe_sub<double>(const int_tp n, const double* a, const double* b,
                       double* y) {
  vdSub(n, a, b, y);
}

template<>
void caffe_mul<float>(const int_tp n, const float* a, const float* b,
                      float* y) {
  vsMul(n, a, b, y);
}

template<>
void caffe_mul<double>(const int_tp n, const double* a, const double* b,
                       double* y) {
  vdMul(n, a, b, y);
}

template<>
void caffe_div<float>(const int_tp n, const float* a, const float* b,
                      float* y) {
  vsDiv(n, a, b, y);
}

template<>
void caffe_div<double>(const int_tp n, const double* a, const double* b,
                       double* y) {
  vdDiv(n, a, b, y);
}

template<>
void caffe_powx<float>(const int_tp n, const float* a, const float b,
                       float* y) {
  vsPowx(n, a, b, y);
}

template<>
void caffe_powx<double>(const int_tp n, const double* a, const double b,
                        double* y) {
  vdPowx(n, a, b, y);
}

template<>
void caffe_sqr<float>(const int_tp n, const float* a, float* y) {
  vsSqr(n, a, y);
}

template<>
void caffe_sqr<double>(const int_tp n, const double* a, double* y) {
  vdSqr(n, a, y);
}

template <>
void caffe_sqrt<float>(const int_tp n, const float* a, float* y) {
  vsSqrt(n, a, y);
}

template <>
void caffe_sqrt<double>(const int_tp n, const double* a, double* y) {
  vdSqrt(n, a, y);
}

template <>
void caffe_exp<float>(const int_tp n, const float* a, float* y) {
  vsExp(n, a, y);
}

template<>
void caffe_exp<double>(const int_tp n, const double* a, double* y) {
  vdExp(n, a, y);
}

template<>
void caffe_log<float>(const int_tp n, const float* a, float* y) {
  vsLn(n, a, y);
}

template<>
void caffe_log<double>(const int_tp n, const double* a, double* y) {
  vdLn(n, a, y);
}

template<>
void caffe_abs<float>(const int_tp n, const float* a, float* y) {
  vsAbs(n, a, y);
}

template<>
void caffe_abs<double>(const int_tp n, const double* a, double* y) {
  vdAbs(n, a, y);
}

uint_tp caffe_rng_rand() {
  return (*caffe_rng())();
}

template<typename Dtype>
Dtype caffe_nextafter(const Dtype b) {
  return boost::math::nextafter<Dtype>(b, std::numeric_limits<Dtype>::max());
}

template
float caffe_nextafter(const float b);

template
double caffe_nextafter(const double b);

void caffe_rng_uniform(const int_tp n, uint_tp* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  boost::uniform_int<int_tp> random_distribution(
      std::numeric_limits<int_tp>::min(), std::numeric_limits<int_tp>::max());
  boost::variate_generator<caffe::rng_t*,
  boost::uniform_int<int_tp>> variate_generator(
      caffe_rng(), random_distribution);
  for (int_tp i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template<typename Dtype>
void caffe_rng_uniform(const int_tp n, const Dtype a, const Dtype b, Dtype* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_LE(a, b);
  boost::uniform_real<Dtype> random_distribution(a, caffe_nextafter<Dtype>(b));
  boost::variate_generator<caffe::rng_t*,
  boost::uniform_real<Dtype>> variate_generator(
      caffe_rng(), random_distribution);
  for (int_tp i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template
void caffe_rng_uniform<float>(const int_tp n, const float a, const float b,
                              float* r);

template
void caffe_rng_uniform<double>(const int_tp n, const double a, const double b,
                               double* r);

template<typename Dtype>
void caffe_rng_gaussian(const int_tp n, const Dtype a, const Dtype sigma,
                        Dtype* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GT(sigma, 0);
  boost::normal_distribution<Dtype> random_distribution(a, sigma);
  boost::variate_generator<caffe::rng_t*,
  boost::normal_distribution<Dtype>> variate_generator(
      caffe_rng(), random_distribution);
  for (int_tp i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template
void caffe_rng_gaussian<float>(const int_tp n, const float mu,
                               const float sigma, float* r);

template
void caffe_rng_gaussian<double>(const int_tp n, const double mu,
                                const double sigma, double* r);

template<typename Dtype, typename Itype>
void caffe_rng_bernoulli(const int_tp n, const Dtype p, Itype* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GE(p, 0);
  CHECK_LE(p, 1);
  boost::bernoulli_distribution<Dtype> random_distribution(p);
  boost::variate_generator<caffe::rng_t*,
  boost::bernoulli_distribution<Dtype>> variate_generator(
      caffe_rng(), random_distribution);
  for (int_tp i = 0; i < n; ++i) {
    r[i] = static_cast<Itype>(variate_generator());
  }
}

template
void caffe_rng_bernoulli<double, unsigned long>(const int_tp n, const double p,  // NOLINT
                                                unsigned long* r);  // NOLINT

template
void caffe_rng_bernoulli<float, unsigned long>(const int_tp n, const float p,  // NOLINT
                                               unsigned long* r);  // NOLINT

template
void caffe_rng_bernoulli<double, long>(const int_tp n, const double p, long* r);  // NOLINT

template
void caffe_rng_bernoulli<float, long>(const int_tp n, const float p, long* r);  // NOLINT

template
void caffe_rng_bernoulli<double, unsigned int>(const int_tp n, const double p,
                                               unsigned int* r);

template
void caffe_rng_bernoulli<float, unsigned int>(const int_tp n, const float p,
                                              unsigned int* r);

template
void caffe_rng_bernoulli<double, int>(const int_tp n, const double p, int* r);

template
void caffe_rng_bernoulli<float, int>(const int_tp n, const float p, int* r);

template<>
float caffe_cpu_strided_dot<float>(const int_tp n, const float* x,
                                   const int_tp incx, const float* y,
                                   const int_tp incy) {
  return cblas_sdot(n, x, incx, y, incy);
}

template<>
double caffe_cpu_strided_dot<double>(const int_tp n, const double* x,
                                     const int_tp incx, const double* y,
                                     const int_tp incy) {
  return cblas_ddot(n, x, incx, y, incy);
}

template<typename Dtype>
Dtype caffe_cpu_dot(const int_tp n, const Dtype* x, const Dtype* y) {
  return caffe_cpu_strided_dot(n, x, 1, y, 1);
}

template
float caffe_cpu_dot<float>(const int_tp n, const float* x, const float* y);

template
double caffe_cpu_dot<double>(const int_tp n, const double* x, const double* y);

template<>
float caffe_cpu_asum<float>(const int_tp n, const float* x) {
  return cblas_sasum(n, x, 1);
}

template<>
double caffe_cpu_asum<double>(const int_tp n, const double* x) {
  return cblas_dasum(n, x, 1);
}

template<>
void caffe_cpu_scale<float>(const int_tp n, const float alpha, const float *x,
                            float* y) {
  cblas_scopy(n, x, 1, y, 1);
  cblas_sscal(n, alpha, y, 1);
}

template<>
void caffe_cpu_scale<double>(const int_tp n, const double alpha,
                             const double *x, double* y) {
  cblas_dcopy(n, x, 1, y, 1);
  cblas_dscal(n, alpha, y, 1);
}

}  // namespace caffe


// ======================================================================
/*
 * \file NFmiKKJArea.cpp
 * \brief Implementation of class NFmiKKJArea
 */
// ======================================================================
/*!
 * \class NFmiKKJArea
 *
 *
 * Implements transformation from rectangular PKJ or YKJ coordinates into geodetic
 * coordinates and vice versa for the Finnish National Grid Coordinate System KKJ
 * (KartastoKoordinaattiJärjestelmä). For details see e.g. Matti Ollikainen in:
 * Geodeettinen Laitos Tiedote 8, 1993.
 *
 * The rectangular PKJ (PerusKoordinaattiJärjestelmä) coordinates are located on KKJ
 * zones 1-4 with easting and northing in meters.
 *
 * The rectangular YKJ (YhtenäisKoordinaattiJärjestelmä) coordinates are aligned to KKJ
 * zone 3 only (central meridian 27 degrees) with easting and northing in meters.
 *
 * Projection used is Gauss-Krueger projection with Hayford's International Reference ellipsoid.
 *
 * INHERETANCE:
 *
 * NOTES:
 *
 * RECTANGULAR LOCAL COORDINATES are relative unitless cartesian XY-coordinates.
 * The range for these coordinates will be derived directly from the size of the
 * local rectangle used. The upper left corner XY-point and the opposite lower right
 * corner XY-point will define the LOCAL RECTANGLE.
 *
 * RECTANGULAR WORLD COORDINATES are cartesian XY-coordinates measured in meters
 * on "true world terrain". The upper left corner XY-point and the opposite lower right
 * corner XY-point (in meters) will define the WORLD RECTANGLE.
 *
 * GEODETIC COORDINATES are latitude-longitude coordinates in degrees.
 *
 * \see NFmiPKJArea and NFmiYKJArea
 *
 * INPUT PARAMETERS/ARGUMENT LISTS
 *
 *	theBottomLeftLatLon = lower left corner point of the rectangle in GEODETIC
 *								 latitude-longitude coordinates
 *
 * theTopRightLatLon =	 upper right corner point of the rectangle in GEODETIC
 *								 latitude-longitude coordinates for
 *the
 *rectangle
 *
 * theTopLeftXY = upper left corner of the rectangle in rectangular LOCAL coordinates
 *
 * theBottomRightXY = lower right corner of the rectangle in rectangular LOCAL coordinates
 *
 * theKKJArea = KKJArea object to be copied
 *
 * NOTES
 *
 * The first constructor is meant to be used when both of the geodetic corner points
 * are known, whereas the second constructor is used when the upper right geodetic
 * corner point is unknown but the metric dimensions of the world rectangle are known.
 *
 */
// ======================================================================

#include "NFmiKKJArea.h"
#include <boost/functional/hash.hpp>
#include <macgyver/Exception.h>
#include <cmath>
#include <cstdlib>
#include <stdexcept>

using namespace std;

// ----------------------------------------------------------------------
/*!
 * Void constructor
 */
// ----------------------------------------------------------------------

NFmiKKJArea::NFmiKKJArea()
    : NFmiArea(),
      itsTopRightLatLon(),
      itsBottomLeftLatLon(),
      a(),
      p(),
      b(),
      e(),
      dn(),
      dn2(),
      dn3(),
      dn4(),
      a1(),
      h1(),
      h2(),
      itsXScaleFactor(),
      itsYScaleFactor(),
      itsWorldRect()
{
  try
  {
    SetupGaussKruger();
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Copy constructor
 *
 * \param theKKJArea The other object being copied
 */
// ----------------------------------------------------------------------

NFmiKKJArea::NFmiKKJArea(const NFmiKKJArea &theKKJArea)
    : NFmiArea(theKKJArea),
      itsTopRightLatLon(theKKJArea.itsTopRightLatLon),
      itsBottomLeftLatLon(theKKJArea.itsBottomLeftLatLon),
      a(),
      p(),
      b(),
      e(),
      dn(),
      dn2(),
      dn3(),
      dn4(),
      a1(),
      h1(),
      h2(),
      itsXScaleFactor(theKKJArea.itsXScaleFactor),
      itsYScaleFactor(theKKJArea.itsYScaleFactor),
      itsWorldRect(theKKJArea.itsWorldRect)
{
  try
  {
    SetupGaussKruger();
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Constructor
 * \param theBottomLeftLatLon Undocumented
 * \param theTopRightLatLon Undocumented
 * \param theTopLeftXY Undocumented
 * \param theBottomRightXY Undocumented
 */
// ----------------------------------------------------------------------

NFmiKKJArea::NFmiKKJArea(const NFmiPoint &theBottomLeftLatLon,
                         const NFmiPoint &theTopRightLatLon,
                         const NFmiPoint &theTopLeftXY,
                         const NFmiPoint &theBottomRightXY,
                         bool usePacificView)
    : NFmiArea(theTopLeftXY, theBottomRightXY, usePacificView),
      itsTopRightLatLon(theTopRightLatLon),
      itsBottomLeftLatLon(theBottomLeftLatLon),
      a(),
      p(),
      b(),
      e(),
      dn(),
      dn2(),
      dn3(),
      dn4(),
      a1(),
      h1(),
      h2(),
      itsXScaleFactor(),
      itsYScaleFactor(),
      itsWorldRect()
{
  try
  {
    SetupGaussKruger();
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

/*!
 * Constructor
 *
 * \param theRadialRangeInMeters Undocumented
 * \param theCenterLatLon Undocumented
 * \param theTopLeftXY Undocumented
 * \param theBottomRightXY Undocumented
 */
NFmiKKJArea::NFmiKKJArea(double /* theRadialRangeInMeters */,
                         const NFmiPoint & /* theCenterLatLon */,
                         const NFmiPoint &theTopLeftXY,
                         const NFmiPoint &theBottomRightXY)

    : NFmiArea(theTopLeftXY, theBottomRightXY),
      itsTopRightLatLon(),
      itsBottomLeftLatLon(),
      a(),
      p(),
      b(),
      e(),
      dn(),
      dn2(),
      dn3(),
      dn4(),
      a1(),
      h1(),
      h2(),
      itsXScaleFactor(),
      itsYScaleFactor(),
      itsWorldRect()
{
  try
  {
    SetupGaussKruger();
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Constructor
 *
 * \param theBottomLeftLatLon Undocumented
 * \param theTopLeftXY Undocumented
 * \param theBottomRightXY Undocumented
 */
// ----------------------------------------------------------------------

NFmiKKJArea::NFmiKKJArea(const NFmiPoint &theBottomLeftLatLon,
                         const NFmiPoint &theTopLeftXY,
                         const NFmiPoint &theBottomRightXY)
    : NFmiArea(theTopLeftXY, theBottomRightXY),
      itsTopRightLatLon(),
      itsBottomLeftLatLon(theBottomLeftLatLon),
      a(),
      p(),
      b(),
      e(),
      dn(),
      dn2(),
      dn3(),
      dn4(),
      a1(),
      h1(),
      h2(),
      itsXScaleFactor(),
      itsYScaleFactor(),
      itsWorldRect()
{
  try
  {
    SetupGaussKruger();
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Constructor
 *
 * \param theTopLeftXY Undocumented
 * \param theBottomRightXY Undocumented
 */
// ----------------------------------------------------------------------

NFmiKKJArea::NFmiKKJArea(const NFmiPoint &theTopLeftXY, const NFmiPoint &theBottomRightXY)
    : NFmiArea(theTopLeftXY, theBottomRightXY),
      itsTopRightLatLon(),
      itsBottomLeftLatLon(),
      a(),
      p(),
      b(),
      e(),
      dn(),
      dn2(),
      dn3(),
      dn4(),
      a1(),
      h1(),
      h2(),
      itsXScaleFactor(),
      itsYScaleFactor(),
      itsWorldRect()
{
  try
  {
    SetupGaussKruger();
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Initializes parameters for Gauss-Kruger projection.
 */
// ----------------------------------------------------------------------

void NFmiKKJArea::SetupGaussKruger()
{
  try
  {
    // Set up Gauss-Krueger projection parameters
    a = 6378388.;                         // Major radius
    p = 1. / 297.;                        // Flatness
    b = (1. - p) * a;                     // Minor radius
    e = sqrt((a * a - b * b) / (a * a));  // Eccentricity

    dn = (a - b) / (a + b);
    dn2 = dn * dn;
    dn3 = dn * dn * dn;
    dn4 = dn * dn * dn * dn;

    a1 = a / (1. + dn) * (1. + dn2 / 4. + dn4 / 64.);

    h1[0] = dn / 2. - 2. * dn2 / 3. + 5. * dn3 / 16. + 41. * dn4 / 180.;
    h1[1] = 13. * dn2 / 48. - 3. * dn3 / 5. + 557. * dn4 / 1440.;
    h1[2] = 61. * dn3 / 240. - 103. * dn4 / 140.;
    h1[3] = 49561. * dn4 / 161280.;

    h2[0] = dn / 2. - (2. / 3.) * dn2 + 37. * dn3 / 96. - dn4 / 360.;
    h2[1] = dn2 / 48. + dn3 / 15. - 437. * dn4 / 1440.;
    h2[2] = 17. * dn3 / 480. - 37. * dn4 / 840.;
    h2[3] = 4397. * dn4 / 161280.;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \param theA Undocumented
 * \param theP Undocumented
 */
// ----------------------------------------------------------------------

void NFmiKKJArea::SetupAnyEllipsoid(double theA, double theP)
{
  try
  {
    a = theA;
    p = theP;

    // Set up any ellipsoid parameters
    // a = 6378388.;	 // Major radius specific to Hayford
    // p = 1./297.;	 // Flatness  specific to Hayford
    b = (1. - p) * a;                     // Minor radius
    e = sqrt((a * a - b * b) / (a * a));  // Eccentricity

    dn = (a - b) / (a + b);
    dn2 = dn * dn;
    dn3 = dn * dn * dn;
    dn4 = dn * dn * dn * dn;

    a1 = a / (1. + dn) * (1. + dn2 / 4. + dn4 / 64.);

    h1[0] = dn / 2. - 2. * dn2 / 3. + 5. * dn3 / 16. + 41. * dn4 / 180.;
    h1[1] = 13. * dn2 / 48. - 3. * dn3 / 5. + 557. * dn4 / 1440.;
    h1[2] = 61. * dn3 / 240. - 103. * dn4 / 140.;
    h1[3] = 49561. * dn4 / 161280.;

    h2[0] = dn / 2. - (2. / 3.) * dn2 + 37. * dn3 / 96. - dn4 / 360.;
    h2[1] = dn2 / 48. + dn3 / 15. - 437. * dn4 / 1440.;
    h2[2] = 17. * dn3 / 480. - 37. * dn4 / 840.;
    h2[3] = 4397. * dn4 / 161280.;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Works as ToLatLon() but the input rectangular parameter theXYPoint is
 * expected to be given in rectangular world coordinates (meters) instead of
 * local coordinates.
 *
 * \param theXYPoint Undocumented
 * \return Undocumented
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiKKJArea::WorldXYToLatLon(const NFmiPoint &theXYPoint) const
{
  try
  {
    double xeast, ynorth, zone, xeastForCentralMeridian, ksi, eta, ksi1, eta1, lambda, q, temp, q2,
        d, testv, lat, lon, dind;
    int ind;

    // Transform world xy-coordinates into geodetic coordinates.
    xeast = theXYPoint.X();   // Easting
    ynorth = theXYPoint.Y();  // Northing

    zone = double(ZoneNumberByEasting(xeast));

    xeastForCentralMeridian = 1000000. * (6. + zone) - 5500000.;
    ksi = ynorth / a1;
    eta = (xeast - xeastForCentralMeridian) / a1;
    ksi1 = 2. * ksi;
    eta1 = 2. * eta;

    for (ind = 1, dind = 1.0; ind <= 4; ind++, dind += 1.0)
    {
      ksi = ksi - h2[ind - 1] * sin(dind * ksi1) * cosh(dind * eta1);
      eta = eta - h2[ind - 1] * cos(dind * ksi1) * sinh(dind * eta1);
    }

    lambda = atan(sinh(eta) / cos(ksi));
    q = Arsinh(cos(lambda) * tan(ksi));
    temp = sin(atan(sinh(1.0040685 * q)));

    do
    {
      q2 = Artanh(e * temp);
      d = sinh(q + e * q2);
      d = d / sqrt(1. + d * d);
      testv = fabs(d - temp);
      temp = d;
    } while (testv >= 1.0E-11);

    lat = Deg(asin(d));
    lon = 18. + zone * 3.0 + Deg(lambda);

    return NFmiPoint(lon, lat);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Returns the geodetic latitude-longitude point corresponding the
 * rectangular local point theXYPoint. Point will be returned in degrees.
 *
 * \param theXYPoint Undocumented
 * \return Undocumented
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiKKJArea::ToLatLon(const NFmiPoint &theXYPoint) const
{
  try
  {
    double xeast, ynorth;

    // Transform local xy-coordinates into world xy-coordinates (meters).
    xeast = itsWorldRect.Left() + (theXYPoint.X() - Left()) / itsXScaleFactor;    // Easting
    ynorth = itsWorldRect.Bottom() - (theXYPoint.Y() - Top()) / itsYScaleFactor;  // Northing

    // Transform world xy-coordinates into geodetic coordinates.
    return WorldXYToLatLon(NFmiPoint(xeast, ynorth));
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \param theLatLonPoint Undocumented
 * \return Undocumented
 * \todo Remove the method as unnecessary duplicate
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiKKJArea::ToWorldXY(const NFmiPoint &theLatLonPoint) const
{
  try
  {
    return LatLonToWorldXY(theLatLonPoint);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \param theXYPoint Undocumented
 * \return Undocumented
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiKKJArea::XYToWorldXY(const NFmiPoint &theXYPoint) const
{
  try
  {
    double xeast = itsWorldRect.Left() + (theXYPoint.X() - Left()) / itsXScaleFactor;
    double ynorth = itsWorldRect.Bottom() - (theXYPoint.Y() - Top()) / itsYScaleFactor;

    return NFmiPoint(xeast, ynorth);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \param theWorldXYPoint Undocumented
 * \return Undocumented
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiKKJArea::WorldXYToXY(const NFmiPoint &theWorldXYPoint) const
{
  try
  {
    double x = itsXScaleFactor * (theWorldXYPoint.X() - itsWorldRect.Left()) + Left();
    double y = Top() - itsYScaleFactor * (theWorldXYPoint.Y() - itsWorldRect.Bottom());
    return NFmiPoint(x, y);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \param theLatLonPoint Undocumented
 * \return Undocumented
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiKKJArea::LatLonToWorldXY(const NFmiPoint &theLatLonPoint) const
{
  try
  {
    // Transforms input geodetic coordinates (longitude,latitude) into world coordinates
    // (meters) for the Finnish National Grid Coordinate System (KKJ).

    double lat, lon, fii, d, zone, xeastForCentralMeridian, lambda, xeast, ynorth, q, ksi, ksi1,
        eta, eta1, dind;
    int ind;

    lat = theLatLonPoint.Y();
    lon = theLatLonPoint.X();

    fii = Rad(lat);
    d = sin(fii);

    zone = double(ZoneNumberByLongitude(lon));
    xeastForCentralMeridian = 1000000.0 * (6. + zone) - 5500000.;
    lambda = Rad(lon - zone * 3. - 18.);

    q = sinh(Arsinh(tan(fii)) - e * Artanh(e * d));
    ksi = atan(q / cos(lambda));
    ksi1 = 2. * ksi;
    eta = Artanh(sin(lambda) / sqrt(1. + q * q));
    eta1 = 2. * eta;

    for (ind = 1, dind = 1.0; ind <= 4; ind++, dind += 1.0)
    {
      ksi = ksi + h1[ind - 1] * sin(dind * ksi1) * cosh(dind * eta1);
      eta = eta + h1[ind - 1] * cos(dind * ksi1) * sinh(dind * eta1);
    }

    ynorth = a1 * ksi;                           // Northing
    xeast = a1 * eta + xeastForCentralMeridian;  // Easting

    return NFmiPoint(xeast, ynorth);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Returns the local rectangular XY-point corresponding the input geodetic
 * latitude-longitude point theLatLonPoint.
 *
 * \param theLatLonPoint Undocumented
 * \return Undocumented
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiKKJArea::ToXY(const NFmiPoint &theLatLonPoint) const
{
  try
  {
    double xLocal, yLocal;

    // Transform input geodetic coordinates into world coordinates (meters).
    NFmiPoint latlon(FixLongitude(theLatLonPoint.X()), theLatLonPoint.Y());
    NFmiPoint xyWorld(LatLonToWorldXY(latlon));

    // Finally, transform world xy-coordinates into local xy-coordinates
    xLocal = Left() + itsXScaleFactor * (xyWorld.X() - itsWorldRect.Left());
    yLocal = Top() + itsYScaleFactor * (itsWorldRect.Bottom() - xyWorld.Y());

    return NFmiPoint(xLocal, yLocal);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Assignment operator
 *
 * \param theArea The other object being copied
 * \return The object assigned to
 * \bug Should protect from self assignment
 */
// ----------------------------------------------------------------------

NFmiKKJArea &NFmiKKJArea::operator=(const NFmiKKJArea &theArea)
{
  try
  {
    NFmiArea::operator=(theArea);

    itsBottomLeftLatLon = theArea.itsBottomLeftLatLon;
    itsTopRightLatLon = theArea.itsTopRightLatLon;
    itsXScaleFactor = theArea.itsXScaleFactor;
    itsYScaleFactor = theArea.itsYScaleFactor;
    itsWorldRect = theArea.itsWorldRect;

    return *this;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Equality comparison
 *
 * \param theArea The object being compared to
 * \return True, if the objects are equal
 */
// ----------------------------------------------------------------------

bool NFmiKKJArea::operator==(const NFmiKKJArea &theArea) const
{
  try
  {
    if ((itsBottomLeftLatLon == theArea.itsBottomLeftLatLon) &&
        (itsTopRightLatLon == theArea.itsTopRightLatLon) &&
        (itsXScaleFactor == theArea.itsXScaleFactor) &&
        (itsYScaleFactor == theArea.itsYScaleFactor) && (itsWorldRect == theArea.itsWorldRect))
      return true;

    return false;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Inequality comparison
 *
 * \param theArea The object being compared to
 * \return True, if the objects are not equal
 */
// ----------------------------------------------------------------------

bool NFmiKKJArea::operator!=(const NFmiKKJArea &theArea) const
{
  try
  {
    return (!(*this == theArea));
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Equality comparison
 *
 * \param theArea The object being compared to
 * \return True, if the objects are equal
 */
// ----------------------------------------------------------------------

bool NFmiKKJArea::operator==(const NFmiArea &theArea) const
{
  try
  {
    return *this == *(static_cast<const NFmiKKJArea *>(&theArea));
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Inequality comparison
 *
 * \param theArea The object being compared to
 * \return True, if the objects are not equal
 */
// ----------------------------------------------------------------------

bool NFmiKKJArea::operator!=(const NFmiArea &theArea) const
{
  try
  {
    return (!(*this == theArea));
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Write the object to the given output stream
 *
 * \param file The output stream to write to
 * \return The output stream written to
 */
// ----------------------------------------------------------------------

std::ostream &NFmiKKJArea::Write(std::ostream &file) const
{
  try
  {
    NFmiArea::Write(file);

    file << itsBottomLeftLatLon;
    file << itsTopRightLatLon;

    // Dummies to replace old removed variables
    file << "0 0\n0 0\n";

    file << itsXScaleFactor << " " << itsYScaleFactor << std::endl;
    file << itsWorldRect;

    return file;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Read new object contents from the given input stream
 *
 * \param file The input stream to read from
 * \return The input stream read from
 */
// ----------------------------------------------------------------------

std::istream &NFmiKKJArea::Read(std::istream &file)
{
  try
  {
    NFmiArea::Read(file);

    double dummy;

    file >> itsBottomLeftLatLon;
    file >> itsTopRightLatLon;
    PacificView(NFmiArea::IsPacificView(itsBottomLeftLatLon, itsTopRightLatLon));

    file >> dummy >> dummy >> dummy >> dummy;  // old removed variables

    file >> itsXScaleFactor >> itsYScaleFactor;
    file >> itsWorldRect;

    Init(true);

    return file;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \param fKeepWorldRect Undocumented
 */
// ----------------------------------------------------------------------

void NFmiKKJArea::Init(bool fKeepWorldRect)
{
  try
  {
    SetupGaussKruger();
    NFmiArea::Init(fKeepWorldRect);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

const std::string NFmiKKJArea::AreaStr() const
{
  try
  {
    std::ostringstream out;
    out << "kkj:" << BottomLeftLatLon().X() << ',' << BottomLeftLatLon().Y() << ','
        << TopRightLatLon().X() << ',' << TopRightLatLon().Y();
    return out.str();
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Return Well Known Text representation of the GCS
 *
 * The value cannot be defined without multiple zones, so we throw instead.
 */
// ----------------------------------------------------------------------

const std::string NFmiKKJArea::WKT() const
{
  try
  {
    throw Fmi::Exception(BCP, "WKT not available for generic KKJ projections");
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Hash value
 */
// ----------------------------------------------------------------------

std::size_t NFmiKKJArea::HashValue() const
{
  try
  {
    std::size_t hash = NFmiArea::HashValue();
    boost::hash_combine(hash, itsTopRightLatLon.HashValue());
    boost::hash_combine(hash, itsBottomLeftLatLon.HashValue());
    // no need to handle a, p, b, e, dn, dn2, dn3, dn4, a1, h1 or h2 here
    boost::hash_combine(hash, boost::hash_value(itsXScaleFactor));
    boost::hash_combine(hash, boost::hash_value(itsYScaleFactor));
    boost::hash_combine(hash, itsWorldRect.HashValue());
    return hash;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}
// ======================================================================


/*******************************************************************************
* Copyright 2016 ROBOTIS CO., LTD.
*
* 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: Hye-Jong KIM */

#ifndef OMKINEMATICS_HPP_
#define OMKINEMATICS_HPP_

#include "../../include/open_manipulator/OMManager.hpp"
#include "../../include/open_manipulator/OMMath.hpp"
#include "../../include/open_manipulator/OMDebug.hpp"

#include <Eigen.h>        // Calls main Eigen matrix class library
#include <Eigen/LU>       // Calls inverse, determinant, LU decomp., etc.
#include <Eigen/Dense>
#include <math.h>

class OMKinematicsMethod
{
 private:
  OMMath math_;

 public:
  OMKinematicsMethod(){};
  ~OMKinematicsMethod(){};

  void getBasePose(Base &base, Eigen::Vector3f base_position, Eigen::Matrix3f base_orientation)
  {
    Pose temp;
    base.setPosition(base_position);
    base.setOrientaion(base_orientation);
    temp.position = base_position + base.inertia_.relative_center_position;
    base.setCenterPosition(temp.position);
  }

  void getBaseJointPose(Manipulator *manipulator, int8_t base_joint_number)
  {
    Pose temp;
    temp.position = manipulator.base_.getPosition() + manipulator.base_.getOrientation()*manipulator.base_.getRelativeBaseJointPosition(base_joint_number);
    temp.orientation = manipulator.base_.getOrientation() * manipulator.base_.getRelativeBaseJointOrientation(base_joint_number) * math_.rodriguesRotationMatrix(manipulator.joint_[base_joint_number].getAxis(), manipulator.joint_[base_joint_number].getAngle());
    manipulator.joint[base_joint_number].setJointPose(temp);
  }

  void getSinglejointPose(Manipulator *manipulator, int8_t joint_number, int8_t mather_joint_number, int8_t link_number)
  {
    Pose temp;
    temp.position = manipulator.joint_[mather_joint_number].getPosition() + manipulator.joint_[mather_joint_number].getOrientation()*manipulator.link_[link_number].getRelativeJointPosition(mather_joint_number,joint_number);
    temp.orientation = manipulator.joint_[mather_joint_number].getOrientation() * manipulator.link_[link_number].getRelativeJointOrientation(mather_joint_number,joint_number) * math_.rodriguesRotationMatrix(manipulator.joint_[joint_number].getAxis(), manipulator.joint[joint_number].getAngle());
    manipulator.joint_[joint_number].setPose(temp);
  }

  void getToolPose(Manipulator *manipulator, int8_t tool_number, int8_t mather_joint_number)
  {
    Pose temp;
    temp.position = manipulator.joint_[mather_joint_number].getPosition() + manipulator.joint_[mather_joint_number].getOrientation()*manipulator.tool_[tool_number].getRelativeToolPosition(mather_joint_number);
    temp.orientation = manipulator.joint_[mather_joint_number].getOrientation() * manipulator.tool_[tool_number].getRelativeToolOrientation(mather_joint_number);
    manipulator.tool_[tool_number].setPose(temp);
  }

  void jacobian(Manipulator *manipulator, int8_t tool_number)
  {
    Eigen::MatrixXf jacobian(6,manipulator.getDOF());
    Eigen::Vector3f position_changed    = Eigen::Vector3f::Zero();
    Eigen::Vector3f orientation_changed = Eigen::Vector3f::Zero();
    Eigen::VectorXf pose_changed(6);

    int8_t j = 0;
    for(int8_t i = 0; i < manipulator.getJointSize(); i++)
    {
      if(manipulator.joint_[i].getId() >= 0)
      {
        position_changed = math_.skewSymmetricMatrix(manipulator.joint_[i].getOrientation()*manipulator.joint_[i].getAxis()) * ( manipulator.tool_[tool_number].getPosition() - manipulator.joint_[i].getPosition());
        orientation_changed = manipulator.joint_[i].getOrientation()*manipulator.joint_[i].getAxis();

        pose_changed(6) << position_changed(0),
                          position_changed(1),
                          position_changed(2),
                          orientation_changed(0),
                          orientation_changed(1),
                          orientation_changed(2);

        jacobian.col(j) = pose_changed;
        j++;
      }
    }
  }
}

class OMChainKinematics
{
  private:
    OMMath math_;

  public:
    OMChainKinematics(){};
    ~OMChainKinematics(){};
};

class OMScaraKinematics
{
  private:
    OMMath math_;

  public:
    OMScaraKinematics(){};
    ~OMScaraKinematics(){};

};

class OMLinkKinematics
{
 private:
  OMMath math_;
  OMKinematicsMethod method_;

  void getPassiveJointAngle(Joint *joint)
  {
    // joint[0].setAngle();
    // joint[1].setAngle(); // 0 < joint[1].setAngle < 3PI/4
    // joint[2].setAngle(); // PI/2 && joint[1].setAngle + 45? < joint[2].setAngle < PI
    joint[3].setAngle(joint[1].getAngle()-joint[2].getAngle());
    joint[4].setAngle(-M_PI-(joint[1].getAngle()-joint[2].getAngle()));
    joint[5].setAngle(-M_PI-(joint[1].getAngle()-joint[2].getAngle()));
    joint[6].setAngle((155 * DEG2RAD) - joint[2].getAngle());
    joint[7].setAngle(joint[1].getAngle());
    joint[8].setAngle((15 * DEG2RAD)-joint[1].getAngle());
    joint[9].setAngle(joint[2].getAngle() - (195 * DEG2RAD));
    joint[10].setAngle((90 * DEG2RAD) - joint[2].getAngle());
  }

 public:
  OMLinkKinematics(){};
  ~OMLinkKinematics(){};

  void forward(Manipulator *omlink, Eigen::Vector3f base_position, Eigen::Matrix3f base_orientation)
  {
    method_.getBasePose(omlink.base_, base_position, base_orientation);
    getPassiveJointAngle(omlink.joint_);
    method_.getBaseJointPose(omlink,0);
    method_.getSinglejointPose(omlink, 1, 0, 0);
    method_.getSinglejointPose(omlink, 2, 0, 0);
    method_.getSinglejointPose(omlink, 3, 2, 2);
    method_.getSinglejointPose(omlink, 4, 3, 3);
    method_.getSinglejointPose(omlink, 5, 1, 1);
    method_.getSinglejointPose(omlink, 6, 5, 4);
    method_.getSinglejointPose(omlink, 7, 0, 0);
    method_.getSinglejointPose(omlink, 8, 7, 5);
    method_.getSinglejointPose(omlink, 9, 8, 6);
    method_.getSinglejointPose(omlink, 10, 9, 7);
    method_.getToolPose(omlink, 0, 6);
  }

  Eigen::VectorXf numericalInverse(Manipulator omlink, int8_t tool_number, Pose target_pose, float gain)
  {
    Eigen::VectorXf target_angle(omlink.getDOF());
    Eigen::MatrixXf jacobian(6,omlink.getDOF());
    Eigen::VectorXf differential_pose(6);
    Eigen::VectorXf previous_differential_pose(6);

    while(differential_pose.norm() < 1E-6)
    {
      forward(omlink);
      jacobian = method_.jacobian(omlink, tool_number);
      differential_pose = math_.differentialPose(target_pose.position, omlink.tool[tool_number].getPosition(), target_pose.orientation, omlink.tool[tool_number].getOrientation())

      Eigen::ColPivHouseholderQR<Eigen::MatrixXf> qrmethod(jacobian);
      target_angle = gain * qrmethod.solve(differential_pose);

      int8_t k = 0;
      for(int8_t j = 0; j < omlink.getJointSize(); j++)
      {
        if(omlink.joint_[j].getId() >= 0)
        {
          omlink.joint_[j].setAngle(target_angle(k));
          k++;
        }
      }

      if(differential_pose.norm()>previous_differential_pose.norm())
      {
        //ERROR
        break;
      }
      previous_differential_pose = differential_pose;
    }

    return target_angle;
  }

  Eigen::VectorXf geometricInverse(Manipulator *omlink, int8_t tool_number, Pose target_pose, float gain) //for basic model
  {
    OMKinematicsMethod method_;
    OMMath math_;

    Eigen::VectorXf target_angle_vector(3);
    Eigen::Vector3f control_position; //joint6-joint1
    Eigen::Vector3f tool_joint6_position = omlink.tool[0].getRelativeToolPosition(6);
    Eigen::Vector3f joint0_position = omlink.joint_[0].getPosition();
    Eigen::Vector3f temp_vector;

    float target_angle[3];
    float link[3];
    float temp_x;
    float temp_y;

    temp_y = target_pose.position(0)-joint0_position(0);
    temp_x = target_pose.position(1)-joint0_position(1);
    target_angle[0] = atan2(temp_y, temp_x);

    control_position(0) = target_pose.position(0) - tool_joint6_position(0)*cos(target_angle[0]);
    control_position(1) = target_pose.position(1) - tool_joint6_position(0)*sin(target_angle[0]);
    control_position(2) = target_pose.position(2) - tool_joint6_position(3)

    temp_vector = omlink.link_[0].getRelativeJointPosition(1,0);
    link[0] = temp_vector(2);
    temp_vector = omlink.link_[1].getRelativeJointPosition(5,1);
    link[1] = temp_vector(0);
    temp_vector = omlink.link_[4].getRelativeJointPosition(6,5);
    link[2] = -temp_vector(0);

    temp_y = control_position(2)-joint0_position(2);
    temp_x = (control_position(0)-joint0_position(0))*cos(target_angle[0]);

    target_angle[1] = acos(((temp_x*temp_x+temp_y*temp_y+link[1]*link[1]-link[2]*link[2]))/(2*link[1]*sqrt(temp_x*temp_x+temp_y*temp_y)) + atan2(temp_y, temp_x);
    target_angle[2] = acos((link[1]*link[1]+link[2]*link[2]-(temp_x*temp_x+temp_y*temp_y))/(2*link[1]*link[2])) + target_angle[1];

    target_angle_vector << target_angle[0],
                           target_angle[1],
                           target_angle[2];
    return target_angle_vector;
  }
};

class OMDeltaKinematics
{
  private:
    OMMath math_;

  public:
    OMDeltaKinematics(){};
    ~OMDeltaKinematics(){};

};

class MYKinematics
{
 private:
   OMMath math_;

 public:
   MYKinematics(){};
   ~MYKinematics(){};
};

#endif // OMKINEMATICS_HPP_


// Copyright (c) 2005 - 2015 Marc de Kamps
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
//
//    * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
//    * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation
//      and/or other materials provided with the distribution.
//    * Neither the name of the 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 OWNER 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.
//
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <TwoDLib.hpp>
#include <vector>
#include <fstream>
#include <iostream>
#include "FixtureOde2DSystemGroup.hpp"

using namespace std;
using namespace TwoDLib;

BOOST_FIXTURE_TEST_CASE(CSRMatrixTest, FixtureOde2DSystemGroup)
{
	// generate transition matrix for mesh 1
	std::ofstream ofst1("mat1.mat");
	ofst1 << "0.1 0.0\n"; // this jump size is immaterial and will not be used
	ofst1 << "10;0,0;1,0:1.0;\n";
	ofst1 << "10;0,1;1,1:1.0;\n";
	ofst1 << "10;1,0;2,0:0.4;2,1:0.6;\n";
	ofst1.close();

	std::ofstream ofst2("mat2.mat");
	ofst2 << "0.1 0.0\n"; // this jump size is immaterial and will not be used
	ofst2 << "10;0,0;1,0:0.3;1,1:0.3;1,2:0.4\n";
	ofst2 << "10;1,0;2,0:0.4;2,1:0.6;\n";
	ofst2.close();

	TransitionMatrix mat1("mat1.mat");
	TransitionMatrix mat2("mat2.mat");

	std::vector<std::vector<Redistribution> > vec_dummy { std::vector<Redistribution>(), std::vector<Redistribution>() };
	std::vector<Mesh> vec_mesh {_mesh1, _mesh2 };
       	Ode2DSystemGroup group(vec_mesh,vec_dummy,vec_dummy);
	group.Initialize(0,0,0);
	group.Initialize(1,0,0);

	CSRMatrix csr1(mat1,group,0);
	CSRMatrix csr2(mat2,group,1);

	std::vector<double> dydt(group.Mass().size(),0.0);
	csr1.MVMapped(dydt,group.Mass(),1000.);
	csr2.MVMapped(dydt,group.Mass(),800.);

	BOOST_CHECK( dydt[0] == -1000.);
	BOOST_CHECK( dydt[2] ==  1000.);
	BOOST_CHECK( dydt[7] == -800.);
	BOOST_CHECK( dydt[8] ==  240.);
	BOOST_CHECK( dydt[9] ==  240.);
	BOOST_CHECK( dydt[10] ==  320.);
}


#pragma once
#include <Eigen/Core>
#include "facets.hpp"

namespace ear {
  /// very simple convex hull implementation
  ///
  /// limitations:
  /// - doesn't deal with degenerate cases (less than 4 points, or all points
  ///   being coplanar)
  /// - doesn't deal with collinear points at all (we only use points a unit
  ///   distance from the origin, so this never occurs)
  /// - doesn't check for valid geometry, so the tolerance has to be high
  ///   enough
  std::vector<Facet> convex_hull(const std::vector<Eigen::Vector3d> &positions,
                                 double tolerance = 1e-5);
}  // namespace ear


#ifndef MATH_TYPES_HPP
#define MATH_TYPES_HPP

#define PI 3.14159265359

#include <Eigen/Dense>

using Vec2f = Eigen::Vector2f;
using Vec2d = Eigen::Vector2d;

#endif // MATH_TYPES_HPP