#include "vtkTemporalTransforms.h"
#include "vtkEigenTools.h"
#include "vtkConversions.h"

#include <vtkCellData.h>
#include <vtkPolyLine.h>
#include <vtkTransform.h>

#include <cmath>

// Eigen
#include <Eigen/Dense>

//-----------------------------------------------------------------------------
vtkStandardNewMacro(vtkTemporalTransforms)

//-----------------------------------------------------------------------------
vtkTemporalTransforms::vtkTemporalTransforms()
{
  auto points = vtkSmartPointer<vtkPoints>::New();

  auto timeArray = vtkSmartPointer<vtkDoubleArray>::New();
  timeArray->SetName(this->TimeArrayName);

  auto orientationArray = vtkSmartPointer<vtkDoubleArray>::New();
  orientationArray->SetName(this->OrientationArrayName);
  orientationArray->SetNumberOfComponents(4);

  this->SetPoints(points);
  this->GetPointData()->AddArray(timeArray);
  this->GetPointData()->AddArray(orientationArray);
}

//-----------------------------------------------------------------------------
vtkSmartPointer<vtkTemporalTransforms> vtkTemporalTransforms::CreateFromPolyData(vtkPolyData *poly)
{
  if (!poly)
  {
    return nullptr;
  }
  // copy the data into a vtkTemporalTransforms before checking that it is well-formed
  auto temporalTransforms = vtkSmartPointer<vtkTemporalTransforms>::New();
  temporalTransforms->ShallowCopy(poly);

  bool isWellFormed = temporalTransforms->GetTimeArray() &&
                      temporalTransforms->GetTranslationArray() &&
                      temporalTransforms->GetOrientationArray();
  if(!isWellFormed)
  {
    return nullptr;
  }

  // create polyline if needed
  if (temporalTransforms->GetLines()->GetNumberOfCells() == 0)
  {
    // create the cell in the same time for visualization
    auto polyLine = vtkSmartPointer<vtkPolyLine>::New();
    polyLine->GetPointIds()->SetNumberOfIds(temporalTransforms->GetNumberOfPoints());
    for (vtkIdType i = 0; i < temporalTransforms->GetNumberOfPoints(); i++)
    {
      polyLine->GetPointIds()->SetId(i,i);
    }
    auto cell = vtkSmartPointer<vtkCellArray>::New();
    cell->InsertNextCell(polyLine);
    temporalTransforms->SetLines(cell);
  }

  return temporalTransforms;
}

vtkSmartPointer<vtkTransform> vtkTemporalTransforms::GetTransform(unsigned int transformNumber)
{
  auto axisAngle = this->GetOrientationArray();
  auto transform = vtkSmartPointer<vtkTransform>::New();
  transform->PostMultiply();
  double x = axisAngle->GetTuple4(transformNumber)[0];
  double y = axisAngle->GetTuple4(transformNumber)[1];
  double z = axisAngle->GetTuple4(transformNumber)[2];
  double w = axisAngle->GetTuple4(transformNumber)[3]
      * 180 / vtkMath::Pi(); // vtk need degrees not radians
  transform->RotateWXYZ(w, x, y, z);
  transform->Translate(this->GetTranslationArray()->GetTuple(transformNumber));

  return transform;
}

vtkSmartPointer<vtkVelodyneTransformInterpolator> vtkTemporalTransforms::CreateInterpolator()
{
  auto interpolator = vtkSmartPointer<vtkVelodyneTransformInterpolator>::New();

  auto timestamp = this->GetTimeArray();

  for (unsigned int i = 0; i < this->GetNumberOfPoints(); i++)
  {
    // get timestamp
    double currentTimestamp = timestamp->GetTuple1(i);

    // create the tranform
    auto transform = this->GetTransform(i);

    // add the tranform to the interpolator
    if (!vtkMath::IsNan(currentTimestamp))
    {
      interpolator->AddTransform(currentTimestamp, transform);
    }
    else
    {
      vtkErrorMacro(<< "Timestamp " << i << "is not a number")
    }
  }
  interpolator->Modified();
  return interpolator;
}

//-----------------------------------------------------------------------------
void vtkTemporalTransforms::SetOrientationArray(vtkDoubleArray *array)
{
  if (array->GetNumberOfComponents() != 4)
  {
    vtkErrorMacro(<< "The orientation array has " << array->GetNumberOfComponents()
                  << ". 4 components are expected")
  }
  array->SetName(this->OrientationArrayName);
  this->GetPointData()->AddArray(array);
}

//-----------------------------------------------------------------------------
void vtkTemporalTransforms::SetTranslationArray(vtkDataArray *array)
{
  if (array->GetNumberOfComponents() != 3)
  {
    vtkErrorMacro(<< "The translation array has " << array->GetNumberOfComponents()
                  << ". 3 components are expected")
  }
  auto points =  vtkSmartPointer<vtkPoints>::New();
  this->SetPoints(points);
  this->GetPoints()->SetData(array);

  // create the cell in the same time for visualization
  auto polyLine = vtkSmartPointer<vtkPolyLine>::New();
  polyLine->GetPointIds()->SetNumberOfIds(this->GetNumberOfPoints());
  for (vtkIdType i = 0; i < array->GetNumberOfTuples(); i++)
  {
    polyLine->GetPointIds()->SetId(i,i);
  }
  auto cell = vtkSmartPointer<vtkCellArray>::New();
  cell->InsertNextCell(polyLine);
  this->SetLines(cell);
}

//-----------------------------------------------------------------------------
void vtkTemporalTransforms::SetTimeArray(vtkDoubleArray *array)
{
  if (array->GetNumberOfComponents() != 1)
  {
    vtkErrorMacro(<< "The time array has " << array->GetNumberOfComponents()
                  << ". 1 component is expected")
  }
  array->SetName(this->TimeArrayName);
  this->GetPointData()->AddArray(array);
}

//-----------------------------------------------------------------------------
vtkSmartPointer<vtkTemporalTransforms> vtkTemporalTransforms::IsometricTransform(vtkSmartPointer<vtkTransform> H)
{
  vtkSmartPointer<vtkTemporalTransforms> outputPoses = vtkSmartPointer<vtkTemporalTransforms>::New();
  outputPoses->DeepCopy(this);

  // First, compute the rotation and translation from H 4x4 matrix
  std::pair<Eigen::Vector3d, Eigen::Vector3d> transParams = GetPoseParamsFromTransform(H);
  Eigen::Matrix3d R0 = RollPitchYawToMatrix(transParams.first);
  Eigen::Vector3d T0 = transParams.second;

  // We need to change the angle axis and position arrays
  vtkDoubleArray* xyzwArray = vtkDoubleArray::SafeDownCast(this->GetOrientationArray());
  vtkDoubleArray* xyzArray = vtkDoubleArray::SafeDownCast(this->GetTranslationArray());

  // Loop over the transforms points
  for (unsigned int transformIndex = 0; transformIndex < this->GetNumberOfPoints(); transformIndex++)
  {
    // Get the angle-axis representation
    double* xyzw = xyzwArray->GetTuple4(transformIndex);
    double* xyz = xyzArray->GetTuple3(transformIndex);

    // Compute the corresponding rotation matrix that
    // correspond to the angle axis representation
    Eigen::Vector3d T(xyz[0], xyz[1], xyz[2]);
    Eigen::AngleAxisd angleAxis(xyzw[3], Eigen::Vector3d(xyzw[0], xyzw[1], xyzw[2]));
    Eigen::Matrix3d R = angleAxis.toRotationMatrix();

    // Compute the new rotation and translation
    R = R0 * R;
    T = R0 * T + T0;

    // Get the axis angle representation of this new rotation
    Eigen::AngleAxisd newAngleAxis(R);
    double newXyzw[4] = {newAngleAxis.axis()(0), newAngleAxis.axis()(1),
                         newAngleAxis.axis()(2), newAngleAxis.angle()};

    // Replace it
    xyzwArray->SetTuple4(transformIndex, newXyzw[0], newXyzw[1], newXyzw[2], newXyzw[3]);
    xyzArray->SetTuple3(transformIndex, T(0), T(1), T(2));
  }
  return outputPoses;
}

//-----------------------------------------------------------------------------
vtkSmartPointer<vtkTemporalTransforms> vtkTemporalTransforms::CycloidicTransform(vtkSmartPointer<vtkTransform> H)
{
  vtkSmartPointer<vtkTemporalTransforms> outputPoses = vtkSmartPointer<vtkTemporalTransforms>::New();
  outputPoses->DeepCopy(this);

  // First, compute the rotation and translation from H 4x4 matrix
  std::pair<Eigen::Vector3d, Eigen::Vector3d> transParams = GetPoseParamsFromTransform(H);
  Eigen::Matrix3d R0 = RollPitchYawToMatrix(transParams.first);
  Eigen::Vector3d T0 = transParams.second;

  // We need to change the angle axis and position arrays
  vtkDoubleArray* xyzwArray = vtkDoubleArray::SafeDownCast(this->GetOrientationArray());
  vtkDoubleArray* xyzArray = vtkDoubleArray::SafeDownCast(this->GetTranslationArray());

  // Loop over the transforms points
  for (unsigned int transformIndex = 0; transformIndex < this->GetNumberOfPoints(); transformIndex++)
  {
    // Get the angle-axis representation
    double* xyzw = xyzwArray->GetTuple4(transformIndex);
    double* xyz = xyzArray->GetTuple3(transformIndex);

    // Compute the corresponding rotation matrix that
    // correspond to the angle axis representation
    Eigen::Vector3d T(xyz[0], xyz[1], xyz[2]);
    Eigen::AngleAxisd angleAxis(xyzw[3], Eigen::Vector3d(xyzw[0], xyzw[1], xyzw[2]));
    Eigen::Matrix3d R = angleAxis.toRotationMatrix();

    // Compute the new rotation and translation
    T = R * T0 + T;
    R = R * R0;

    // Get the axis angle representation of this new rotation
    Eigen::AngleAxisd newAngleAxis(R);
    double newXyzw[4] = {newAngleAxis.axis()(0), newAngleAxis.axis()(1),
                         newAngleAxis.axis()(2), newAngleAxis.angle()};

    // Replace it
    xyzwArray->SetTuple4(transformIndex, newXyzw[0], newXyzw[1], newXyzw[2], newXyzw[3]);
    xyzArray->SetTuple3(transformIndex, T(0), T(1), T(2));
  }
  return outputPoses;
}

//-----------------------------------------------------------------------------
void vtkTemporalTransforms::PushBack(double time, const Eigen::AngleAxisd& orientation , const Eigen::Vector3d translation)
{
  this->GetTimeArray()->InsertNextTuple1(time);
  this->GetOrientationArray()->InsertNextTuple4(orientation.axis()[0],
                                                orientation.axis()[1],
                                                orientation.axis()[2],
                                                orientation.angle());
  this->GetTranslationArray()->InsertNextTuple(static_cast<const double*>(translation.data()));

  // replace the cell by a line with one more point
  auto polyLine = vtkSmartPointer<vtkPolyLine>::New();
  polyLine->GetPointIds()->SetNumberOfIds(this->GetNumberOfPoints());
  for (vtkIdType i = 0; i < this->GetNumberOfPoints(); i++)
  {
    polyLine->GetPointIds()->SetId(i,i);
  }
  auto cell = vtkSmartPointer<vtkCellArray>::New();
  cell->InsertNextCell(polyLine);
  this->SetLines(cell);
}

vtkSmartPointer<vtkTemporalTransforms> vtkTemporalTransforms::ExtractTimes(double tstart, double tend)
{
  auto extract = vtkSmartPointer<vtkTemporalTransforms>::New();

  for (unsigned int i = 0; i < this->GetNumberOfPoints(); i++)
  {
    // get timestamp
    double currentTimestamp = this->GetTimeArray()->GetTuple1(i);
    if (currentTimestamp < tstart || currentTimestamp > tend) {
      continue;
    }

    extract->GetTimeArray()->InsertNextTuple1(currentTimestamp);
    double* aa = this->GetOrientationArray()->GetTuple4(i);
    extract->GetOrientationArray()->InsertNextTuple(aa);
    double* xyz = this->GetTranslationArray()->GetTuple3(i);
    extract->GetTranslationArray()->InsertNextTuple(xyz);
  }

  return extract;
}

vtkSmartPointer<vtkTemporalTransforms> vtkTemporalTransforms::Subsample(int N)
{

  auto extract = vtkSmartPointer<vtkTemporalTransforms>::New();

  for (unsigned int i = 0; i < this->GetNumberOfPoints(); i++)
  {
    if (i % N != 0)
    {
      continue;
    }
    // get timestamp
    double currentTimestamp = this->GetTimeArray()->GetTuple1(i);
    extract->GetTimeArray()->InsertNextTuple1(currentTimestamp);
    double* aa = this->GetOrientationArray()->GetTuple4(i);
    extract->GetOrientationArray()->InsertNextTuple(aa);
    double* xyz = this->GetTranslationArray()->GetTuple3(i);
    extract->GetTranslationArray()->InsertNextTuple(xyz);
  }

  auto polyLine = vtkSmartPointer<vtkPolyLine>::New();
  polyLine->GetPointIds()->SetNumberOfIds(extract->GetNumberOfPoints());
  for (vtkIdType i = 0; i < extract->GetNumberOfPoints(); i++)
  {
    polyLine->GetPointIds()->SetId(i,i);
  }
  auto cell = vtkSmartPointer<vtkCellArray>::New();
  cell->InsertNextCell(polyLine);
  extract->SetLines(cell);

  return extract;
}

vtkSmartPointer<vtkTemporalTransforms> vtkTemporalTransforms::ApplyTimeshift(double shift)
{
  auto timeshifted = vtkSmartPointer<vtkTemporalTransforms>::New();

  for (unsigned int i = 0; i < this->GetNumberOfPoints(); i++)
  {
    double currentTimestamp = this->GetTimeArray()->GetTuple1(i);
    timeshifted->GetTimeArray()->InsertNextTuple1(currentTimestamp + shift);
    double* aa = this->GetOrientationArray()->GetTuple4(i);
    timeshifted->GetOrientationArray()->InsertNextTuple(aa);
    double* xyz = this->GetTranslationArray()->GetTuple3(i);
    timeshifted->GetTranslationArray()->InsertNextTuple(xyz);
  }

  auto polyLine = vtkSmartPointer<vtkPolyLine>::New();
  polyLine->GetPointIds()->SetNumberOfIds(timeshifted->GetNumberOfPoints());
  for (vtkIdType i = 0; i < timeshifted->GetNumberOfPoints(); i++)
  {
    polyLine->GetPointIds()->SetId(i,i);
  }
  auto cell = vtkSmartPointer<vtkCellArray>::New();
  cell->InsertNextCell(polyLine);
  timeshifted->SetLines(cell);

  bool isWellFormed = timeshifted->GetTimeArray() &&
                      timeshifted->GetTranslationArray() &&
                      timeshifted->GetOrientationArray();
  if(!isWellFormed)
  {
    std::cout << "warning ! timeshifted not well formed" << std::endl;
  }

  return timeshifted;
}

vtkSmartPointer<vtkTemporalTransforms> vtkTemporalTransforms::ApplyScale(double scale)
{
  auto scaled = vtkSmartPointer<vtkTemporalTransforms>::New();

  for (unsigned int i = 0; i < this->GetNumberOfPoints(); i++)
  {
    double currentTimestamp = this->GetTimeArray()->GetTuple1(i);
    scaled->GetTimeArray()->InsertNextTuple1(currentTimestamp);
    double* aa = this->GetOrientationArray()->GetTuple4(i);
    scaled->GetOrientationArray()->InsertNextTuple(aa);
    double* xyz = this->GetTranslationArray()->GetTuple3(i);
    xyz[0] *= scale;
    xyz[1] *= scale;
    xyz[2] *= scale;
    scaled->GetTranslationArray()->InsertNextTuple(xyz);
  }

  auto polyLine = vtkSmartPointer<vtkPolyLine>::New();
  polyLine->GetPointIds()->SetNumberOfIds(scaled->GetNumberOfPoints());
  for (vtkIdType i = 0; i < scaled->GetNumberOfPoints(); i++)
  {
    polyLine->GetPointIds()->SetId(i,i);
  }
  auto cell = vtkSmartPointer<vtkCellArray>::New();
  cell->InsertNextCell(polyLine);
  scaled->SetLines(cell);

  bool isWellFormed = scaled->GetTimeArray() && scaled->GetTranslationArray() &&
                      scaled->GetOrientationArray();
  if(!isWellFormed)
  {
    std::cout << "warning ! scaled not well formed" << std::endl;
  }

  return scaled;
}


#include <memory>
#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <sudoku/dimensions.h>
#include <sudoku/groupwise_empty_solver.h>
#include <sudoku/parallel_solver.h>
#include <sudoku/solver.h>
#include <sudoku/solver_interface.h>
#include <sudoku/square.h>
#include <sudoku/cell_value_parser.h>
#include <sudoku/groupwise_solver.h>

enum class SolverType
{
    SEQUENTIAL,
    PARALLEL,
    GROUPWISE_EMPTY,
    GROUPWISE
};

namespace std
{
    ostream& operator<<(ostream& out, const SolverType& solverType)
    {
        switch (solverType) {
            case SolverType::SEQUENTIAL: return out << "sudoku::Solver";
            case SolverType::PARALLEL: return out << "sudoku::ParallelSolver";
            case SolverType::GROUPWISE_EMPTY: return out << "sudoku::GroupwiseEmptySolver";
            case SolverType::GROUPWISE: return out << "sudoku::GroupwiseSolver";
            default: return out << "<Unknown Solver>";
        }
    }
}

std::unique_ptr<sudoku::SolverInterface> solverFactory(const sudoku::Dimensions& dims, SolverType solverType)
{
    switch (solverType) {
        case SolverType::SEQUENTIAL:
            return std::make_unique<sudoku::Solver>(dims);
        case SolverType::PARALLEL:
            return std::make_unique<sudoku::ParallelSolver>(dims, 4, 8);
        case SolverType::GROUPWISE_EMPTY:
            return std::make_unique<sudoku::GroupwiseEmptySolver>(dims);
        case SolverType::GROUPWISE:
            return std::make_unique<sudoku::GroupwiseSolver>(dims);
        default:
            throw std::runtime_error("unrecognised SolverType");
    }
}

std::unique_ptr<sudoku::SolverInterface> solverFactory(const sudoku::Dimensions& dims,
                                                       std::vector<sudoku::CellValue> initialValues,
                                                       SolverType solverType)
{
    sudoku::Grid grid(dims, initialValues);
    switch (solverType) {
        case SolverType::SEQUENTIAL:
            return std::make_unique<sudoku::Solver>(grid);
        case SolverType::PARALLEL:
            return std::make_unique<sudoku::ParallelSolver>(grid, 4, 8);
        case SolverType::GROUPWISE:
            return std::make_unique<sudoku::GroupwiseSolver>(dims, initialValues);
        default:
            throw std::runtime_error("unrecognised SolverType");
    }
}

SolverType emptySolverTypes[] = {
    SolverType::SEQUENTIAL,
    SolverType::PARALLEL,
    SolverType::GROUPWISE_EMPTY,
    SolverType::GROUPWISE
};

BOOST_DATA_TEST_CASE(SolverInterface_empty9x9, emptySolverTypes)
{
    sudoku::square::Dimensions dims(3);
    auto solver = solverFactory(dims, sample);
    BOOST_REQUIRE(solver->computeNextSolution());
    auto cellValues = solver->getCellValues();
    BOOST_REQUIRE_EQUAL(0, std::count(cellValues.begin(), cellValues.end(), 0));

    auto metrics = solver->getMetrics();
    BOOST_REQUIRE_GE(metrics.totalGuesses, dims.getCellCount());
    BOOST_REQUIRE_EQUAL(metrics.totalGuesses - metrics.totalBacktracks, dims.getCellCount());
    BOOST_REQUIRE_GT(std::chrono::duration_cast<std::chrono::nanoseconds>(metrics.duration).count(), 0);

    // Empty sudokus have more than 1 solution.
    BOOST_REQUIRE(solver->computeNextSolution());
    auto cellValues2 = solver->getCellValues();
    BOOST_REQUIRE_EQUAL(0, std::count(cellValues2.begin(), cellValues2.end(), 0));
    BOOST_REQUIRE(cellValues != cellValues2);
}

SolverType solverTypes[] = {
    SolverType::SEQUENTIAL,
    SolverType::PARALLEL,
    SolverType::GROUPWISE
};

BOOST_DATA_TEST_CASE(SolverInterface_initialValues, solverTypes)
{
    sudoku::square::Dimensions dims(3);
    auto inputValueString =
        "000600400 700003600 000091080"
        "000000000 050180003 000306045"
        "040200060 903000000 020000100";
    auto outputValueString =
        "581672439 792843651 364591782"
        "438957216 256184973 179326845"
        "845219367 913768524 627435198";
    auto inputValues = sudoku::parseCellValues(dims, inputValueString);
    auto expectedSolution = sudoku::parseCellValues(dims, outputValueString);
    auto solver = solverFactory(dims, inputValues, sample);
    BOOST_REQUIRE(solver->computeNextSolution());
    auto foundSolution = solver->getCellValues();
    BOOST_REQUIRE_EQUAL_COLLECTIONS(expectedSolution.begin(), expectedSolution.end(),
                                    foundSolution.begin(), foundSolution.end());
}


#include <iostream>
#include <chrono>
#include <thread>
#include <Eigen/Eigen>

#include <pangolin/pangolin.h>
#include <SceneGraph/SceneGraph.h>

using namespace std;

int main( int /*argc*/, char** /*argv[]*/ )
{
    // Create OpenGL window in single line thanks to GLUT
    pangolin::CreateGlutWindowAndBind("Main",640,480, GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH | GLUT_MULTISAMPLE);
    SceneGraph::GLSceneGraph::ApplyPreferredGlSettings();
    glewInit();

    // Scenegraph to hold GLObjects and relative transformations
    SceneGraph::GLSceneGraph glGraph;

    // Create shadow light to illuminate scene. Shadow casters
    // and receivers must be set up individually.
    SceneGraph::GLShadowLight shadowLight(10,10,-100, 2048,2048);
    glGraph.AddChild(&shadowLight);

    // Define grid object
    SceneGraph::GLGrid glGrid(20,1.0, true);
    glGraph.AddChild(&glGrid);
    shadowLight.AddShadowReceiver(&glGrid);

    // Define axis object, and set its pose
    SceneGraph::GLAxis glAxis;
    glAxis.SetPose(-1,-2,-0.1, 0, 0, M_PI/4);
    glAxis.SetScale(0.25);
    glGraph.AddChild(&glAxis);

    // Define 3D spiral using a GLCachedPrimitives object
    SceneGraph::GLCachedPrimitives glSpiral(GL_LINE_STRIP, SceneGraph::GLColor(1.0f,0.7f,0.2f));
    for(double t=0; t < 10*M_PI; t+= M_PI/50) {
        glSpiral.AddVertex(Eigen::Vector3d(cos(t)+2, sin(t)+2, -0.2*t) );
    }
    glGraph.AddChild(&glSpiral);

    SceneGraph::GLCube glCube;
    glCube.SetPose(1.5,1.5,-sqrt(3), M_PI/4, M_PI/4, M_PI/4);
    glGraph.AddChild(&glCube);
    shadowLight.AddShadowCaster(&glCube);

#ifdef HAVE_ASSIMP
    // Define a mesh object and try to load model
    SceneGraph::GLMesh glMesh;
    try {
        glMesh.Init("./model.blend");
        glMesh.SetPosition(0,0,-1);
        glMesh.SetScale(4.0f);
        glGraph.AddChild(&glMesh);
        shadowLight.AddShadowCasterAndReceiver(&glMesh);
    }catch(exception e) {
        cerr << "Cannot load mesh. Check file exists" << endl;
    }
#endif // HAVE_ASSIMP

    // Define Camera Render Object (for view / scene browsing)
    pangolin::OpenGlRenderState stacks3d(
        pangolin::ProjectionMatrix(640,480,420,420,320,240,0.1,1000),
        pangolin::ModelViewLookAt(0,-2,-4, 0,1,0, pangolin::AxisNegZ)
    );

    // Pangolin abstracts the OpenGL viewport as a View.
    // Here we get a reference to the default 'base' view.
    pangolin::View& container = pangolin::DisplayBase();

    // We define a new view which will reside within the container.
    pangolin::View view3d;

    // We set the views location on screen and add a handler which will
    // let user input update the model_view matrix (stacks3d) and feed through
    // to our scenegraph
    view3d.SetBounds(0.0, 1.0, 0.0, 1.0, 640.0f/480.0f)
          .SetHandler(new SceneGraph::HandlerSceneGraph(glGraph,stacks3d,pangolin::AxisNegZ))
          .SetDrawFunction(SceneGraph::ActivateDrawFunctor(glGraph, stacks3d));

    pangolin::View& viewLight = pangolin::CreateDisplay()
            .SetBounds(0.0, 0.3, 0, 0.3)
            .SetDrawFunction(SceneGraph::ActivateScissorClearDrawFunctor(glGraph,shadowLight.GetRenderState()));

    // Add our view as children to the base container.
    container.AddDisplay(view3d);
    container.AddDisplay(viewLight);

    // Default hooks for exiting (Esc) and fullscreen (tab).
    for(int frame=0; !pangolin::ShouldQuit(); ++frame )
    {
        // Clear whole screen
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        // Animate position of light over time.
        shadowLight.SetPosition(100*cos(frame/100.0), 100*sin(frame/100.0), -100 );

        // Swap frames and Process Events
        pangolin::FinishFrame();

        // Pause for 1/60th of a second.
        std::this_thread::sleep_for(std::chrono::milliseconds(1000 / 60));
    }

    return 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2015 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.

// This file was modified by Oracle on 2015-2020.
// Modifications copyright (c) 2015-2020, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_STRATEGY_GEOGRAPHIC_ENVELOPE_HPP
#define BOOST_GEOMETRY_STRATEGY_GEOGRAPHIC_ENVELOPE_HPP

#include <boost/geometry/srs/spheroid.hpp>

#include <boost/geometry/strategy/geographic/envelope_segment.hpp>
#include <boost/geometry/strategy/geographic/expand_segment.hpp>
#include <boost/geometry/strategies/geographic/parameters.hpp>
#include <boost/geometry/strategy/spherical/envelope.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace envelope
{

template
<
    typename FormulaPolicy = strategy::andoyer,
    typename Spheroid = geometry::srs::spheroid<double>,
    typename CalculationType = void
>
class geographic
    : public spherical<CalculationType>
{
public:
    typedef geographic_tag cs_tag;

    typedef Spheroid model_type;

    inline geographic()
        : m_spheroid()
    {}

    explicit inline geographic(Spheroid const& spheroid)
        : m_spheroid(spheroid)
    {}

    Spheroid model() const
    {
        return m_spheroid;
    }

private:
    Spheroid m_spheroid;
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

namespace services
{

template <typename Tag, typename CalculationType>
struct default_strategy<Tag, geographic_tag, CalculationType>
{
    typedef strategy::envelope::geographic
        <
            strategy::andoyer,
            geometry::srs::spheroid<double>,
            CalculationType
        > type;
};

}

#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::envelope

}} //namepsace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGY_GEOGRAPHIC_ENVELOPE_HPP


#include <config.h>
// dune-multiscale
// Copyright Holders: Patrick Henning, Rene Milk
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#include <assert.h>
#include <boost/format.hpp>
#include <dune/grid/io/file/dgfparser/gridptr.hh>
#include <dune/multiscale/common/error_calc.hh>

#include <dune/multiscale/common/traits.hh>
#include <dune/multiscale/msfem/fem_solver.hh>
#include <dune/multiscale/fem/print_info.hh>

#include <dune/multiscale/msfem/msfem_solver.hh>
#include <dune/multiscale/msfem/msfem_traits.hh>
#include <dune/multiscale/problems/selector.hh>
#include <dune/multiscale/common/df_io.hh>

#include <dune/xt/common/logging.hh>
#include <dune/xt/common/configuration.hh>
#include <dune/xt/common/ranges.hh>
#include <dune/stuff/grid/output/entity_visualization.hh>
#include <dune/stuff/grid/information.hh>
#include <dune/stuff/grid/structuredgridfactory.hh>
#include <dune/stuff/grid/provider.hh>
#include <dune/xt/common/timings.hh>
#include <dune/gdt/discretefunction/default.hh>

#include <cmath>
#include <iterator>
#include <memory>
#include <sstream>

#include "algorithm.hh"
#include <dune/multiscale/problems/base.hh>
#include <dune/multiscale/problems/selector.hh>
#include <dune/multiscale/common/grid_creation.hh>
#include <dune/multiscale/tools/misc/outputparameter.hh>
#include <dune/multiscale/msfem/localsolution_proxy.hh>
#include <dune/multiscale/msfem/localproblems/localgridlist.hh>

namespace Dune {
namespace Multiscale {

//! algorithm
std::map<std::string, double> msfem_algorithm()
{
  using namespace Dune;

  Dune::XT::Common::ScopedTiming algo("msfem.algorithm");
  DXTC_TIMINGS.start("msfem.setup.grid");
  const MPIHelper::MPICommunicator& comm = Dune::MPIHelper::getCommunicator();
  DMP::ProblemContainer problem(comm, comm, DXTC_CONFIG);
  auto grid = make_coarse_grid(problem);
  DXTC_TIMINGS.stop("msfem.setup.grid");
  DXTC_TIMINGS.start("msfem.setup.problem");
  DXTC_CONFIG.set("grids.dim", CommonTraits::world_dim, true);
  problem.getMutableModelData().prepare_new_evaluation(problem);
  DXTC_TIMINGS.stop("msfem.setup.problem");

  const CommonTraits::SpaceType coarseSpace(
      CommonTraits::SpaceChooserType::PartViewType::create(*grid, CommonTraits::st_gdt_grid_level));
  std::unique_ptr<LocalsolutionProxy> msfem_solution(nullptr);

  LocalGridList localgrid_list(problem, coarseSpace);
  Elliptic_MsFEM_Solver().apply(problem, coarseSpace, msfem_solution, localgrid_list);

  if (problem.config().get("global.vtk_output", false)) {
    Dune::XT::Common::ScopedTiming algo("msfem.vtk_output");
    CommonTraits::DiscreteFunctionType coarse_grid_visualization(coarseSpace, "Visualization_of_the_coarse_grid");
    coarse_grid_visualization.visualize(
        OutputParameters(problem.config().get("global.datadir", "data")).fullpath(coarse_grid_visualization.name()));
  }

  if (!problem.config().get("msfem.skip_error", false))
    return ErrorCalculator(problem, msfem_solution).print(MS_LOG_INFO_0);
  return decltype(ErrorCalculator(problem, msfem_solution).print(MS_LOG_INFO_0))();
} // function algorithm

} // namespace Multiscale {
} // namespace Dune {


// Test of EPANET's Network Building Functions
//
// This is a test of the API functions EN_setjuncdata, EN_settankdata & EN_setpipedata
//
#define _CRT_SECURE_NO_DEPRECATE

//#define NO_BOOST

#ifndef NO_BOOST
#define BOOST_TEST_MODULE "toolkit"
#include <boost/test/included/unit_test.hpp>
#endif

#include <iostream>
#include <string>
#include "epanet2_2.h"

#define DATA_PATH_INP "./net1.inp"
#define DATA_PATH_RPT "./test.rpt"
#define DATA_PATH_OUT "./test.out"

#ifdef NO_BOOST
#define BOOST_REQUIRE(x) (((x)) ? cout << "\nPassed at line " << __LINE__ : cout << "\nFailed at line " << __LINE__ )
#endif

using namespace std;

#ifndef NO_BOOST
BOOST_AUTO_TEST_SUITE (test_toolkit)
BOOST_AUTO_TEST_CASE(test_setlinktype)
{
#else
int main(int argc, char *argv[])
{
#endif

    int index;
    char id[EN_MAXID+1];
    double p1_1, p2_1, p1_2, p2_2;
    double q1_1, q2_1, q1_2, q2_2;

    // Create & initialize a project
    EN_Project ph = NULL;
    EN_createproject(&ph);
    EN_init(ph, "", "", EN_GPM, EN_HW);

    // Build a network
    EN_addnode(ph, (char *)"N1", EN_JUNCTION);
    EN_addnode(ph, (char *)"N2", EN_JUNCTION);
    EN_addnode(ph, (char *)"N3", EN_RESERVOIR);
    EN_addnode(ph, (char *)"N4", EN_TANK);
    EN_addlink(ph, (char *)"L1", EN_PUMP, (char *)"N3", (char *)"N1");
    EN_addlink(ph, (char *)"L2", EN_PIPE, (char *)"N1", (char *)"N3");
    EN_addlink(ph, (char *)"L3", EN_PIPE, (char *)"N1", (char *)"N2");
    EN_addcurve(ph, (char *)"C1");

    // Set network data using the new helper functions
    EN_setcurvevalue(ph, 1, 1, 1500, 250);
    EN_setjuncdata(ph, 1, 700, 500, "");
    EN_setjuncdata(ph, 2, 710, 500, "");
    EN_setnodevalue(ph, 3, EN_ELEVATION, 800);
    EN_settankdata(ph, 4, 850, 120, 100, 150, 50.5, 0, "");
    EN_setlinkvalue(ph, 1, EN_PUMP_HCURVE, 1);
    EN_setpipedata(ph, 2, 10560, 12, 100, 0);
    EN_setpipedata(ph, 3, 5280, 14, 100, 0);

    // Run hydraulics
    EN_solveH(ph);

    // Save results
    EN_getnodevalue(ph, 1, EN_PRESSURE, &p1_1);
    EN_getnodevalue(ph, 2, EN_PRESSURE, &p2_1);
    EN_getlinkvalue(ph, 1, EN_FLOW, &q1_1);
    EN_getlinkvalue(ph, 2, EN_FLOW, &q2_1);

    // Save project
    EN_saveinpfile(ph, "test2.inp");

    // Close project
    EN_close(ph);

    // Open the saved project file
    EN_open(ph, "test2.inp", "", "");

    // Run hydraulics
    EN_solveH(ph);

    // Save these new results
    EN_getnodevalue(ph, 1, EN_PRESSURE, &p1_2);
    EN_getnodevalue(ph, 2, EN_PRESSURE, &p2_2);
    EN_getlinkindex(ph, (char *)"L1", &index);
    EN_getlinkvalue(ph, index, EN_FLOW, &q1_2);
    EN_getlinkindex(ph, (char *)"L2", &index);
    EN_getlinkvalue(ph, index, EN_FLOW, &q2_2);

    // Display old & new results
    cout << "\n  Node N1 Pressure: " << p1_1 << "  " << p1_2;
    cout << "\n  Node N2 Pressure: " << p2_1 << "  " << p2_2;
    cout << "\n  Link L1 Flow:     " << q1_1 << "  " << q1_2;
    cout << "\n  Link L2 Flow:     " << q2_1 << "  " << q2_2;

    // Compare old & new results
    BOOST_REQUIRE(abs(p1_1 - p1_2) < 1.e-5);
    BOOST_REQUIRE(abs(p2_1 - p2_2) < 1.e-5);
    BOOST_REQUIRE(abs(q1_1 - q1_2) < 1.e-5);
    BOOST_REQUIRE(abs(q2_1 - q2_2) < 1.e-5);

    // Close project
    EN_close(ph);
    EN_deleteproject(&ph);
}
#ifndef NO_BOOST
BOOST_AUTO_TEST_SUITE_END()
#endif


/**********************************************************************
*  Copyright (c) 2008-2013, 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 <model/test/ModelFixture.hpp>
#include <model/Model_Impl.hpp>
#include <model/Space.hpp>
#include <model/SpaceType.hpp>
#include <model/ThermalZone.hpp>
#include <model/ElectricEquipment.hpp>
#include <model/ElectricEquipment_Impl.hpp>
#include <model/ElectricEquipmentDefinition.hpp>
#include <model/ElectricEquipmentDefinition_Impl.hpp>
#include <model/Building.hpp>
#include <model/Building_Impl.hpp>
#include <model/LifeCycleCost.hpp>

#include <utilities/geometry/Point3d.hpp>

#include <boost/foreach.hpp>

using namespace openstudio;
using namespace openstudio::model;

TEST_F(ModelFixture, ElectricEquipment)
{
  Model model;

  ElectricEquipmentDefinition definition(model);
  ElectricEquipment electricEquipment(definition);
  EXPECT_EQ(2u, model.numObjects());
}

TEST_F(ModelFixture, ElectricEquipment_Cost) {
  Model model;
  Building building = model.getUniqueModelObject<Building>();
  SpaceType spaceType(model);

  Point3dVector floorPrint;
  floorPrint.push_back(Point3d(0, 10, 0));
  floorPrint.push_back(Point3d(10, 10, 0));
  floorPrint.push_back(Point3d(10, 0, 0));
  floorPrint.push_back(Point3d(0, 0, 0));

  boost::optional<Space> space1 = Space::fromFloorPrint(floorPrint, 3, model);
  ASSERT_TRUE(space1);
  boost::optional<Space> space2 = Space::fromFloorPrint(floorPrint, 3, model);
  ASSERT_TRUE(space2);

  EXPECT_DOUBLE_EQ(200, building.floorArea());

  ElectricEquipmentDefinition definition(model);
  ElectricEquipment electricEquipment1(definition);
  ElectricEquipment electricEquipment2(definition);

  boost::optional<LifeCycleCost> cost = LifeCycleCost::createLifeCycleCost("Dusting", definition, 0.5, "CostPerArea", "Maintenance", 1);
  ASSERT_TRUE(cost);
  EXPECT_EQ(1, cost->repeatPeriodYears());

  EXPECT_DOUBLE_EQ(0, cost->totalCost());

  electricEquipment1.setSpace(*space1);

  EXPECT_DOUBLE_EQ(50, cost->totalCost());

  electricEquipment2.setSpace(*space2);

  EXPECT_DOUBLE_EQ(100, cost->totalCost());

  ThermalZone thermalZone(model);
  space1->setThermalZone(thermalZone);
  thermalZone.setMultiplier(4);

  EXPECT_DOUBLE_EQ(250.0, cost->totalCost());

  building.setSpaceType(spaceType);
  electricEquipment1.setSpaceType(spaceType);

  EXPECT_DOUBLE_EQ(300.0, cost->totalCost());
}


//=============================================================================================================
/**
 * @file     warp.cpp
 * @author   Lorenz Esch <lesch@mgh.harvard.edu>
 * @version  dev
 * @date     November, 2015
 *
 * @section  LICENSE
 *
 * Copyright (C) 2015, Lorenz Esch. 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 MNE-CPP authors 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.
 *
 *
 * @brief   Warp class definition.
 */

//=============================================================================================================
// INCLUDES
//=============================================================================================================

#include "warp.h"

#include <iostream>

//=============================================================================================================
// EIGEN INCLUDES
//=============================================================================================================

#include <Eigen/LU>

//=============================================================================================================
// QT INCLUDES
//=============================================================================================================

#include <QDebug>
#include <QFile>
#include <QList>

//=============================================================================================================
// USED NAMESPACES
//=============================================================================================================

using namespace UTILSLIB;
using namespace Eigen;

//=============================================================================================================
// DEFINE MEMBER METHODS
//=============================================================================================================

MatrixXf Warp::calculate(const MatrixXf &sLm, const MatrixXf &dLm, const MatrixXf &sVert)
{
    MatrixXf warpWeight, polWeight;
    calcWeighting(sLm, dLm, warpWeight, polWeight);
    MatrixXf wVert = warpVertices(sVert, sLm, warpWeight, polWeight);
    return wVert;
}

//=============================================================================================================

void Warp::calculate(const MatrixXf & sLm, const MatrixXf &dLm, QList<MatrixXf> & vertList)
{
    MatrixXf warpWeight, polWeight;
    calcWeighting(sLm, dLm, warpWeight, polWeight);

    for (int i=0; i<vertList.size(); i++)
    {
        vertList.replace(i,warpVertices(vertList.at(i), sLm, warpWeight, polWeight));
    }
    return;
}

//=============================================================================================================

bool Warp::calcWeighting(const MatrixXf &sLm, const MatrixXf &dLm, MatrixXf& warpWeight, MatrixXf& polWeight)
{
    MatrixXf K = MatrixXf::Zero(sLm.rows(),sLm.rows());     //K(i,j)=||sLm(i)-sLm(j)||
    for (int i=0; i<sLm.rows(); i++)
        K.col(i)=((sLm.rowwise()-sLm.row(i)).rowwise().norm());

//    std::cout << "Here is the matrix K:" << std::endl << K << std::endl;

    MatrixXf P (sLm.rows(),4);                              //P=[ones,sLm]
    P << MatrixXf::Ones(sLm.rows(),1),sLm;
//    std::cout << "Here is the matrix P:" << std::endl << P << std::endl;

    MatrixXf L ((sLm.rows()+4),(sLm.rows()+4));             //L=Full Matrix of the linear eq.
    L <<    K,P,
            P.transpose(),MatrixXf::Zero(4,4);
//    std::cout << "Here is the matrix L:" << std::endl << L << std::endl;

    MatrixXf Y ((dLm.rows()+4),3);                          //Y=[dLm,Zero]
    Y <<    dLm,
            MatrixXf::Zero(4,3);
//    std::cout << "Here is the matrix Y:" << std::endl << Y << std::endl;

    //
    // calculate the weighting matrix (Y=L*W)
    //
    MatrixXf W ((dLm.rows()+4),3);                          //W=[warpWeight,polWeight]
    Eigen::FullPivLU <MatrixXf> Lu(L);                      //LU decomposition is one method to solve lin. eq.
    W=Lu.solve(Y);
//    std::cout << "Here is the matrix W:" << std::endl << W << std::endl;

    warpWeight = W.topRows(sLm.rows());
    polWeight = W.bottomRows(4);

    return true;
}

//=============================================================================================================

MatrixXf Warp::warpVertices(const MatrixXf &sVert, const MatrixXf & sLm, const MatrixXf& warpWeight, const MatrixXf& polWeight)
{
    MatrixXf wVert = sVert * polWeight.bottomRows(3);         //Pol. Warp
    wVert.rowwise() += polWeight.row(0);                      //Translation

    //
    // TPS Warp
    //
    MatrixXf K = MatrixXf::Zero(sVert.rows(),sLm.rows());     //K(i,j)=||sLm(i)-sLm(j)||
    for (int i=0; i<sVert.rows(); i++)
        K.row(i)=((sLm.rowwise()-sVert.row(i)).rowwise().norm().transpose());
//    std::cout << "Here is the matrix K:" << std::endl << K << std::endl;

    wVert += K*warpWeight;
//    std::cout << "Here is the matrix wVert:" << std::endl << wVert << std::endl;
    return wVert;
}

//=============================================================================================================

MatrixXf Warp::readsLm(const QString &electrodeFileName)
{
    MatrixXf electrodes;
    QFile file(electrodeFileName);

    if(!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
        qDebug()<<"Error opening file";
//        return false;
    }

    //Start reading from file
    double numberElectrodes;
    QTextStream in(&file);
    int i=0;

    while(!in.atEnd())
    {
        QString line = in.readLine();
        QStringList fields = line.split(QRegExp("\\s+"));

        //Delete last element if it is a blank character
        if(fields.at(fields.size()-1) == "")
            fields.removeLast();

        //Read number of electrodes
        if(i == 0){
            numberElectrodes = fields.at(fields.size()-1).toDouble();
            electrodes = MatrixXf::Zero(numberElectrodes, 3);
        }
        //Read actual electrode positions
        else{
            Vector3f x;
            x << fields.at(fields.size()-3).toFloat(),fields.at(fields.size()-2).toFloat(),fields.at(fields.size()-1).toFloat();
            electrodes.row(i-1)=x.transpose();
        }
        i++;
    }
    return electrodes;
}


//This program checks to see if the camera can see the target at different poses.
//Assumes a flat rectangular target.

#include <vector>
#include <industrial_extrinsic_cal/conical_pose_generator.h>
#include <industrial_extrinsic_cal/targets_yaml_parser.h>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <boost/shared_ptr.hpp>
#include <industrial_extrinsic_cal/target.h>
#include <industrial_extrinsic_cal/targets_yaml_parser.h>
#include <ros/ros.h>
#include <ros/node_handle.h>
#include <geometry_msgs/PoseArray.h>
#include <eigen_conversions/eigen_msg.h>

#include "applicable_pose_generator/pose_reachability_filter.h"

#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Twist.h>

#include <industrial_extrinsic_cal/ros_transform_interface.h>
#include <industrial_extrinsic_cal/check_if_points_in_pic.h>

#include <industrial_extrinsic_cal/camera_yaml_parser.h>

using std::vector;
using std::string;
using industrial_extrinsic_cal::Target;
using YAML::Node;

// local function prototypes
int main(int argc, char **argv)
{
  ros::init(argc, argv, "node");
  ros::NodeHandle n("~");
  ros::NodeHandle pivnh("~");

  //loads all known values and parameters
  check_if_point_in_pic initialization(pivnh);

  initialization.center_point_of_target = initialization.finds_middle_of_target(initialization.center_Of_TargetX, initialization.center_Of_TargetY);

  // creates camera transforms in the shape of a cone
  ros::Publisher pub = n.advertise<geometry_msgs::PoseArray>("topic", 1, true);

  // radius of the cone = (poseHeight/ std::tan(angleOfCone))
  geometry_msgs::PoseArray msg = initialization.create_all_poses(initialization.poseHeight, initialization.spacing_in_z, initialization.angleOfCone, initialization.numberOfStopsForPhotos,  initialization.center_point_of_target );

  //creates rectanglular target in rviz
  initialization.create_rviz_target(msg);

  // convert the messages back into poses
  EigenSTL::vector_Affine3d AllcameraPoses(msg.poses.size());
  geometry_msgs::PoseArray msg2;
  for(int i=0; i< msg.poses.size(); i++)
  {
    tf::poseMsgToEigen(msg.poses[i],AllcameraPoses[i]);
  }

  // filters out unreachable and unseeable poses
  geometry_msgs::PoseArray filtered_msgs = initialization.pose_filters(msg2,initialization.tf_transform, initialization.image_width,initialization.image_height, AllcameraPoses, initialization.fx,  initialization.fy,  initialization.cx_,  initialization.cy_ );

  ros::Publisher pub2 = n.advertise<geometry_msgs::PoseArray>("topic2", 1, true);
  filtered_msgs.header.frame_id = "target";
  filtered_msgs.header.stamp = ros::Time::now();

  // publishes all transforms
  ros::Rate r(10); // 10 hz
  while (ros::ok())
  {
    pub2.publish(filtered_msgs);
    pub.publish(msg);
    ros::spinOnce();
    r.sleep();
  }
} //end of main

// Transforms a point into the camera frame then projects the new point into the 2d screen
void check_if_point_in_pic::imagePoint(Eigen::Vector3d TargetPoint , double fx, double fy, double cx, double cy, double &u, double &v,const Eigen::Affine3d &cameraPose )
{
 Eigen::Vector3d cp;
 cp = cameraPose.inverse() * TargetPoint;
 double divider = cp.z();
 if(divider <= .0001)
 {
   ROS_ERROR("Camera can't be here");
 }
 if(divider == 0) divider = 1.0;

 u = fx*(cp.x()/divider) + cx;
 v = fy*(cp.y()/divider) + cy;
}

int check_if_point_in_pic::addingFactorial(int lastAdded)
{
    int sumation = 0;
    while(lastAdded>0)
    {
       sumation = sumation +lastAdded;
       lastAdded = lastAdded-1;
    }
    return sumation;
}
int findingMidpoint(int pointOne, int pointTwo)
{
  int midpoint;
  return midpoint;
}

Eigen::Vector2d check_if_point_in_pic::finds_middle_of_target( double center_Of_TargetX, double center_Of_TargetY)
{
  Eigen::Vector2d center_point;
  for(int i=0; i<4; i++)
  {
    for(int j=0; j<4; j++)
    {
      if(corner_points_[i][0] != corner_points_[j][0])
      {
        center_Of_TargetX = (corner_points_[i][0] + corner_points_[j][0]) / 2;
      }
      if(corner_points_[i][1] != corner_points_[j][1])
      {
        center_Of_TargetY = (corner_points_[i][1] + corner_points_[j][1]) / 2;
      }
    }
  }
  center_point[0]=center_Of_TargetX;
  center_point[1]=center_Of_TargetY;
  return center_point;
}

geometry_msgs::PoseArray check_if_point_in_pic::create_all_poses(double poseHeight, double spacing_in_z, double angleOfCone, int numberOfStopsForPhotos, Eigen::Vector2d center_point_of_target )
{
  double pi = 3.14159265359;
  double angleInRadians = angleOfCone*(pi/180);
  ROS_INFO ("angle= %lf degrees", angleInRadians *pi/180.0 );
  geometry_msgs::PoseArray msg;
  int extra_Counter = 0;
  for(double j=0; j<=poseHeight; j=j+spacing_in_z)
  {
    for(double l = (std::tan(angleInRadians))*spacing_in_z ; l<=(std::tan(angleInRadians) * (j) ); l = l+(std::tan(angleInRadians))*spacing_in_z)
    {
      EigenSTL::vector_Affine3d cameraPoses = getConicalPoses(numberOfStopsForPhotos, j, l);
      msg.header.frame_id = "target";
      msg.header.stamp = ros::Time::now();
      extra_Counter ++;
      for(int i=0; i<cameraPoses.size(); i++ )
      {
        geometry_msgs::Pose pose;
        tf::poseEigenToMsg(cameraPoses[i], pose);
        ROS_INFO("adding a pose");
        pose.position.x = pose.position.x + center_point_of_target[0];

        pose.position.y = pose.position.y + center_point_of_target[1];

        msg.poses.push_back(pose);

      }
    }
    numberOfStopsForPhotos ++;
  }
  return msg;
}

geometry_msgs::PoseArray check_if_point_in_pic::pose_filters(geometry_msgs::PoseArray msg2, tf::StampedTransform tf_transform, int image_width, int image_height, EigenSTL::vector_Affine3d AllcameraPoses, double fx, double fy, double cx, double cy )
{
  CreateChain::chain_creation reachability_filter;
  int num_created_poses = AllcameraPoses.size();
  for(int j=0;j<num_created_poses;j++)
  {
      bool accept_pose = true;
      for(int i=0;i<4;i++)
      {
          double u=0;
          double v=0;
          imagePoint(corner_points_[i], fx, fy, cx_, cy_, u, v,AllcameraPoses[j]);
          if(u<0 || u>=image_width || v<0 || v>= image_height )
          {
            ROS_ERROR("u,v = %lf %lf but image size = %d %d",u,v,image_width,image_height);
            accept_pose = false;
          }
          double x_part = tf_transform.getOrigin().x();
          double y_part = tf_transform.getOrigin().y();
          double z_part = tf_transform.getOrigin().z();

          //ROS_INFO("%lf %lf %lf", tf_transform.getOrigin().x(), tf_transform.getOrigin().y(), tf_transform.getOrigin().z());

          tf::Quaternion Q;
          tf_transform.getBasis().getRotation(Q);
          Eigen::Affine3d R;
          R.setIdentity();
          R.translate(Eigen::Vector3d(x_part,y_part,z_part));
          R.rotate(Eigen::Quaterniond(Q.getW(), Q.getX(), Q.getY(), Q.getZ()));
          Eigen::Matrix3Xd m = R.rotation();
          Eigen::Vector3d vec = R.translation();
          if(!reachability_filter.chain_Parse(R*AllcameraPoses[j]))
          {
            ROS_ERROR("Robot unable to reach the location");
            accept_pose = false;
          }
      }
      if(accept_pose)
      {
          geometry_msgs::Pose pose;
          ROS_INFO("adding an accepted pose");
          tf::poseEigenToMsg(AllcameraPoses[j], pose);
          msg2.poses.push_back(pose);
      }
  }
  return msg2;
}

void check_if_point_in_pic::create_transform_listener(tf::StampedTransform& tf_transform,tf::TransformListener& tf_listen)
{
  ros::Time now = ros::Time::now();
  while (!tf_listen.waitForTransform(from_frame_param_, to_frame_param_, now, ros::Duration(1.0)))
   {
     now = ros::Time::now();
     ROS_INFO("waiting for tranform from %s to %s", from_frame_param_.c_str(), to_frame_param_.c_str());
   }
   try
   {
    tf_listen.lookupTransform(from_frame_param_, to_frame_param_, now, tf_transform);
   }
   catch (tf::TransformException& ex)
   {
     ROS_ERROR("%s", ex.what());
   }
}
check_if_point_in_pic::check_if_point_in_pic(ros::NodeHandle pivnh)
{
  //gets camera parameters
  if (!pivnh.getParam("image_width", image_width)){
    ROS_ERROR("did not set parameter image_width");
  }
  if (!pivnh.getParam("image_height", image_height)){
    ROS_ERROR("did not set parameter image_height");
  }
  if (!pivnh.getParam("focal_length", fx)){
    ROS_ERROR("did not set parameter focal_length");
  }
  if (!pivnh.getParam("numberOfStopsForPhotos", numberOfStopsForPhotos)){
    ROS_ERROR("did not set parameter numberOfStopsForPhotos");
  }
  if (!pivnh.getParam("poseHeight", poseHeight)){
    ROS_ERROR("did not set parameter poseHeight");
  }
  if (!pivnh.getParam("spacing_in_z", spacing_in_z)){
    ROS_ERROR("did not set parameter spacing_in_z");
  }
  if (!pivnh.getParam("angleOfCone", angleOfCone)){
    ROS_ERROR("did not set parameter angleOfCone");
  }

  fy = fx;
  cx_ = image_width/2;//assumes that lense is ligned up with sensor
  cy_ = image_height/2;//assums that lense is ligned up with sensor

  if (!pivnh.getParam("world", from_frame_param_))
  {
    ROS_ERROR("did not set parameter world");
  }
  if (!pivnh.getParam("target", to_frame_param_))
  {
    ROS_ERROR("did not set parameter target");
  }

  create_transform_listener(tf_transform, tf_listener);

  //reads in information about target
  string targetFile = "/home/lawrencelewis/catkin_ws/src/industrial_calibration/industrial_extrinsic_cal/yaml/ical_srv_target.yaml";
  vector<boost::shared_ptr<Target>>  myYamlDefinedTargets;
  if(!parseTargets(targetFile , myYamlDefinedTargets))
  {
    ROS_ERROR("could not parse target file %s", targetFile.c_str());
    return ;
  }
   boost::shared_ptr<Target> Mytarget = myYamlDefinedTargets[0];

  //finds the max values to be used as corner points
  ROS_INFO("target name = %s num_points = %d size of pts_ = %d",Mytarget->target_name_,Mytarget->num_points_,(int)Mytarget->pts_.size());
  xMax = Mytarget->pts_[0].x;
  xMin = Mytarget->pts_[0].x;
  yMax = Mytarget->pts_[0].y;
  yMin = Mytarget->pts_[0].y;
  for(int i=0;i<Mytarget->num_points_;i++)
  {
    if(Mytarget->pts_[i].x > xMax) xMax = Mytarget->pts_[i].x;
    if(Mytarget->pts_[i].y > yMax) yMax = Mytarget->pts_[i].y;
    if(Mytarget->pts_[i].x < xMin) xMin = Mytarget->pts_[i].x;
    if(Mytarget->pts_[i].y < yMin) yMin = Mytarget->pts_[i].y;
  }

  corner_points_[0] = Eigen::Vector3d((xMin-xMax/2)/5,(yMin-yMax/2)/5,0);
  corner_points_[1] = Eigen::Vector3d((xMax-xMax/2)/5,(yMin-yMax/2)/5,0);
  corner_points_[2] = Eigen::Vector3d((xMin-xMax/2)/5,(yMax-yMax/2)/5,0);
  corner_points_[3] = Eigen::Vector3d((xMax-xMax/2)/5,(yMax-yMax/2)/5,0);
}
void check_if_point_in_pic::create_rviz_target( geometry_msgs::PoseArray& msg)
{
  geometry_msgs::Pose pnt1,pnt2,pnt3,pnt4;
  pnt1.position.x = corner_points_[0][0];
  pnt1.position.y = corner_points_[0][1];
  pnt1.position.z = corner_points_[0][2];
  pnt1.orientation.w = 1.0;
  pnt1.orientation.x = 0.0;
  pnt1.orientation.y = 0.0;
  pnt1.orientation.z = 0.0;

  pnt2.position.x = corner_points_[1][0];
  pnt2.position.y = corner_points_[1][1];
  pnt2.position.z = corner_points_[1][2];
  pnt2.orientation.w = 1.0;
  pnt2.orientation.x = 0.0;
  pnt2.orientation.y = 0.0;
  pnt2.orientation.z = 0.0;

  pnt3.position.x = corner_points_[2][0];
  pnt3.position.y = corner_points_[2][1];
  pnt3.position.z = corner_points_[2][2];
  pnt3.orientation.w = 1.0;
  pnt3.orientation.x = 0.0;
  pnt3.orientation.y = 0.0;
  pnt3.orientation.z = 0.0;

  pnt4.position.x = corner_points_[3][0];
  pnt4.position.y = corner_points_[3][1];
  pnt4.position.z = corner_points_[3][2];
  pnt4.orientation.w = 1.0;
  pnt4.orientation.x = 0.0;
  pnt4.orientation.y = 0.0;
  pnt4.orientation.z = 0.0;

  msg.poses.push_back(pnt1);
  msg.poses.push_back(pnt2);
  msg.poses.push_back(pnt3);
  msg.poses.push_back(pnt4);

  return;
}


//
// Created by 李卫东 on 2019-02-19.
//
#include <hb/monitor_price_plugin/monitor_pairs.h>
#include <boost/regex.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/algorithm/string/regex.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <math.h>
#include <hb/http/http.h>
#include <hb/https/https.h>
#include <hb/send_mail_plugin/send_mail_plugin.h>
#include <sstream>
#include <hb/time/time.h>

namespace hb{ namespace plugin {

        void monitor_pairs::deal(const shared_ptr<monitor_targets> &targets){
            log_info<<"begin monitor_pair";
            auto &all_targets_list = targets->all_tagets();
            for(auto &it : pairs_list_) {
                if(!it.active)
                    continue;
                auto &target1 = all_targets_list[it.id1];
                auto &target2 = all_targets_list[it.id2];
                if(target1.status!=target_status::target_status_ok || target2.status!=target_status::target_status_ok){
                    log_info<<"【pair " << target1.id<< "~" << target2.id << "】status is wrong "<< target1.status << "~" << target2.status;
                    continue;
                }
                if(target1.date!=target2.date){
                    log_info<<"【pair " << target1.id<< "~" << target2.id << "】date is wrong "<< target1.date << "~" << target2.date;
                    continue;
                }
                // auto val = std::fabs(target2.value-target1.value);
                auto val = target2.value-target1.value;
                std::ostringstream contents1;
                contents1<<"(";
                for(auto &it: target1.contents){
                    contents1<<" "<<it<<" ";
                }
                contents1<<")";
                std::ostringstream contents2;
                contents2<<"(";
                for(auto &it: target2.contents){
                    contents2<<" "<<it<<" ";
                }
                contents2<<")";
                if(it.min<val && val<it.max){
                    log_info<< "【pair " << target1.id<< "~" << target2.id << "】normal " << val << contents1.str() << contents2.str();
                }else{
                    log_info<< "【pair " << target1.id<< "~" << target2.id << "】attent " << val << contents1.str() << contents2.str();
                    //snd msg small
                    auto cur_seconds = hb::time::timestamp();
                    if(cur_seconds - it.send_msg_time >=sendmsg_seconds_){
                        it.send_msg_time = cur_seconds;
                        log_info<< "【pair send msg " << target1.id <<  "~" << target2.id << " 】 value is " << val;
                        //snd msg
                        auto &mail_plugin = app().get_plugin<send_mail_plugin>();
                        auto api = mail_plugin.get_api();
                        std::ostringstream subject;
                        subject<<"[pair "<<target1.id<<"~"<<target2.id<<"]" << val << contents1.str() << contents2.str();
                        api->send_mail(it.mail, subject.str(), subject.str()+"\n\rPlease pay attention to the pair !!!");
                    }
                }
            }
        }
} }


#ifndef __aerodynamics_HH__
#define __aerodynamics_HH__
/********************************* TRICK HEADER *******************************
PURPOSE:
      (Describe the AERODYNAMICS Module On Board)
LIBRARY DEPENDENCY:
      ((../src/Aerodynamics.cpp))
PROGRAMMERS:
      (((Chun-Hsu Lai) () () () ))
*******************************************************************************/
#include <armadillo>
#include <functional>
#include "Module.hh"
#include "Propulsion.hh"
#include "aux.hh"
#include "datadeck.hh"
#include "global_constants.hh"

class Propulsion;

class AeroDynamics : public Dynamics {
  TRICK_INTERFACE(AeroDynamics);

 public:
  explicit AeroDynamics(Propulsion& prop, Data_exchang& input);
  AeroDynamics(const AeroDynamics& other);

  AeroDynamics& operator=(const AeroDynamics& other);

  void load_aerotable(const char* filename);

  virtual void init();
  virtual void algorithm(double int_step);

  void set_refa(double in);
  void set_refd(double in);
  void set_XCP(double in);

 private:
  Propulsion* propulsion;
  Datadeck aerotable; /* ** (--) Aero Deck */

  double refa;  /* *o (m2)     Reference area for aero coefficients - m^2*/
  double refd;  /* *o (m)      Reference length for aero coefficients - m*/
  double cy;    /* *o (--)     Side force coefficient - ND*/
  double cll;   /* *o (--)     Rolling moment coefficient - ND*/
  double clm;   /* *o (--)     Pitching moment coefficient - ND*/
  double cln;   /* *o (--)     Yawing moment coefficient - ND*/
  double cx;    /* *o (--)     X-force coefficient - ND*/
  double cz;    /* *o (--)     Z-force coefficient - ND*/
  double cn;    /* *o (--)     Normal force coefficient */
  double ca;    /* *o (--)     Axial force coefficient */
  double ca_on; /* *o (--)     Axial force coefficient when engine is running */
  double cnq;   /* *o (--)     Normal damping coefficient */
  double cmq;   /* *o (--)     Pitch moment derivative coefficient */
  double ca0;
  double caa;
  double ca0b;
  double cn0;
  double clm0;
  double clmq;
  double cna;
  double clma;

  VECTOR(XCP,
         3); /* *o (m)      Non-dimensional center of pressure (from nose)*/
};

#endif  // __aerodynamics_HH__


// Copyright 2017-2020 Coen Tempelaars (Falcons)
// SPDX-License-Identifier: Apache-2.0
/*
 * abstractHeightMap.hpp
 *
 *  Created on: Nov 9, 2017
 *      Author: Coen Tempelaars
 */

#ifndef ABSTRACTHEIGHTMAP_HPP_
#define ABSTRACTHEIGHTMAP_HPP_

#include <map>
#include <string>
#include <vector>
#include <boost/numeric/ublas/matrix.hpp>
#include <opencv2/opencv.hpp>

#include "vector2d.hpp"

namespace teamplay
{

typedef std::map<std::string, std::string> parameterMap_t;

namespace heightMapValues
{
    /* The resolution is the size (in meters) of a square inside a heightmap. */
    static const float RESOLUTION = 0.25;

    static const float MIN = -100.0;
    static const float NEUTRAL = 0.0;
    static const float MAX = 100.0;
}

class heightMapField
{
public:
    void setValue (const float);
    void setValue (const float, const float, const float);
    float getValue() const;

    Point2D _center;

private:
    float _value;
};

/* A heightmap is a grid of squares */
class abstractHeightMap
{
public:
    abstractHeightMap();
    virtual ~abstractHeightMap();

    virtual void precalculate();
    virtual abstractHeightMap refine(const parameterMap_t&);
    virtual std::string getDescription() const;
    virtual std::string getFilename() const;

    cv::Mat generateOpenCVMatrix () const;
    heightMapField getOptimum() const;
    heightMapField getFieldAtCoordinate(const Point2D&) const;
    abstractHeightMap scale(const float) const;
    abstractHeightMap operator+(const abstractHeightMap&) const;

protected:
    /* reset() sets the size of the heightmap and gives every heightmapfield a center  */
    void reset();

    unsigned int getNrOfHeightMapFieldsInX() const;
    unsigned int getNrOfHeightMapFieldsInY() const;

    double angleOnPath (const Point2D& source, const Point2D& at, const Point2D& destination) const;

    /* this is the heightmap. heightmapfield _heightmap(0,0) is at field coordinate (-x, -y).
     * heightmapfield _heightmap(nrInX, nrInY) is at field coordinate (+x, +y). */
    typedef boost::numeric::ublas::matrix<heightMapField> heightMap_t;
    heightMap_t _heightMap;
};

} /* namespace teamplay */

#endif /* ABSTRACTHEIGHTMAP_HPP_ */


#include <crest/wave/solver.hpp>

#include <crest/wave/constraints.hpp>

#include <crest/basis/lagrange_basis2d.hpp>
#include <crest/geometry/refinement.hpp>
#include <crest/quadrature/simpsons.hpp>
#include <crest/basis/amg_corrector_solver.hpp>

#include <experiments/experiments.hpp>

#include <json.hpp>

#include <iostream>
#include <iomanip>
#include <string>
#include <memory>
#include <sstream>

#include <boost/optional.hpp>

using std::cout;
using std::endl;
using std::cerr;
using std::setw;

auto sampled_data_summary_as_json(const crest::AccumulatedDensityHistogram & histogram)
{
    nlohmann::json j;
    auto & hist = j["distribution"];
    for (const auto & bin : histogram)
    {
        hist.push_back({
                               { "lower", bin.lower_bound() },
                               { "upper", bin.upper_bound() },
                               { "accumulated", bin.accumulated() }
                       });
    }

    return j;
}

auto stats_as_json(const std::unordered_map<std::string, crest::AccumulatedDensityHistogram> & stats)
{
    nlohmann::json j;
    for (const auto pair : stats)
    {
        j[pair.first] = sampled_data_summary_as_json(pair.second);
    }

    return j;
}

auto experiment_result_as_json(const ExperimentResult & result)
{
    using nlohmann::json;

    const auto offline_timing = result.offline_result.timing;
    const auto mesh_details = result.offline_result.mesh_details;

    json output = {
            { "experiment", result.name },
            { "offline", {
                                    { "parameters", {
                                                            { "mesh_resolution", result.offline_parameters.mesh_resolution },
                                                            { "oversampling", result.offline_parameters.oversampling },
                                                            { "basis_import_file", result.offline_parameters.basis_import_file },
                                                            { "basis_export_file", result.offline_parameters.basis_export_file },
                                                            { "dense_fallback_threshold", result.offline_parameters.dense_fallback_threshold },
                                                            { "iterative_tolerance", result.offline_parameters.iterative_tolerance }
                                                    }},
                                    { "result", {
                                                        { "mesh_details", {
                                                                                { "num_coarse_vertices", mesh_details.num_coarse_vertices },
                                                                                { "num_coarse_elements", mesh_details.num_coarse_elements },
                                                                                { "num_fine_vertices", mesh_details.num_fine_vertices },
                                                                                { "num_fine_elements", mesh_details.num_fine_elements },
                                                                                { "h_min_coarse", mesh_details.h_min_coarse },
                                                                                { "h_max_coarse", mesh_details.h_max_coarse },
                                                                                { "h_min_fine", mesh_details.h_min_fine },
                                                                                { "h_max_fine", mesh_details.h_max_fine }
                                                                          }},
                                                        { "timing", {
                                                                            { "mesh_construction", offline_timing.mesh_construction },
                                                                            { "basis_construction", offline_timing.basis_construction },
                                                                            { "total", offline_timing.total }
                                                                    }},
                                                        { "stats", stats_as_json(result.offline_result.stats) }
                                                }}
                            }}
    };

    if (result.online_parameters) {
        output["online"]["parameters"] = {
                { "end_time", result.online_parameters->end_time },
                { "sample_count", result.online_parameters->sample_count },
                { "integrator", result.online_parameters->integrator_name },
                { "load_quadrature_strength", result.online_parameters->load_quadrature_strength },
                { "use_coarse_rhs", result.online_parameters->use_coarse_rhs },
                { "use_coarse_mass_matrix", result.online_parameters->use_coarse_mass_matrix },
                { "iterative_tolerance", result.online_parameters->iterative_tolerance },
                { "measure_error", result.online_parameters->measure_error },
                { "max_error_before_abort", result.online_parameters->max_error_before_abort }
        };
    }

    if (result.online_result)
    {
        const auto errors = result.online_result->error_summary;
        const auto timing = result.online_result->timing;
        output["online"]["result"] = {
                { "converged", result.online_result->converged },
                { "error_summary", {
                                       { "h1", errors.h1 },
                                       { "h1_semi", errors.h1_semi },
                                       { "l2", errors.l2 }
                               }},
                { "timing", {
                                       { "assembly_time", timing.assembly_time },
                                       { "load_time", timing.load_time },
                                       { "initializer_time", timing.initializer_time },
                                       { "integrator_setup_time", timing.integrator_setup_time },
                                       { "integrator_solve_time", timing.integrator_solve_time },
                                       { "transform_time", timing.transform_time },
                                       { "total_time", timing.total_time }
                               }}
        };
    }

    return output;
}

std::unique_ptr<Experiment> make_experiment(const std::string & name)
{
    if (name == "homogeneous_dirichlet_unit_square") {
        return std::make_unique<HomogeneousDirichletUnitSquare>();
    } else if (name == "inhomogeneous_dirichlet_unit_square") {
        return std::make_unique<InhomogeneousDirichletUnitSquare>();
    } else if (name == "homogenized_l_shaped") {
        return std::make_unique<HomogenizedLShaped<>>();
    } else if (name == "homogenized_l_shaped_sparselu") {
        return std::make_unique<HomogenizedLShaped<crest::SparseLuCorrectorSolver<double>>>();
    } else if (name == "homogenized_l_shaped_schur") {
        return std::make_unique<HomogenizedLShaped<crest::SchurCorrectorSolver<double>>>();
    } else if (name == "homogenized_l_shaped_amg") {
        return std::make_unique<HomogenizedLShaped<crest::AmgCorrectorSolver<double>>>();
    } else if (name == "refined_standard_l_shaped") {
        return std::make_unique<RefinedStandardLShaped>();
    } else if (name == "quasi_uniform_standard_l_shaped") {
        return std::make_unique<QuasiUniformStandardLShaped>();
    } else {
        throw std::invalid_argument("Unknown experiment requested.");
    }
}

std::unique_ptr<crest::wave::Integrator<double>> make_integrator(const std::string & name)
{
    using namespace crest::wave;
    if (name == "direct_crank_nicolson")
    {
        return std::make_unique<DirectCrankNicolson<double>>();
    } else if (name == "iterative_crank_nicolson")
    {
        return std::make_unique<IterativeCrankNicolson<double>>();
    } else if (name == "iterative_leapfrog")
    {
        return std::make_unique<IterativeLeapfrog<double>>();
    } else if (name == "lumped_leapfrog")
    {
        return std::make_unique<LumpedLeapfrog<double>>();
    } else {
        return std::unique_ptr<crest::wave::Integrator<double>>();
    }
}

template <typename OutputType = nlohmann::json>
OutputType extract_mandatory_field(const nlohmann::json & json, const std::string & field_name)
{
    const auto it = json.find(field_name);
    if (it == json.end())
    {
        std::stringstream ss;
        ss << "Mandatory field `" << field_name << "` not found in JSON input.";
        throw std::runtime_error(ss.str());
    } else {
        return *it;
    }
}

template <typename OutputType = nlohmann::json>
boost::optional<OutputType> extract_optional_field(const nlohmann::json & json,
                                                   const std::string & field_name)
{
    const auto it = json.find(field_name);
    if (it == json.end())
    {
        return boost::optional<OutputType>();
    } else {
        return *it;
    }
}

OfflineParameters parse_offline_parameters(const nlohmann::json & offline_json) {
    OfflineParameters offline;
    offline.mesh_resolution = extract_mandatory_field(offline_json, "mesh_resolution");
    if (const auto oversampling = extract_optional_field(offline_json, "oversampling")) {
        offline.oversampling = *oversampling;
    }
    if (const auto basis_import_file = extract_optional_field(offline_json, "basis_import_file")) {
        offline.basis_import_file = *basis_import_file;
    }
    if (const auto basis_export_file = extract_optional_field(offline_json, "basis_export_file")) {
        offline.basis_export_file = *basis_export_file;
    }
    if (const auto threshold = extract_optional_field(offline_json, "dense_fallback_threshold")) {
        offline.dense_fallback_threshold = *threshold;
    }
    if (const auto iterative_tolerance = extract_optional_field(offline_json, "iterative_tolerance")) {
        offline.iterative_tolerance = *iterative_tolerance;
    }
    return offline;
}

OnlineParameters parse_online_parameters(const nlohmann::json & online_json) {
    OnlineParameters parameters;
    parameters.integrator_name = extract_mandatory_field(online_json, "integrator");
    parameters.end_time = extract_mandatory_field(online_json, "end_time");
    parameters.sample_count = extract_mandatory_field(online_json, "sample_count");

    if (const auto load_strength = extract_optional_field(online_json, "load_quadrature_strength")) {
        parameters.load_quadrature_strength = *load_strength;
    }

    if (const auto use_coarse_rhs = extract_optional_field(online_json, "use_coarse_rhs")) {
        parameters.use_coarse_rhs = *use_coarse_rhs;
    }

    if (const auto use_coarse_mass_matrix = extract_optional_field(online_json, "use_coarse_mass_matrix")) {
        parameters.use_coarse_mass_matrix = *use_coarse_mass_matrix;
    }

    if (const auto iterative_tol = extract_optional_field(online_json, "iterative_tolerance")) {
        parameters.iterative_tolerance = *iterative_tol;
    }

    if (const auto measure_error = extract_optional_field(online_json, "measure_error")) {
        parameters.measure_error = *measure_error;
    }

    if (const auto max_error_before_abort = extract_optional_field(online_json, "max_error_before_abort")) {
        parameters.max_error_before_abort = *max_error_before_abort;
    }

    return parameters;
}

int main(int, char **)
{
    H5::Exception::dontPrint();

    // Disable automatic multi-threading in Eigen, so that we have predictable behavior. In order to get
    // decent measurements, we'd rather parallelize explicitly where we actually want to parallelize.
    // Remember that the main purpose of this binary is not to solve problems as fast as possible,
    // but to facilitate numerical experimentation, including making inferences about runtime behavior.
    Eigen::setNbThreads(1);
    try
    {
        nlohmann::json j;
        std::cin >> j;

        const auto experiment_name = extract_mandatory_field<std::string>(j, "experiment");

        const auto offline_json = extract_mandatory_field(j, "offline");
        const auto optional_online_json = extract_optional_field(j, "online");

        const auto offline_param = parse_offline_parameters(offline_json);
        const auto online_param = optional_online_json
                                  ? parse_online_parameters(*optional_online_json)
                                  : boost::optional<OnlineParameters>();

        auto experiment = make_experiment(experiment_name);
        const auto offline_result = experiment->run_offline(offline_param);
        boost::optional<OnlineResult> online_result;
        boost::optional<std::string> error;

        try {
            if (online_param) {
                auto integrator = make_integrator(online_param->integrator_name);

                if (integrator) {
                    integrator->set_tolerance(online_param->iterative_tolerance);
                    online_result = experiment->run_online(*online_param, *integrator);
                } else {
                    throw std::runtime_error("Unknown integrator requested.");
                }
            }
        } catch(const std::exception & e) {
            // This catch block mainly catches errors related to the simulation
            error = e.what();
        }

        const auto result = ExperimentResult()
                .with_name(experiment_name)
                .with_offline_parameters(offline_param)
                .with_online_parameters(online_param)
                .with_offline_result(offline_result)
                .with_online_result(online_result);
        auto json = experiment_result_as_json(result);
        if (error) {
            json["error"] = *error;
        }
        cout << json.dump(4) << endl;
        return 0;
    } catch (const std::exception & e)
    {
        // This catch block mainly catches errors related to parsing (missing arguments or malformed input)
        nlohmann::json j = {
                { "error", e.what() }
        };
        cout << j.dump(4) << endl;

        return 1;
    }
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_COMMON_FAST_TOINT_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_COMMON_FAST_TOINT_HPP_INCLUDED
#include <boost/simd/arithmetic/functions/fast_toint.hpp>
#include <boost/simd/include/functions/simd/toint.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( fast_toint_
                                    , tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<arithmetic_<A0>,X>))
                                    )
  {
    typedef typename dispatch::meta::as_integer<A0, signed> ::type result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      return toint(a0);
    }
  };
} } }

#endif