/** This is free and unencumbered software released into the public domain.

The authors of ISIS do not claim copyright on the contents of this file.
For more details about the LICENSE terms and the AUTHORS, you will
find files of those names at the top level of this repository. **/

/* SPDX-License-Identifier: CC0-1.0 */

#include "Isis.h"

#include <QFile>
#include <QtGlobal>
#include <QTextStream>
#include <QScopedPointer>
#include <QVector>

// boost library
#include <boost/foreach.hpp>

#include "nanoflann.hpp"

#include "ControlNet.h"
#include "ControlPointCloudPt.h"
#include "DatumFunctoid.h"
#include "FileList.h"
#include "FileName.h"
#include "IString.h"
#include "Latitude.h"
#include "Longitude.h"
#include "MapPointCollector.h"
#include "PointCloud.h"
#include "PointCloudTree.h"
#include "ProcessByLine.h"
#include "Progress.h"
#include "ProjectionFactory.h"
#include "Projection.h"
#include "SerialNumber.h"
#include "SpecialPixel.h"
#include "SurfacePoint.h"
#include "IException.h"
#include "TProjection.h"

using namespace std;
using namespace Isis;

// Control network manager
typedef QList<QSharedPointer<ControlNet> >    ControlNetList;

/**
 * Functor for normalized 3D-to-2D Euclidean distances
 *
 * @author 2015-03-14 Kris Becker
 *
 * @internal
 *   @history 2015-03-14 Kris Becker - Original version.
 */
template <class T> class Dist3Dto2D {
public:
  enum { Dimension = 3 };  // Needs all three coordinates
  Dist3Dto2D()  : m_znorm(1.0) { }
  Dist3Dto2D(const double znorm) : m_znorm(znorm) {  }
  ~Dist3Dto2D() { }

  inline int dimension() const {
    return ( Dimension );
  }

  double getZNorm() const {
    return ( m_znorm );
  }

  void setZNorm(const double znorm = 1.0) {
    m_znorm = znorm;
    return;
  }

  inline double operator()(const T &datum1, const T &datum2) const {
    return ( normalize(datum1.x(), datum1.y(), datum1.z(),
                       datum2.x(), datum2.y(), datum2.z()) );
  }

  inline double operator()(const double *datum1, const T &datum2) const {
    return ( normalize(datum1[0], datum1[1], datum1[2],
                      datum2.x(), datum2.y(), datum2.z()) );
  }

  inline double normalize(const double dx1, const double dy1, const double dz1,
                          const double dx2, const double dy2, const double dz2)
                          const {
    double scale = m_znorm / radius(dx2, dy2, dz2);
    double nx = dx2 * scale;
    double ny = dy2 * scale;
    double nz = dz2 * scale;
    return ( distance(dx1, dy1, dz1, nx, ny, nz) );
  }

  inline double distance(const double dx1, const double dy1, const double dz1,
                         const double dx2, const double dy2, const double dz2)
                         const {
    double dx = dx1 - dx2;
    double dy = dy1 - dy2;
    double dz = dz1 - dz2;
    return ( dx*dx + dy*dy + dz*dz );
  }

  inline double radius(const double dx, const double dy, const double dz) const {
    double v1max = qMax(qAbs(dx), qMax(qAbs(dy), qAbs(dz)));

    // We're done if its the zero vector
    if ( qFuzzyCompare(v1max+1.0, 1.0) ) { return ( 0.0 ); }

    // Compute magnitude of the vector
    double tmp0( dx / v1max );
    double tmp1( dy / v1max );
    double tmp2( dz / v1max );
    double normsqr = tmp0*tmp0 + tmp1*tmp1 + tmp2*tmp2;
    return ( v1max * std::sqrt(normsqr) );
  }

  private:
    double m_znorm;   // Normalization radius
};

// ControlNet point cloud types. Dist3d distance works for all types.
typedef ControlPointCloudPt             PointType;
typedef Dist3Dto2D<ControlPointCloudPt> DistanceType;

// Point cloud/kd-tree template definitions
typedef PointCloud<PointType, DistanceType>             CNetPointCloud;
typedef PointCloudTree<PointType, DistanceType>         CNetPointCloudTree;
typedef PointCloudSearchResult<PointType, DistanceType> ResultType;

void IsisMain() {

  // We will be processing by line
  ProcessByLine p;
  UserInterface &ui = Application::GetUserInterface();

  // Now parse the input datum string to determine the bands of the output
  // cube. Note the order of the bands will be as the user has specified them.
  QString algorithm = ui.GetString("ALGORITHM");
  DatumFunctoidFactory *dfactory = DatumFunctoidFactory::getInstance();

//  QStringList functions = dfactory->algorithms();
//  std::cout << "\nList of Algorithms\n" << functions.join("\n") << "\n";

  DatumFunctoidList functors = dfactory->create(algorithm);

  QStringList cnetfiles;
  if ( ui.WasEntered("CNET") ) {
    cnetfiles.append(ui.GetAsString("CNET"));
  }

  if ( ui.WasEntered("CNETLIST") ) {
    FileList list_o_nets(ui.GetFileName("CNETLIST"));
    BOOST_FOREACH ( FileName cfile, list_o_nets ) {
      cnetfiles.append( cfile.original() );
    }
  }

  // Ok, if we end up with one net, it must have been entered in CNETBASE.
  if ( (cnetfiles.size() < 1) ) {
    QString mess = "Must enter a control net inc CNET or a list in CNETLIST";
    throw IException(IException::User, mess, _FILEINFO_);
  }
  // Create the point cloud container and load the control networks
  QScopedPointer<CNetPointCloud> cloud(new CNetPointCloud());

  // Collect some stuff from input nets for the output net
  QString netid;
  QString target;
  QString description;
  BigInt allPoints(0);
  BOOST_FOREACH ( QString cfile, cnetfiles ) {
    std::cout << "\nLoading " << cfile << "...\n";
    Progress c_progress;
    QScopedPointer<ControlNet> cnet( new ControlNet(cfile, &c_progress) );
    if ( netid.isEmpty() )       { netid = cnet->GetNetworkId(); }
    if ( target.isEmpty() )      { target = cnet->GetTarget(); }
    if ( description.isEmpty() ) { description = cnet->Description(); }

    // Get all control points by taking ownership from the control net
    int npoints(0);
    QList<ControlPoint *> points = cnet->take();
    BOOST_FOREACH ( ControlPoint *point, points) {
      ControlPointCloudPt cpt(point, ControlPointCloudPt::Ground,
                              ControlPointCloudPt::Exclusive);
      if ( cpt.isValid() ) {
        cloud->addPoint(cpt);
        npoints++;
      }
    }
    std::cout << "Added " << npoints << " of " << points.size() << "\n";
    allPoints += points.size();

    // Instead of having to save the ControlNet instances, we take ownership
    // of all the points from ControlNet and turn it over to the cloud...
    //
    //  cnetlist.append( QSharedPointer<ControlNet> ( cnet.take() ) );
  }
  std::cout << "\nTotal " << cloud->size() << " of " << allPoints << "\n";

  std::cout << "\nCreating output DEM to determine 3-D normalization...\n";
  //Get the map projection file provided by the user
  Pvl userMap;
  userMap.read(ui.GetFileName("MAP"));
  // PvlGroup &mapGrp = userMap.FindGroup("Mapping", Pvl::Traverse);

  int csamps, clines;
  Projection *proj = ProjectionFactory::CreateForCube(userMap, csamps, clines, true);
  PvlGroup cleanMap = proj->Mapping();
  Cube *ocube = p.SetOutputCube( "TO", csamps, clines, functors.size() );

  ocube->putGroup(cleanMap);
  delete proj;
  TProjection *tproj = (TProjection *) ocube->projection();
  double znorm = tproj->LocalRadius(tproj->TrueScaleLatitude());
  std::cout << "3D Normalization: " << znorm << "\n";

  std::cout << "\nCreating cloud kd-tree..\n";
  int kd_nodes = ui.GetInteger("KDNODES");
  cloud->getDistance().setZNorm(znorm);
  CNetPointCloudTree cloud_t( cloud.take(), kd_nodes );
  std::cout << "Done...\n";

  //  Set trimming option
  bool trim = ui.GetBoolean("TRIM");

  //  Set up efficient test variables
  QString search_type = ui.GetString("SEARCH").toLower();
  bool both_searches =   ("both" == search_type);
  bool radial_search =   ( both_searches || ( "radial" == search_type) );
 // bool neighbor_search = ( both_searches || ( "neighbors" == search_type ) );

  double search_radius(Null);
  double search_radius_sq(Null);

  // Only worry about this if a range search is requested
  if ( radial_search) {
    if (ui.WasEntered("DISTANCE")) {
      search_radius = ui.GetDouble("DISTANCE");
    }
    else {
      // Compute range from center of pixel to corner in meters
      double res = tproj->Resolution();
      double half_res_sq  = (res / 2.0) * (res / 2.0);
      search_radius = sqrt(half_res_sq + half_res_sq);
      cout << "Search RANGE computed from Map Resolution: " << search_radius
           << " <meters>\n";
    }
    search_radius_sq = search_radius * search_radius;
  }

  // Determine search criteria
  int neighbors = ui.GetInteger("NEIGHBORS");
  int minpoints = ui.GetInteger("MINPOINTS");

  // Now determine if radius noise filtering is requested
  bool do_radius_filter = ui.WasEntered("SIGMARADIUS");
  double sigma = 999.0;
  if ( do_radius_filter ) {
    sigma = ui.GetDouble("SIGMARADIUS");
  }
  // Get the real tile sizes and allocate the buffer accordingly
  PvlObject &icube = ocube->label()->findObject("IsisCube");
  PvlObject &core = icube.findObject("Core");
  int tsamps = core["TileSamples"];
  int tlines = core["TileLines"];
  Brick tile(*ocube, tsamps, tlines, functors.size() );
  int bOffset = tsamps * tlines;  // Index offset to next band (spectrum) value

  Progress mapper;
  mapper.SetText("mapping");
  mapper.SetMaximumSteps(tile.Bricks());
  mapper.CheckStatus();

 //  Process data using 3-D brick
  int npixels = tsamps * tlines;
  int nbands = tile.BandDimension();
  SurfacePoint point;

  for ( int brick = 1 ; brick <= tile.Bricks() ; brick++ ) {
    tile.SetBrick(brick);

    // Tile processing...
    for ( int index = 0 ; index < npixels ; index++ ) {

      int samp, line, band;
      tile.Position(index, samp, line, band);

      // Intialize output spectrum to NULLs
      QVector<double> datum(nbands, Null);

      //  Map only valid projection translation
      if ( (samp <= csamps) && (line <= clines) &&
           ( tproj->SetWorld(samp, line) ) ) {
#if 0
        std::cout << "Line: " << line << "  Sample: " << samp << "\n";
        std::cout << "Lat:  " << tproj->UniversalLatitude()
                  << " Long: " << tproj->UniversalLongitude() << "\n";
#endif

        // Trim if requested
        bool mapit(true);
        if ( ( trim ) && ( tproj->HasGroundRange() ) ) {
          if ( tproj->Latitude()  < tproj->MinimumLatitude()  ) mapit = false;
          if ( tproj->Latitude()  > tproj->MaximumLatitude()  ) mapit = false;
          if ( tproj->Longitude() < tproj->MinimumLongitude() ) mapit = false;
          if ( tproj->Longitude() > tproj->MaximumLongitude() ) mapit = false;
        }

        // Plot it only if its within mapping boundary conditions
        if ( mapit ) {
          double lat = tproj->UniversalLatitude();
          double lon = tproj->UniversalLongitude();
          double radius = tproj->LocalRadius(lat);

          point.SetSphericalCoordinates(Latitude(lat, Angle::Degrees),
                                        Longitude(lon, Angle::Degrees),
                                        Distance(radius, Distance::Meters));

          // Search the PC cloud
          ControlPoint pt;
          pt.SetAprioriSurfacePoint(point);
          ControlPointCloudPt cpt(&pt, ControlPointCloudPt::Ground,
                                  ControlPointCloudPt::Shared,
                                  "MapPoint");

          // There are several combinations to consider
          //    1) RADIAL search from RANGE <meters> at the lat/lon pixel center
          //    2) NEIGHBOR search selecting the NEIGHBORS closest to the center
          //    3) BOTH searches requested will apply the RADIAL search first, then
          //        and only if MINPOINTS points resulting from the RADIAL
          //        search are within RANGE <meters>, otherwise a NEIGHBOR
          //        search is performed.
          ResultType results;
          if ( both_searches ) {
            results = cloud_t.radius_query(cpt, search_radius_sq);
            if ( minpoints > results.size() ) {
              results = cloud_t.neighbor_query(cpt, neighbors);
            }
          }
          else if ( radial_search ) {
            results = cloud_t.radius_query(cpt, search_radius_sq);
          }
          else {  // ( neighbor_search == search_type)
            results = cloud_t.neighbor_query(cpt, neighbors);
          }

          // Extract points and prepare for processing
          MapPointCollector mpoint;
          if ( ResultType::Radius == results.type() ) mpoint.setSearchType(MapPointCollector::Radius);
          else                                        mpoint.setSearchType(MapPointCollector::NearestNeighbor);

          // Extract point set and optionally apply noise filter
          results.forEachPair(mpoint);
          if ( do_radius_filter ) {  mpoint.removeNoise(sigma); }

          // Compute values for each functor
          for ( int i = 0 ; i < functors.size() ; i++) {
            datum[i] = functors[i]->value(mpoint);
          }
        }
      }

      // Copy data values to output data brick
      int ndx = index;
      for (int v = 0 ; v < datum.size() ; v++) {
        tile[ndx] = datum[v];
        ndx += bOffset;
      }
    }

    ocube->write(tile);
    mapper.CheckStatus();
  }

  PvlKeyword fname("Name");
  PvlKeyword cnumber("Number");
  for (int i = 0 ; i < functors.size() ; i++) {
    fname.addValue(functors[i]->name());
    cnumber.addValue(QString::number(i+1));
  }

  // Create the BandBin group
  PvlGroup bbin("BandBin");
  bbin += fname;
  bbin += cnumber;

  // Dup the Number for Center and Width
  cnumber.setName("Center");
  bbin += cnumber;
  cnumber.setName("Width");
  bbin += cnumber;

  // Write BandBin
  icube.addGroup(bbin);

  p.EndProcess();
}


/*
Copyright (c)  Hemanth Kapila
All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

1) Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
2) Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
The name of the Hemanth Kapila may NOT be used to endorse or promote products derived from this software without specific prior written permission.
3) 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 "QInternal.h"
#include <boost/lexical_cast.hpp>
#include <iostream>
#include <sstream>

namespace Quarry {

    // The below code is messy. It needs to be improved. But later on.
    // Following is the suggested approach -
    // Let the lexer just scan and return only the relevant number of bytes + some metadata
    // The C++ guy takes the metadata and the length and creates an instance of appropriate class.
    // The C flow will just consume the bytes, ignoring the metadata.
    // But all that for a later date . TODO
    template<typename numeric>
    static numeric toNumber(unsigned char c) {
	numeric v = 0;
	if( c >= '0' && c <= '9') {
	    v = (numeric)(c - '0');
	}
	else if (c >= 'A' && c <= 'F') {
	    v = (numeric)(10 + (c - 'A'));
	}
	else{
	    v = (numeric)(10 + (c - 'a'));
	}
	//std::cout << c << " = " << v << " ";
	return v;
    }

    template<typename T, typename numeric>
    static Token * tokenOrError(QReader &reader, std::string &text, const unsigned char *begin, std::size_t initPos) {
	reader.addColumns(text.length());
	std::istringstream iss(text);
	numeric v;
	if (iss >> v){
	    return new T(reader.getLine(), reader.getCol(), v, (reader.currPosition() - initPos), begin);
	}
	return new ErrorToken(reader.getLine(), reader.getCol(), "Invalid Numerical Constant");
    }

    template<typename T, typename numeric>
    static Token * tokenOrError(QReader &reader, std::string &text, int base, int pos, const unsigned char *begin, std::size_t initPos) {
	reader.addColumns(text.length());
	std::istringstream iss(text);
	numeric v = 0;
	int len = text.length();
	for (int p = pos; p < text.length(); p++){
	    if((text[p] == 'u') || (text[p] == 'U') || (text[p] == 'l') || (text[p] == 'L'))
		break;
	    v = (v * base) + toNumber<numeric>(text[p]);
	}
	return new T(reader.getLine(), reader.getCol(), v, (reader.currPosition() - initPos), begin);
    }

    static bool isDigit(unsigned char c) {
	return (c >= '0' && c <= '9');
    }

    static Token *appendMantissa(std::string &text, QReader &reader, const unsigned char *begin, std::size_t initPos)
    {
	reader.appendWhile(isDigit, text);
	if( reader.hasMore() && ((reader.peekNext() == 'E') || (reader.peekNext() == 'e'))) {
	    reader.next();
	    text.push_back('E');
	    if(reader.hasMore() &&( ( reader.peekNext() == '+') || (reader.peekNext() == '-')) ) {
		text.push_back(reader.next());
	    }

	    reader.appendWhile(isDigit, text);
	}

	if(reader.hasMore() && ((reader.peekNext() == 'f') || (reader.peekNext() == 'F'))) {
	    text.push_back(reader.next());
	    return tokenOrError<FlToken, float>(reader, text, begin, initPos);
	}
	if(reader.hasMore() && ((reader.peekNext() == 'l') || (reader.peekNext() == 'L'))) {
	    text.push_back(reader.next());
	    return tokenOrError<LDblToken, long double>(reader, text, begin, initPos);
	}

	return tokenOrError<DblToken, double>(reader, text, begin, initPos);
    }

    static Token *tryOptionalU(std::string &text, QReader &reader,const unsigned char *begin, std::size_t initPos, int base = 10, int pos = 0) {
	if(reader.hasMore() && ((reader.peekNext() == 'u') || (reader.peekNext() == 'U'))) {
	    reader.next();
	    text.push_back('U');

	    if(reader.hasMore() && ((reader.peekNext() == 'l') || (reader.peekNext() == 'L'))) {
		text.push_back('L');
		reader.next();

		if (reader.hasMore() && ((reader.peekNext() == 'l') || (reader.peekNext() == 'L'))) {
		    text.push_back('L');
		    reader.next();
		    return tokenOrError<ULongLongToken, unsigned long long>(reader, text, base, pos,begin, initPos);
		}

		return tokenOrError<ULongToken, unsigned long>(reader, text, base, pos, begin,initPos);
	    }

	    return tokenOrError<UIntToken, unsigned int>(reader, text, base, pos, begin, initPos);
	}

	return nullptr;
    }

    static Token *tryOptionalLL(std::string &text, QReader &reader, const unsigned char *begin, std::size_t initPos, int base = 10, int pos = 0) {
	bool isUnsigned = false;
	bool isLongLong = false;
	bool isLong  = false;

	if(reader.hasMore() && ((reader.peekNext() == 'l') || (reader.peekNext() == 'L'))) {
	    text.push_back('L');
	    reader.next();
	    isLong = true;
	    if (reader.hasMore() && ((reader.peekNext() == 'l') || (reader.peekNext() == 'L'))) {
		text.push_back('L');
		reader.next();
		isLongLong = true;
	    }
	    if(reader.hasMore() && ((reader.peekNext() == 'u') || (reader.peekNext() == 'U'))) {
		reader.next();
		text.push_back('U');
		isUnsigned = true;
	    }
	}

	if (isLong) {
	    if (isLongLong) {
		if (isUnsigned)
		    return tokenOrError<ULongLongToken, unsigned long long>(reader, text, base, pos, begin, initPos);
		else
		    return tokenOrError<LongLongToken, long long>(reader, text, base, pos, begin, initPos);
	    }
	    if (isUnsigned)
		return tokenOrError<ULongToken, unsigned long>(reader, text, base, pos, begin, initPos);

	    return tokenOrError<LongToken, long>(reader, text,base, pos, begin, initPos);
	}

	return tryOptionalU(text, reader, begin, initPos, base, pos);
    }

    Token* numberLexer(QReader &reader, QContext &context)
    {
	const unsigned char *begin = reader.current();
	std::size_t initPos = reader.currPosition();
	auto c = reader.next();

	std::string text;
	text.push_back(c);
	if (c == '-' && (!reader.hasMore() || (reader.peekNext() < '0') || (reader.peekNext() > '9'))) {
            return new Operator(reader.getLine(), reader.getCol(), context.operatorIndex(text), 1, begin);
	}

	if (c != '0') {
	    reader.appendWhile(isDigit, text);

	    auto ullPtr = tryOptionalLL(text,reader, begin, initPos);
	    if (ullPtr != nullptr) {
		return ullPtr;
	    }
	    else if(reader.hasMore() && ((reader.peekNext() == 'f') || (reader.peekNext() == 'f'))) {
		reader.next();
		return tokenOrError<FlToken,float>(reader, text, begin, initPos);
	    }else if(reader.hasMore() && ((reader.peekNext() == 'd') || (reader.peekNext() == 'D'))) {
		reader.next();
		return tokenOrError<DblToken, double>(reader, text, begin, initPos);
	    }else if(reader.hasMore() && (reader.peekNext() == '.')) {
		text.push_back(reader.next());
		return appendMantissa(text, reader, begin, initPos);
	    }

	    return tokenOrError<IntToken,int>(reader, text, begin, initPos);
	}
	if (reader.hasMore() && (reader.peekNext() == '.')){
	    text.push_back(reader.next());
	    return appendMantissa(text, reader, begin, initPos);
	}
	int base = 10;
	int pos  = 0;
	if (reader.hasMore() && ((reader.peekNext() == 'x') || (reader.peekNext() == 'X'))) {
	    text.push_back(reader.next());
	    base = 16;
	    pos = 2;
	    while(reader.hasMore() &&
		  (
		   ((reader.peekNext() >= '0') && (reader.peekNext() <= '9')) ||
		   ((reader.peekNext() >= 'A') && (reader.peekNext() <= 'F')) ||
		   ((reader.peekNext() >= 'a') && (reader.peekNext() <= 'f')) )) {
		text.push_back(reader.next());
	    }
	}
	else if(reader.hasMore() && ((reader.peekNext() == 'b') || (reader.peekNext() == 'B'))) {
	    text.push_back(reader.next());
	    base = 2;
	    pos = 2;
	    while(reader.hasMore() && ((reader.peekNext() == '0') || (reader.peekNext() == '1'))) {
		text.push_back(reader.next());
	    }
	}
	else{
	    base = 8;
	    pos = 1;
	    while(reader.hasMore() && ((reader.peekNext() >= '0') && (reader.peekNext() <= '7'))) {
		text.push_back(reader.next());
	    }
	}

	auto ullPtr = tryOptionalLL(text, reader, begin, initPos,base, pos);
	if (ullPtr != nullptr) {
	    return ullPtr;
	}

	return tokenOrError<IntToken,int>(reader, text, base, pos, begin, initPos);
    }
}


// Author(s): Gijs Kant
// Copyright: see the accompanying file COPYING or copy at
// https://svn.win.tue.nl/trac/MCRL2/browser/trunk/COPYING
//
// 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)
//
/// \file quotienting_test.cpp
/// \brief Add your file description here.

#include <iostream>
#include <iterator>
#include <boost/test/included/unit_test_framework.hpp>
#include "mcrl2/utilities/test_utilities.h"
#include "mcrl2/lps/parse.h"
#include "mcrl2/modal_formula/quotienting.h"
#include "mcrl2/modal_formula/parse.h"

using namespace mcrl2;

BOOST_AUTO_TEST_CASE(test_quotienting)
{
  const std::string LPS1 =
    "act a, b, c, d, e : Nat;                \n"
    "                                        \n"
    "proc P1(n:Nat) = true -> a(n) . P1(n);  \n"
    "                                        \n"
    "init P1(1);                             \n"
    ;

  const std::string LPS2 =
    "act a, b, c, d, e : Nat;                \n"
    "                                        \n"
    "proc P2(m:Nat) = true -> b(m) . P2(m+1) \n"
    "               + true -> e(m) . P2(1);  \n"
    "                                        \n"
    "init P2(1);                             \n"
    ;

  lps::specification spec1 = lps::parse_linear_process_specification(LPS1);
  lps::specification spec2 = lps::parse_linear_process_specification(LPS2);

  std::clog << "spec1: " << pp(spec1) << std::endl;
  std::clog << "spec2: " << pp(spec2) << std::endl;

  lps::synchronization_vector v(2);
  process::action_label_list labels = process::parse_action_declaration("c:Nat;");
  process::action_label label = labels.front();
  v.add_vector(std::vector<std::string> {"a", "b"}, label);
  v.add_vector(std::vector<std::string> {"d", "e"}, label);

  std::clog << "synchronization vector: " << pp(v) << std::endl;

  state_formulas::state_formula formula = state_formulas::parse_state_formula("nu X. exists j:Nat . <c(j)>X", spec1);

  std::clog << "formula: " << pp(formula) << std::endl;

  std::clog << "quotienting formula..." << std::endl;
  //state_formulas::state_formula q_formula = state_formulas::algorithms::quotient(formula, spec1, v, 0);
  state_formulas::quotient_builder f(spec1, v, 0);
  state_formulas::state_formula q_formula = f.apply(formula);
  process::label_generator label_generator = f.label_generator();
  for(auto it = label_generator.generated_labels().begin(); it != label_generator.generated_labels().end(); ++it)
  {
    spec1.action_labels().push_front(*it);
  }

  std::clog << "result: " << pp(q_formula) << std::endl;

  std::string q_formula_text = pp(q_formula);
  state_formulas::state_formula formula2 = state_formulas::parse_state_formula(q_formula_text, spec1);
  BOOST_CHECK(formula2 == q_formula);

  BOOST_CHECK(true);
}

boost::unit_test::test_suite* init_unit_test_suite(int argc, char* argv[])
{
  return 0;
}


// Author(s): Gijs Kant
// Copyright: see the accompanying file COPYING or copy at
// https://svn.win.tue.nl/trac/MCRL2/browser/trunk/COPYING
//
// 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)
//
/// \file ppg_rewriter_test.cpp
/// \brief Test for the ppg rewriter.

#include <iostream>
#include <boost/test/minimal.hpp>
#include "mcrl2/pbes/normalize.h"
#include "mcrl2/pbes/txt2pbes.h"
#include "mcrl2/pbes/rewriter.h"
#include "mcrl2/pbes/detail/ppg_visitor.h"
#include "mcrl2/pbes/detail/bqnf2ppg_rewriter.h"
#include "mcrl2/pbes/detail/bqnf_traverser.h"
#include "mcrl2/pbes/detail/ppg_traverser.h"
#include "mcrl2/pbes/detail/ppg_rewriter.h"

using namespace mcrl2;
using namespace mcrl2::pbes_system;

void rewrite_ppg(const std::string& bqnf_text, const std::string& ppg_text)
{
  std::clog << "rewrite_ppg" << std::endl;
  std::clog << "Parsing text..." << std::endl;
  //std::clog << bqnf_text << std::endl;
  pbes p = txt2pbes(bqnf_text);
  std::clog << "done." << std::endl;

  bool is_bqnf = pbes_system::detail::is_bqnf(p);
  std::clog << "bqnf_traverser says: p is " << (is_bqnf ? "" : "NOT ") << "in BQNF." << std::endl;
  bool is_ppg = pbes_system::detail::is_ppg(p);
  std::clog << "ppg_traverser says: p is " << (is_ppg ? "" : "NOT ") << "a PPG." << std::endl;
  std::clog << "Try the new rewriter:" << std::endl;
  pbes q = pbes_system::detail::to_ppg(p);
  std::clog << "The new rewriter is done." << std::endl;
  //std::clog << "result:" << std::endl << pbes_system::pp(q) << std::endl << std::endl;
  is_ppg = pbes_system::detail::is_ppg(q);
  std::clog << "ppg_traverser says: result is " << (is_ppg ? "" : "NOT ") << "a PPG." << std::endl;
  p = q;
  normalize(p);
  std::clog << "Parsing text..." << std::endl;
  pbes ppg = txt2pbes(ppg_text);
  normalize(ppg);
  std::clog << "Checking for equality..." << std::endl;
  if (!(p==ppg))
  {
    std::clog << "target:" << std::endl << ppg_text << std::endl << std::endl;
    std::clog << "result:" << std::endl << pbes_system::pp(p) << std::endl;
  }
  BOOST_CHECK(p==ppg);
  std::clog << "done." << std::endl;
}

void test_ppg_rewriter()
{
  // buffer.always_send_and_receive
  std::string bqnf_text =
      "sort D = struct d1 | d2;\n"
      "map  N: Pos;\n"
      "eqn  N  =  2;\n"
      "pbes nu X(q_Buffer: List(D)) =\n"
      "  (exists d: D. (val(#q_Buffer < 2) && X(q_Buffer <| d))) && (exists d: D. val(head(q_Buffer) == d) && val(!(q_Buffer == [])) && X(tail(q_Buffer)));\n"
      "init X([]);"
  ;
  std::string ppg_text =
      "sort D = struct d1 | d2;\n"
      "map  N: Pos;\n"
      "eqn  N  =  2;\n"
      "pbes nu X_1(q_Buffer: List(D)) =\n"
      "  exists d: D. val(#q_Buffer < 2) && X(q_Buffer <| d);\n"
      "nu X_2(q_Buffer: List(D)) =\n"
      "  exists d: D. val(head(q_Buffer) == d) && val(!(q_Buffer == [])) && X(tail(q_Buffer));\n"
      "nu X(q_Buffer: List(D)) =\n"
      "  X_1(q_Buffer) && X_2(q_Buffer);\n"
      "init X([]);"
  ;
  rewrite_ppg(bqnf_text, ppg_text);
}

int test_main(int argc, char* argv[])
{
  test_ppg_rewriter();

  return 0;
}


#include <boost/test/unit_test.hpp>

#include <string>
#include <vector>

#include "key.h"
#include "base58.h"
#include "uint256.h"
#include "util.h"

using namespace std;

// BlockShares keys
static const string str8Secret1     ("6AB5RJLZnaEj6R2V8vpnN2UP9PJgMH1oifZCS2duhW4KxHceNee");
static const string str8Secret2     ("69zWmgDULAixiNHWizXbNYdtAUCR5FYpSXj1EYUNzjPPmPAUYem");
static const string str8Secret1C    ("PmVUWFXMi9hanu3ZBzZuDoVrk1T2gVgZCuB7TUDubmFgeqbNPhXL");
static const string str8Secret2C    ("PkgrQC63dbvCuawtCLv2i6dnpo3PFmDSPgcQ2mur5p5qUbUoEfyH");

// BlockCredits keys
static const string strCSecret1     ("6C7pbXnbdzuxEEcC3WSnKM5A4VvPzyrgWJdJgBKohA4iSsjFoWx");
static const string strCSecret2     ("6BwFwufWBbQBrBsDda9bKsEf5ap8ixPhEAo7UhAGzPPnFyKZ4yc");
static const string strCSecret1C    ("Pv575QtFtJcimR4Kk4xo1ybLzSegWuxHNLoF1va16F1PwAhBCTtx");
static const string strCSecret2C    ("PuGUyMSwokqLt6xekRJvWGjH5EF36BVAZ8EXbEFwaHqYkveSG4nK");

// The resulting BKS addresses for the above keys
static const CBitcoinAddress addr81 ("8ZJqrcmJCfNb9DVudDDWBduKRpnLqca5vq");
static const CBitcoinAddress addr82 ("8VeSFyegJHg2bUBpnwpN14akHm53knrSpV");
static const CBitcoinAddress addr81C("8Z6Y2kXWmci4wWFg9FW9xC9hxTjs72bwyS");
static const CBitcoinAddress addr82C("8S5p4yC31f1qL3KUiqSPaRgE5eZt5C1muQ");

// The resulting BKC addresses for the above keys
static const CBitcoinAddress addrC1 ("CagshhkBJU1MKYtmsQYh2tdBisMmu6GRwy");
static const CBitcoinAddress addrC2 ("CX2U74dZQ6Jnmoah399YrKJcaoeUmzxr6P");
static const CBitcoinAddress addrC1C("CaUZsqWPsRLq7qeYPSqLoSsaFWKJDzgqXF");
static const CBitcoinAddress addrC2C("CTTqv4Av7TebWNiLy2maRgQ6Nh9KCEqZWD");

static const string strAddressBad("1HV9Lc3sNHZxwj4Zk6fB38tEmBryq2cBiF");

#ifdef KEY_TESTS_DUMPINFO
void dumpKeyInfo(uint256 privkey)
{
    CSecret secret;
    secret.resize(32);
    memcpy(&secret[0], &privkey, 32);
    vector<unsigned char> sec;
    sec.resize(32);
    memcpy(&sec[0], &secret[0], 32);
    printf("  * secret (hex): %s\n", HexStr(sec).c_str());

    for (int nCompressed=0; nCompressed<2; nCompressed++)
    {
        bool fCompressed = nCompressed == 1;
        printf("  * %s:\n", fCompressed ? "compressed" : "uncompressed");
        CBitcoinSecret bsecret;
        bsecret.SetSecret(secret, fCompressed, '8');
        printf("    * secret (base58, BKS): %s\n", bsecret.ToString().c_str());
        bsecret.SetSecret(secret, fCompressed, 'C');
        printf("    * secret (base58, BKC): %s\n", bsecret.ToString().c_str());
        CKey key;
        key.SetSecret(secret, fCompressed);
        CPubKey pubKey = key.GetPubKey();
        printf("    * pubkey (hex): %s\n", HexStr(pubKey.Raw()).c_str());
        printf("    * address (base58, BKS): %s\n", CBitcoinAddress(pubKey.GetID(), '8').ToString().c_str());
        printf("    * address (base58, BKC): %s\n", CBitcoinAddress(pubKey.GetID(), 'C').ToString().c_str());
    }
}
#endif

BOOST_AUTO_TEST_SUITE(key_tests)

BOOST_AUTO_TEST_CASE(key_test_8)
{
    /*
    dumpKeyInfo(uint256(1000));
    dumpKeyInfo(uint256(2000));
    */
    CBitcoinSecret bsecret1, bsecret2, bsecret1C, bsecret2C, baddress1;
    BOOST_CHECK( bsecret1.SetString (str8Secret1,   '8'));
    BOOST_CHECK( bsecret2.SetString (str8Secret2,   '8'));
    BOOST_CHECK( bsecret1C.SetString(str8Secret1C,  '8'));
    BOOST_CHECK( bsecret2C.SetString(str8Secret2C,  '8'));
    BOOST_CHECK(!baddress1.SetString(strAddressBad, '8'));

    bool fCompressed;
    CSecret secret1  = bsecret1.GetSecret (fCompressed);
    BOOST_CHECK(fCompressed == false);
    CSecret secret2  = bsecret2.GetSecret (fCompressed);
    BOOST_CHECK(fCompressed == false);
    CSecret secret1C = bsecret1C.GetSecret(fCompressed);
    BOOST_CHECK(fCompressed == true);
    CSecret secret2C = bsecret2C.GetSecret(fCompressed);
    BOOST_CHECK(fCompressed == true);

    BOOST_CHECK(secret1 == secret1C);
    BOOST_CHECK(secret2 == secret2C);

    CKey key1, key2, key1C, key2C;
    key1.SetSecret(secret1, false);
    key2.SetSecret(secret2, false);
    key1C.SetSecret(secret1, true);
    key2C.SetSecret(secret2, true);

    BOOST_CHECK(addr81.Get()  == CTxDestination(key1.GetPubKey().GetID()));
    BOOST_CHECK(addr82.Get()  == CTxDestination(key2.GetPubKey().GetID()));
    BOOST_CHECK(addr81C.Get() == CTxDestination(key1C.GetPubKey().GetID()));
    BOOST_CHECK(addr82C.Get() == CTxDestination(key2C.GetPubKey().GetID()));

    for (int n=0; n<16; n++)
    {
        string strMsg = strprintf("Very secret message %i: 11", n);
        uint256 hashMsg = Hash(strMsg.begin(), strMsg.end());

        // normal signatures

        vector<unsigned char> sign1, sign2, sign1C, sign2C;

        BOOST_CHECK(key1.Sign (hashMsg, sign1));
        BOOST_CHECK(key2.Sign (hashMsg, sign2));
        BOOST_CHECK(key1C.Sign(hashMsg, sign1C));
        BOOST_CHECK(key2C.Sign(hashMsg, sign2C));

        BOOST_CHECK( key1.Verify(hashMsg, sign1));
        BOOST_CHECK(!key1.Verify(hashMsg, sign2));
        BOOST_CHECK( key1.Verify(hashMsg, sign1C));
        BOOST_CHECK(!key1.Verify(hashMsg, sign2C));

        BOOST_CHECK(!key2.Verify(hashMsg, sign1));
        BOOST_CHECK( key2.Verify(hashMsg, sign2));
        BOOST_CHECK(!key2.Verify(hashMsg, sign1C));
        BOOST_CHECK( key2.Verify(hashMsg, sign2C));

        BOOST_CHECK( key1C.Verify(hashMsg, sign1));
        BOOST_CHECK(!key1C.Verify(hashMsg, sign2));
        BOOST_CHECK( key1C.Verify(hashMsg, sign1C));
        BOOST_CHECK(!key1C.Verify(hashMsg, sign2C));

        BOOST_CHECK(!key2C.Verify(hashMsg, sign1));
        BOOST_CHECK( key2C.Verify(hashMsg, sign2));
        BOOST_CHECK(!key2C.Verify(hashMsg, sign1C));
        BOOST_CHECK( key2C.Verify(hashMsg, sign2C));

        // compact signatures (with key recovery)

        vector<unsigned char> csign1, csign2, csign1C, csign2C;

        BOOST_CHECK(key1.SignCompact (hashMsg, csign1));
        BOOST_CHECK(key2.SignCompact (hashMsg, csign2));
        BOOST_CHECK(key1C.SignCompact(hashMsg, csign1C));
        BOOST_CHECK(key2C.SignCompact(hashMsg, csign2C));

        CKey rkey1, rkey2, rkey1C, rkey2C;

        BOOST_CHECK(rkey1.SetCompactSignature (hashMsg, csign1));
        BOOST_CHECK(rkey2.SetCompactSignature (hashMsg, csign2));
        BOOST_CHECK(rkey1C.SetCompactSignature(hashMsg, csign1C));
        BOOST_CHECK(rkey2C.SetCompactSignature(hashMsg, csign2C));

        BOOST_CHECK(rkey1.GetPubKey()  == key1.GetPubKey());
        BOOST_CHECK(rkey2.GetPubKey()  == key2.GetPubKey());
        BOOST_CHECK(rkey1C.GetPubKey() == key1C.GetPubKey());
        BOOST_CHECK(rkey2C.GetPubKey() == key2C.GetPubKey());
    }
}

BOOST_AUTO_TEST_CASE(key_test_C)
{
    /*
    dumpKeyInfo(uint256(1000));
    dumpKeyInfo(uint256(2000));
    */
    CBitcoinSecret bsecret1, bsecret2, bsecret1C, bsecret2C, baddress1;
    BOOST_CHECK( bsecret1.SetString (strCSecret1,   'C'));
    BOOST_CHECK( bsecret2.SetString (strCSecret2,   'C'));
    BOOST_CHECK( bsecret1C.SetString(strCSecret1C,  'C'));
    BOOST_CHECK( bsecret2C.SetString(strCSecret2C,  'C'));
    BOOST_CHECK(!baddress1.SetString(strAddressBad, 'C'));

    bool fCompressed;
    CSecret secret1  = bsecret1.GetSecret (fCompressed);
    BOOST_CHECK(fCompressed == false);
    CSecret secret2  = bsecret2.GetSecret (fCompressed);
    BOOST_CHECK(fCompressed == false);
    CSecret secret1C = bsecret1C.GetSecret(fCompressed);
    BOOST_CHECK(fCompressed == true);
    CSecret secret2C = bsecret2C.GetSecret(fCompressed);
    BOOST_CHECK(fCompressed == true);

    BOOST_CHECK(secret1 == secret1C);
    BOOST_CHECK(secret2 == secret2C);

    CKey key1, key2, key1C, key2C;
    key1.SetSecret(secret1, false);
    key2.SetSecret(secret2, false);
    key1C.SetSecret(secret1, true);
    key2C.SetSecret(secret2, true);

    BOOST_CHECK(addrC1.Get()  == CTxDestination(key1.GetPubKey().GetID()));
    BOOST_CHECK(addrC2.Get()  == CTxDestination(key2.GetPubKey().GetID()));
    BOOST_CHECK(addrC1C.Get() == CTxDestination(key1C.GetPubKey().GetID()));
    BOOST_CHECK(addrC2C.Get() == CTxDestination(key2C.GetPubKey().GetID()));

    for (int n=0; n<16; n++)
    {
        string strMsg = strprintf("Very secret message %i: 11", n);
        uint256 hashMsg = Hash(strMsg.begin(), strMsg.end());

        // normal signatures

        vector<unsigned char> sign1, sign2, sign1C, sign2C;

        BOOST_CHECK(key1.Sign (hashMsg, sign1));
        BOOST_CHECK(key2.Sign (hashMsg, sign2));
        BOOST_CHECK(key1C.Sign(hashMsg, sign1C));
        BOOST_CHECK(key2C.Sign(hashMsg, sign2C));

        BOOST_CHECK( key1.Verify(hashMsg, sign1));
        BOOST_CHECK(!key1.Verify(hashMsg, sign2));
        BOOST_CHECK( key1.Verify(hashMsg, sign1C));
        BOOST_CHECK(!key1.Verify(hashMsg, sign2C));

        BOOST_CHECK(!key2.Verify(hashMsg, sign1));
        BOOST_CHECK( key2.Verify(hashMsg, sign2));
        BOOST_CHECK(!key2.Verify(hashMsg, sign1C));
        BOOST_CHECK( key2.Verify(hashMsg, sign2C));

        BOOST_CHECK( key1C.Verify(hashMsg, sign1));
        BOOST_CHECK(!key1C.Verify(hashMsg, sign2));
        BOOST_CHECK( key1C.Verify(hashMsg, sign1C));
        BOOST_CHECK(!key1C.Verify(hashMsg, sign2C));

        BOOST_CHECK(!key2C.Verify(hashMsg, sign1));
        BOOST_CHECK( key2C.Verify(hashMsg, sign2));
        BOOST_CHECK(!key2C.Verify(hashMsg, sign1C));
        BOOST_CHECK( key2C.Verify(hashMsg, sign2C));

        // compact signatures (with key recovery)

        vector<unsigned char> csign1, csign2, csign1C, csign2C;

        BOOST_CHECK(key1.SignCompact (hashMsg, csign1));
        BOOST_CHECK(key2.SignCompact (hashMsg, csign2));
        BOOST_CHECK(key1C.SignCompact(hashMsg, csign1C));
        BOOST_CHECK(key2C.SignCompact(hashMsg, csign2C));

        CKey rkey1, rkey2, rkey1C, rkey2C;

        BOOST_CHECK(rkey1.SetCompactSignature (hashMsg, csign1));
        BOOST_CHECK(rkey2.SetCompactSignature (hashMsg, csign2));
        BOOST_CHECK(rkey1C.SetCompactSignature(hashMsg, csign1C));
        BOOST_CHECK(rkey2C.SetCompactSignature(hashMsg, csign2C));

        BOOST_CHECK(rkey1.GetPubKey()  == key1.GetPubKey());
        BOOST_CHECK(rkey2.GetPubKey()  == key2.GetPubKey());
        BOOST_CHECK(rkey1C.GetPubKey() == key1C.GetPubKey());
        BOOST_CHECK(rkey2C.GetPubKey() == key2C.GetPubKey());
    }
}

BOOST_AUTO_TEST_CASE(key_test2)
{
    CBitcoinSecret bsecret1, bsecret2, bsecret1C, bsecret2C;
    // New encoding for BlockShares
    BOOST_CHECK( bsecret1.SetString (str8Secret1,  '8'));
    BOOST_CHECK( bsecret2.SetString (str8Secret2,  '8'));
    BOOST_CHECK( bsecret1C.SetString(str8Secret1C, '8'));
    BOOST_CHECK( bsecret2C.SetString(str8Secret2C, '8'));
    // Set BlockShares private key with BlockCredits unit, should fail
    BOOST_CHECK(!bsecret1.SetString (str8Secret1,  'C'));
    BOOST_CHECK(!bsecret2.SetString (str8Secret2,  'C'));
    BOOST_CHECK(!bsecret1C.SetString(str8Secret1C, 'C'));
    BOOST_CHECK(!bsecret2C.SetString(str8Secret2C, 'C'));

    // New encoding for BlockCredits
    BOOST_CHECK( bsecret1.SetString (strCSecret1,  'C'));
    BOOST_CHECK( bsecret2.SetString (strCSecret2,  'C'));
    BOOST_CHECK( bsecret1C.SetString(strCSecret1C, 'C'));
    BOOST_CHECK( bsecret2C.SetString(strCSecret2C, 'C'));
    // Set BlockCredits private key with BlockShares unit, should fail
    BOOST_CHECK(!bsecret1.SetString (strCSecret1,  '8'));
    BOOST_CHECK(!bsecret2.SetString (strCSecret2,  '8'));
    BOOST_CHECK(!bsecret1C.SetString(strCSecret1C, '8'));
    BOOST_CHECK(!bsecret2C.SetString(strCSecret2C, '8'));

    // Check the CBitcoinSecret
    bool fCompressed;
    CSecret secret = bsecret1.GetSecret(fCompressed);
    BOOST_CHECK(CBitcoinSecret(secret, fCompressed, '8').ToString() == str8Secret1);
    BOOST_CHECK(CBitcoinSecret(secret, fCompressed, 'C').ToString() == strCSecret1);
    secret = bsecret2.GetSecret(fCompressed);
    BOOST_CHECK(CBitcoinSecret(secret, fCompressed, '8').ToString() == str8Secret2);
    BOOST_CHECK(CBitcoinSecret(secret, fCompressed, 'C').ToString() == strCSecret2);
    secret = bsecret1C.GetSecret(fCompressed);
    BOOST_CHECK(CBitcoinSecret(secret, fCompressed, '8').ToString() == str8Secret1C);
    BOOST_CHECK(CBitcoinSecret(secret, fCompressed, 'C').ToString() == strCSecret1C);
    secret = bsecret2C.GetSecret(fCompressed);
    BOOST_CHECK(CBitcoinSecret(secret, fCompressed, '8').ToString() == str8Secret2C);
    BOOST_CHECK(CBitcoinSecret(secret, fCompressed, 'C').ToString() == strCSecret2C);
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef FASTSIM_RADAR_HPP_
#define FASTSIM_RADAR_HPP_

#include <boost/shared_ptr.hpp>
#include "posture.hpp"
#include "map.hpp"

namespace fastsim
{
  /// a radar detects a given goal in one of the n slices
  /// radar SEE TROUGH OBSTACLES
  class Radar
  {
  public:
    Radar(int color, int nb_slices, bool through_walls = true) :
      _color(color), _nb_slices(nb_slices), _inc(2 * M_PI / nb_slices),
      _activated_slice(0),
      _through_walls(through_walls)
    {}
    int update(const Posture& pos,
	       const boost::shared_ptr<Map>& map);
    int get_activated_slice() const { return _activated_slice; }
    int get_nb_slices() const { return _nb_slices; }
    int get_color() const { return _color; }
    float get_inc() const { return _inc; }
  protected:
    // the "color" (i.e. goal identifier) detected by this "radar"
    int _color;
    // the number of pie-slices
    int _nb_slices;
    // the size of a slice
    float _inc;
    // the activated slices (-1 if the goal is not visible)
    int _activated_slice;
    // true if the radar can see trough walls
    // get_activated_slices() will return -1 if the goal is not visible
    bool _through_walls;
  };
}
#endif


#include "mex.h"
#include "RigidBodyManipulator.h"
#include "constraint/RigidBodyConstraint.h"
#include "IKoptions.h"
#include "RigidBodyIK.h"
#include <Eigen/Dense>
#include "drakeUtil.h"

using namespace std;
using namespace Eigen;

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
  if(nrhs < 6)
  {
    mexErrMsgIdAndTxt("Drake:inverseKinPointwisemex:NotEnoughInputs","Usage inverseKinPointwisemex(model_ptr,t,q_seed,q_nom,constraint1,constraint2,...,ikoptions");
  }
  RigidBodyManipulator* model = (RigidBodyManipulator*) getDrakeMexPointer(prhs[0]);
  int nq = model->num_dof;
  int nT = mxGetNumberOfElements(prhs[1]);
  double* t = mxGetPr(prhs[1]);
  Map<MatrixXd> q_seed(mxGetPr(prhs[2]),nq,nT);
  Map<MatrixXd> q_nom(mxGetPr(prhs[3]),nq,nT);
  int num_constraints = nrhs-5;
  RigidBodyConstraint** constraint_array = new RigidBodyConstraint*[num_constraints];
  for(int i = 0;i<num_constraints;i++)
  {
    constraint_array[i] = (RigidBodyConstraint*) getDrakeMexPointer(prhs[4+i]);
  }
  IKoptions* ikoptions = (IKoptions*) getDrakeMexPointer(prhs[nrhs-1]);
  plhs[0] = mxCreateDoubleMatrix(nq,nT,mxREAL);
  Map<MatrixXd> q_sol(mxGetPr(plhs[0]),nq,nT);
  int* info = new int[nT];
  vector<string> infeasible_constraint;
  inverseKinPointwise(model,nT,t,q_seed,q_nom,num_constraints,constraint_array,q_sol,info,infeasible_constraint,*ikoptions);
  plhs[1] = mxCreateDoubleMatrix(1,nT,mxREAL);
  for(int i = 0;i<nT;i++)
  {
    *(mxGetPr(plhs[1])+i) = (double) info[i];
  }
  mwSize name_dim[1] = {static_cast<mwSize>(infeasible_constraint.size())};
  plhs[2] = mxCreateCellArray(1,name_dim);
  for(int i = 0;i<infeasible_constraint.size();i++)
  {
    mxArray* name_ptr = mxCreateString(infeasible_constraint[i].c_str());
    mxSetCell(plhs[2],i,name_ptr);
  }
  delete[] info;
  delete[] constraint_array;
}


#pragma once

#include <boost/iostreams/constants.hpp>
#include <boost/iostreams/categories.hpp>
#include <openssl/sha.h>

class sha256_filter {
public:
	SHA256_CTX hasher;
	unsigned char digest[SHA256_DIGEST_LENGTH]{};
	typedef char char_type;

	struct category :
		boost::iostreams::output,
		boost::iostreams::input,
		boost::iostreams::filter_tag,
		boost::iostreams::multichar_tag,
		boost::iostreams::closable_tag { };

	/* FIXME TODO Signal that errors happened somehow */
	sha256_filter()
	{
		SHA256_Init(&hasher);
	}

	template<typename Sink>
	std::streamsize write(Sink& dest, const char* s, std::streamsize n)
	{
		SHA256_Update(&hasher, s, n);
		boost::iostreams::write(dest, s, n);
		return n;
	}

	template<typename Source>
	std::streamsize read(Source& src, char* s, std::streamsize n)
	{
		std::streamsize result = boost::iostreams::read(src, s, n);

		if (result == -1) return result;

		SHA256_Update(&hasher, s, result);
		return result;
	}

	template <class Device>
	void close(Device &device)
	{
		SHA256_Final(&digest[0], &hasher);
	}
};


#ifdef _CARTO_SEARCH_SUPPORT

#include "SearchProxy.h"
#include "core/MapPos.h"
#include "core/Variant.h"
#include "components/Exceptions.h"
#include "geometry/Geometry.h"
#include "geometry/PointGeometry.h"
#include "geometry/LineGeometry.h"
#include "geometry/PolygonGeometry.h"
#include "geometry/MultiPointGeometry.h"
#include "geometry/MultiLineGeometry.h"
#include "geometry/MultiPolygonGeometry.h"
#include "geometry/MultiGeometry.h"
#include "search/query/QueryContext.h"
#include "search/query/QueryExpressionParser.h"
#include "projections/Projection.h"
#include "projections/EPSG3857.h"
#include "utils/Const.h"
#include "utils/Log.h"

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/variant.hpp>

#include <limits>
#include <algorithm>
#include <numeric>

namespace {

    typedef boost::geometry::model::d2::point_xy<double> BoostPointType;
    typedef boost::geometry::model::linestring<BoostPointType> BoostLinestringType;
    typedef boost::geometry::model::polygon<BoostPointType> BoostPolygonType;
    typedef boost::variant<BoostPointType, BoostLinestringType, BoostPolygonType> BoostGeometryType;

    carto::MapBounds convertToEPSG3857(const carto::MapBounds& mapBounds, const std::shared_ptr<carto::Projection>& proj) {
        if (std::dynamic_pointer_cast<carto::EPSG3857>(proj)) {
            return mapBounds;
        }

        carto::MapPos mapPos0 = mapBounds.getMin();
        carto::MapPos mapPos1 = mapBounds.getMax();

        carto::EPSG3857 epsg3857;
        carto::MapBounds epsg3857Bounds;
        epsg3857Bounds.expandToContain(epsg3857.fromWgs84(proj->toWgs84(mapPos0)));
        epsg3857Bounds.expandToContain(epsg3857.fromWgs84(proj->toWgs84(carto::MapPos(mapPos0.getX(), mapPos1.getY()))));
        epsg3857Bounds.expandToContain(epsg3857.fromWgs84(proj->toWgs84(mapPos1)));
        epsg3857Bounds.expandToContain(epsg3857.fromWgs84(proj->toWgs84(carto::MapPos(mapPos1.getX(), mapPos0.getY()))));
        return epsg3857Bounds;
    }

    std::shared_ptr<carto::Geometry> convertToEPSG3857(const std::shared_ptr<carto::Geometry>& geometry, const std::shared_ptr<carto::Projection>& proj) {
        if (std::dynamic_pointer_cast<carto::EPSG3857>(proj)) {
            return geometry;
        }

        carto::EPSG3857 epsg3857;
        if (auto pointGeometry = std::dynamic_pointer_cast<carto::PointGeometry>(geometry)) {
            carto::MapPos mapPos = epsg3857.fromWgs84(proj->toWgs84(pointGeometry->getPos()));
            return std::make_shared<carto::PointGeometry>(mapPos);
        } else if (auto lineGeometry = std::dynamic_pointer_cast<carto::LineGeometry>(geometry)) {
            std::vector<carto::MapPos> mapPoses = lineGeometry->getPoses();
            std::for_each(mapPoses.begin(), mapPoses.end(), [&epsg3857, &proj](carto::MapPos& mapPos) { mapPos = epsg3857.fromWgs84(proj->toWgs84(mapPos)); });
            return std::make_shared<carto::LineGeometry>(mapPoses);
        } else if (auto polygonGeometry = std::dynamic_pointer_cast<carto::PolygonGeometry>(geometry)) {
            std::vector<std::vector<carto::MapPos> > rings = polygonGeometry->getRings();
            for (std::vector<carto::MapPos>& ring : rings) {
                std::for_each(ring.begin(), ring.end(), [&epsg3857, &proj](carto::MapPos& mapPos) { mapPos = epsg3857.fromWgs84(proj->toWgs84(mapPos)); });
            }
            return std::make_shared<carto::PolygonGeometry>(rings);
        } else if (auto multiGeometry = std::dynamic_pointer_cast<carto::MultiGeometry>(geometry)) {
            std::vector<std::shared_ptr<carto::Geometry> > geometries;
            for (int i = 0; i < multiGeometry->getGeometryCount(); i++) {
                geometries.push_back(convertToEPSG3857(multiGeometry->getGeometry(i), proj));
            }
            return std::make_shared<carto::MultiGeometry>(geometries);
        } else {
            throw carto::GenericException("Unsupported geometry type");
        }
    }

    BoostGeometryType convertToBoostGeometry(const std::shared_ptr<carto::Geometry>& geometry) {
        if (auto pointGeometry = std::dynamic_pointer_cast<carto::PointGeometry>(geometry)) {
            carto::MapPos mapPos = pointGeometry->getPos();
            BoostPointType boostPoint(mapPos.getX(), mapPos.getY());
            return boostPoint;
        } else if (auto lineGeometry = std::dynamic_pointer_cast<carto::LineGeometry>(geometry)) {
            const std::vector<carto::MapPos>& mapPoses = lineGeometry->getPoses();
            BoostLinestringType boostLinestring;
            std::for_each(mapPoses.begin(), mapPoses.end(), [&boostLinestring](const carto::MapPos& mapPos) { boostLinestring.push_back(BoostPointType(mapPos.getX(), mapPos.getY())); });
            return boostLinestring;
        } else if (auto polygonGeometry = std::dynamic_pointer_cast<carto::PolygonGeometry>(geometry)) {
            const std::vector<std::vector<carto::MapPos> >& rings = polygonGeometry->getRings();
            BoostPolygonType boostPolygon;
            for (std::size_t i = 0; i < rings.size(); i++) {
                BoostPolygonType::ring_type boostRing;
                std::for_each(rings[i].begin(), rings[i].end(), [&boostRing](const carto::MapPos& mapPos) { boostRing.push_back(BoostPointType(mapPos.getX(), mapPos.getY())); });
                if (!boostRing.empty()) {
                    BoostPointType pos = boostRing.front();
                    boostRing.push_back(pos);
                }
                if (i == 0) {
                    boostPolygon.outer() = std::move(boostRing);
                } else {
                    boostPolygon.inners().push_back(std::move(boostRing));
                }
            }
            return boostPolygon;
        } else {
            throw carto::GenericException("Unsupported geometry type");
        }
    }

    double calculateDistance(const std::shared_ptr<carto::Geometry>& geometry1, const std::shared_ptr<carto::Geometry>& geometry2) {
        if (auto multiGeometry1 = std::dynamic_pointer_cast<carto::MultiGeometry>(geometry1)) {
            double dist = std::numeric_limits<double>::infinity();
            for (int i = 0; i < multiGeometry1->getGeometryCount(); i++) {
                dist = std::min(dist, calculateDistance(multiGeometry1->getGeometry(i), geometry2));
            }
            return dist;
        }

        if (auto multiGeometry2 = std::dynamic_pointer_cast<carto::MultiGeometry>(geometry2)) {
            double dist = std::numeric_limits<double>::infinity();
            for (int i = 0; i < multiGeometry2->getGeometryCount(); i++) {
                dist = std::min(dist, calculateDistance(geometry1, multiGeometry2->getGeometry(i)));
            }
            return dist;
        }

        return boost::geometry::distance(convertToBoostGeometry(geometry1), convertToBoostGeometry(geometry2));
    }

    bool matchRegexFilter(const carto::Variant& variant, const std::regex& re) {
        std::string str;
        switch (variant.getType()) {
        case carto::VariantType::VARIANT_TYPE_NULL:
            return false;
        case carto::VariantType::VARIANT_TYPE_STRING:
        case carto::VariantType::VARIANT_TYPE_BOOL:
        case carto::VariantType::VARIANT_TYPE_INTEGER:
        case carto::VariantType::VARIANT_TYPE_DOUBLE:
            return std::regex_match(variant.getString(), re);
        case carto::VariantType::VARIANT_TYPE_ARRAY:
            for (int i = 0; i < variant.getArraySize(); i++) {
                if (matchRegexFilter(variant.getArrayElement(i), re)) {
                    return true;
                }
            }
            return false;
        case carto::VariantType::VARIANT_TYPE_OBJECT:
            for (const std::string& key : variant.getObjectKeys()) {
                if (matchRegexFilter(variant.getObjectElement(key), re)) {
                    return true;
                }
            }
            return false;
        }
        return false;
    }

    class SearchQueryContext : public carto::QueryContext {
    public:
        explicit SearchQueryContext(const std::shared_ptr<carto::Geometry>& geometry, const std::string* layerName, const carto::Variant& var) : _geometry(geometry), _layerName(layerName), _variant(var) { }
        virtual ~SearchQueryContext() { }

        virtual bool getVariable(const std::string& name, carto::Variant& value) const {
            if (name == "layer::name") {
                value = (_layerName ? carto::Variant(*_layerName) : carto::Variant());
                return true;
            }

            if (name == "geometry::type") {
                value = carto::Variant(GetGeometryType(_geometry));
                return true;
            }

            if (name == "geometry::vertices") {
                value = carto::Variant(static_cast<long long>(GetGeometryVerticesCount(_geometry)));
                return true;
            }

            switch (_variant.getType()) {
            case carto::VariantType::VARIANT_TYPE_OBJECT:
                if (_variant.containsObjectKey(name)) {
                    value = _variant.getObjectElement(name);
                    return true;
                }
                return false;
            case carto::VariantType::VARIANT_TYPE_ARRAY:
                return false;
            default:
                if (name == "value") {
                    value = _variant;
                    return true;
                }
                return false;
            }
        }

    private:
        static std::string GetGeometryType(const std::shared_ptr<carto::Geometry>& geometry) {
            if (std::dynamic_pointer_cast<carto::PointGeometry>(geometry)) {
                return "point";
            } else if (std::dynamic_pointer_cast<carto::LineGeometry>(geometry)) {
                return "linestring";
            } else if (std::dynamic_pointer_cast<carto::PolygonGeometry>(geometry)) {
                return "polygon";
            } else if (std::dynamic_pointer_cast<carto::MultiPointGeometry>(geometry)) {
                return "multipoint";
            } else if (std::dynamic_pointer_cast<carto::MultiLineGeometry>(geometry)) {
                return "multilinestring";
            } else if (std::dynamic_pointer_cast<carto::MultiPolygonGeometry>(geometry)) {
                return "multipolygon";
            } else if (std::dynamic_pointer_cast<carto::MultiGeometry>(geometry)) {
                return "multigeometry";
            } else if (std::dynamic_pointer_cast<carto::Geometry>(geometry)) {
                return "geometry";
            }
            return "unknown";
        }

        static std::size_t GetGeometryVerticesCount(const std::shared_ptr<carto::Geometry>& geometry) {
            if (std::dynamic_pointer_cast<carto::PointGeometry>(geometry)) {
                return 1;
            } else if (auto lineGeometry = std::dynamic_pointer_cast<carto::LineGeometry>(geometry)) {
                return lineGeometry->getPoses().size();
            } else if (auto polygonGeometry = std::dynamic_pointer_cast<carto::PolygonGeometry>(geometry)) {
                return std::accumulate(polygonGeometry->getHoles().begin(), polygonGeometry->getHoles().end(), polygonGeometry->getPoses().size(), [](std::size_t size, const std::vector<carto::MapPos>& ring) { return size + ring.size(); });
            } else if (auto multiGeometry = std::dynamic_pointer_cast<carto::MultiGeometry>(geometry)) {
                std::size_t count = 0;
                for (int i = 0; i < multiGeometry->getGeometryCount(); i++) {
                    count += GetGeometryVerticesCount(multiGeometry->getGeometry(i));
                }
                return count;
            }
            return 0;
        }

        const std::shared_ptr<carto::Geometry>& _geometry;
        const std::string* _layerName;
        const carto::Variant& _variant;
    };

}

namespace carto {

    SearchProxy::SearchProxy(const std::shared_ptr<SearchRequest>& request, const MapBounds& mapBounds, const std::shared_ptr<Projection>& proj) :
        _request(request),
        _geometry(),
        _searchBounds(),
        _searchRadius(0),
        _projection(proj),
        _expr(),
        _re()
    {
        if (!request) {
            throw NullArgumentException("Null request");
        }
        if (!proj) {
            throw NullArgumentException("Null proj");
        }

        if (!request->getRegexFilter().empty()) {
            try {
                _re = std::regex(request->getRegexFilter());
            }
            catch (const std::exception& ex) {
                throw ParseException(std::string("Failed to parse regex: ") + ex.what(), request->getRegexFilter());
            }
        }

        if (!request->getFilterExpression().empty()) {
            try {
                _expr = QueryExpressionParser::parse(request->getFilterExpression());
            }
            catch (const std::exception& ex) {
                throw ParseException(std::string("Failed to parse expression: ") + ex.what(), request->getFilterExpression());
            }
        }

        if (request->getGeometry()) {
            if (!request->getProjection()) {
                throw NullArgumentException("Null projection while geometry is not null");
            }

            MapPos wgs84CenterPos = request->getProjection()->toWgs84(request->getGeometry()->getCenterPos());
            _geometry = convertToEPSG3857(request->getGeometry(), request->getProjection());
            MapBounds geometryBounds = _geometry->getBounds();
            _searchRadius = request->getSearchRadius() / std::cos(std::min(89.9, std::abs(wgs84CenterPos.getY())) * Const::DEG_TO_RAD);
            MapPos boundsPos0 = geometryBounds.getMin() - MapVec(_searchRadius, _searchRadius);
            MapPos boundsPos1 = geometryBounds.getMax() + MapVec(_searchRadius, _searchRadius);
            boundsPos0[0] = std::max(boundsPos0[0], EPSG3857().getBounds().getMin()[0] * 0.9999);
            boundsPos1[0] = std::min(boundsPos1[0], EPSG3857().getBounds().getMax()[0] * 0.9999);
            _searchBounds = MapBounds(boundsPos0, boundsPos1);
        } else {
            _searchBounds = convertToEPSG3857(mapBounds, proj);
        }
    }

    const MapBounds& SearchProxy::getSearchBounds() const {
        return _searchBounds;
    }

    bool SearchProxy::testBounds(const MapBounds& bounds) const {
        if (_geometry) {
            std::vector<MapPos> points(4);
            points[0] = bounds.getMin();
            points[1] = MapPos(bounds.getMin().getX(), bounds.getMax().getY());
            points[2] = bounds.getMax();
            points[3] = MapPos(bounds.getMax().getX(), bounds.getMin().getY());
            auto geometry = std::make_shared<PolygonGeometry>(std::move(points));
            double dist = calculateDistance(convertToEPSG3857(geometry, _projection), _geometry);
            if (dist > _searchRadius) {
                return false;
            }
        }

        return true;
    }

    bool SearchProxy::testElement(const std::shared_ptr<Geometry>& geometry, const std::string* layerName, const Variant& var) const {
        if (_re) {
            if (!matchRegexFilter(var, *_re)) {
                return false;
            }
        }

        if (_expr) {
            SearchQueryContext context(geometry, layerName, var);
            if (!_expr->evaluate(context)) {
                return false;
            }
        }

        if (_geometry) {
            if (calculateDistance(convertToEPSG3857(geometry, _projection), _geometry) > _searchRadius) {
                return false;
            }
        }

        return true;
    }

}

#endif