/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkBoostDividedEdgeBundling.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
/*-------------------------------------------------------------------------
  Copyright 2008 Sandia Corporation.
  Under the terms of Contract DE-AC04-94AL85000 with Sandia Corporation,
  the U.S. Government retains certain rights in this software.
-------------------------------------------------------------------------*/
#include "vtkBoostDividedEdgeBundling.h"

#include "vtkBoostGraphAdapter.h"
#include "vtkDataSetAttributes.h"
#include "vtkDirectedGraph.h"
#include "vtkFloatArray.h"
#include "vtkInformation.h"
#include "vtkInformationVector.h"
#include "vtkMath.h"
#include "vtkNew.h"
#include "vtkObjectFactory.h"
#include "vtkPoints.h"
#include "vtkVectorOperators.h"

#include <boost/property_map/property_map.hpp>
#include <boost/graph/johnson_all_pairs_shortest.hpp>
#include <algorithm>

vtkStandardNewMacro(vtkBoostDividedEdgeBundling);

vtkBoostDividedEdgeBundling::vtkBoostDividedEdgeBundling()
{
}

class vtkBundlingMetadata
{
public:
  vtkBundlingMetadata(vtkBoostDividedEdgeBundling *alg, vtkDirectedGraph *g)
    : Outer(alg), Graph(g)
  {
    this->Nodes = reinterpret_cast<vtkVector3f*>(
      vtkArrayDownCast<vtkFloatArray>(g->GetPoints()->GetData())->GetPointer(0));
    this->Edges.resize(g->GetNumberOfEdges());
    for (vtkIdType e = 0; e < g->GetNumberOfEdges(); ++e)
    {
      this->Edges[e] = std::make_pair(g->GetSourceVertex(e), g->GetTargetVertex(e));
    }
    this->VelocityDamping = 0.1f;
    this->EdgeCoulombConstant = 0.5f;
    //this->EdgeCoulombConstant = 50.0f;
    this->EdgeCoulombDecay = 35.0f;
    this->EdgeSpringConstant = 0.1f;
    //this->EdgeSpringConstant = 0.0005f;
    this->EdgeLaneWidth = 25.0f;
    this->UseNewForce = true;
  }

  void ProjectOnto(vtkIdType e1, vtkIdType e2, vtkVector3f& s, vtkVector3f& t);
  void NormalizeNodePositions();
  void DenormalizeNodePositions();
  void CalculateNodeDistances();
  float AngleCompatibility(vtkIdType e1, vtkIdType e2);
  float ScaleCompatibility(vtkIdType e1, vtkIdType e2);
  float PositionCompatibility(vtkIdType e1, vtkIdType e2);
  float VisibilityCompatibility(vtkIdType e1, vtkIdType e2);
  float ConnectivityCompatibility(vtkIdType e1, vtkIdType e2);
  void CalculateEdgeLengths();
  void CalculateEdgeCompatibilities();
  void InitializeEdgeMesh();
  void DoubleEdgeMeshResolution();
  void SimulateEdgeStep();
  void LayoutEdgePoints();
  void SmoothEdges();

  float SimulationStep;
  int CycleIterations;
  int MeshCount;
  float VelocityDamping;
  float EdgeCoulombConstant;
  float EdgeCoulombDecay;
  float EdgeSpringConstant;
  float EdgeLaneWidth;
  bool UseNewForce;
  vtkBoostDividedEdgeBundling *Outer;
  vtkDirectedGraph *Graph;
  vtkVector3f *Nodes;
  std::vector<std::pair<vtkIdType, vtkIdType> > Edges;
  std::vector<std::vector<float> > NodeDistances;
  std::vector<float> EdgeLengths;
  std::vector<std::vector<float> > EdgeCompatibilities;
  std::vector<std::vector<float> > EdgeDots;
  std::vector<std::vector<vtkVector3f> > EdgeMesh;
  std::vector<std::vector<vtkVector3f> > EdgeMeshVelocities;
  std::vector<std::vector<vtkVector3f> > EdgeMeshAccelerations;
  //std::vector<std::vector<float> > EdgeMeshGroupCounts;
  vtkVector2f XRange;
  vtkVector2f YRange;
  vtkVector2f ZRange;
  float Scale;
};

void vtkBundlingMetadata::NormalizeNodePositions()
{
  this->XRange = vtkVector2f(VTK_FLOAT_MAX, VTK_FLOAT_MIN);
  this->YRange = vtkVector2f(VTK_FLOAT_MAX, VTK_FLOAT_MIN);
  this->ZRange = vtkVector2f(VTK_FLOAT_MAX, VTK_FLOAT_MIN);
  for (vtkIdType i = 0; i < this->Graph->GetNumberOfVertices(); ++i)
  {
    vtkVector3f p = this->Nodes[i];
    this->XRange[0] = std::min(this->XRange[0], p[0]);
    this->XRange[1] = std::max(this->XRange[1], p[0]);
    this->YRange[0] = std::min(this->YRange[0], p[1]);
    this->YRange[1] = std::max(this->YRange[1], p[1]);
    this->ZRange[0] = std::min(this->ZRange[0], p[2]);
    this->ZRange[1] = std::max(this->ZRange[1], p[2]);
  }
  float dx = this->XRange[1] - this->XRange[0];
  float dy = this->YRange[1] - this->YRange[0];
  float dz = this->ZRange[1] - this->ZRange[0];
  this->Scale = std::max(dx, std::max(dy, dz));
  for (vtkIdType i = 0; i < this->Graph->GetNumberOfVertices(); ++i)
  {
    vtkVector3f p = this->Nodes[i];
    this->Nodes[i] = vtkVector3f(
      (p[0] - this->XRange[0])/this->Scale * 1000.0f,
      (p[1] - this->YRange[0])/this->Scale * 1000.0f,
      (p[2] - this->ZRange[0])/this->Scale * 1000.0f);
  }
}

void vtkBundlingMetadata::DenormalizeNodePositions()
{
  for (vtkIdType i = 0; i < this->Graph->GetNumberOfVertices(); ++i)
  {
    vtkVector3f p = this->Nodes[i];
    this->Nodes[i] = vtkVector3f(
      p[0] / 1000.0f * this->Scale + this->XRange[0],
      p[1] / 1000.0f * this->Scale + this->YRange[0],
      p[2] / 1000.0f * this->Scale + this->ZRange[0]);
  }
  for (vtkIdType i = 0; i < (int)this->EdgeMesh.size(); ++i)
  {
    for (vtkIdType j = 0; j < (int)this->EdgeMesh[i].size(); ++j)
    {
      vtkVector3f p = this->EdgeMesh[i][j];
      this->EdgeMesh[i][j] = vtkVector3f(
        p[0] / 1000.0f * this->Scale + this->XRange[0],
        p[1] / 1000.0f * this->Scale + this->YRange[0],
        p[2] / 1000.0f * this->Scale + this->ZRange[0]);
    }
  }
}

void vtkBundlingMetadata::CalculateNodeDistances()
{
  vtkIdType numVerts = this->Graph->GetNumberOfVertices();
  vtkIdType numEdges = this->Graph->GetNumberOfEdges();
  this->NodeDistances.resize(numVerts, std::vector<float>(numVerts, VTK_FLOAT_MAX));
  std::vector<float> weights(numVerts, 1.0f);
  vtkNew<vtkFloatArray> weightMap;
  weightMap->SetNumberOfTuples(numEdges);
  for (vtkIdType e = 0; e < numEdges; ++e)
  {
    weightMap->SetValue(e, 1.0f);
  }
  boost::vtkGraphEdgePropertyMapHelper<vtkFloatArray*> weightProp(weightMap);
  boost::johnson_all_pairs_shortest_paths(
    this->Graph, this->NodeDistances,
    boost::weight_map(weightProp));
}

float vtkBundlingMetadata::AngleCompatibility(vtkIdType e1, vtkIdType e2)
{
  if (this->EdgeLengths[e1] == 0.0f || this->EdgeLengths[e2] == 0.0f)
  {
    return 0.0f;
  }
  vtkVector3f s1 = this->Nodes[this->Edges[e1].first];
  vtkVector3f t1 = this->Nodes[this->Edges[e1].second];
  vtkVector3f s2 = this->Nodes[this->Edges[e2].first];
  vtkVector3f t2 = this->Nodes[this->Edges[e2].second];
  vtkVector3f p1 = s1 - t1;
  vtkVector3f p2 = s2 - t2;
  float compatibility = p1.Dot(p2) / (this->EdgeLengths[e1]*this->EdgeLengths[e2]);
  return fabs(compatibility);
}

float vtkBundlingMetadata::ScaleCompatibility(vtkIdType e1, vtkIdType e2)
{
  float len1 = this->EdgeLengths[e1];
  float len2 = this->EdgeLengths[e2];
  float average = (len1 + len2) / 2.0f;
  if (average == 0.0f)
  {
    return 0.0f;
  }
  return 2.0f / (average / std::min(len1, len2) + std::max(len1, len2) / average);
}

float vtkBundlingMetadata::PositionCompatibility(vtkIdType e1, vtkIdType e2)
{
  float len1 = this->EdgeLengths[e1];
  float len2 = this->EdgeLengths[e2];
  float average = (len1 + len2) / 2.0f;
  if (average == 0.0f)
  {
    return 0.0f;
  }
  vtkVector3f s1 = this->Nodes[this->Edges[e1].first];
  vtkVector3f t1 = this->Nodes[this->Edges[e1].second];
  vtkVector3f s2 = this->Nodes[this->Edges[e2].first];
  vtkVector3f t2 = this->Nodes[this->Edges[e2].second];
  vtkVector3f mid1 = 0.5*(s1 + t1);
  vtkVector3f mid2 = 0.5*(s2 + t2);
  return average / (average + (mid1 - mid2).Norm());
}

void vtkBundlingMetadata::ProjectOnto(vtkIdType e1, vtkIdType e2, vtkVector3f& s, vtkVector3f& t)
{
  vtkVector3f s1 = this->Nodes[this->Edges[e1].first];
  vtkVector3f t1 = this->Nodes[this->Edges[e1].second];
  vtkVector3f s2 = this->Nodes[this->Edges[e2].first];
  vtkVector3f t2 = this->Nodes[this->Edges[e2].second];
  vtkVector3f norm = t2 - s2;
  norm.Normalize();
  vtkVector3f toHead = s1 - s2;
  vtkVector3f toTail = t1 - s2;
  vtkVector3f headOnOther = norm * norm.Dot(toHead);
  vtkVector3f tailOnOther = norm * norm.Dot(toTail);
  s = s2 + headOnOther;
  t = s2 + tailOnOther;
}

float vtkBundlingMetadata::VisibilityCompatibility(vtkIdType e1, vtkIdType e2)
{
  vtkVector3f is;
  vtkVector3f it;
  vtkVector3f js;
  vtkVector3f jt;
  this->ProjectOnto(e1, e2, is, it);
  this->ProjectOnto(e2, e1, js, jt);
  float ilen = (is - it).Norm();
  float jlen = (js - jt).Norm();
  if (ilen == 0.0f || jlen == 0.0f)
  {
    return 0.0f;
  }
  vtkVector3f s1 = this->Nodes[this->Edges[e1].first];
  vtkVector3f t1 = this->Nodes[this->Edges[e1].second];
  vtkVector3f s2 = this->Nodes[this->Edges[e2].first];
  vtkVector3f t2 = this->Nodes[this->Edges[e2].second];
  vtkVector3f mid1 = 0.5*(s1 + t1);
  vtkVector3f mid2 = 0.5*(s2 + t2);
  vtkVector3f imid = 0.5*(is + it);
  vtkVector3f jmid = 0.5*(js + jt);
  float midQI = (mid2 - imid).Norm();
  float vpq = std::max(0.0f, 1.0f - (2.0f * midQI) / ilen);
  float midPJ = (mid1 - jmid).Norm();
  float vqp = std::max(0.0f, 1.0f - (2.0f * midPJ) / jlen);

  return std::min(vpq, vqp);
}

float vtkBundlingMetadata::ConnectivityCompatibility(vtkIdType e1, vtkIdType e2)
{
  vtkIdType s1 = this->Edges[e1].first;
  vtkIdType t1 = this->Edges[e1].second;
  vtkIdType s2 = this->Edges[e2].first;
  vtkIdType t2 = this->Edges[e2].second;
  if (s1 == s2 || s1 == t2 || t1 == s2 || t1 == t2)
  {
    return 1.0f;
  }
  float minPath = std::min(this->NodeDistances[s1][s2], std::min(this->NodeDistances[s1][t2],
    std::min(this->NodeDistances[t1][s2], this->NodeDistances[t1][t2])));
  return 1.0f / (minPath + 1.0f);
}

void vtkBundlingMetadata::CalculateEdgeLengths()
{
  vtkIdType numEdges = this->Graph->GetNumberOfEdges();
  this->EdgeLengths.resize(numEdges);
  for (vtkIdType e = 0; e < numEdges; ++e)
  {
    vtkVector3f s = this->Nodes[this->Edges[e].first];
    vtkVector3f t = this->Nodes[this->Edges[e].second];
    this->EdgeLengths[e] = (s - t).Norm();
  }
}

void vtkBundlingMetadata::CalculateEdgeCompatibilities()
{
  vtkIdType numEdges = this->Graph->GetNumberOfEdges();
  this->EdgeCompatibilities.resize(numEdges, std::vector<float>(numEdges, 1.0f));
  this->EdgeDots.resize(numEdges, std::vector<float>(numEdges, 1.0f));
  for (vtkIdType e1 = 0; e1 < numEdges; ++e1)
  {
    vtkVector3f s1 = this->Nodes[this->Edges[e1].first];
    vtkVector3f t1 = this->Nodes[this->Edges[e1].second];
    vtkVector3f r1 = s1 - t1;
    r1.Normalize();
    for (vtkIdType e2 = e1 + 1; e2 < numEdges; ++e2)
    {
      float compatibility = 1.0f;
      compatibility *= this->AngleCompatibility(e1, e2);
      compatibility *= this->ScaleCompatibility(e1, e2);
      compatibility *= this->PositionCompatibility(e1, e2);
      compatibility *= this->VisibilityCompatibility(e1, e2);
      compatibility *= this->ConnectivityCompatibility(e1, e2);
      this->EdgeCompatibilities[e1][e2] = compatibility;
      this->EdgeCompatibilities[e2][e1] = compatibility;

      vtkVector3f s2 = this->Nodes[this->Edges[e2].first];
      vtkVector3f t2 = this->Nodes[this->Edges[e2].second];
      vtkVector3f r2 = s2 - t2;
      r2.Normalize();
      float dot = r1.Dot(r2);
      this->EdgeDots[e1][e2] = dot;
      this->EdgeDots[e2][e1] = dot;
    }
  }
}

void vtkBundlingMetadata::InitializeEdgeMesh()
{
  this->MeshCount = 2;
  vtkIdType numEdges = this->Graph->GetNumberOfEdges();
  this->EdgeMesh.resize(numEdges, std::vector<vtkVector3f>(2));
  this->EdgeMeshVelocities.resize(numEdges, std::vector<vtkVector3f>(2));
  this->EdgeMeshAccelerations.resize(numEdges, std::vector<vtkVector3f>(2));
  //this->EdgeMeshGroupCounts.resize(numEdges, std::vector<float>(2, 1.0f));
  for (vtkIdType e = 0; e < numEdges; ++e)
  {
    this->EdgeMesh[e][0] = this->Nodes[this->Edges[e].first];
    this->EdgeMesh[e][1] = this->Nodes[this->Edges[e].second];
  }
}

void vtkBundlingMetadata::DoubleEdgeMeshResolution()
{
  int newMeshCount = (this->MeshCount - 1)*2 + 1;
  vtkIdType numEdges = this->Graph->GetNumberOfEdges();
  std::vector<std::vector<vtkVector3f> > newEdgeMesh(
      numEdges, std::vector<vtkVector3f>(newMeshCount));
  std::vector<std::vector<vtkVector3f> > newEdgeMeshVelocities(
      numEdges, std::vector<vtkVector3f>(newMeshCount, vtkVector3f(0.0f, 0.0f, 0.0f)));
  std::vector<std::vector<vtkVector3f> > newEdgeMeshAccelerations(
      numEdges, std::vector<vtkVector3f>(newMeshCount, vtkVector3f(0.0f, 0.0f, 0.0f)));
  //std::vector<std::vector<float> > newEdgeMeshGroupCounts(
  //    numEdges, std::vector<float>(newMeshCount, 1.0f));
  for (vtkIdType e = 0; e < numEdges; ++e)
  {
    for (int m = 0; m < newMeshCount; ++m)
    {
      float indexFloat = (this->MeshCount - 1.0f)*m/(newMeshCount - 1.0f);
      int index = static_cast<int>(indexFloat);
      float alpha = indexFloat - index;
      vtkVector3f before = this->EdgeMesh[e][index];
      if (alpha > 0)
      {
        vtkVector3f after = this->EdgeMesh[e][index+1];
        newEdgeMesh[e][m] = before + alpha*(after - before);
      }
      else
      {
        newEdgeMesh[e][m] = before;
      }
    }
  }
  this->MeshCount = newMeshCount;
  this->EdgeMesh = newEdgeMesh;
  this->EdgeMeshVelocities = newEdgeMeshVelocities;
  this->EdgeMeshAccelerations = newEdgeMeshAccelerations;
  //this->EdgeMeshGroupCounts = newEdgeMeshGroupCounts;
}

void vtkBundlingMetadata::SimulateEdgeStep()
{
  vtkIdType numEdges = this->Graph->GetNumberOfEdges();

  for (vtkIdType e1 = 0; e1 < numEdges; ++e1)
  {
    float weight1 = 1.0f;
    for (int m1 = 0; m1 < this->MeshCount; ++m1)
    {
      // Immovable
      if (m1 <= 0 || m1 >= this->MeshCount - 1)
      {
        continue;
      }

      // Move the point according to dynamics
      vtkVector3f position = this->EdgeMesh[e1][m1];
      vtkVector3f velocity = this->EdgeMeshVelocities[e1][m1];
      vtkVector3f acceleration = this->EdgeMeshAccelerations[e1][m1];
      velocity = velocity + acceleration * this->SimulationStep * 0.5f;
      velocity = velocity * this->VelocityDamping;
      position = position + velocity * this->SimulationStep;
      this->EdgeMesh[e1][m1] = position;

      acceleration = vtkVector3f(0.0f, 0.0f, 0.0f);

      // Spring force
      vtkVector3f prevPosition = this->EdgeMesh[e1][m1-1];
      vtkVector3f prevDirection = prevPosition - position;
      float prevDist = prevDirection.Norm();
      float prevForce = this->EdgeSpringConstant / 1000.0f * (this->MeshCount - 1) * prevDist * weight1;
      prevDirection.Normalize();
      acceleration = acceleration + prevForce * prevDirection;

      vtkVector3f nextPosition = this->EdgeMesh[e1][m1+1];
      vtkVector3f nextDirection = nextPosition - position;
      float nextDist = nextDirection.Norm();
      float nextForce = this->EdgeSpringConstant / 1000.0f * (this->MeshCount - 1) * nextDist * weight1;
      nextDirection.Normalize();
      acceleration = acceleration + nextForce * nextDirection;

      // Coulomb force
      float normalizedEdgeCoulombConstant = this->EdgeCoulombConstant / sqrt(static_cast<float>(numEdges));

      for (vtkIdType e2 = 0; e2 < numEdges; ++e2)
      {
        if (e1 == e2)
        {
          continue;
        }

        float compatibility = this->EdgeCompatibilities[e1][e2];
        if (compatibility <= 0.05)
        {
          continue;
        }

        float dot = this->EdgeDots[e1][e2];
        float weight2 = 1.0f;

        int m2;
        if (dot >= 0.0f)
        {
          m2 = m1;
        }
        else
        {
          m2 = this->MeshCount - 1 - m1;
        }

        vtkVector3f position2;
        // If we're going the same direction is edge1, then the potential minimum is at the point.
        if (dot >= 0.0f)
        {
          position2 = this->EdgeMesh[e2][m2];
        }
        // If we're going the opposite direction, the potential minimum is edgeLaneWidth to the "right."
        else
        {
          vtkVector3f tangent = this->EdgeMesh[e2][m2+1] - this->EdgeMesh[e2][m2-1];
          tangent.Normalize();
          // This assumes 2D
          vtkVector3f normal(-tangent[1], tangent[0], 0.0f);
          position2 = this->EdgeMesh[e2][m2] + normal*this->EdgeLaneWidth;
        }

        vtkVector3f direction = position2 - position;
        float distance = direction.Norm();

        // Inverse force.
        float force;
        if (!this->UseNewForce)
        {
          force = normalizedEdgeCoulombConstant * 30.0f / (this->MeshCount - 1) / (distance + 0.01f);
        }
        // New force.
        else
        {
          force = 4.0f * 10000.0f / (this->MeshCount - 1) * this->EdgeCoulombDecay * normalizedEdgeCoulombConstant * distance / (3.1415926f * pow(this->EdgeCoulombDecay * this->EdgeCoulombDecay + distance * distance, 2));
        }
        force *= weight2;
        force *= compatibility;

        if (distance > 0.0f)
        {
          direction.Normalize();
          acceleration = acceleration + force * direction;
        }
      }

      velocity = velocity + acceleration * this->SimulationStep * 0.5f;
      this->EdgeMeshVelocities[e1][m1] = velocity;
      this->EdgeMeshAccelerations[e1][m1] = acceleration;
    }
  }
}

void vtkBundlingMetadata::SmoothEdges()
{
  // From Mathematica Total[GaussianMatrix[{3, 3}]]
  int kernelSize = 3;
  // Has to sum to 1.0 to be correct.
  float gaussianKernel[] = {0.10468, 0.139936, 0.166874, 0.177019, 0.166874, 0.139936, 0.10468};
  vtkIdType numEdges = this->Graph->GetNumberOfEdges();
  std::vector<std::vector<vtkVector3f> > smoothedEdgeMesh(
      numEdges, std::vector<vtkVector3f>(this->MeshCount));
  for (vtkIdType e = 0; e < numEdges; ++e)
  {
    for (int m = 1; m < this->MeshCount - 1; ++m)
    {
      vtkVector3f smoothed(0.0f, 0.0f, 0.0f);
      for (int kernelIndex = 0; kernelIndex < kernelSize * 2 + 1; kernelIndex++)
      {
        int m2 = m + kernelIndex - kernelSize;
        m2 = std::max(0, std::min(this->MeshCount - 1, m2));

        vtkVector3f pt = this->EdgeMesh[e][m2];
        smoothed = smoothed + gaussianKernel[kernelIndex] * pt;
      }
      smoothedEdgeMesh[e][m] = smoothed;
    }
  }
  this->EdgeMesh = smoothedEdgeMesh;
}

void vtkBundlingMetadata::LayoutEdgePoints()
{
  this->InitializeEdgeMesh();
  this->SimulationStep = 40.0f;
  this->CycleIterations = 30;
  for (int i = 0; i < 5; ++i)
  {
    vtkDebugWithObjectMacro(this->Outer, "vtkBoostDividedEdgeBundling cycle " << i);
    this->CycleIterations = this->CycleIterations * 2 / 3;
    this->SimulationStep = 0.85f*this->SimulationStep;
    this->DoubleEdgeMeshResolution();
    for (int j = 0; j < this->CycleIterations; ++j)
    {
      vtkDebugWithObjectMacro(this->Outer, "vtkBoostDividedEdgeBundling iteration " << j);
      this->SimulateEdgeStep();
    }
  }
  this->SmoothEdges();
}

int vtkBoostDividedEdgeBundling::RequestData(
  vtkInformation *vtkNotUsed(request),
  vtkInformationVector **inputVector,
  vtkInformationVector *outputVector)
{
  // get the info objects
  vtkInformation *graphInfo = inputVector[0]->GetInformationObject(0);
  vtkInformation *outInfo = outputVector->GetInformationObject(0);

  // get the input and output
  vtkDirectedGraph *g = vtkDirectedGraph::SafeDownCast(
    graphInfo->Get(vtkDataObject::DATA_OBJECT()));
  vtkDirectedGraph *output = vtkDirectedGraph::SafeDownCast(
    outInfo->Get(vtkDataObject::DATA_OBJECT()));

  vtkBundlingMetadata *meta = new vtkBundlingMetadata(this, g);

  meta->NormalizeNodePositions();
  meta->CalculateEdgeLengths();
  meta->CalculateNodeDistances();
  meta->CalculateEdgeCompatibilities();
  meta->LayoutEdgePoints();
  meta->DenormalizeNodePositions();

  output->ShallowCopy(g);

  for (vtkIdType e = 0; e < g->GetNumberOfEdges(); ++e)
  {
    output->ClearEdgePoints(e);
    for (int m = 1; m < meta->MeshCount-1; ++m)
    {
      vtkVector3f edgePoint = meta->EdgeMesh[e][m];
      output->AddEdgePoint(e, edgePoint[0], edgePoint[1], edgePoint[2]);
    }
  }

  delete meta;

  return 1;
}

void vtkBoostDividedEdgeBundling::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os,indent);
}


/*

Copyright (c) 2005-2017, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

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 University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#include <cmath>
#include <iostream>
#include <boost/math/tools/roots.hpp>
#include <boost/bind.hpp>

#include "BoostTolerance.hpp"
#include "LaPradAirwayWall.hpp"
#include "MathsCustomFunctions.hpp"
#include "Exception.hpp"

LaPradAirwayWall::LaPradAirwayWall() : mTargetPressure(0),
                                       mRIn(0),
                                       mROut(0),
                                       mk1(0),
                                       mk2(0),
                                       mk3(0)
{
}

LaPradAirwayWall::~LaPradAirwayWall() {}

void LaPradAirwayWall::SetTimestep(double dt) {}

double LaPradAirwayWall::CalculatePressureRadiusResidual(double radius)
{

    mTargetPressure = mAirwayPressure - mPleuralPressure;

    double rin = radius;

    double areaOfAirwayWall = M_PI*(mROut*mROut - mRIn*mRIn);
    double rout = sqrt(rin*rin + areaOfAirwayWall/M_PI);
    long double functionValues[10000];
    double rValues[10000];
    double pressure;

    for (int i = 0; i < 10000; i++)
    {

        double RVal = mRIn + (double)i*(mROut - mRIn)/(10000. - 1.);
        rValues[i] = rin + (double)i*(rout - rin)/(10000. - 1.);
        functionValues[i] = ((rValues[i]/RVal)*(rValues[i]/RVal) - (RVal/rValues[i])*(RVal/rValues[i]))*(mk1*sqrt(1. + (RVal/rValues[i])*(RVal/rValues[i]) + (rValues[i]/RVal)*(rValues[i]/RVal) - 3.) + mk2*sqrt(1 + (RVal/rValues[i])*(RVal/rValues[i]) + (rValues[i]/RVal)*(rValues[i]/RVal) - 3.)*exp(mk3*(1. + (RVal/rValues[i])*(RVal/rValues[i]) + (rValues[i]/RVal)*(rValues[i]/RVal) - 3.)*(1 + (RVal/rValues[i])*(RVal/rValues[i]) + (rValues[i]/RVal)*(rValues[i]/RVal) - 3.)))/rValues[i];
    }

    pressure = (0.5*(functionValues[0] + functionValues[10000 - 1]));
    for (int i = 1; i < (10000 - 1); i++)
    {
        pressure = pressure + functionValues[i];
    }
    pressure = pressure*(rValues[1] - rValues[0]);

    double residual = mTargetPressure - pressure;

    return residual;

}

void LaPradAirwayWall::SolveAndUpdateState(double tStart, double tEnd)
{

    double guess = (mRIn + mROut)/2.;
    double factor = 2.;

    Tolerance tol = 0.000001;
    boost::uintmax_t maxIterations = 500u;

    std::pair<double, double> found = boost::math::tools::bracket_and_solve_root(boost::bind(&LaPradAirwayWall::CalculatePressureRadiusResidual, this, _1), guess, factor, false, tol, maxIterations);
    mDeformedAirwayRadius = found.first;
}

void LaPradAirwayWall::SetRIn(double RIn)
{
    assert (RIn >= 0.0);
    mRIn = RIn;
}

void LaPradAirwayWall::SetROut(double ROut)
{
    assert (ROut >= 0.0);
    mROut = ROut;
}

void LaPradAirwayWall::Setk1(double k1)
{
    assert (k1 >= 0.0);
    mk1 = k1;
}

void LaPradAirwayWall::Setk2(double k2)
{
    assert (k2 >= 0.);
    mk2 = k2;
}

void LaPradAirwayWall::Setk3(double k3)
{
    assert (k3 >= 0.);
    mk3 = k3;
}

double LaPradAirwayWall::GetLumenRadius()
{
    return mDeformedAirwayRadius;
}

void LaPradAirwayWall::SetAirwayPressure(double pressure)
{
    mAirwayPressure = pressure;
}

void LaPradAirwayWall::SetPleuralPressure(double pressure)
{
    mPleuralPressure = pressure;
}


#include <math.h>
#include <float.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <stdbool.h>
#include "matrix.h"
#include "omxCsolnp.h"
#include <Eigen/Dense>
//#include <iostream>
//#include <iomanip>
//using std::cout;
//using std::endl;

struct CSOLNP {

    int flag, flag_NormgZ, minr_rec;
    Eigen::VectorXd LB_e;
    Eigen::VectorXd UB_e;
    Eigen::MatrixXd resP;
    double resLambda;
    Eigen::MatrixXd resHessv;
    Eigen::MatrixXd resY;
    Eigen::MatrixXd sx_Matrix; // search direction
    Eigen::RowVectorXd resGrad;
    int mode;
    int neq, nineq;
    bool optimize_initial_inequality_constraints;
    int numCallsToCSOLNP;
    GradientOptimizerContext &fit;

	CSOLNP(GradientOptimizerContext &_fit) : fit(_fit) {};

	void solnp(double *pars, int verbose);

    template <typename T1, typename T2>
    void obj_constr_eval(Eigen::MatrixBase<T2>& objVal, Eigen::MatrixBase<T2>& eqval, Eigen::MatrixBase<T2>& ineqval, Eigen::MatrixBase<T1>& fitVal, int verbose);

    template <typename T1, typename T2>
    void subnp(Eigen::MatrixBase<T2>& pars, Eigen::MatrixBase<T1>& yy_e, Eigen::MatrixBase<T1>& ob_e, Eigen::MatrixBase<T1>& hessv_e, double lambda, Eigen::MatrixBase<T2>& vscale_e,
                 const Eigen::Array<double, 4, 1> &ctrl, int verbose);

	enum indParam {
		indNumParam=0,
		indHasGradient,
		indHasHessian,
		indHasIneq,
		indHasJacobianIneq,
		indHasEq,
		indHasJacobianEq,
		indVectorLength  // must be last
	};

	Eigen::Array<double, int(indVectorLength), 1> ind;
};

void solnp(double *solPars, GradientOptimizerContext &fit)
{
	CSOLNP context(fit);
	fit.setupIneqConstraintBounds();
	context.solnp(solPars, fit.verbose);
}

void CSOLNP::solnp(double *solPars, int verbose)
{
	fit.informOut = -1;
    LB_e = fit.solLB;
    UB_e = fit.solUB;
    //verbose = 3;

    flag = 0;
    flag_NormgZ = 0; minr_rec = 0;

    double funv;
    double resultForTT;
    double solnp_nfn = 0;

    //time_t sec;
    //sec = time (NULL);

    int maxit_trace = 0;
    //free(matrices.front().t);

    Eigen::Map< Eigen::RowVectorXd > pars(solPars, LB_e.size());

    const int np = pars.size();

    ind.setZero();
    ind[indNumParam] = np;

    // does not have a function gradient (currently not supported in Rsolnp)
    ind[indHasGradient] = 0;
    //# do function checks and return starting value

    mode = 1;
    funv = fit.solFun(pars.data(), &mode);

    // does not have a hessian (currently not supported in Rsolnp)
    ind[indHasHessian] = 0;
    // no jacobian (not implemented)
    ind[indHasJacobianIneq] = 0;

    neq = fit.equality.size();

    ind[indHasEq] = neq > 0;
    ind[indHasJacobianEq] = 0;

    fit.myineqFun();
    Eigen::RowVectorXd ineqv_e(fit.inequality.size());
    ineqv_e= fit.inequality;

    Eigen::MatrixXd hessv_e;

    if ((ineqv_e.array() < 0).any())
    {
        optimize_initial_inequality_constraints = TRUE;
        numCallsToCSOLNP = 2;
    }
    else{
        optimize_initial_inequality_constraints = FALSE;
        numCallsToCSOLNP = 1;
    }

    for ( int i = 1; i <= numCallsToCSOLNP; i++){

        if (optimize_initial_inequality_constraints){
            nineq = 0;
        }
        else {
            nineq = fit.inequality.size();
        }

        ind[indHasIneq] = nineq > 0;

        if (verbose >= 2){
            mxLog("ind is: \n");
            for (i = 0; i < ind.size(); i++) mxLog("%f",ind[i]);
        }

        Eigen::RowVectorXd ineqx0_e(nineq); ineqx0_e.setZero();
        Eigen::MatrixXd pb_e;

    if(nineq) {
        pb_e.setZero(nineq, 2);
        pb_e.col(1) = Eigen::VectorXd::Constant(pb_e.rows(), INF);
        Eigen::MatrixXd pb_cont_e;
        pb_cont_e.setZero(np, 2);
        pb_cont_e.col(0) = LB_e;
        pb_cont_e.col(1) = UB_e;
        pb_e.transposeInPlace();
        pb_cont_e.transposeInPlace();
        Eigen::MatrixXd pbJoined(2, nineq + np);
        pbJoined << pb_e, pb_cont_e;
        pbJoined.transposeInPlace();
        pb_e.resize(pbJoined.rows(), pbJoined.cols());
        pb_e = pbJoined;

    } else {
        pb_e.setZero(np, 2);
        pb_e.col(0) = LB_e;
        pb_e.col(1) = UB_e;
    }

    double rho   = fit.ControlRho;
    int maxit = fit.ControlMajorLimit;
    int minit = fit.ControlMinorLimit;
    double delta = fit.ControlFuncPrecision;
    double tol   = fit.ControlTolerance;

    int tc = nineq + neq;

    double j = funv;
    Eigen::VectorXd tt_e(3); tt_e.setZero();

    Eigen::MatrixXd constraint_e;

    fit.solEqBFun();
    Eigen::RowVectorXd eqv_e(neq);
    eqv_e = fit.equality;

    fit.myineqFun();
    Eigen::RowVectorXd ineqv_e(nineq);
    ineqv_e= fit.inequality;

    Eigen::MatrixXd lambda_e;

    if (tc > 0){
        lambda_e.setZero(tc, 1);

        if (nineq){
            if(neq)
            {
                constraint_e.resize(1, eqv_e.size() + ineqv_e.size());
                constraint_e << eqv_e, ineqv_e;
            }
            else{
                constraint_e = ineqv_e;
            }
        }
        else
            constraint_e = eqv_e;

        if( ind[indHasIneq] > 0 ) {

            // 	tmpv = cbind(constraint[ (neq[0]):(tc[0]-1) ] - .fit.solIneqLB, .fit.solIneqUB - constraint[ (neq + 1):tc ] )
            Eigen::MatrixXd diff1 = constraint_e.block(0, neq, 1, tc-neq).transpose();
            Eigen::MatrixXd diff2 = constraint_e.block(0, neq, 1, tc-neq).transpose();
            Eigen::VectorXd infVec = Eigen::VectorXd::Constant(diff2.rows(), INF);
            diff2 = infVec - diff2;
            Eigen::MatrixXd tmpv_e(nineq, 2);
            tmpv_e.col(0) = diff1;
            tmpv_e.col(1) = diff2;
            Eigen::MatrixXd testMin_e = tmpv_e.rowwise().minCoeff();

            if ((testMin_e.array() > 0).all()) {
                ineqx0_e = constraint_e.block(0, neq, 1, tc-neq);
            }

            constraint_e.block(0, neq, 1, tc-neq) = constraint_e.block(0, neq, 1, tc-neq) - ineqx0_e;
        }

        tt_e[1] = sqrt(constraint_e.squaredNorm());
        double zeroCheck = tt_e[1] - (10 * tol);
        if( std::max(zeroCheck, (double)nineq) <= 0 ) {
            rho = 0;
        }
    } // end if tc > 0
    else {
        lambda_e.setZero(1, 1);
    }

    Eigen::RowVectorXd p_e;

    if (nineq){
        p_e.resize(1, ineqx0_e.size() + pars.size());
        p_e << ineqx0_e, pars;
    }
    else{
        p_e = pars;
    }

    hessv_e.resize(np + nineq, np + nineq);
    hessv_e.setIdentity();

    double mu = np;

    int solnp_iter = 0;

    Eigen::MatrixXd ob_e(1, 1 + neq + nineq);
    Eigen::RowVectorXd funvMatrix_e(1);
    funvMatrix_e[0] = funv;
    fit.solEqBFun();
    eqv_e = fit.equality;

    fit.myineqFun();
    ineqv_e = fit.inequality;

    obj_constr_eval(funvMatrix_e, eqv_e, ineqv_e, ob_e, verbose);

    Eigen::RowVectorXd vscale_e;

    while(solnp_iter < maxit){
        solnp_iter = solnp_iter + 1;
        Eigen::Array<double, 4, 1> subnp_ctrl;
        subnp_ctrl[0] = rho;
        subnp_ctrl[1] = minit;
        subnp_ctrl[2] = delta;
        subnp_ctrl[3] = tol;

        if ( ind[indHasEq] > 0){

            double max = ob_e.block(0, 1, 1, neq).cwiseAbs().maxCoeff();
            Eigen::MatrixXd temp2_e(1, neq);
            temp2_e = temp2_e.setOnes() * max;
            Eigen::MatrixXd temp1_e(1, 1); temp1_e(0, 0) = ob_e(0, 0);
            vscale_e.resize(1, temp1_e.cols() + temp2_e.cols());
            vscale_e << temp1_e, temp2_e;
        }
        else{
            vscale_e.resize(1, 1); vscale_e.setOnes();
        }
        Eigen::RowVectorXd onesMatrix(1, p_e.size()); onesMatrix.setOnes();
        Eigen::RowVectorXd vscale_t = vscale_e;
        vscale_e.resize(1, vscale_t.cols() + onesMatrix.cols());
        vscale_e << vscale_t, onesMatrix;

        minMaxAbs(vscale_e, tol);

        if (mode == -1)
        {
            fit.informOut = 0;
            memcpy(pars.data(), p_e.data(), pars.size() * sizeof(double));
            return;
        }

        sx_Matrix.setZero(p_e.rows(), p_e.cols());

        subnp(p_e, lambda_e, ob_e, hessv_e, mu, vscale_e, subnp_ctrl, verbose);

        p_e = resP;

        if (flag == 1)
        {
            mode = 0;
            Eigen::MatrixXd temp;
            temp = p_e.block(0, nineq, 1, np);
            funv = fit.solFun(temp.data(), &mode);
            funvMatrix_e[0] = funv;
            fit.solEqBFun();
            eqv_e = fit.equality;
            fit.myineqFun();
            ineqv_e = fit.inequality;
            obj_constr_eval(funvMatrix_e, eqv_e, ineqv_e, ob_e, verbose);

            if ( ind[indHasEq] > 0){

                double max = ob_e.block(0, 1, 1, neq).cwiseAbs().maxCoeff();
                Eigen::MatrixXd temp2_e(1, neq);
                temp2_e = temp2_e.setOnes() * max;
                Eigen::MatrixXd temp1_e(1, 1); temp1_e(0, 0) = ob_e(0, 0);
                vscale_e.resize(1, temp1_e.cols() + temp2_e.cols());
                vscale_e << temp1_e, temp2_e;
            }
            else{
                vscale_e.resize(1, 1); vscale_e.setOnes();
            }
            Eigen::RowVectorXd onesMatrix(1, p_e.size()); onesMatrix.setOnes();
            Eigen::RowVectorXd vscale_t = vscale_e;
            vscale_e.resize(1, vscale_t.cols() + onesMatrix.cols());
            vscale_e << vscale_t, onesMatrix;

            lambda_e = resY;
            hessv_e = resHessv;
            mu = resLambda;
            subnp(p_e, lambda_e, ob_e, hessv_e, mu, vscale_e, subnp_ctrl, verbose);
        }

        lambda_e = resY;
        hessv_e = resHessv;
        mu = resLambda;

        Eigen::MatrixXd temp;
        temp = p_e.block(0, nineq, 1, np);

        mode = 1;
        funv = fit.solFun(temp.data(), &mode);
        if (mode == -1)
        {
            fit.informOut = 0;
            memcpy(pars.data(), p_e.data(), pars.size() * sizeof(double));
            return;
        }

        solnp_nfn = solnp_nfn + 1;

        fit.solEqBFun();
        eqv_e = fit.equality;
        funvMatrix_e[0] = funv;
        fit.myineqFun();
        ineqv_e = fit.inequality;

        obj_constr_eval(funvMatrix_e, eqv_e, ineqv_e, ob_e, verbose);

        resultForTT = (j - ob_e(0, 0)) / std::max(ob_e.cwiseAbs().maxCoeff(), 1.0);
        tt_e[0] = resultForTT;
        if (verbose >= 1){
            mxLog("resultForTT \n");
            mxLog("%f", resultForTT);
        }
        j = ob_e(0, 0);

        if (tc > 0){
            // constraint = ob[ 2:(tc + 1) ]
            constraint_e = ob_e.block(0, 1, 1, tc);

            if ( ind[indHasIneq] > 0.5){
                //tempv = rbind( constraint[ (neq + 1):tc ] - pb[ 1:nineq, 1 ], pb[ 1:nineq, 2 ] - constraint[ (neq + 1):tc ] )
                Eigen::MatrixXd subsetOne = constraint_e.block(0, neq, 1, tc-neq) - pb_e.col(0).transpose().block(0, 0, 1, nineq);
                Eigen::MatrixXd subsetTwo = pb_e.col(1).transpose().block(0, 0, 1, nineq);
                subsetTwo -= subsetOne;
                Eigen::MatrixXd tempv(2, nineq);
                tempv.row(0) = subsetOne;
                tempv.row(1) = subsetTwo;

                if (tempv.minCoeff() > 0){
                    p_e.block(0, 0, 1, nineq) = constraint_e.block(0, neq, 1, tc-neq);
                }
                constraint_e.block(0, neq, 1, tc-neq) = constraint_e.block(0, neq, 1, tc-neq) - p_e.block(0, 0, 1, nineq);

            } // end if (ind[0][3] > 0.5){

            tt_e[2] = sqrt(constraint_e.squaredNorm());

            if ( tt_e[2] < (10 *tol)){
                rho =0;
                mu = std::min(mu, tol);
            }

            if ( tt_e[2] < (5 * tt_e[1])){
                rho = rho/5;
            }

            if ( tt_e[2] > (10 * tt_e[1])){
                rho = 5 * std::max(rho, sqrt(tol));
            }

            Eigen::MatrixXd llist(1, 2);

            llist(0, 0) = tol + tt_e[0];
            llist(0, 1) = tt_e[1] - tt_e[2];

            if (llist.maxCoeff() <= 0){
                lambda_e.resize(1, 1); lambda_e(0, 0) = 0;
                hessv_e = hessv_e.diagonal().asDiagonal();
            }

            tt_e[1] = tt_e[2];

        } // end if (tc > 0){

        Eigen::VectorXd tempTTVals(2);
        tempTTVals[0] = tt_e[0];
        tempTTVals[1] = tt_e[1];
        double vnormValue = sqrt(tempTTVals.squaredNorm());

        if (vnormValue <= tol){
            maxit_trace = maxit;
            maxit = solnp_iter;
        }

        if (verbose >= 3)
        {
            mxLog("vnormValue in while \n");
            mxLog("%f", vnormValue);
        }
    } // end while(solnp_iter < maxit){

    Eigen::RowVectorXd p_e_copy = p_e;
    p_e.resize(np);
    p_e = p_e_copy.block(0, nineq, 1, np);

    {
        Eigen::VectorXd tempTTVals(2);
        tempTTVals[0] = tt_e[0];
        tempTTVals[1] = tt_e[1];
        double vnormValue = sqrt(tempTTVals.squaredNorm());

        if (verbose >= 1) {
		mxLog("vnormValue %.20f, flag_NormgZ=%d, minr_rec=%d",
		      vnormValue, flag_NormgZ, minr_rec);
	}
        if (vnormValue <= tol && flag_NormgZ == 1 && minr_rec == 1){
		double iterateConverge = delta * pow(sqrt(sx_Matrix.squaredNorm()),(double)2.0);
		double iterateConvergeCond = sqrt(tol) * ((double)1.0 + pow(sqrt(p_e.squaredNorm()), (double)2.0));
		if (verbose >= 1) {
			mxLog("vnorm(sx_Matrix) is %.20f, iterateConverge is %.20f, iterateConvergeCond is: %.20f",
			      sqrt(sx_Matrix.squaredNorm()), iterateConverge, iterateConvergeCond);
		}

            if (iterateConverge <= iterateConvergeCond){
		    if (verbose >= 1) { mxLog("The solution converged in %d iterations", solnp_iter); }
		    fit.informOut = INFORM_CONVERGED_OPTIMUM;
            } else {
                if (verbose >= 1){
                    mxLog("The final iterate x satisfies the optimality conditions to the accuracy requested, but the sequence of iterates has not yet converged. CSOLNP was terminated because no further improvement could be made in the merit function.");}
                fit.informOut = INFORM_UNCONVERGED_OPTIMUM;
            }
        }
        else{
            if (solnp_iter == maxit_trace) {
                if (verbose >= 1){
                    mxLog("Exiting after maximum number of iterations. Tolerance not achieved\n");}
                fit.informOut = INFORM_ITERATION_LIMIT;
            } else {
                if (verbose >= 1) { mxLog("Solution failed to converge."); }
                fit.informOut = INFORM_NOT_AT_OPTIMUM;
            }
        }
    }
        memcpy(pars.data(), p_e.data(), pars.size() * sizeof(double));
        optimize_initial_inequality_constraints = FALSE;
    }

    fit.gradOut.resize(resGrad.size());
    memcpy(fit.gradOut.data(), resGrad.data(), fit.gradOut.size() * sizeof(double));
    fit.hessOut.resize(hessv_e.rows(), hessv_e.cols());
    memcpy(fit.hessOut.data(), hessv_e.data(), fit.hessOut.size() * sizeof(double));
}

template <typename T1, typename T2>
void CSOLNP::subnp(Eigen::MatrixBase<T2>& pars, Eigen::MatrixBase<T1>& yy_e, Eigen::MatrixBase<T1>& ob_e, Eigen::MatrixBase<T1>& hessv_e,
                     double lambda, Eigen::MatrixBase<T2>& vscale_e, const Eigen::Array<double, 4, 1> &ctrl, int verbose)
{
    int yyRows = yy_e.rows();
    //int yyCols = yy.cols;
    double j;

    //mxLog("ctrl is: ");
    //for (int ilog = 0; ilog < ctrl.cols; ilog++) mxLog("%f",ctrl.t[ilog]);
    double rho   = ctrl[0];
    int maxit = ctrl[1];
    double delta = ctrl[2];
    double tol =   ctrl[3];

    int neq =  fit.equality.size();
    int nineq;
    if (optimize_initial_inequality_constraints) nineq = 0;
    else nineq = fit.inequality.size();

    int np = (int)ind[indNumParam];

    double ch = 1;

    if (verbose >= 2){
        mxLog("ind inside subnp is: \n");
        for (int i = 0; i < ind.size(); i++) mxLog("%f",ind[i]);
    }

    Eigen::Array<double, 3, 1> alp;
    alp.setZero();

    int nc = neq + nineq;
    int npic = np + nineq;

    Eigen::RowVectorXd p0_e = pars;

    if (verbose >= 3) {
	    mxPrintMat("p0", p0_e);
    }

    Eigen::MatrixXd pb_e;
    /*Eigen::Map< Eigen::VectorXd > LB_e(LB.t, LB.cols);
    Eigen::Map< Eigen::VectorXd > UB_e(UB.t, UB.cols);*/

    if(nineq) {
        pb_e.setZero(nineq, 2);
        pb_e.col(1) = Eigen::VectorXd::Constant(pb_e.rows(), INF);
        Eigen::MatrixXd pb_cont_e;
        pb_cont_e.setZero(np, 2);
        pb_cont_e.col(0) = LB_e;
        pb_cont_e.col(1) = UB_e;
        pb_e.transposeInPlace();
        pb_cont_e.transposeInPlace();
        Eigen::MatrixXd pbJoined(2, nineq + np);
        pbJoined << pb_e, pb_cont_e;
        pbJoined.transposeInPlace();
        pb_e.resize(pbJoined.rows(), pbJoined.cols());
        pb_e = pbJoined;
    } else {
        pb_e.setZero(np, 2);
        pb_e.col(0) = LB_e;
        pb_e.col(1) = UB_e;
    }

    Eigen::Array<double, 3, 1> sob;
    sob.setZero();

    //Matrix yyMatrix = duplicateIt(yy);

    ob_e = ob_e.cwiseQuotient(vscale_e.block(0, 0, 1, nc + 1));
    p0_e = p0_e.cwiseQuotient(vscale_e.block(0, neq + 1, 1, nc + np - neq));

    int mm = 0;
    {
        mm=npic;
        Eigen::MatrixXd pbCopied;
        pbCopied.setZero(pb_e.rows(), pb_e.cols());
        pbCopied.col(0) = vscale_e.block(0, neq + 1, 1, mm).transpose();
        pbCopied.col(1) = vscale_e.block(0, neq + 1, 1, mm).transpose();
        pb_e = pb_e.cwiseQuotient(pbCopied);
    }

    // scale the lagrange multipliers and the Hessian
    if( nc > 0) {
        // yy [total constraints = nineq + neq]
        // scale here is [tc] and dot multiplied by yy
        //yy = vscale[ 2:(nc + 1) ] * yy / vscale[ 1 ]

        yy_e = vscale_e.block(0, 1, 1, nc).transpose().array() * yy_e.array();
        yy_e = yy_e / vscale_e[0];
    }

    // hessv [ (np+nineq) x (np+nineq) ]
    // hessv = hessv * (vscale[ (neq + 2):(nc + np + 1) ] %*% t(vscale[ (neq + 2):(nc + np + 1)]) ) / vscale[ 1 ]

    Eigen::MatrixXd result_e;
    result_e = vscale_e.block(0, neq + 1, 1, nc + np - neq).transpose() * vscale_e.block(0, neq + 1, 1, nc + np - neq);
    hessv_e = hessv_e.cwiseProduct(result_e);
    hessv_e = hessv_e / vscale_e[0];

    j = ob_e(0, 0);
    if (verbose >= 3){
        mxLog("j j is: \n");
        mxLog("%f", j);
    }
    Eigen::MatrixXd a_e;
    if( ind[indHasIneq] > 0){
        if ( ind[indHasEq] <= 0)
        {
            // arrays, rows, cols
            Eigen::MatrixXd negDiag;
            negDiag.setIdentity(nineq, nineq);
            negDiag.diagonal() *= -1;
            //std::cout << "Here is the matrix negDiag:\n" << negDiag << std::endl;
            Eigen::MatrixXd zeroMatrix(nineq, np);
            zeroMatrix.setZero();
            //std::cout << "Here is the matrix  zeroMatrix:\n" << zeroMatrix << std::endl;
            a_e.resize(nineq, np + nineq);
            a_e << negDiag, zeroMatrix;
            //std::cout << "Here is the matrix a_e:\n" << a_e << std::endl;
        }
        else{
            // [ (neq+nineq) x (nineq+np)]
            //a = rbind( cbind( 0 * .ones(neq, nineq), matrix(0, ncol = np, nrow = neq) ),
            //      cbind( -diag(nineq), matrix(0, ncol = np, nrow = nineq) ) )

            Eigen::MatrixXd zeroMatrix(nineq, np);
            zeroMatrix.setZero();
            //Matrix zeroMatrix = fill(np, nineq, (double)0.0);
            Eigen::MatrixXd firstHalf_e(neq, nineq + np);
            firstHalf_e.setZero();
            //Matrix firstHalf = copy(fill(nineq, neq, (double)0.0), fill(np, neq, (double)0.0));
            Eigen::MatrixXd negDiag;
            negDiag.setIdentity(nineq, nineq);
            negDiag.diagonal() *= -1;
            //Matrix onesMatrix = fill(nineq, 1, (double)-1.0);
            //Matrix negDiag = diag(onesMatrix);

            //Matrix secondHalf = copy(negDiag, zeroMatrix);

            firstHalf_e.transpose();
            Eigen::MatrixXd secondHalf_e(nineq, np + nineq);
            secondHalf_e << negDiag, zeroMatrix;
            a_e.resize(nineq + np, neq + nineq);
            a_e << firstHalf_e.transpose(), secondHalf_e.transpose();
            a_e.transposeInPlace();
            //a = transpose(copy(transpose(firstHalf), transpose(secondHalf)));
        }
    }	// end 	if(ind[0][3] > 0){

    if ( (ind[indHasEq] > 0) && ind[indHasIneq] <= 0 ){
        a_e.resize(neq, np);
        a_e.setZero();
        //a = fill(np, neq, (double)0.0);
    }
    if (ind[indHasEq]<= 0 && (ind[indHasIneq] <= 0)){
        a_e.resize(1, np);
        a_e.setZero();
        //a = fill(np, 1, (double)0.0);
    }

    Eigen::RowVectorXd g_e; g_e.setZero(npic);
    Eigen::RowVectorXd p_e;
    p_e = p0_e.block(0, 0, 1, npic);

    Eigen::MatrixXd b_e;
    double funv;

    int solnp_nfn = 0;
    double go, reduce = 1e-300;
    int minit;
    double lambdaValue = lambda;

    Eigen::MatrixXd constraint_e(1, nc);
    Eigen::MatrixXd y_e;

    if (nc > 0) {
        constraint_e = ob_e.block(0, 1, 1, nc);

        for (int i=0; i<np; i++){
            int index = nineq + i;
            p0_e[index] = p0_e[index] + delta;
            Eigen::MatrixXd tmpv_e;
            tmpv_e = p0_e.block(0, nineq, 1, npic - nineq);
            tmpv_e = tmpv_e.array() * vscale_e.block(0, nc+1, 1, np).array();
            if (verbose >= 2){
                mxLog("7th call is \n");
            }
            funv = fit.solFun(tmpv_e.data(), &mode);

            fit.solEqBFun();
            fit.myineqFun();

            solnp_nfn = solnp_nfn + 1;

            Eigen::MatrixXd firstPart_e(1, 1 + neq + nineq);

            Eigen::RowVectorXd funv_e(1); funv_e[0] = funv;
            Eigen::RowVectorXd eqv_e(neq); eqv_e = fit.equality;
            Eigen::RowVectorXd ineqv_e(nineq); ineqv_e= fit.inequality;

            obj_constr_eval(funv_e, eqv_e, ineqv_e, firstPart_e, verbose);

            Eigen::RowVectorXd secondPart_e;
            secondPart_e = vscale_e.block(0, 0, 1, nc+1);
            firstPart_e = firstPart_e.cwiseQuotient(secondPart_e);
            ob_e = firstPart_e;

            g_e[index] = (ob_e(0, 0)-j) / delta;

            if (verbose >= 3){
		    mxPrintMat("g", g_e);
            }

            a_e.col(index) = (ob_e.block(0, 1, 1, nc) - constraint_e).transpose() / delta;
            p0_e[index] = p0_e[index] - delta;
        } // end for (int i=0; i<np, i++){

        if (mode == -1)
        {
            funv = 1e24;
            mode = 0;
        }

        if(ind[indHasIneq] > 0){
            //constraint[ (neq + 1):(neq + nineq) ] = constraint[ (neq + 1):(neq + nineq) ] - p0[ 1:nineq ]
            constraint_e.block(0, neq, 1, nineq) = (constraint_e.block(0, neq, 1, nineq) - p0_e.block(0, 0, 1, nineq)).block(0, 0, 1, nineq);
        }

        if (false && solvecond(a_e) > 1/DBL_EPSILON) { // this can't be the cheapest way to check TODO
            Rf_error("Redundant constraints were found. Poor intermediate results may result. "
                     "Remove redundant constraints and re-OPTIMIZE.");
        }

        b_e = (a_e * p0_e.transpose()).transpose();
        //  b [nc,1]
        b_e -= constraint_e;
        ch = -1;
        alp[0] = tol - constraint_e.cwiseAbs().maxCoeff();
        if (alp[0] <= 0){

            ch = 1;

        } // end if (alp[0][0] <= 0){

        if (alp[0] <= 0){
            int npic_int = npic;
            Eigen::RowVectorXd onesMatrix_e;
            onesMatrix_e.setOnes(1, 1);
            Eigen::RowVectorXd p0_e_copy = p0_e;
            p0_e.resize(p0_e_copy.rows(), p0_e_copy.cols() + onesMatrix_e.cols());
            p0_e << p0_e_copy, onesMatrix_e;
            constraint_e *= (-1.0);
            Eigen::MatrixXd a_e_copy = a_e;
            a_e.resize(a_e.rows(), a_e.cols() + constraint_e.transpose().cols());
            a_e << a_e_copy, constraint_e.transpose();
            Eigen::MatrixXd firstMatrix_e(1, npic);
            firstMatrix_e.setZero();
            Eigen::MatrixXd cx_e(firstMatrix_e.rows(), firstMatrix_e.cols() + onesMatrix_e.cols());
            cx_e << firstMatrix_e, onesMatrix_e;
            Eigen::MatrixXd dx_e(npic + 1, 1);
            dx_e.setOnes();
            go = 1;
            minit = 0;

            while(go >= tol)
            {
                minit = minit + 1;
                Eigen::MatrixXd gap_e(mm, 2);
                gap_e.setZero();
                gap_e.col(0) = p0_e.block(0, 0, 1, mm).transpose() - pb_e.col(0);
                gap_e.col(1) = pb_e.col(1) - p0_e.block(0, 0, 1, mm).transpose();
                rowSort_e(gap_e);
                dx_e.transpose().block(0, 0, 1, mm) = gap_e.col(0).transpose().block(0, 0, 1, mm);
                dx_e(npic_int, 0) = p0_e(0, npic_int);
                Eigen::MatrixXd argum1_e;
                argum1_e = a_e * dx_e.asDiagonal();
                argum1_e.transposeInPlace();
                Eigen::MatrixXd argum2_e;
                argum2_e = cx_e.asDiagonal() * dx_e;
                y_e = QRdsolve(argum1_e, argum2_e);
                Eigen::MatrixXd cx_e_r;
                cx_e_r = cx_e.transpose() - (a_e.transpose() * y_e);
                dx_e = (cx_e_r.asDiagonal() * dx_e).asDiagonal() * dx_e;
                Eigen::MatrixXd v_e = dx_e.transpose();
                int indexx = npic;

                if (v_e(0, indexx) > 0)
                {
                    double z = p0_e(indexx)/v_e(0, indexx);

                    for (int i=0; i<mm; i++)
                    {
                        if(v_e(0, i) < 0)
                        {
                            z = std::min(z, -(pb_e(i, 1) - p0_e(i))/v_e(0, i));

                        }
                        else if(v_e(0, i) > 0)
                        {

                            z = std::min(z, (p0_e(i) - pb_e(i, 0))/v_e(0, i));
                        }
                    }

                    if(z < (p0_e(indexx)/v_e(0, indexx))) {
                        z *= 0.9;
                    }

                    p0_e -= v_e * z;
                    go = p0_e(indexx);
                    if(minit >= 10){
                        go = 0;
                    }
                }
                else{
                    go = 0;
                    minit = 10;
                }
            }// end while(go >= tol)

            if (minit >= 10){
                mxLog("The linearized problem has no feasible solution. The problem may not be feasible.");
            }

            int h;
            Eigen::MatrixXd a_e_c(nc, npic);

            for (h = 0; h<a_e.rows(); h++)
            {
                a_e_c.row(h) = a_e.row(h).block(0, 0, 1, npic);
            }
            a_e.resize(a_e_c.rows(), a_e_c.cols());
            a_e = a_e_c;
            b_e = (a_e * p0_e.block(0, 0, 1, npic).transpose()).transpose();
        }// end if(M(alp, 0, 0) <= 0)
    } // end if (nc > 0){

    p_e = p0_e.block(0, 0, 1, npic);

    if (nc == 0){
        y_e.resize(1,1);
        y_e(0, 0) = 0;
    }

    if (ch > 0){

        Eigen::MatrixXd tmpv_e;
        tmpv_e = p_e.block(0, nineq, 1, npic-nineq);
        tmpv_e = tmpv_e.array() * vscale_e.block(0, nc+1, 1, np).array();
        funv = fit.solFun(tmpv_e.data(), &mode);
        if (verbose >= 3){
            mxLog("funv is: \n");
            mxLog("%f", funv);
        }

        if (mode == -1)
        {
            funv = 1e24;
            mode = 0;
        }

        fit.solEqBFun();

        fit.myineqFun();

        solnp_nfn = solnp_nfn + 1;
        Eigen::MatrixXd firstPart_e(1, 1 + neq + nineq);
        Eigen::RowVectorXd funv_e(1); funv_e[0] = funv;
        Eigen::RowVectorXd eqv_e(neq); eqv_e = fit.equality;
        Eigen::RowVectorXd ineqv_e(nineq); ineqv_e= fit.inequality;

        obj_constr_eval(funv_e, eqv_e, ineqv_e, firstPart_e, verbose);

        Eigen::RowVectorXd secondPart_e;
        secondPart_e = vscale_e.block(0, 0, 1, nc+1);
        firstPart_e = firstPart_e.cwiseQuotient(secondPart_e);
        ob_e = firstPart_e;

    } // end of if (ch>0)

    j = ob_e(0, 0);

    if (ind[indHasIneq] > 0){
        ob_e.block(0, neq+1, 1, nc-neq) -= p_e.block(0, 0, 1, nineq);
    }

    if (nc > 0){
        Eigen::MatrixXd result_e = ob_e.block(0, 1, 1, nc);
        result_e -= (a_e * p_e.transpose()).transpose();
        result_e += b_e;
        ob_e.block(0, 1, 1, nc) = result_e;
        double vnormTerm = ob_e.block(0, 1, 1, nc).squaredNorm();
        double dotProductTerm = yy_e.transpose().row(0).dot(ob_e.block(0, 1, 1, nc).row(0));
        j = ob_e(0, 0) - dotProductTerm + rho * vnormTerm;
    }

    minit = 0;
    Eigen::MatrixXd yg_e;
    Eigen::MatrixXd yg_rec(1, 2);
    Eigen::MatrixXd sx_e;
    sx_e.setZero(p_e.rows(), p_e.cols());
    Eigen::MatrixXd obm_e;

    while (minit < maxit){
        minit = minit + 1;
        if (ch > 0){

            for (int i=0; i<np; i++){
                int index = nineq + i;
                p_e[index] = p_e[index] + delta;
                Eigen::MatrixXd tmpv_e = p_e.block(0, nineq, 1, npic - nineq).array() * vscale_e.block(0, nc+1, 1, np).array();
                if (verbose >= 3){
                    mxLog("9th call is \n");
                }
                mode = 0;
                funv = fit.solFun(tmpv_e.data(), &mode);
                if (verbose >= 3){
                    mxLog("funv is: \n");
                    mxLog("%f", funv);
                }

                if (mode == -1)
                {
                    funv = 1e24;
                    mode = 0;
                }
                fit.solEqBFun();
                fit.myineqFun();

                solnp_nfn = solnp_nfn + 1;

                Eigen::MatrixXd firstPart_e(1, 1 + neq + nineq);
                Eigen::RowVectorXd funv_e(1); funv_e[0] = funv;
                Eigen::RowVectorXd eqv_e(neq); eqv_e = fit.equality;
                Eigen::RowVectorXd ineqv_e(nineq); ineqv_e= fit.inequality;

                obj_constr_eval(funv_e, eqv_e, ineqv_e, firstPart_e, verbose);

                Eigen::RowVectorXd secondPart_e;
                secondPart_e = vscale_e.block(0, 0, 1, nc+1);

                firstPart_e = firstPart_e.cwiseQuotient(secondPart_e);
                obm_e = firstPart_e;

                if (verbose >= 3){
                    mxLog("j is: \n");
                    mxLog("%f", j);
                }

                if (ind[indHasIneq] > 0.5){
                    obm_e.block(0, neq+1, 1, nc-neq) -= p_e.block(0, 0, 1, nineq);
                }

                double obm = obm_e(0, 0);
                if (nc > 0){
                    Eigen::MatrixXd result_e = obm_e.block(0, 1, 1, nc);
                    result_e -= (a_e * p_e.transpose()).transpose();
                    result_e += b_e;
                    obm_e.block(0, 1, 1, nc) = result_e;
                    double vnormTerm = obm_e.block(0, 1, 1, nc).squaredNorm();
                    double dotProductTerm = yy_e.transpose().row(0).dot(obm_e.block(0, 1, 1, nc).row(0));
                    obm = obm_e(0, 0) - dotProductTerm + rho * vnormTerm;
                }

                if (verbose >= 3)   mxLog("obm is: %.20f", obm);

                g_e[index] = (obm - j)/delta;
                p_e[index] = p_e[index] - delta;

                if (verbose >= 3){
			mxPrintMat("g", g_e);
			mxPrintMat("p", p_e);
                }
            } // end for (i=0; i<np; i++){

            if (ind[indHasIneq] > 0.5){
                Eigen::RowVectorXd temp;
                temp.setZero(1, nineq);
                g_e.block(0, 0, 1, nineq) = temp;
            }
        } // end if (ch > 0){

        if (minit > 1){
            yg_e = g_e - yg_e;
            sx_e = p_e - sx_e;
            Eigen::MatrixXd sc_m1 = (sx_e * hessv_e) * sx_e.transpose();
            Eigen::MatrixXd sc_m2 = sx_e * yg_e.transpose();
            Eigen::RowVectorXd sc_e(2);
            sc_e[0] = sc_m1(0, 0);
            sc_e[1] = sc_m2(0, 0);
            if ((sc_e[0] * sc_e[1]) > 0){
                //hessv  = hessv - ( sx %*% t(sx) ) / sc[ 1 ] + ( yg %*% t(yg) ) / sc[ 2 ]
                Eigen::MatrixXd sx_t = sx_e.transpose();
                sx_e.resize(hessv_e.rows(), sx_t.cols());
                sx_e = hessv_e * sx_t;

                Eigen::MatrixXd sxMatrix = sx_e * sx_e.transpose();
                sxMatrix /= sc_e[0];
                Eigen::MatrixXd ygMatrix = yg_e.transpose() * yg_e;
                ygMatrix /= sc_e[1];
                hessv_e -= sxMatrix;
                hessv_e += ygMatrix;
            }
        }

        Eigen::MatrixXd dx_e(1, npic);
        dx_e.setOnes();
        dx_e *= 0.01;

        {
            Eigen::MatrixXd gap_e(pb_e.rows(), pb_e.cols());
            gap_e.setZero();
            gap_e.col(0) = p_e.block(0, 0, 1, mm).transpose() - pb_e.col(0);
            gap_e.col(1) = pb_e.col(1) - p_e.block(0, 0, 1, mm).transpose();
            rowSort_e(gap_e);
            Eigen::MatrixXd temp(mm, 1);
            temp.setOnes();
            Eigen::MatrixXd gap_eTemp(mm, 1);
            gap_eTemp = gap_e.col(0) + (temp * sqrt(DBL_EPSILON));
            gap_e.resize(mm, 1);
            gap_e = gap_eTemp;
            dx_e.block(0, 0, 1, mm) = temp.cwiseQuotient(gap_e).transpose();
        }

        go = -1;
        lambdaValue = lambdaValue/10.0;

        if (verbose >= 3){
            mxLog("lambdaValue is: \n");
            mxLog("%.20f", lambdaValue);
        }

        while(go <= 0){
            Eigen::RowVectorXd dxDiagValues(dx_e.cols());
            dxDiagValues = dx_e.cwiseProduct(dx_e);
            Eigen::MatrixXd cz_e;
            cz_e = dxDiagValues.asDiagonal();
            cz_e = hessv_e + (cz_e * lambdaValue);
            Eigen::MatrixXd cz_chol = cz_e.llt().matrixL();
            cz_chol.transposeInPlace();

            if (!R_FINITE((cz_e.maxCoeff())))
            {
                if (verbose >= 3){
                    mxLog("here in findMax");
                }
                flag = 1;
                p_e = p_e.cwiseProduct(vscale_e.block(0, neq+1, 1, nc+np-neq));
                if (nc > 0){ y_e.resize(1, 1); y_e(0, 0) = 0;}
                hessv_e = hessv_e.cwiseQuotient(vscale_e.block(0, neq+1, 1, nc+np-neq).transpose() * vscale_e.block(0, neq+1, 1, nc+np-neq)) *vscale_e(0);
                resP = p_e;
                resY = y_e;
                resHessv = hessv_e;
                resLambda = lambda;
                resGrad = g_e;
                return;
            }

            Eigen::MatrixXd cz_inv;
            cz_inv = cz_chol.inverse();

            if (verbose >= 3){
                mxLog("cz.rows: %d", cz_chol.rows());
                mxLog("cz.cols: %d", cz_chol.cols());
            }

            yg_e.resize(cz_inv.cols(), g_e.rows());
            yg_e = cz_inv.transpose() * g_e.transpose();
            if (minit == 1) yg_rec(0, 0) = yg_e.squaredNorm();

            Eigen::MatrixXd u_e;
            if (nc <= 0){
                u_e = (cz_inv * (-1.0)) * yg_e;
                u_e.transposeInPlace();
            }
            else{
                //y = qr.solve(t(cz) %*% t(a), yg)
                Eigen::MatrixXd argum1_e;
                argum1_e = cz_inv.transpose() * a_e.transpose();
                Eigen::MatrixXd solution;

                solution = QRdsolve(argum1_e, yg_e);

                y_e.resize(solution.cols(), solution.rows());
                y_e = solution.transpose();
                u_e = (cz_inv * (-1.0)) * (yg_e - (argum1_e * solution));
                u_e.transposeInPlace();
            }

            p0_e.resize(npic);
            p0_e = u_e.block(0, 0, 1, npic) + p_e;

            {
                Eigen::MatrixXd listPartOne = p0_e.block(0, 0, 1, mm).transpose() - pb_e.col(0);
                Eigen::MatrixXd listPartTwo = pb_e.col(1) - p0_e.block(0, 0, 1, mm).transpose();
                Eigen::MatrixXd llist(listPartOne.rows(), listPartOne.cols() + listPartTwo.cols());
                llist << listPartOne, listPartTwo;
                go = llist.minCoeff();
                lambdaValue = 3 * lambdaValue;
                if (verbose >= 3){
                    mxLog("go is: \n");
                    mxLog("%f", go);
                    mxLog("lambdaValue is: \n");
                    mxLog("%f", lambdaValue);

                }
            }
        } // end while(go <= 0){

        alp[0] = 0;
        Eigen::MatrixXd ob1_e = ob_e;
        Eigen::MatrixXd ob2_e = ob1_e;
        sob[0] = j;
        sob[1] = j;

        if (verbose >= 3){
		mxPrintMat("sob", sob);
        }

        Eigen::MatrixXd ptt_e(p_e.cols(), p_e.rows() + p_e.rows());
        ptt_e << p_e.transpose(), p_e.transpose();
        alp[2] = 1.0;

        Eigen::MatrixXd ptt_temp(ptt_e.rows(), ptt_e.cols() + p0_e.rows());
        ptt_temp << ptt_e, p0_e.transpose();
        ptt_e.resize(ptt_temp.rows(), ptt_temp.cols());
        ptt_e = ptt_temp;
        Eigen::MatrixXd pttCol;
        pttCol = ptt_e.col(2);
        Eigen::MatrixXd tmpv_e = pttCol.transpose().block(0, nineq, 1, npic - nineq).cwiseProduct(vscale_e.block(0, nc+1, 1, np));

        mode = 1;
        funv = fit.solFun(tmpv_e.data(), &mode);
        if (verbose >= 3){
		mxPrintMat("g", g_e);
            mxLog("funv is: \n");
            mxLog("%f", funv);
        }

        if (mode == -1)
        {
            funv = 1e24;
            mode = 0;
        }

        fit.solEqBFun();
        fit.myineqFun();

        solnp_nfn = solnp_nfn + 1;

        Eigen::MatrixXd firstPart_e(1, 1 + neq + nineq);
        Eigen::RowVectorXd funv_e(1); funv_e[0] = funv;
        Eigen::RowVectorXd eqv_e(neq); eqv_e = fit.equality;
        Eigen::RowVectorXd ineqv_e(nineq); ineqv_e= fit.inequality;

        obj_constr_eval(funv_e, eqv_e, ineqv_e, firstPart_e, verbose);

        Eigen::RowVectorXd secondPart_e;
        secondPart_e = vscale_e.block(0, 0, 1, nc+1);
        firstPart_e = firstPart_e.cwiseQuotient(secondPart_e);
        Eigen::MatrixXd ob3_e = firstPart_e;
        sob[2] = ob3_e(0, 0);

        if (ind[indHasIneq] > 0.5){
            // ob3[ (neq + 2):(nc + 1) ] = ob3[ (neq + 2):(nc + 1) ] - ptt[ 1:nineq, 3 ]
            Eigen::MatrixXd partOne = ob3_e.block(0, neq+1, 1, nc-neq);
            Eigen::MatrixXd partTwo = ptt_e.col(2).transpose().block(0, 0, 1, nineq);
            ob3_e.block(0, neq+1, 1, nc-neq) = partOne - partTwo;
        }

        if (nc > 0){
            //sob[ 3 ] = ob3[ 1 ] - t(yy) %*% ob3[ 2:(nc + 1) ] + rho * .vnorm(ob3[ 2:(nc + 1) ]) ^ 2
            Eigen::MatrixXd result_e = ob3_e.block(0, 1, 1, nc);
            result_e -= (a_e * ptt_e.col(2)).transpose();
            result_e += b_e;
            ob3_e.block(0, 1, 1, nc) = result_e;
            double vnormTerm = ob3_e.block(0, 1, 1, nc).squaredNorm();
            double dotProductTerm = yy_e.transpose().row(0).dot(ob3_e.block(0, 1, 1, nc).row(0));
            sob[2] = ob3_e(0, 0) - dotProductTerm + (rho * vnormTerm);
        }

        go = 1;

        while(go > tol){
            alp[1] = (alp[0] + alp[2]) / 2.0;

            ptt_e.col(1) = (p_e * (1 - alp[1])) + p0_e * alp[1];
            Eigen::MatrixXd tmpv_e = ptt_e.col(1).transpose().block(0, nineq, 1, npic - nineq).cwiseProduct(vscale_e.block(0, nc+1, 1, np));

            if (verbose >= 3){
                mxLog("11th call is \n");
            }

            mode = 0;
            funv = fit.solFun(tmpv_e.data(), &mode);
            if (verbose >= 3){
                mxLog("funv is: \n");
                mxLog("%f", funv);
            }

            if (mode == -1)
            {
                funv = 1e24;
                mode = 0;
            }

            fit.solEqBFun();
            fit.myineqFun();

            solnp_nfn = solnp_nfn + 1;
            Eigen::MatrixXd firstPart_e(1, 1 + neq + nineq);
            Eigen::RowVectorXd funv_e(1); funv_e[0] = funv;
            Eigen::RowVectorXd eqv_e(neq); eqv_e = fit.equality;
            Eigen::RowVectorXd ineqv_e(nineq); ineqv_e= fit.inequality;

            obj_constr_eval(funv_e, eqv_e, ineqv_e, firstPart_e, verbose);

            Eigen::RowVectorXd secondPart_e;
            secondPart_e = vscale_e.block(0, 0, 1, nc+1);
            firstPart_e = firstPart_e.cwiseQuotient(secondPart_e);
            ob2_e = firstPart_e;

            sob[1] = ob2_e(0, 0);
            if (verbose >= 3){
		    mxPrintMat("sob", sob);
            }
            if (ind[indHasIneq] > 0.5){
                Eigen::MatrixXd partOne = ob2_e.block(0, neq+1, 1, nc-neq);
                Eigen::MatrixXd partTwo = ptt_e.col(1).transpose().block(0, 0, 1, nineq);
                ob2_e.block(0, neq+1, 1, nc-neq) = partOne - partTwo;
            }
            if (nc > 0){
                Eigen::MatrixXd result_e = ob2_e.block(0, 1, 1, nc);
                result_e -= (a_e * ptt_e.col(1)).transpose();
                result_e += b_e;
                ob2_e.block(0, 1, 1, nc) = result_e;
                double vnormTerm = ob2_e.block(0, 1, 1, nc).squaredNorm();
                Eigen::MatrixXd temp = ob2_e.block(0, 1, 1, nc);
                double dotProductTerm = yy_e.transpose().row(0).dot(temp.row(0));
                sob[1] = ob2_e(0, 0) - dotProductTerm + (rho * vnormTerm);
            }

            const double sobMax = sob.maxCoeff();
            if (verbose >= 3){
                mxLog("sobMax is: %f", sobMax);
            }
            if (sobMax < j){
                go = tol * (sobMax - sob.minCoeff()) / (j - sobMax);
            }

            const bool condif1 = (sob[1] >= sob[0]);
            const bool condif2 = (sob[0] <= sob[2]) && (sob[1] < sob[0]);
            const bool condif3 = (sob[1] <  sob[0]) && (sob[0] > sob[2]);

            if (condif1){
                sob[2] = sob[1];
                ob3_e = ob2_e;
                alp[2] = alp[1];
                ptt_e.col(2) = ptt_e.col(1);

            }

            if (condif2){
                sob[2] = sob[1];
                ob3_e = ob2_e;
                alp[2] = alp[1];
                ptt_e.col(2) = ptt_e.col(1);
            }

            if (condif3){
                sob[0] = sob[1];
                ob1_e = ob2_e;
                alp[0] = alp[1];
                ptt_e.col(0) = ptt_e.col(1);
            }

            if (go >= tol){
                go = alp[2] - alp[0];
                if (verbose >= 3){
                    mxLog("go is: \n");
                    mxLog("%f", go);
                }
            }

        } // 	while(go > tol){

        if (verbose >= 3){
            mxLog("go is: \n");
            mxLog("%.16f", go);
        }

        sx_Matrix = sx_e;
        sx_e.resize(p_e.rows(), p_e.cols());
        sx_e = p_e;
        yg_e.resize(g_e.rows(), g_e.cols());
        yg_e = g_e;

        ch = 1;

        double obn = sob.minCoeff();
        if (verbose >= 3){
            mxLog("obn is: \n");
            mxLog("%f", obn);
        }
        if (j <= obn){
            maxit = minit;
        }
        if (verbose >= 3){
            mxLog("j is: \n");
            mxLog("%f", j);
        }
        double reduce = (j - obn) / ((double)1.0 + (double)fabs(j));
        if (verbose >= 3){
            mxLog("reduce is: \n");
            mxLog("%f", reduce);
        }
        if (reduce < tol){
            maxit = minit;
        }

        const bool condif1 = (sob[0] <  sob[1]);
        const bool condif2 = (sob[2] <  sob[1]) && (sob[0] >= sob[1]);
        const bool condif3 = (sob[0] >= sob[1]) && (sob[2] >= sob[1]);

        if (condif1){
            j = sob[0];
            p_e = ptt_e.col(0).transpose();
            ob_e = ob1_e;
            if (verbose >= 3){
                mxLog("condif1\n");
                mxLog("j is: \n");
                mxLog("%f", j);
            }
        }

        if (condif2){

            j = sob[2];
            p_e = ptt_e.col(2).transpose();
            ob_e = ob3_e;
            if (verbose >= 3){
                mxLog("condif2\n");
                mxLog("j is: \n");
                mxLog("%f", j);
            }

        }

        if (condif3){
            j = sob[1];
            p_e = ptt_e.col(1).transpose();
            ob_e = ob2_e;
            if (verbose >= 3){
                mxLog("condif3\n");
                mxLog("j is: \n");
                mxLog("%f", j);
            }
        }
    } // end while (minit < maxit){

    yg_rec(0, 1) = yg_e.squaredNorm();
    if(yg_rec(0, 0) / yg_rec(0, 1) > 1000)  flag_NormgZ = 1;

    minr_rec = minit;

    p_e = p_e.cwiseProduct(vscale_e.block(0, neq+1, 1, nc+np-neq));
    // I need vscale, p, y, hessv
    if (nc > 0){
        y_e *= vscale_e(0);
        y_e = y_e.cwiseQuotient(vscale_e.block(0, 1, 1, nc));
    }

    // hessv = vscale[ 1 ] * hessv / (vscale[ (neq + 2):(nc + np + 1) ] %*%
    //                                t(vscale[ (neq + 2):(nc + np + 1) ]) )

    Eigen::MatrixXd transposePart;
    transposePart = vscale_e.block(0, neq+1, 1, nc+np-neq).transpose() * vscale_e.block(0, neq+1, 1, nc+np-neq);
    hessv_e = hessv_e.cwiseQuotient(transposePart);
    hessv_e = hessv_e * vscale_e(0);

    if (verbose >= 1 && reduce > tol) {
        mxLog("m3 solnp Rf_error message being reported.");
    }

    resP = p_e;
    resY = y_e.block(0, 0, 1, yyRows).transpose();
    resHessv = hessv_e;
    resLambda = lambdaValue;
    resGrad = g_e;

} // end subnp

template <typename T1, typename T2>
void CSOLNP::obj_constr_eval(Eigen::MatrixBase<T2>& objVal, Eigen::MatrixBase<T2>& eqval, Eigen::MatrixBase<T2>& ineqval, Eigen::MatrixBase<T1>& fitVal, int verbose)
{
	if (!std::isfinite(objVal(0))) {
		fitVal.setConstant(1e24);
		return;
	}

    if (optimize_initial_inequality_constraints){
        double total = ineqval.array().min(0).sum();
        fitVal << fabs(total) - 1e-4, eqval;
    }
    else{
	    fitVal << objVal, eqval, ineqval;
    }

    if (!std::isfinite(fitVal.sum())) {
		fitVal.setConstant(1e24);
		return;
    }
    if (verbose >= 4) mxPrintMat("fitVal", fitVal);
}


/*
  Copyright (c) 2019 Matthew H. Reilly (kb1vc)
  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.

  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
  HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "Point.hxx"
#ifndef __NO_MATH_INLINES
#define __NO_MATH_INLINES 1
#endif

#include <boost/format.hpp>
#include <regex>
#include <stdexcept>
#include <iostream>
#include <cmath>
#include <math.h>
#include <limits>

namespace GeoProf {

  std::regex Point::grid_regexp("[A-R][A-R][0-9][0-9][A-X][A-X]", std::regex_constants::icase);

    // Useful constants
  const double Point::clarke_66_al = 6378206.4;    /*Clarke 1866 ellipsoid*/
  const double Point::clarke_66_bl = 6356583.8;
  const double Point::rad_per_deg = (M_PI / 180.0);

  double Point::bearingTo(const Point & other) const {
    double br, rbr, dst;
    bearingDistanceTo(other, br, rbr, dst);

    return br;
  }

  double Point::distanceTo(const Point & other) const {
    double br, rbr, dst;
    bearingDistanceTo(other, br, rbr, dst);

    return dst;
  }

  // In g++ we get some very bad behavior around generated NANs -- these get handled better with more cautious math.
  __attribute__((optimize("-fno-fast-math")))
  void Point::bearingDistanceTo(const Point & other, double & bearing, double & reverse_bearing, double & distance) const {
    /*Taken directly from:       */
    /*Thomas, P.D., 1970, Spheroidal Geodesics, reference systems,*/
    /*    & local geometry, U.S. Naval Oceanographic Office SP-138,*/
    /*    165 pp.*/

    /*assumes North Latitude and East Longitude are positive*/
    // Translated from C, translated from fortran as in this comment
    /* forward.f -- translated by f2c (version 19960717).
       then hacked up a bit by hand to remove the dependence on
       the f2c libraries and such. */

    double BOA, F, P1R, P2R, L1R, L2R, DLR, T1R, T2R, TM, DTM, STM, CTM, SDTM,
      CDTM, KL, KK, SDLMR, L, CD, DL, SD, T, U, V, D, X, E, Y, A, FF64,
      TDLPM, HAPBR, HAMBR, A1M2, A2M1;

    if((fabs(other.lat - this->lat) < 1.0e-10) && (fabs(other.lon - this->lon) < 1.0e-10)) {
      bearing = 0.0;
      reverse_bearing = 0.0;
      distance = 0.0;
      return;
    }
    BOA = clarke_66_bl / clarke_66_al;
    F = 1.0 - BOA;
    P1R = this->lat * rad_per_deg;
    P2R = other.lat * rad_per_deg;
    L1R = this->lon * rad_per_deg;
    L2R = other.lon * rad_per_deg;
    DLR = L1R - L2R;
    T1R = atan(BOA * tan(P1R));
    T2R = atan(BOA * tan(P2R));
    TM = (T1R + T2R) / 2.0;
    DTM = (T2R - T1R) / 2.0;
    STM = sin(TM);
    CTM = cos(TM);
    SDTM = sin(DTM);
    CDTM = cos(DTM);
    KL = STM * CDTM;
    KK = SDTM * CTM;
    SDLMR = sin(DLR / 2.0);
    L = SDTM * SDTM + SDLMR * SDLMR * (CDTM * CDTM - STM * STM);
    CD = 1.0 - 2.0 * L;
    DL = acos(CD);
    SD = sin(DL);

    // Believe it or not, sometimes CD is 1.0 (when L, for instance, is 0)
    // All this NAN stuff is pretty hazardous. Make sure 0/0 is really
    // lim x -> 0  x/x = 1.
    if(fabs(DL - SD) < 1e-25) {
      T = 1.0;
    }
    else {
      T = DL / SD;
    }
    U = 2.0 * KL * KL / (1.0 - L);
    V = 2.0 * KK * KK / L;
    D = 4.0 * T * T;
    X = U + V;
    E = -2.0 * CD;
    Y = U - V;
    A = -D * E;
    FF64 = F * F / 64.0;
    distance = clarke_66_al * SD * (T -
				    F / 4.0 * (T * X - Y) +
				    FF64 * (X * (A + (T - (A + E) / 2.0) * X) +
					    Y * (E * Y - 2.0 * D) + D * X * Y)) / 1000.0;

    double dlrtan = tan(DLR);
    // At times DLR is 2pi... or pi... then we need to
    // fixup the atan calculation, as TDLPM is going to be very very large.

    double tanarg = (DLR - (E * (4.0 - X) + 2.0 * Y)
		     * (F / 2.0 * T + FF64 * (32.0 * T + (A - 20.0 * T) * X - 2.0 * (D + 2.0) * Y)) / 4.0 * dlrtan) / 2.0;

    TDLPM = tan(tanarg);

    HAPBR = atan2Pt(SDTM, CTM * TDLPM);
    HAMBR = atan2Pt(CDTM, STM * TDLPM);

    A1M2 = 2.0 * M_PI + HAMBR - HAPBR;
    A2M1 = 2.0 * M_PI - HAMBR - HAPBR;

    // bring A1M2 into the range 0..2pi
    A1M2 = inSpan(A1M2);
    A2M1 = inSpan(A2M1);

    /* these 360 degree corrections were added to
       fix a disagreement in the semantics of
       the original implementation of ATAN2, vs
       the implementation from the gnu c math rtl. */
    bearing = (A1M2 == 0.0) ? 0.0 : (360.0 - (A1M2 / rad_per_deg));
    reverse_bearing = (A2M1 == 0.0) ? 0.0 : (360.0 - (A2M1 / rad_per_deg));
    if((bearing - 360.0) >= 0.0) bearing -= 360.0;
    if((reverse_bearing - 360.0) >= 0.0) reverse_bearing -= 360.0;
  }

  bool Point::recCorrectBearingDistanceTo(const Point & other,
					  const double bearing,
					  const double distance,
					  const double b_span,
					  const double d_span,
					  double & new_bearing,
					  double & new_distance
					  ) const {

    // minimize distance error;
    double err_dist = 1e6; // we're closer than that..
    new_bearing = bearing;
    new_distance = distance;
    bool ret = false;
    // sweep +/- 1 degree
    for(double b_inc = -1.0 * b_span; b_inc < b_span; b_inc += (b_span * 0.25)) {
      double az = bearing + b_inc;
      if(az < 0.0) az = az + 360.0;
      if(az > 360.0) az = az - 360.0;
      bool got_better = false;
      double last_err = 1e6;
      for(double distinc = -1.0 * d_span; distinc < d_span; distinc += (d_span * 0.25)) {
	double rng = distance + distinc;
	if(rng < 0.1) rng = 0.1;
	// now go to the remote point
	Point next;
	stepTo(az, rng, next);
	// and calculate the distance between that and "other"
	double d_err = other.distanceTo(next);
	if(d_err < err_dist) {
	  err_dist = d_err;
	  new_bearing = az;
	  new_distance = rng;
	  got_better = true;
	  ret = true;
	}
	if(d_err > last_err) break; // we're getting worse.
	last_err = d_err;
      }
      // if we didn't improve on the last azimuth, bail out
      if(!got_better) break;
    }
    return (err_dist < 0.01); // if we're within 10m, let's quit.
  }

  void Point::correctBearingDistanceTo(const Point & other,
				       const double bearing,
				       const double distance,
				       double & new_bearing,
				       double & new_distance) const {
    new_bearing = bearing;
    new_distance = distance;
    double b_span = 1.0;
    double d_span = 30.0;
    for(int i = 0; i < 16; i++) {
      if(recCorrectBearingDistanceTo(other, new_bearing, new_distance, b_span, d_span, new_bearing, new_distance)) return;
      b_span = b_span * 0.25;
      d_span = d_span * 0.25;
      if((b_span < 0.01) && (d_span < 0.01)) break;
    }
  }

  double Point::inSpan(double ang) const {
    while((ang < 0.0) || (ang >= (2.0 * M_PI))) {
      if(ang < 0.0) {
	ang += 2.0 * M_PI;
      }
      else {
	ang -= 2.0 * M_PI;
      }
    }
    return ang;
  }

  __attribute__((optimize("-fno-fast-math")))
  void Point::stepTo(double bearing, double distance, Point & next) const {

    /* Initialized data */
    const double eps = 5e-14;
    const double a = 6378206.4; /* (meters) */
    const double f = 1.0/298.25722210088;

    /* System generated locals */
    double d1;

    bool debug = ((bearing < 180.00001) && (bearing > 179.99999)) ||
      (bearing < 0.00001) || (bearing > 359.99999);

    /* Local variables */
    double c, d, e, r, x, y, cf, sa, cu, sf, cy, cz, su, tu,
      sy, c2a, s, faz, rbaz;

    double glat1, glon1, glat2, glon2;

    // *** SOLUTION OF THE GEODETIC DIRECT PROBLEM AFTER T.VINCENTY
    // *** MODIFIED RAINSFORD'S METHOD WITH HELMERT'S ELLIPTICAL TERMS
    // *** EFFECTIVE IN ANY AZIMUTH AND AT ANY DISTANCE SHORT OF ANTIPODAL
    //
    // *** A IS THE SEMI-MAJOR AXIS OF THE REFERENCE ELLIPSOID
    // *** F IS THE FLATTENING OF THE REFERENCE ELLIPSOID
    // *** LATITUDES AND LONGITUDES IN RADIANS POSITIVE NORTH AND EAST
    // *** AZIMUTHS IN RADIANS CLOCKWISE FROM NORTH
    // *** GEODESIC DISTANCE S ASSUMED IN UNITS OF SEMI-MAJOR AXIS A
    //
    // *** PROGRAMMED FOR CDC-6600 BY LCDR L.PFEIFER NGS ROCKVILLE MD 20FEB75
    //
    // *** MODIFIED FOR SYSTEM 360 BY JOHN G GERGEN NGS ROCKVILLE MD 750608
    //
    // *** HACKED TO HELL AND GONE BY F2C (July-17-96 version) and by
    // *** Matt Reilly (KB1VC) to make it fit with the dem-gridlib routines.
    // *** Feb 24, 1997.
    // *** pounded on again by Matt Reilly (kb1vc) June 2019 for the GeoProfII
    // *** code

    // distance is in Km... */
    s = distance * 1000.0;
    faz = bearing * M_PI / 180.0;

    glat1 = this->lat * M_PI / 180.0;
    glon1 = this->lon * M_PI / 180.0;

    r = 1.0 - f;

    tu = r * sin(glat1) / cos(glat1);

    sf = sin(faz);
    cf = cos(faz);
    rbaz = 0.0;
    if (cf != 0.0) {
      rbaz = atan2(tu, cf) * 2.0;
    }

    cu = 1.0 / sqrt(tu * tu + 1.0);
    su = tu * cu;
    sa = cu * sf;
    c2a = -sa * sa + 1.0;
    x = sqrt((1.0 / r / r - 1.0) * c2a + 1.0) + (float)
      1.;
    x = (x - 2.0) / x;
    c = 1.0 - x;
    c = (x * x / 4.0 + 1) / c;
    d = (x * .375 * x - 1.0) * x;
    tu = s / r / a / c;
    y = tu;
  L100:
    sy = sin(y);
    cy = cos(y);
    cz = cos(rbaz + y);
    e = cz * cz * 2.0 - 1.0;
    c = y;
    x = e * cy;
    y = e + e - 1.0;
    y = (((sy * sy * 4.0 - 3.0) * y * cz * d / 6.0 + x) *
	 d / (float)4. - cz) * sy * d + tu;
    if (fabs(y - c) > eps) {
      goto L100;
    }
    rbaz = cu * cy * cf - su * sy;
    c = r * sqrt(sa * sa + rbaz * rbaz);
    d = su * cy + cu * sy * cf;
    glat2 = atan2(d, c);
    c = cu * cy - su * sy * cf;
    x = atan2(sy * sf, c);
    c = ((c2a * -3.0 + 4.0) * f + 4.0) * c2a *
      f / 16.0;
    d = ((e * cy * c + cz) * sy * c + y) * sa;
    glon2 = glon1 + x - (1.0 - c) * d * f;

    next.lat = glat2 * 180.0 / M_PI;
    next.lon = glon2 * 180.0 / M_PI;

    if(next.lon < 180.0) next.lon = 360.0 + next.lon;
    if(next.lon > 180.0) next.lon = next.lon - 360.0;
    return;

  }

  void Point::pt2Grid(std::string & grid) const {
    std::string ret(6, ' ');

    // First convert lat and lon to degrees and minutes.
    // Remember the basis is lon 180 is Grenwich, lat 0 is the pole
    // so we need to correct;
    double llon = lon + 180;
    double llat = lat + 90;

    int ilat = ((int) floor(llat));
    int ilat_mins = ((int) floor(60.0 * (llat - floor(llat))));

    int ilon = ((int) floor(llon));
    int ilon_mins = ((int) floor(60.0 * (llon - floor(llon))));

    // set lon positions
    ret[0] = 'A' + ((int) (ilon / 20));
    ilon = ilon % 20;
    ret[2] = '0' + (ilon / 2);
    ilon = ilon % 2;
    ret[4] = 'a' + ((ilon * 60 + ilon_mins) / 5);

    ret[1] = 'A' + ((int) (ilat / 10));
    ilat = ilat % 10;
    ret[3] = '0' + ilat;
    ret[5] = 'a' + ((ilat_mins * 2) / 5);

    grid = ret;

    return;
  }

  void Point::dms2Pt(double lat_d, double lat_m, double lat_s, char ns,
		      double lon_d, double lon_m, double lon_s, char ew) {
    lat = lat_d + (lat_m / 60.0) + (lat_s / 3600.0);
    if((ns == 'S') || (ns == 's')) lat = -1 * lat;

    lon = lon_d + (lon_m / 60.0) + (lon_s / 3600.0);
    if((ew == 'W') || (ew == 'w')) lon = -1 * lon;
  }

  double Point::gridDiff(char v, char s, double mul) const {
    return mul * ((double) (v - s));
  }

  void Point::grid2Pt(const std::string & grid) {
    if(!checkGrid(grid)) {
      throw std::runtime_error((boost::format("Bad grid specifier: [%s] is not in the proper form.") % grid).str());
    }

    std::string lgrid = grid;

    // upcase all of it... Is anybody embarrassed by the awkwardness here?
    std::transform(lgrid.begin(), lgrid.end(), lgrid.begin(), ::toupper);

    lon = gridDiff(lgrid[0], 'A', 20.0) +
      gridDiff(lgrid[2], '0', 2.0);

    if(lgrid.size() > 4) {
      lon += (gridDiff(lgrid[4], 'A', 5.0) + 2.5) / 60.0;
    }

    lon = lon - 180.0;

    lat = gridDiff(lgrid[1], 'A', 10.0) +
      gridDiff(lgrid[3], '0', 1.0);

    if(lgrid.size() > 5) {
      lat += (gridDiff(lgrid[5], 'A', 2.5) + 1.25) / 60.0;
    }

    lat = lat - 90.0; // correct for south pole being 0deg lat.
  }

  bool Point::checkGrid(const std::string & grid) const
  {
    //  verifies that the grid square is legitimate
    //  return true for a good grid.

    return regex_match(grid, grid_regexp);
  }

  double Point::atan2Pt(double y, double x) const {
    double retval;

    retval = atan2(y, x);
    if(retval < 0.0) retval = ((2.0 * M_PI) + retval);

    return retval;
  }

}


#include <iostream>
#include <iomanip>
#include <fstream>
#include <list>
#include <ctime>

#include <boost/numeric/ublas/io.hpp>

#include <flame/constants.h>
#include <flame/base.h>
#include <flame/moment.h>
#include <flame/chg_stripper.h>
#include <flame/state/vector.h>
#include <flame/state/matrix.h>

#include <flame/moment_sup.h>

typedef MomentState state_t;
typedef state_t::vector_t value_vec;
typedef state_t::matrix_t value_mat;

extern int glps_debug;

static
void PrtVec(const MomentState::vector_t &a)
{
    for (size_t k = 0; k < a.size(); k++)
        std::cout << std::scientific << std::setprecision(10)
                      << std::setw(18) << a[k];
    std::cout << "\n";
}

static
void PrtMat(const value_mat &M)
{
    for (size_t j = 0; j < M.size1(); j++) {
        for (size_t k = 0; k < M.size2(); k++)
            std::cout << std::scientific << std::setprecision(10)
                      << std::setw(18) << M(j, k);
        std::cout << "\n";
    }
}

void PrtState(const state_t& ST)
{
    MomentState::vector_t CenofChg, BeamRMS;

    if (true)
        for (size_t k = 0; k < ST.size(); k++) {
            std::cout << "\nState: "<<k<<" s = "<<std::fixed << std::setprecision(5) << ST.pos << "\n"
                      <<"\n Ref:  "<<ST.ref
                     <<"\n Real: "<<ST.real[k]
                       <<"\n moment0\n";
            PrtVec(ST.moment0[k]);
            std::cout << " moment1\n";
            PrtMat(ST.moment1[k]);
        }

    std::cout<<"\nCenter:        "<< std::scientific << std::setprecision(10)
             << std::setw(18)<<ST.moment0_env<<"\n";
    std::cout<<"RMS Beam Size: "<< std::scientific << std::setprecision(10)
             << std::setw(18)<<ST.moment0_rms<<"\n";
    PrtMat(ST.moment1_env);
}

static
void prt_initial_cond(Machine &sim,
                      state_t &ST)
{
    sim.propagate(&ST, 0, 1);
    PrtState(ST);
}

static
void PrtOut(std::ofstream &outf1, std::ofstream &outf2, std::ofstream &outf3, const state_t& ST)
{
    outf1 << std::scientific << std::setprecision(14) << std::setw(22) << ST.pos;
    for (size_t j = 0; j < ST.size(); j++)
        for (int k = 0; k < 6; k++)
            outf1 << std::scientific << std::setprecision(14) << std::setw(22) << ST.moment0[j][k];
    for (int k = 0; k < 6; k++)
        outf1 << std::scientific << std::setprecision(14) << std::setw(22) << ST.moment0_env[k];
    outf1 << "\n";

    outf2 << std::scientific << std::setprecision(14) << std::setw(22) << ST.pos;
    for (size_t j = 0; j < ST.size(); j++)
        for (int k = 0; k < 6; k++)
            outf2 << std::scientific << std::setprecision(14) << std::setw(22) << sqrt(ST.moment1[j](k, k));
    for (int k = 0; k < 6; k++)
        outf2 << std::scientific << std::setprecision(14) << std::setw(22) << sqrt(ST.moment1_env(k, k));
    outf2 << "\n";

    outf3 << std::scientific << std::setprecision(14)
          << std::setw(22) << ST.pos << std::setw(22) << ST.ref.phis << std::setw(22) << ST.ref.IonEk << "\n";
}

static
void propagate(const Config &conf)
{
    // Propagate element-by-element for each charge state.
    Machine                  sim(conf);
    std::auto_ptr<StateBase> state(sim.allocState());
    state_t                  *StatePtr = dynamic_cast<state_t*>(state.get());
    std::ofstream            outf1, outf2, outf3;

    if(!StatePtr) throw std::runtime_error("Only sim_type MomentMatrix is supported");

    outf1.open("moment0.txt",   std::ios::out);
    outf2.open("moment1.txt",   std::ios::out);
    outf3.open("ref_orbit.txt", std::ios::out);

    prt_initial_cond(sim, *StatePtr);

    clock_t tStamp[2];

    tStamp[0] = clock();

    prt_initial_cond(sim, *StatePtr);

    Machine::iterator it = sim.begin()+1;
    while (it != sim.end()) {
        ElementVoid* elem = *it;

        elem->advance(*state);
        ++it;

//        PrtOut(outf1, outf2, outf3, *StatePtr);

//        PrtState(*StatePtr);
    }

    outf1.close();
    outf2.close();
    outf3.close();

    tStamp[1] = clock();

    PrtState(*StatePtr);

    std::cout << std::fixed << std::setprecision(5)
              << "\npropagate: " << double(tStamp[1]-tStamp[0])/CLOCKS_PER_SEC << " sec" << "\n";
}

int main(int argc, char *argv[])
{
    try {
        std::auto_ptr<Config> conf;

        if(argc>2)

        glps_debug = 0; // 0 or 1.

        try {
            GLPSParser P;
            conf.reset(P.parse_file(argc>1 ? argv[1] : NULL));
            fprintf(stderr, "Parsing succeeds\n");
        } catch(std::exception& e) {
            fprintf(stderr, "Parse error: %s\n", e.what());
            return 1;
        }

//        std::cout<<"# Reduced lattice\n";
//        GLPSPrint(std::cout, *conf);
//        std::cout<<"\n";

        registerMoment();

        propagate(*conf);

        return 0;
    } catch(std::exception& e) {
        std::cerr << "Main exception: " << e.what() << "\n";
        Machine::registeryCleanup();
        return 1;
    }
}