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

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

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

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

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

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_DISTANCE_PROJECTED_POINT_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_DISTANCE_PROJECTED_POINT_HPP

#include <boost/concept_check.hpp>
#include <boost/core/ignore_unused.hpp>
#include <boost/mpl/if.hpp>
#include <boost/type_traits/is_void.hpp>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/point_type.hpp>

#include <boost/geometry/algorithms/convert.hpp>
#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/arithmetic/dot_product.hpp>

#include <boost/geometry/strategies/tags.hpp>
#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/strategies/default_distance_result.hpp>
#include <boost/geometry/strategies/cartesian/distance_pythagoras.hpp>

#include <boost/geometry/util/select_coordinate_type.hpp>

// Helper geometry (projected point on line)
#include <boost/geometry/geometries/point.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace distance
{

/*!
\brief Strategy for distance point to segment
\ingroup strategies
\details Calculates distance using projected-point method, and (optionally) Pythagoras
\author Adapted from: http://geometryalgorithms.com/Archive/algorithm_0102/algorithm_0102.htm
\tparam CalculationType \tparam_calculation
\tparam Strategy underlying point-point distance strategy
\par Concepts for Strategy:
- cartesian_distance operator(Point,Point)
\note If the Strategy is a "comparable::pythagoras", this strategy
    automatically is a comparable projected_point strategy (so without sqrt)

\qbk{
[heading See also]
[link geometry.reference.algorithms.distance.distance_3_with_strategy distance (with strategy)]
}

*/
template
<
    typename CalculationType = void,
    typename Strategy = pythagoras<CalculationType>
>
class projected_point
{
public :
    // The three typedefs below are necessary to calculate distances
    // from segments defined in integer coordinates.

    // Integer coordinates can still result in FP distances.
    // There is a division, which must be represented in FP.
    // So promote.
    template <typename Point, typename PointOfSegment>
    struct calculation_type
        : promote_floating_point
          <
              typename strategy::distance::services::return_type
                  <
                      Strategy,
                      Point,
                      PointOfSegment
                  >::type
          >
    {};

    template <typename Point, typename PointOfSegment>
    inline typename calculation_type<Point, PointOfSegment>::type
    apply(Point const& p, PointOfSegment const& p1, PointOfSegment const& p2) const
    {
        assert_dimension_equal<Point, PointOfSegment>();

        typedef typename calculation_type<Point, PointOfSegment>::type calculation_type;

        // A projected point of points in Integer coordinates must be able to be
        // represented in FP.
        typedef model::point
            <
                calculation_type,
                dimension<PointOfSegment>::value,
                typename coordinate_system<PointOfSegment>::type
            > fp_point_type;

        // For convenience
        typedef fp_point_type fp_vector_type;

        /*
            Algorithm [p: (px,py), p1: (x1,y1), p2: (x2,y2)]
            VECTOR v(x2 - x1, y2 - y1)
            VECTOR w(px - x1, py - y1)
            c1 = w . v
            c2 = v . v
            b = c1 / c2
            RETURN POINT(x1 + b * vx, y1 + b * vy)
        */

        // v is multiplied below with a (possibly) FP-value, so should be in FP
        // For consistency we define w also in FP
        fp_vector_type v, w, projected;

        geometry::convert(p2, v);
        geometry::convert(p, w);
        geometry::convert(p1, projected);
        subtract_point(v, projected);
        subtract_point(w, projected);

        Strategy strategy;
        boost::ignore_unused(strategy);

        calculation_type const zero = calculation_type();
        calculation_type const c1 = dot_product(w, v);
        if (c1 <= zero)
        {
            return strategy.apply(p, p1);
        }
        calculation_type const c2 = dot_product(v, v);
        if (c2 <= c1)
        {
            return strategy.apply(p, p2);
        }

        // See above, c1 > 0 AND c2 > c1 so: c2 != 0
        calculation_type const b = c1 / c2;

        multiply_value(v, b);
        add_point(projected, v);

        return strategy.apply(p, projected);
    }

    template <typename CT>
    inline CT vertical_or_meridian(CT const& lat1, CT const& lat2) const
    {
        return lat1 - lat2;
    }

};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename CalculationType, typename Strategy>
struct tag<projected_point<CalculationType, Strategy> >
{
    typedef strategy_tag_distance_point_segment type;
};

template <typename CalculationType, typename Strategy, typename P, typename PS>
struct return_type<projected_point<CalculationType, Strategy>, P, PS>
    : projected_point<CalculationType, Strategy>::template calculation_type<P, PS>
{};

template <typename CalculationType, typename Strategy>
struct comparable_type<projected_point<CalculationType, Strategy> >
{
    // Define a projected_point strategy with its underlying point-point-strategy
    // being comparable
    typedef projected_point
        <
            CalculationType,
            typename comparable_type<Strategy>::type
        > type;
};

template <typename CalculationType, typename Strategy>
struct get_comparable<projected_point<CalculationType, Strategy> >
{
    typedef typename comparable_type
        <
            projected_point<CalculationType, Strategy>
        >::type comparable_type;
public :
    static inline comparable_type apply(projected_point<CalculationType, Strategy> const& )
    {
        return comparable_type();
    }
};

template <typename CalculationType, typename Strategy, typename P, typename PS>
struct result_from_distance<projected_point<CalculationType, Strategy>, P, PS>
{
private :
    typedef typename return_type<projected_point<CalculationType, Strategy>, P, PS>::type return_type;
public :
    template <typename T>
    static inline return_type apply(projected_point<CalculationType, Strategy> const& , T const& value)
    {
        Strategy s;
        return result_from_distance<Strategy, P, PS>::apply(s, value);
    }
};

// Get default-strategy for point-segment distance calculation
// while still have the possibility to specify point-point distance strategy (PPS)
// It is used in algorithms/distance.hpp where users specify PPS for distance
// of point-to-segment or point-to-linestring.
// Convenient for geographic coordinate systems especially.
template <typename Point, typename PointOfSegment, typename Strategy>
struct default_strategy
    <
        point_tag, segment_tag, Point, PointOfSegment,
        cartesian_tag, cartesian_tag, Strategy
    >
{
    typedef strategy::distance::projected_point
    <
        void,
        typename boost::mpl::if_
            <
                boost::is_void<Strategy>,
                typename default_strategy
                    <
                        point_tag, point_tag, Point, PointOfSegment,
                        cartesian_tag, cartesian_tag
                    >::type,
                Strategy
            >::type
    > type;
};

template <typename PointOfSegment, typename Point, typename Strategy>
struct default_strategy
    <
        segment_tag, point_tag, PointOfSegment, Point,
        cartesian_tag, cartesian_tag, Strategy
    >
{
    typedef typename default_strategy
        <
            point_tag, segment_tag, Point, PointOfSegment,
            cartesian_tag, cartesian_tag, Strategy
        >::type type;
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_DISTANCE_PROJECTED_POINT_HPP


#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <cmath>

#include <Eigen/Dense>

#include "variablesExt.h"
#include "recContours.h"
#include "pathfinding.h"

using namespace cv;
using namespace std;
using namespace Eigen;

void pathfinding(double offset)
{
	cout << "--- Nouveau path ---" << endl;
	convexHull(approxPoly, approxConvex, false, false); // on approxime par un poly convexe
	if(approxConvex.size() < 3) return; // si ce n'est pas au moins un triangle on sort

	Point A1,A2,B1,B2,L1,L2;

	A1 = approxConvex[0];
	A2 = approxConvex[approxConvex.size()-1];
	B1 = approxConvex[1];
	B2 = approxConvex[2];
	size_t iB2 = 2;
	size_t iA2 = approxConvex.size()-1;

	Vector2d vOffset = offsetVector(A1,B1,offset);
	Vector2d vAB = points2Vec(A1,B1);

	path.clear();

	// on commence par aller de A1 à B1

	path.push_back(A1);
	path.push_back(B1);

	bool continuer = true;
	bool altern = true;

	Point B = B1;
	int cmpt = 1;

	while(continuer)
	{
		// on recherche les points d'intersection de la parallèle à A1B1 décalée de vOffset

		Point O = vec2Point(B, vOffset*cmpt); // à partir du point 0, on cherche ces points
		Vector2d vB12 = points2Vec(B1,B2); // on a besoin de croiser vB12 avec vAB, on vérifie qu'ils ne sont pas colinéaires
		Vector2d vA12 = points2Vec(A1,A2);

		if(vAB == vB12 || vAB == vA12 || vAB == vB12 * -1 || vAB == vA12 * -1)
		{
			cout << "vecteurs colineaires" << endl;
			continuer = false; // c'est possible mais ça signifie qu'on est arrivé au bout du champ
		}
		else
		{
			Matrix4d MB;
			MB << 	1,-1*vAB[0],0,0,
				1,0,0,-1*vB12[0],
				0,-1*vAB[1],1,0,
				0,0,1,-1*vB12[1];
			//cout << "MB : " << endl << MB << endl;

			Vector4d CB;
			CB << O.x,B1.x,O.y,B1.y;
			//cout << "CB : " << endl << CB << endl;

			FullPivLU<Matrix4d> decompositionB(MB);

		    	Vector4d XB = decompositionB.solve(CB); // X : (x, t, y, t')
		    	cout << "XB : " << endl << XB << endl;

		    	Matrix4d MA;
			MA << 	1,-1*vAB[0],0,0,
				1,0,0,-1*vA12[0],
				0,-1*vAB[1],1,0,
				0,0,1,-1*vA12[1];
			//cout << "MA : " << endl << MA << endl;

			Vector4d CA;
			CA << O.x,A1.x,O.y,A1.y;
			//cout << "CA : " << endl << CA << endl;

			FullPivLU<Matrix4d> decompositionA(MA);

		    	Vector4d XA = decompositionA.solve(CA); // X : (x, t, y, t')
		    	cout << "XA : " << endl << XA << endl;

		    	Point BB = Point(XB[0],XB[2]);
		    	Point AA = Point(XA[0],XA[2]);

		    	// il faut regarder si on est sorti de l'une des deux arêtes

		    	Vector2d vNBB = points2VecN(B1,BB);
		    	Vector2d vNAA = points2VecN(A1,AA);
		    	Vector2d vNB12 = points2VecN(B1,B2);
		    	Vector2d vNA12 = points2VecN(A1,A2);

		    	if(vNBB.squaredNorm() > vNB12.squaredNorm())
		    	{
		    		iB2++;
		    		B1 = B2;
		    		B2 = approxConvex[iB2];
		    		if(iA2 >= iB2) continue;
		    	}
		    	else
		    	{
		    		B1 = BB;
		    	}

		    	if(vNAA.squaredNorm() > vNA12.squaredNorm())
		    	{
		    		iA2--;
		    		A1 = A2;
		    		A2 = approxConvex[iA2];
		    		if(iA2 >= iB2) continue;
		    	}
		    	else
		    	{
		    		A1 = AA;
		    	}

		    	if(iA2 < iB2) // on est arrivé au bout du champ
		    	{
		    		continuer = false;
		    		cout << "iA2:"<<iA2<<"<iB2:"<<iB2<<endl;
		    		break;
		    	}

		    	// ici, on vérifie que nos points sont dans l'approxPoly, s'ils n'y sont pas, on les force à y rentrer.
		    	double distAA = pointPolygonTest(approxPoly, AA, true);
		    	if(distAA < 0)
		    	{
		    		// on cherche le point d'intersection entre AA-BB et l'approxPoly (à la bourrin)
		    		int t = 0;
		    		while(true)
		    		{
		    			t++;
			    		//Point AAA = vec2Point(AA, t * vAB);
			    		Point AAA = vec2Point(AA, t * points2Vec(AA,BB));
			    		double distAAA = pointPolygonTest(approxPoly, AAA, true);

			    		AA = AAA;

			    		if(distAAA >= 0)
			    		{
			    			cout << "On est rentré dans le polygone pour AA" << endl;
			    			break;
			    		}
			    		else if(distAAA < distAA - 1000)
			    		{
			    			cout << "ERREUR : on s'eloigne !" << endl;
			    			break;
			    		}
		    		}
		    	}
		    	double distBB = pointPolygonTest(approxPoly, BB, true);
		    	if(distBB < 0)
		    	{
		    		// on cherche le point d'intersection entre AA-BB et l'approxPoly (à la bourrin)
		    		int t = 0;
		    		while(true)
		    		{
		    			t++;
			    		//Point BBB = vec2Point(BB, (-1*t) * vAB);
			    		Point BBB = vec2Point(BB, t * points2Vec(BB,AA));
			    		double distBBB = pointPolygonTest(approxPoly, BBB, true);

			    		BB = BBB;

			    		if(distBBB >= 0)
			    		{
			    			cout << "On est rentré dans le polygone pour BB" << endl;
			    			break;
			    		}
			    		else if(distBBB < distBB - 1000)
			    		{
			    			cout << "ERREUR : on s'eloigne !" << endl;
			    			break;
			    		}
		    		}
		    	}

		    	if(altern)
		    	{
			    	path.push_back(BB);
			    	path.push_back(AA);
			    	cout << "nouveau trait : " << BB<< "-"<< AA<<endl;
		    	}
		    	else
		    	{
		    		path.push_back(AA);
			    	path.push_back(BB);
			    	cout << "nouveau trait : " << AA<< "-"<< BB<<endl;
		    	}
	    	}
    	altern = !altern;
    	cmpt++;

    	cv::polylines(rgb_copy, path, false, Scalar(0,255,0), 1, LINE_8, 0);
	displayPicture(0,NULL);

    	//cv::waitKey(0);
    	}
}

Point vec2Point(Point depart, Vector2d vec)
{
	double lx = (double)depart.x + vec[0];
	double ly = (double)depart.y + vec[1];

	return Point(ceil(lx),ceil(ly));
}

Vector2d points2Vec(Point A, Point B) // transforme AB en vecteur unitaire
{
	double moduleX = (B.x - A.x);
	double moduleY = (B.y - A.y);
	double module = sqrt(moduleX*moduleX + moduleY*moduleY);

	Vector2d V;
	V[0] = moduleX/module;
	V[1] = moduleY/module;

	return V;
}

Vector2d points2VecN(Point A, Point B) // transforme AB en vecteur normé
{
	double moduleX = (B.x - A.x);
	double moduleY = (B.y - A.y);

	Vector2d V;
	V[0] = moduleX;
	V[1] = moduleY;

	return V;
}

Vector2d offsetVector(Point A, Point B, double offset) // (unitaire) A et B sont les points de l'arrête à couper, leur ordre spatial ne change pas, et le polygone est codé en sens horraire
{
	Vector2d edge = points2Vec(A,B);

	Vector2d voffset;

	voffset[0] = (-1 * edge[1]) * offset;
	voffset[1] = (edge[0]) * offset;

	return voffset;
}


/* Copyright © 2019 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at
 * https://opensource.org/licenses/BSD-3-Clause
 */

#ifndef TURI_ONE_SHOT_MAPPING_FUNCTION_H_
#define TURI_ONE_SHOT_MAPPING_FUNCTION_H_

#include <Eigen/Core>
#include <Eigen/Dense>

#include <algorithm>
#include <cmath>
#include <limits>
#include <random>
#include <vector>

#include <boost/gil/gil_all.hpp>

namespace boost {
namespace gil {

/* Matrix - Vector multiplication */
template <typename T, typename F>
boost::gil::point2<F> operator*(const boost::gil::point2<T>& p,
                                const Eigen::Matrix<F, 3, 3>& m) {
  float denominator = m(2, 0) * p.x + m(2, 1) * p.y + m(2, 2);
  if (denominator == 0) {
    // TODO: Figure out the right failure behavior for when denominator is 0.
    return boost::gil::point2<F>(0, 0);
  }
  return boost::gil::point2<F>(
      (m(0, 0) * p.x + m(0, 1) * p.y + m(0, 2)) / denominator,
      (m(1, 0) * p.x + m(1, 1) * p.y + m(1, 2)) / denominator);
}

/* Conforming to the MapFn concept required by Boost GIL, for Eigen::Matrix3f
 */
template <typename T>
struct mapping_traits;

template <typename F, typename F2>
boost::gil::point2<F> transform(const Eigen::Matrix<F, 3, 3>& mat,
                                const boost::gil::point2<F2>& src) {
  return src * mat;
}

template <typename F>
struct mapping_traits<Eigen::Matrix<F, 3, 3> > {
  using result_type = boost::gil::point2<F>;
};

}  // namespace gil
}  // namespace boost

#endif  // TURI_ONE_SHOT_MAPPING_FUNCTION_H_


#include <ql/quantlib.hpp>
#include <boost/make_shared.hpp>

using namespace QuantLib;

int main() {

    Date refDate(24, August, 2015);

    Settings::instance().evaluationDate() = refDate;

    Handle<YieldTermStructure> yts(
        boost::make_shared<FlatForward>(refDate, 0.02, Actual365Fixed()));

    boost::shared_ptr<IborIndex> euribor6m =
        boost::make_shared<Euribor>(6 * Months, yts);

    Handle<OptionletVolatilityStructure> capletVol(
        boost::make_shared<ConstantOptionletVolatility>(
            refDate, TARGET(), Following, 0.40, Actual365Fixed()));

    Real rho = atof(getenv("RHO"));
    boost::shared_ptr<SimpleQuote> correlationQuote =
        boost::make_shared<SimpleQuote>(rho);
    Handle<Quote> correlation(correlationQuote);

    Date startDate =
        TARGET().advance(TARGET().advance(refDate, 2 * Days), 10 * Years);
    Date endDate = TARGET().advance(startDate, 6 * Months);
    Natural fixingDays = 2;

    boost::shared_ptr<IborCoupon> coupon = boost::make_shared<IborCoupon>(
        endDate, 1.0, startDate, endDate, fixingDays, euribor6m, 1.0, 0.0,
        Date(), Date(), DayCounter(), true);

    boost::shared_ptr<IborCouponPricer> black76Pricer =
        boost::make_shared<BlackIborCouponPricer>(
            capletVol, BlackIborCouponPricer::Black76);
    boost::shared_ptr<IborCouponPricer> bivariateLnPricer =
        boost::make_shared<BlackIborCouponPricer>(
            capletVol, BlackIborCouponPricer::BivariateLognormal, correlation);

    coupon->setPricer(black76Pricer);
    std::clog << "coupon fixing  = " << coupon->fixingDate() << std::endl;
    std::clog << "coupon start   = " << coupon->accrualStartDate() << std::endl;
    std::clog << "coupon end     = " << coupon->accrualEndDate() << std::endl;
    std::clog << "coupon payment = " << coupon->date() << std::endl;

    std::clog << "Black76: fixing = " << coupon->indexFixing()
              << " adjustment = " << coupon->convexityAdjustment() << std::endl;

    Date paymentDate = startDate;

    while (paymentDate <= TARGET().advance(endDate, 6 * Months)) {
        boost::shared_ptr<IborCoupon> coupon = boost::make_shared<IborCoupon>(
            paymentDate, 1.0, startDate, endDate, fixingDays, euribor6m, 1.0,
            0.0, Date(), Date(), DayCounter(), false);
        coupon->setPricer(bivariateLnPricer);
        Real tau = Actual365Fixed().yearFraction(paymentDate, endDate);
        if (tau >= 0.0) {
            correlationQuote->setValue(rho + (1.0 - rho) * exp(-10.0 * tau));
        } else {
            correlationQuote->setValue(rho);
        }
        std::cout << "\"" << paymentDate << "\" "
                  << yts->timeFromReference(paymentDate) << " "
                  << coupon->indexFixing() << " "
                  << coupon->convexityAdjustment() << std::endl;
        paymentDate++;
    }
}


// -*- compile-command: "c++ -std=c++14 -O3 -DNDEBUG -ffast-math -march=native lcp.cpp -o lcp `pkg-config --cflags eigen3` -lstdc++" -*-

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <iostream>
#include <array>

template<class U>
struct lcp {
  using real = U;

  template<int N>
  using vector = Eigen::Matrix<real, N, 1>;

  template<int N>
  using indices = Eigen::Matrix<Eigen::Index, N, 1>;

  template<int N>
  using matrix = Eigen::Matrix<real, N, N>;

  static void solve(vector<1>& x, const matrix<1>& M, const vector<1>& q) {
    assert(M(0, 0) > 0);
    x(0) = std::max<real>(0, -q(0) / M(0, 0));
  }

  template<int N>
  static void solve(vector<N>& x, matrix<N> M, vector<N> q) {
    static_assert(N > 1, "size error");
    indices<N - 1> sub;
    vector<N> w;

    std::array<bool, N> active;

    for(std::size_t i = 0; i < N; ++i) {
      // filter all but ith component
      auto ptr = sub.data();
      for(std::size_t j = 0; (j += (j == i)) < N; ++j, ++ptr) {
        *ptr = j;
      }

      vector<N - 1> qq = sub.unaryExpr(q);
      matrix<N - 1> MM;
      for(int j = 0; j < N - 1; ++j) {
        MM.col(j) = sub.unaryExpr(M.col(sub(j)));
      }

      vector<N - 1> xx;
      solve(xx, MM, qq);

      w(i) = std::max<real>(sub.unaryExpr(M.col(i)).dot(xx) + q(i), 0);
      active[i] = w(i) == 0;
    }

    for(int i = 0; i < N; ++i) {
      if(!active[i]) {
        M.row(i) = vector<N>::Zero().transpose();
        M.col(i) = vector<N>::Zero();
        M(i, i) = 1;
        q(i) = w(i);
      }
    }

    // TODO use LLT when N > 4?
    x.noalias() = M.inverse() * (w - q);
  }
};

template<class U>
struct closest {
  using real = U;
  using vec3 = Eigen::Matrix<real, 3, 1>;
  using vec2 = Eigen::Matrix<real, 2, 1>;

  using mat3x3 = Eigen::Matrix<real, 3, 3>;
  using mat2x2 = Eigen::Matrix<real, 2, 2>;

  static double clamp(double x) {
    return x;
  }

  // static real clamp(real x) {
  //   return std::max<real>(x, 0);
  // }

  static void fast_lcp(vec3& x, mat3x3 M, vec3 q) {
    const real x1[3] = {
      clamp(-q(0) / M(0, 0)),
      clamp(-q(1) / M(1, 1)),
      clamp(-q(2) / M(2, 2)),
    };

    using vec2i = Eigen::Matrix<Eigen::Index, 2, 1>;
    // TODO make this constexpr?
    const vec2i ind[3] =  {{1, 2}, {0, 2}, {0, 1}};

    mat2x2 M2inv[3];

    vec2 x2[3];
    vec3 w;
    int skip = 0;
    for(int i = 0; i < 3; ++i) {
      mat2x2 sub;               // TODO optimize (symmetric)
      for(int j = 0; j < 2; ++j) {
        sub.col(j) = ind[i].unaryExpr(M.col(ind[i][j]));
      }

      vec2 w2;
      for(int j = 0; j < 2; ++j) {
        const int other = (j + 1) % 2;
        // w2(j) = clamp(sub(j, other) * x1[ind[i][other]] + q(ind[i][j]));
        w2(j) = clamp(M(ind[i][j], ind[i][other]) * x1[ind[i][other]] + q(ind[i][j]));
      }

      // TODO optimize?
      M2inv[i] = sub.inverse();
      x2[i].noalias() = M2inv[i] * (w2 - ind[i].unaryExpr(q));

      w[i] = clamp(ind[i].unaryExpr(M.col(i)).dot(x2[i]) + q(i));

      if(w[i] > 0) {
        skip = i;
      }

    }

    const vec2 res = M2inv[skip] * ind[skip].unaryExpr(w - q);
    x[ind[skip][0]] = res[0];
    x[ind[skip][1]] = res[1];
    x[skip] = 0;
  }

  static vec3 project_triangle(vec3 a, vec3 b, vec3 c, vec3 p) {
    const vec3* points[3] = {&a, &b, &c};

    const vec3 edges[3] = {
      b - a,
      c - b,
      a - c
    };

    const vec3 n = edges[0].cross(edges[1]);

    mat3x3 JT;
    for(int i = 0; i < 3; ++i) {
      JT.col(i) = n.cross(edges[i]);
    }

    vec3 bounds;

    for(int i = 0; i < 3; ++i) {
      bounds(i) = JT.col(i).dot(*points[i]);
    }

    const mat3x3 M = JT.transpose() * JT;
    const vec3 q = (JT.transpose() * p - bounds);

    vec3 lambda;
    // lcp<real>::solve(lambda, M, q);
    fast_lcp(lambda, M, q);

    // std::clog << lambda.transpose() << std::endl;
    // std::clog << (M * lambda + q).transpose() << std::endl;
    // std::clog << lambda.dot(M * lambda + q) << std::endl;

    const vec3 origin = a;
    const vec3 delta = p - origin;
    const vec3 proj = p - n * n.dot(delta) / n.dot(n);

    return JT * lambda + proj;
  }

  struct projector_ref {
    vec3 p1, p2, p3, p1p2, p1p3;
    real distp1p2;
    real fa, fb, fc;
    real fdet;

    projector_ref(vec3 p1, vec3 p2, vec3 p3):
        p1(p1),
        p2(p2),
        p3(p3),
        p1p2(p2 - p1),
        p1p3(p3 - p1),
        distp1p2(p1p2.dot(p1p2)),
        fa(distp1p2),
        fb(p1p2.dot(p1p3)),
        fc(p1p3.dot(p1p3)),
        fdet(fa * fc - fb * fb) {

    }

    vec3 operator()(vec3 p) const {
      const vec3 pp1 = p1 - p;
      const real fd = p1p2.dot(pp1), fe = p1p3.dot(pp1);
      real fs = fb * fe - fc * fd, ft = fb * fd - fa * fe;

      if(fs + ft <= fdet) {
        if(fs < 0) {
          if(ft < 0) {
            // region 4
            if(fd < 0) {
              ft = 0;
              if(-fd >= fa) {
                fs = 1;
              } else {
                fs = -fd / fa;
              }
            } else {
              fs = 0;
              if(fe >= 0) {
                ft = 0;
              } else if(-fe >= fc) {
                ft = 1;
              } else {
                ft = -fe / fc;
              }
            }
          } else {
            // region 3
            fs = 0;
            if(fe >= 0) {
              ft = 0;
            } else if(-fe >= fc) {
              ft = 1;
            } else {
              ft = -fe / fc;
            }
          }
        } else if(ft < 0) {
          // region 5
          ft = 0;
          if(fd >= 0) {
            fs = 0;
          } else if(-fd >= fa) {
            fs = 1;
          } else {
            fs = -fd / fa;
          }

        } else {
          // region 0
          // minimum at interior point
          fs /= fdet;
          ft /= fdet;
        }
      } else {
        real ftmp0, ftmp1, fNumer, fDenom;

        if(fs < 0) {
          // region 2
          ftmp0 = fb + fd;
          ftmp1 = fc + fe;
          if(ftmp1 > ftmp0) {
            fNumer = ftmp1 - ftmp0;
            fDenom = fa - 2 * fb + fc;
            if(fNumer >= fDenom) {
              fs = 1;
              ft = 0;
            } else {
              fs = fNumer / fDenom;
              ft = 1 - fs;
            }
          } else {
            fs = 0;
            if(ftmp1 <= 0) {
              ft = 1;
            } else if(fe >= 0) {
              ft = 0;
            } else {
              ft = -fe / fc;
            }
          }
        } else if(ft < 0) {
          // region 6
          ftmp0 = fb + fe;
          ftmp1 = fa + fd;
          if(ftmp1 > ftmp0) {
            fNumer = ftmp1 - ftmp0;
            fDenom = fa - 2 * fb + fc;
            if(fNumer >= fDenom) {
              ft = 1;
              fs = 0;
            } else {
              ft = fNumer / fDenom;
              fs = 1 - ft;
            }
          } else {
            ft = 0;
            if(ftmp1 <= 0) {
              fs = 1;
            } else if(fd >= 0) {
              fs = 0;
            } else {
              fs = -fd / fa;
            }
          }
        } else {
          // region 1
          fNumer = fc + fe - fb - fd;
          if(fNumer <= 0) {
            fs = 0;
            ft = 1;
          } else {
            fDenom = fa - 2 * fb + fc;
            if(fNumer >= fDenom) {
              fs = 1;
              ft = 0;
            } else {
              fs = fNumer / fDenom;
              ft = 1 - fs;
            }
          }
        }
      }

      return (1 - fs - ft) * p1 + fs * p2 + ft * p3;
    }

  };

  static vec3 project_triangle_ref(vec3 p1, vec3 p2, vec3 p3, vec3 p) {
    const vec3 p1p2 = p2 - p1, p1p3 = p3 - p1, pp1 = p1 - p;
    const real distp1p2 = p1p2.dot(p1p2) // distp2p3 = (p3 - p2).dot(p3 - p2),
               // distp3p1 = p1p3.dot(p1p3)
      ;

    const real fa = distp1p2, fb = p1p2.dot(p1p3), fc = p1p3.dot(p1p3), fd = p1p2.dot(pp1),
      fe = p1p3.dot(pp1);

    const real fdet = fa * fc - fb * fb;
    real fs = fb * fe - fc * fd, ft = fb * fd - fa * fe;

    if(fs + ft <= fdet) {
      if(fs < 0) {
        if(ft < 0) {
          // region 4
          if(fd < 0) {
            ft = 0;
            if(-fd >= fa) {
              fs = 1;
            } else {
              fs = -fd / fa;
            }
          } else {
            fs = 0;
            if(fe >= 0) {
              ft = 0;
            } else if(-fe >= fc) {
              ft = 1;
            } else {
              ft = -fe / fc;
            }
          }
        } else {
          // region 3
          fs = 0;
          if(fe >= 0) {
            ft = 0;
          } else if(-fe >= fc) {
            ft = 1;
          } else {
            ft = -fe / fc;
          }
        }
      } else if(ft < 0) {
        // region 5
        ft = 0;
        if(fd >= 0) {
          fs = 0;
        } else if(-fd >= fa) {
          fs = 1;
        } else {
          fs = -fd / fa;
        }

      } else
      {
        // region 0
        // minimum at interior point
        fs /= fdet;
        ft /= fdet;
      }
    } else {
      real ftmp0, ftmp1, fNumer, fDenom;

      if(fs < 0)
        {
          // region 2
          ftmp0 = fb + fd;
          ftmp1 = fc + fe;
          if(ftmp1 > ftmp0) {
            fNumer = ftmp1 - ftmp0;
            fDenom = fa - 2 * fb + fc;
            if(fNumer >= fDenom) {
              fs = 1;
              ft = 0;
            } else {
              fs = fNumer / fDenom;
              ft = 1 - fs;
            }
          } else {
            fs = 0;
            if(ftmp1 <= 0) {
              ft = 1;
            } else if(fe >= 0) {
              ft = 0;
            } else {
              ft = -fe / fc;
            }
          }
        } else if(ft < 0)
        {
          // region 6
          ftmp0 = fb + fe;
          ftmp1 = fa + fd;
          if(ftmp1 > ftmp0) {
            fNumer = ftmp1 - ftmp0;
            fDenom = fa - 2 * fb + fc;
            if(fNumer >= fDenom) {
              ft = 1;
              fs = 0;
            } else {
              ft = fNumer / fDenom;
              fs = 1 - ft;
            }
          } else {
            ft = 0;
            if(ftmp1 <= 0) {
              fs = 1;
            } else if(fd >= 0) {
              fs = 0;
            } else {
              fs = -fd / fa;
            }
          }
        } else
        {
          // region 1
          fNumer = fc + fe - fb - fd;
          if(fNumer <= 0) {
            fs = 0;
            ft = 1;
          } else {
            fDenom = fa - 2 * fb + fc;
            if(fNumer >= fDenom) {
              fs = 1;
              ft = 0;
            } else {
              fs = fNumer / fDenom;
              ft = 1 - fs;
            }
          }
        }
    }

    return (1 - fs - ft) * p1 + fs * p2 + ft * p3;
  }

  struct projector_alt {
    mat3x3 points;
    mat3x3 edges;
    mat3x3 points_inv;
    vec3 n;
    real n2;

    projector_alt(vec3 a, vec3 b, vec3 c) {
      points << a, b, c;
      edges << c - b, a - c, b - a;

      points_inv = points.inverse();
      n = edges.col(0).cross(edges.col(1));
      n2 = n.dot(n);

    }

    vec3 operator()(vec3 q) const {
      const vec3 delta = q - points.col(0);
      const vec3 proj = q - n * n.dot(delta) / n2;
      const vec3 coords = points_inv * proj;

      int negative = 0;

      // negative, positive
      int last_index[2];
      for(int i = 0; i < 3; ++i) {
        const auto neg = coords[i] < 0;
        negative += neg;
        last_index[neg] = i;
      }

      switch(negative) {
      case 0: return proj;
      case 1: {
        const vec3 e = edges.col(last_index[0]);
        const vec3 origin = points.col(last_index[1]);
        const vec3 delta = q - origin;

        real alpha = e.dot(delta) / e.dot(e);

        // TODO branchless
        // alpha = (alpha > 0) * alpha - (alpha > 1) * (alpha - 1);
        alpha = std::max<real>(0, alpha);
        alpha = std::min<real>(1, alpha);

        return origin + e * alpha;
      }
      case 2: return points.col(last_index[1]);
      default:
        throw std::logic_error("unreachable");
      }
    }
  };

  static vec3 project_triangle_alt(vec3 a, vec3 b, vec3 c, vec3 q) {
    mat3x3 points;
    points << a, b, c;

    mat3x3 edges;
    edges << c - b, a - c, b - a;

    const vec3 n = edges.col(0).cross(edges.col(1));

    const vec3 origin = a;
    const vec3 delta = q - origin;
    const vec3 proj = q - n * n.dot(delta) / n.dot(n);

    const vec3 coords = points.inverse() * proj;

    int negative = 0;

    // negative, positive
    int last_index[2];
    for(int i = 0; i < 3; ++i) {
      const auto neg = coords[i] < 0;
      negative += neg;
      last_index[neg] = i;
    }

    switch(negative) {
    case 0: return proj;
    case 1: {
      const vec3 e = edges.col(last_index[0]);
      const vec3 origin = points.col(last_index[1]);
      const vec3 delta = q - origin;

      real alpha = e.dot(delta) / e.dot(e);
      // alpha = (alpha > 0) * alpha - (alpha > 1) * (alpha - 1);
      alpha = std::max<real>(0, alpha);
      alpha = std::min<real>(1, alpha);

      return origin + e * alpha;
    }
    case 2: return points.col(last_index[1]);
    default:
      throw std::logic_error("unreachable");
    }

  }
};

#include "timer.hpp"
#include <bitset>

Eigen::Matrix<double, 3, 1> solution;

int main(int argc, char** argv) {
  std::srand(time(0));

  {
    using lcp = struct lcp<double>;

    static constexpr int N = 3;

    lcp::matrix<N> M;
    lcp::vector<N> q, x, w;

    M.setRandom();
    M = M.transpose() * M;

    q.setRandom();
    lcp::solve(x, M, q);

    w.noalias() = M * x + q;

    std::cout << "x: " << x.transpose() << std::endl;
    std::cout << "w: " << w.transpose() << std::endl;
    std::cout << "xw: " << x.dot(w) << std::endl;
  }

  {
    using closest = struct closest<double>;

    std::clog << "=================" << std::endl;

    static const auto make_problem = [](auto k) {
      closest::vec3 a, b, c, p;
      a.setRandom();
      b.setRandom();
      c.setRandom();

      p.setRandom();

      return k(a, b, c, p);
    };

    static const int n = 1000000;

    const auto lcp_duration = with_time([] {
      for(int i = 0; i < n; ++i) {
        solution = make_problem([](auto a, auto b, auto c, auto p) {
          return closest::project_triangle(a, b, c, p);
        });
      }
    });

    const auto alt_duration = with_time([] {
      for(int i = 0; i < n; ++i) {
        solution = make_problem([](auto a, auto b, auto c, auto p) {
          return closest::project_triangle_alt(a, b, c, p);
        });
      }
    });

    const auto ref_duration = with_time([] {
      for(int i = 0; i < n; ++i) {
        solution = make_problem([](auto a, auto b, auto c, auto p) {
          return closest::project_triangle_ref(a, b, c, p);
        });
      }
    });

    std::clog << "lcp: " << lcp_duration << std::endl;
    std::clog << "alt: " << alt_duration << std::endl;
    std::clog << "ref: " << ref_duration << std::endl;

    ////////////////////////////////////////////////////////////////////////////////
    const int m = 100;
    const auto proj_alt_duration = with_time([] {
      std::vector<closest::projector_alt> projs;

      for(int i = 0; i < n; ++i) {
        projs.emplace_back(closest::vec3::Random(),
                           closest::vec3::Random(),
                           closest::vec3::Random());
      }

      for(auto& proj: projs) {
        for(int i = 0; i < m; ++i) {
          solution = proj(closest::vec3::Random());
        }
      }
    });

    const auto proj_ref_duration = with_time([] {
      std::vector<closest::projector_ref> projs;

      for(int i = 0; i < n; ++i) {
        projs.emplace_back(closest::vec3::Random(),
                           closest::vec3::Random(),
                           closest::vec3::Random());
      }

      for(auto& proj: projs) {
        for(int i = 0; i < m; ++i) {
          solution = proj(closest::vec3::Random());
        }
      }
    });

    std::clog << "proj alt: " << proj_alt_duration << std::endl;
    std::clog << "proj ref: " << proj_ref_duration << std::endl;

    // closest::mat3x3 B;
    // B.col(0) = a;
    // B.col(1) = b;
    // B.col(2) = c;

    // const closest::vec3 coords = B.inverse() * x;
    // std::clog << "coords sum: " << coords.sum() << std::endl;
    // std::clog << "coords: " << coords.transpose() << std::endl;
  }

  return 0;
}


// Copyright (C) 2017 Vicente J. Botet Escriba
//
//  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)

// <experimental/chrono.hpp>

#include <experimental/chrono.hpp>
#include <experimental/ordinal_array.hpp>
#include <experimental/ordinal_range.hpp>
#include <experimental/ordinal_set.hpp>

namespace stdex = std::experimental;

// Basic frame durations
using half_days     = std::chrono::duration<int32_t,  std::ratio_multiply<std::ratio<12>, std::chrono::hours::period>::type>;
using days     = std::chrono::duration<int32_t,  std::ratio_multiply<std::ratio<24>, std::chrono::hours::period>::type>;
using weeks    = std::chrono::duration<int32_t,  std::ratio_multiply<std::ratio<7>, days::period>::type>;
using years    = std::chrono::duration<int32_t,  std::ratio_multiply<std::ratio<146097, 400>, days::period>::type>;
using months   = std::chrono::duration<int32_t,  std::ratio_divide<years::period, std::ratio<12>>::type>;

// relative numbers
// The interface provided here differs to the one provided in p0355r4
// Missing interfaces
//constexpr weekday_number(const sys_days& dp) noexcept;
//constexpr explicit weekday_number(const local_days& dp) noexcept;
//constexpr explicit operator unsigned() const noexcept;
//constexpr bool ok() const noexcept;
//constexpr weekday_indexed operator[](unsigned index) const noexcept;
//constexpr weekday_last    operator[](last_spec) const noexcept;
// operator<<(OSTREAM) differes

using month_number = stdex::chrono::modulo<months,   years,    std::uint8_t>; //12
using weekday_number = stdex::chrono::modulo<days,   weeks,    std::uint8_t>; //7
using am_pm_number   = stdex::chrono::modulo<half_days,   days,    std::uint8_t>; //2
using hour_number   = stdex::chrono::modulo<std::chrono::hours,   days,    std::uint8_t>; //24
using am_pm_hour   = stdex::chrono::modulo<std::chrono::hours,   half_days,    std::uint8_t>; //12
using minute_number = stdex::chrono::modulo<std::chrono::minutes, std::chrono::hours,   std::uint8_t>; //60
using second_number = stdex::chrono::modulo<std::chrono::seconds, std::chrono::minutes, std::uint8_t>; //60

// todo: time_of_day must be specialized to provide access to hour_number/minute_number/second_number/subseconds depending on Duration
// So the following can be only a representation, not the interface
template <class Duration>
using time_of_day = stdex::chrono::modulo<Duration, days, std::uint32_t >;

// fixme: Should the following conversions work only for time_of_day?
template <class ModuloFrom>
constexpr hour_number to_hour(ModuloFrom m) noexcept
{
    return stdex::chrono::modulo_cast<hour_number, days>(m);
}

template <class ModuloFrom>
constexpr am_pm_number to_am_pm(ModuloFrom m) noexcept
{
  return stdex::chrono::modulo_cast<am_pm_number, days>(m);
}
template <class ModuloFrom>
constexpr am_pm_hour to_am_pm_hour(ModuloFrom m) noexcept
{
  return stdex::chrono::modulo_cast<am_pm_hour, days>(m);
}

// This doesn't works if the time_of_day is in hours
// todo: use SFINAE to remove the overload when the precision of ModuloFrom is less fine grained than minutes
template <class ModuloFrom>
constexpr minute_number to_minute(ModuloFrom m) noexcept
{
  return stdex::chrono::modulo_cast<minute_number, days>(m);
}

template <class ModuloFrom>
constexpr second_number to_second(ModuloFrom m) noexcept
{
  return stdex::chrono::modulo_cast<second_number, days>(m);
}

// todo: add to_subsecond

static_assert(am_pm_hour::cardinal  == 12, "12 hours are not a half-day");
static_assert(am_pm_number::cardinal  == 2, "2 half-days are not a day");
static_assert(hour_number::cardinal  == 24, "24 hours are not a day");
static_assert(minute_number::cardinal  == 60, "60 minutes are not an hour_number");
static_assert(second_number::cardinal  == 60, "60 seconds are not a minute_number");
static_assert(hour_number::min()  == hour_number{0}, "0 is not the min of hour_number");
static_assert(hour_number::max()  == hour_number{23}, "23 is not the min of hour_number");

#include <boost/detail/lightweight_test.hpp>
#include <iostream>

int main()
{
  {
                                            //   ms        sec       min  h
      constexpr time_of_day<std::chrono::milliseconds> t2{ 234+ 1000*(2 + 60 * ( 4 + 8 * 60 ) )};
      std::cout << "t2 = " << t2.count() << "\n";
      time_of_day<std::chrono::milliseconds> t{ std::chrono::milliseconds(234)+ std::chrono::seconds(2) + std::chrono::minutes(4) + std::chrono::hours(20)};
      std::cout << "t = " << t.count() << "\n";

      std::cout << "am_pm_number = " << int(to_am_pm(t).count()) << "\n";
      std::cout << "am_pm_hour = " << int(to_am_pm_hour(t).count()) << "\n";
      std::cout << "hour_number = " << int(to_hour(t).count()) << "\n";
      std::cout << "minute_number = " << int(to_minute(t).count()) << "\n";
      std::cout << "second_number = " << int(to_second(t).count()) << "\n";

      BOOST_TEST(to_am_pm(t).count() == 1);
      BOOST_TEST(to_am_pm_hour(t).count() == 8);
      BOOST_TEST_EQ(to_hour(t).count(), 20);
      BOOST_TEST_EQ(to_minute(t).count(), 4);
      BOOST_TEST_EQ(to_second(t).count(), 2);
  }
  {
      constexpr weekday_number wd{2};
      BOOST_TEST(wd.count() == 2);
  }
  {
    stdex::ordinal_array<int, weekday_number> arr;
    BOOST_TEST(arr.size() == 7);
  }
  {
    constexpr stdex::ordinal_array<int, weekday_number> c {{0, 1, 2, 3, 4, 5, 6}};
    BOOST_TEST(c.size() == 7);
    BOOST_TEST(c[weekday_number{1}] == 1);
    BOOST_TEST(c[weekday_number{2}] == 2);
    BOOST_TEST(c[weekday_number{3}] == 3);
  }
  {
    // fixme:  the type ‘const std::experimental::fundamental_v3::ordinal_range<std::experimental::fundamental_v3::chrono::modulo<std::chrono::duration<int, std::ratio<86400l, 1l> >, std::chrono::duration<int, std::ratio<604800l, 1l> >, unsigned char> >’ of constexpr variable ‘rng’ is not literal
    // Surely it comes from boost::iterator
    //constexpr
    stdex::ordinal_range<weekday_number> rng;
    auto b = rng.begin();
    BOOST_TEST(b->count()==0);
    for (auto w : rng)
    {
      std::cout << w << "\n";
    }
  }
  {
      stdex::ordinal_set<weekday_number> os;
      os[weekday_number{1}]=true;
      BOOST_TEST( 1 == os.count() );
      BOOST_TEST( os.any() );
      BOOST_TEST(os[weekday_number{1}]);
  }
  return ::boost::report_errors();
}


#include <test/unit/math/prim/prob/hmm_util.hpp>
#include <test/unit/math/prim/prob/vector_rng_test_helper.hpp>
#include <stan/math/prim/prob/hmm_latent_rng.hpp>
#include <stan/math/prim/prob/chi_square_lcdf.hpp>
#include <boost/math/distributions.hpp>
#include <boost/random.hpp>
#include <test/unit/math/test_ad.hpp>
#include <test/unit/util.hpp>
#include <gtest/gtest.h>
#include <limits>
#include <vector>

TEST(hmm_rng_test, chiSquareGoodnessFitTest) {
  // With identity transition and constant log_omegas, the sampled latent
  // states are identifcal and follow a Bernoulli distribution parameterized
  // by rho.
  // The samples live on {1, 2}, so we need to deduct the error_index to
  // to make the indices 0-indexed.
  using stan::math::hmm_latent_rng;

  int n_states = 2;
  int n_transitions = 10;
  Eigen::MatrixXd Gamma = Eigen::MatrixXd::Identity(n_states, n_states);
  Eigen::VectorXd rho(n_states);
  rho << 0.65, 0.35;
  Eigen::MatrixXd log_omegas
      = Eigen::MatrixXd::Ones(n_states, n_transitions + 1);

  boost::random::mt19937 rng;
  int N = 10000;

  std::vector<double> expected;
  expected.push_back(N * rho(0));
  expected.push_back(N * rho(1));

  std::vector<int> counts(2);
  std::vector<int> state;

  for (int i = 0; i < N; ++i) {
    state = hmm_latent_rng(log_omegas, Gamma, rho, rng);
    for (int j = 1; j < n_states; ++j)
      EXPECT_EQ(state[j], state[0]);

    ++counts[state[0] - stan::error_index::value];
  }

  assert_chi_squared(counts, expected, 1e-6);
}

TEST(hmm_rng_test, chiSquareGoodnessFitTest_symmetric) {
  // In this two states situation, the latent states are
  // symmetric, based on the observational log density,
  // and transition matrix.
  // The initial conditions introduces an asymmetry in the first
  // state. The other hidden states all have probability 0.5.
  // Note 1: the hidden states are also uncorrelated.
  // Note 2: as before, to do a chi-squared test, we subtract
  //  the error_index from hidden_state, to produce variables
  //  on {0, 1}.
  using stan::math::hmm_latent_rng;

  int n_states = 2;
  int n_transitions = 1;
  Eigen::MatrixXd Gamma(n_states, n_states);
  Gamma << 0.5, 0.5, 0.5, 0.5;
  Eigen::VectorXd rho(n_states);
  rho << 0.3, 0.7;
  Eigen::MatrixXd log_omegas
      = Eigen::MatrixXd::Ones(n_states, n_transitions + 1);

  boost::random::mt19937 rng;
  int N = 10000;

  std::vector<double> expected_0;
  expected_0.push_back(N * rho(0));
  expected_0.push_back(N * rho(1));

  std::vector<double> expected_1;
  expected_1.push_back(N * 0.5);
  expected_1.push_back(N * 0.5);

  std::vector<int> counts_0(2);
  std::vector<int> counts_1(2);
  // int product = 0;
  std::vector<int> states;
  int a = 0, b = 0, c = 0, d = 0;
  for (int i = 0; i < N; ++i) {
    states = hmm_latent_rng(log_omegas, Gamma, rho, rng);
    ++counts_0[states[0] - stan::error_index::value];
    ++counts_1[states[1] - stan::error_index::value];
    // product += states[0] * states[1];
    a += (states[0] == stan::error_index::value
          && states[1] == stan::error_index::value);
    b += (states[0] == stan::error_index::value
          && states[1] == 1 + stan::error_index::value);
    c += (states[0] == 1 + stan::error_index::value
          && states[1] == stan::error_index::value);
    d += (states[0] == 1 + stan::error_index::value
          && states[1] == 1 + stan::error_index::value);
  }

  // Test the marginal probabilities of each variable
  assert_chi_squared(counts_0, expected_0, 1e-6);
  assert_chi_squared(counts_1, expected_1, 1e-6);

  // Test for independence (0 correlation by construction).
  // By independence E(XY) = E(X)E(Y). We compute the R.H.S
  // analytically and the L.H.S numerically.
  std::vector<int> counts_xy(2);
  counts_xy[0] = a;
  counts_xy[1] = c;
  std::vector<double> expected_xy;
  expected_xy.push_back(N * rho(0) * 0.5);
  expected_xy.push_back(N * rho(1) * 0.5);
  assert_chi_squared(counts_xy, expected_xy, 1e-6);

  // DRAFT -- code for chi-squared independence test.
  // (overkill, since we have analytical prob for each cell)
  // Test that the two states are independent, using a chi squared
  // test for independence.
  // Eigen::MatrixXd Expected(n_states, (n_transitions + 1));
  // Expected << (a + b) * (a + c), (a + b) * (b + d),
  //             (c + d) * (a + c), (c + d) * (b + d);
  // Expected = Expected / N;
  //
  // Eigen::MatrixXd Observed(n_states, (n_transitions + 1));
  // Observed << a, b, c, d;
  // double chi = 0;
  //
  // for (int i = 0; i < n_states; ++i)
  //   for (int j = 0; j < n_transitions + 1; ++j)
  //     chi += (Observed(i, j) - Expected(i, j))
  //             * (Observed(i, j) - Expected(i, j)) / Expected(i, j);
  //
  // int nu = 1;
  // double p_value = exp(stan::math::chi_square_lcdf(chi, nu));
  // double threshold = 0.1;  // CHECK -- what is an appropriate threshold?
  // EXPECT_TRUE(p_value > threshold);
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2016 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#define FUSION_MAX_VECTOR_SIZE 25  // increase maximal vector size for boost/fusion vectors
#include <iostream>
#include <algorithm>  // min, max

#include <boost/timer/timer.hpp>

#include "dune/grid/config.h"
#include "dune/grid/uggrid.hh"
#include "dune/istl/solvers.hh"

#include "fem/assemble.hh"
#include "fem/istlinterface.hh"
#include "fem/gridmanager.hh"
#include "fem/lagrangespace.hh"
//#include "fem/hierarchicspace.hh"   // ContinuousHierarchicMapper
#include "fem/norms.hh"
#include "linalg/iluprecond.hh"
#include "io/vtk.hh"
//#include "io/amira.hh"
#include "utilities/kaskopt.hh"
#include "utilities/gridGeneration.hh" //  createUnitSquare

using namespace Kaskade;
#include "sst.hh"

struct InitialValue
{
  using Scalar = double;
  static constexpr int components = 1;
  using ValueType = Dune::FieldVector<Scalar,components>;

  InitialValue(int c): component(c) {}

  template <class Cell> int order(Cell const&) const { return std::numeric_limits<int>::max(); }
  template <class Cell>
  ValueType value(Cell const& cell,
          Dune::FieldVector<typename Cell::Geometry::ctype,Cell::Geometry::coorddimension> const& localCoordinate) const
  {
  Dune::FieldVector<typename Cell::Geometry::ctype,Cell::Geometry::coorddimension> x = cell.geometry().global(localCoordinate);
  if (component==0)
    return 1.306028e6;
  else if (component==1)
    return 1.076508e12;
  else if (component==2)
    return 6.457715e10;
  else if (component==3)
    return 3.542285e10;
  else
    assert("wrong index!\n"==0);
  return 0;

  }

private:
  int component;
};

int main(int argc, char *argv[])
{
  using Scalar = double;
  using namespace boost::fusion;

  std::cout << "Start sst transfer tutorial program with ordinary Newton iteration" << std::endl;

  boost::timer::cpu_timer totalTimer;

  int verbosityOpt = 1;
  bool dump = true;
  std::unique_ptr<boost::property_tree::ptree> pt = getKaskadeOptions(argc, argv, verbosityOpt, dump);

  int  direct,
     refinements = getParameter(pt, "refinement", 5),
     order =  getParameter(pt, "order", 2),
     verbosity   = getParameter(pt, "verbosity", 0);
  Scalar tol = getParameter(pt, "tolerance", 1.0e-10);
  //   IterateType iterateType = IterateType::CG;
  //   PrecondType precondType = PrecondType::NONE;
  Scalar fsign = static_cast<Scalar>(getParameter(pt, "sign", 0.0));
  std::string empty;

  std::cout << "refinements of original mesh   : " << refinements << std::endl;
  std::cout << "discretization order           : " << order << std::endl;
  std::cout << "tolerance for Newton iteration : " << tol << std::endl;

  std::string s("names.type.");
  s += getParameter(pt, "solver.type", empty);
  direct = getParameter(pt, s, 0);

  //   s = "names.iterate." + getParameter(pt, "solver.iterate", empty);
  //   iterateType = static_cast<IterateType>(getParameter(pt, s, 0));
  //   s = "names.preconditioner." + getParameter(pt, "solver.preconditioner", empty);
  //   precondType = static_cast<PrecondType>(getParameter(pt, s, 0));

  constexpr int dim=2;
  using Grid = Dune::UGGrid<dim>;
  using LeafView = Grid::LeafGridView;
  using H1Space = FEFunctionSpace<ContinuousLagrangeMapper<Scalar,LeafView> >;
// using H1Space = FEFunctionSpace<ContinuousHierarchicMapper<Scalar,LeafView> >;
  using Spaces = boost::fusion::vector<H1Space const*>;
  using VariableDescriptions = boost::fusion::vector<Variable<SpaceIndex<0>,Components<1>,VariableId<0> >,
                               Variable<SpaceIndex<0>,Components<1>,VariableId<1> >,
                               Variable<SpaceIndex<0>,Components<1>,VariableId<2> >,
                               Variable<SpaceIndex<0>,Components<1>,VariableId<3> > >;
  using VariableSet = VariableSetDescription<Spaces,VariableDescriptions>;
  using Functional = SSTFunctional<Scalar,VariableSet>;
  using Assembler = VariationalFunctionalAssembler<LinearizationAt<Functional> >;
  constexpr int neq = SSTFunctional<Scalar,VariableSet>::TestVars::noOfVariables;
  using CoefficientVectors = VariableSet::CoefficientVectorRepresentation<0,neq>::type;
  using LinearSpace = VariableSet::CoefficientVectorRepresentation<0,neq>::type;

  GridManager<Grid> gridManager( createUnitSquare<Grid>() );
  gridManager.globalRefine(refinements);
  std::cout << std::endl << "Grid: " << gridManager.grid().size(0) << " triangles, " << std::endl;
  std::cout << "      " << gridManager.grid().size(1) << " edges, " << std::endl;
  std::cout << "      " << gridManager.grid().size(2) << " points" << std::endl;

  // construction of finite element space for the scalar solution T
  H1Space temperatureSpace(gridManager,gridManager.grid().leafGridView(),
               order);
  Spaces spaces(&temperatureSpace);
  // VariableDescription<int spaceId, int components, int Id>
  // spaceId: number of associated FEFunctionSpace
  // components: number of components in this variable
  // Id: number of this variable
  std::string varNames[4] = { "u0", "u1", "u2", "u3" };
  VariableSet variableSet(spaces,varNames);

  Functional F;
  Assembler assembler(gridManager,spaces);
  VariableSet::VariableSet x(variableSet);
  VariableSet::VariableSet newtonCorr(variableSet);
  VariableSet::VariableSet tmp(variableSet);

  constexpr int nvars = SSTFunctional<Scalar,VariableSet>::AnsatzVars::noOfVariables;
  size_t  nnz = assembler.nnz(0,neq,0,nvars,false);
  size_t  size = variableSet.degreesOfFreedom(0,nvars);
  std::cout << "nvars=" << nvars << ", neq=" << neq << ", size=" << size << ", nnz=" << nnz << "\n";

  std::vector<Scalar> xdata(size), scal(size), dxdata(size);

  int k=0;
  L2Norm l2Norm;
  Scalar norm_dx, norm_rhs;
  std::vector<Scalar> norm_dx_comp(4), norm_rhs_comp(4);

  F.scaleInitialValue<0>(InitialValue(0),x);
  F.scaleInitialValue<1>(InitialValue(1),x);
  F.scaleInitialValue<2>(InitialValue(2),x);
  F.scaleInitialValue<3>(InitialValue(3),x);
  x.write(xdata.begin());
  for (int i=0;i<size;i++) scal[i]=std::max(std::abs(xdata[i]),1.0);

  CoefficientVectors solution(VariableSet::CoefficientVectorRepresentation<0,neq>::init(spaces));

  writeVTKFile(x,"graph/sst_start",IoOptions().setOrder(std::min(order,2)).setPrecision(7));
  gridManager.enforceConcurrentReads(false);
  Dune::InverseOperatorResult res;

  std::cout << std::endl << "Newton iteration starts:" << std::endl <<
            "iter  scaled ||corr||            ||F||    itsol: eps     result  steps      rate        time"
            << std::endl;

// begin of ordinary Newton iteration loop
  do {
    boost::timer::cpu_timer assembTimer;
    assembler.assemble(linearization(F,x));
    size_t  nnz = assembler.nnz(0,neq,0,nvars,false);
    AssembledGalerkinOperator<Assembler,0,neq,0,neq> A(assembler);
    CoefficientVectors rhs(assembler.rhs());

    tmp.data = rhs.data;
    norm_rhs_comp[0]=l2Norm(boost::fusion::at_c<0>(tmp.data));
    norm_rhs_comp[1]=l2Norm(boost::fusion::at_c<1>(tmp.data));
    norm_rhs_comp[2]=l2Norm(boost::fusion::at_c<2>(tmp.data));
    norm_rhs_comp[3]=l2Norm(boost::fusion::at_c<3>(tmp.data));
    norm_rhs = sqrt(norm_rhs_comp[0]*norm_rhs_comp[0]+norm_rhs_comp[1]*norm_rhs_comp[1]+
                    norm_rhs_comp[2]*norm_rhs_comp[2]+norm_rhs_comp[3]*norm_rhs_comp[3]);

    boost::timer::cpu_timer iteTimer;
    int iteSteps = getParameter(pt, "solver.iteMax", 1000);
    Scalar iteEps = getParameter(pt, "solver.iteEps", 1.0e-12);
    int fill_lev = getParameter(pt, "solver.ILUK.fill_lev", 0);
    ILUKPreconditioner<AssembledGalerkinOperator<Assembler,0,neq,0,nvars> > iluk(A,fill_lev,verbosity);
    Dune::BiCGSTABSolver<LinearSpace> cg(A,iluk,iteEps,iteSteps,verbosity);
    solution = 0;
    cg.apply(solution,rhs,res);
    newtonCorr.data = solution.data;
    newtonCorr *= -1;
    // add Newton-correction
    x += newtonCorr;

    newtonCorr.write(dxdata.begin());
    for (int i=0;i<size;i++) dxdata[i] /= scal[i];
    tmp.read(dxdata.begin());
    norm_dx_comp[0]=l2Norm(boost::fusion::at_c<0>(tmp.data));
    norm_dx_comp[1]=l2Norm(boost::fusion::at_c<1>(tmp.data));
    norm_dx_comp[2]=l2Norm(boost::fusion::at_c<2>(tmp.data));
    norm_dx_comp[3]=l2Norm(boost::fusion::at_c<3>(tmp.data));
    norm_dx = sqrt(norm_dx_comp[0]*norm_dx_comp[0]+norm_dx_comp[1]*norm_dx_comp[1]+
                   norm_dx_comp[2]*norm_dx_comp[2]+norm_dx_comp[3]*norm_dx_comp[3]);

    x.write(xdata.begin());
    for (int i=0;i<size;i++) scal[i]=std::max(std::abs(xdata[i]),1.0);

    std::cout << std::setw(4) << k+1 << "  "
              << std::setw(15) << std::setprecision(5) << std::scientific << norm_dx << "  "
              << std::setw(15) << norm_rhs << "  " << std::setprecision(6);
    std::cout.unsetf(std::ios::fixed | std::ios::scientific);
    std::cout << std::setw(12) << iteEps << "  "
              << (res.converged?"converged":"failed  ") << "  "
              << std::setw(5) << res.iterations << "  "
              << std::setw(8) << res.conv_rate << "  "
              << std::setw(9) << (Scalar)(iteTimer.elapsed().user)/1e9 << "s " << std::endl;

    // output of solution in VTK format for visualization,
    // the data are written as ascii stream into file temperature.vtu,
    // possible is also binary
    std::ostringstream fname;
    fname << "graph/sst_";
    fname.width(2);
    fname.fill('0');
    fname.setf(std::ios_base::right,std::ios_base::adjustfield);
    fname << k;
    fname.flush();
    writeVTKFile(x,fname.str(),IoOptions().setOrder(std::min(order,2)).setPrecision(7));

    // output of solution for Amira visualization,
    // the data are written in binary format into file temperature.am,
    // possible is also ascii
    //IoOptions options;
    //options.outputType = IoOptions::ascii;
    //LeafView leafGridView = gridManager.grid().leafGridView();
    //writeAMIRAFile(leafGridView,x,"sst",options);
  k ++;
  }
  while ( norm_dx > tol );
 // end of ordinary Newton iteration loop

  std::cout << "total computing time: " << boost::timer::format(totalTimer.elapsed()) << "\n";
  std::cout << "End sst transfer tutorial program" << std::endl;
}


#pragma once

#include <mona/rect.hpp>
#include <mona/mesh.hpp>

#include "line_renderer.hpp"
#include "colors.hpp"

#include <armadillo>

namespace mona
{
    class line: public mesh
    {
        line_renderer renderer;
        mona::rect span_area;

    public:
        glm::vec4 color{};

        line(const arma::fvec& x, const arma::fvec& y, const arma::fvec& z,
             glm::vec4 color = colors::black);
        line(const arma::fvec& x, const arma::fvec& y,
             glm::vec4 color = colors::black);
        line();

        // returns the area that this series cover
        // this is to be used to calculate the axes tick values
        auto span() const -> mona::rect override
        {
            return span_area;
        }

        auto draw(const glm::mat4& mvp) const -> void override
        {
            renderer.draw(mvp, color, 1.5f);
        }

        auto reset(const arma::fvec& x, const arma::fvec& y) -> void;

        // TODO: I am not sure if this is to stay
        auto empty() const -> bool;

        auto set_strip(bool strip) -> void
        {
            renderer.set_strip(strip);
        }
    };
};


//          Copyright Carl Philipp Reh 2009 - 2016.
// 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)

#include <fcppt/math/matrix/row.hpp>
#include <fcppt/math/matrix/static.hpp>
#include <fcppt/math/vector/comparison.hpp>
#include <fcppt/math/vector/output.hpp>
#include <fcppt/math/vector/static.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <fcppt/config/external_end.hpp>

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	math_matrix_view
)
{
FCPPT_PP_POP_WARNING

	typedef int value_type;

	constexpr std::size_t const size = 3;

	typedef
	fcppt::math::matrix::static_<
		value_type,
		size,
		size
	>
	matrix_type;

	typedef
	fcppt::math::vector::static_<
		value_type,
		size
	>
	vector_type;

	matrix_type const t(
		fcppt::math::matrix::row(
			-3, 2, -5
		),
		fcppt::math::matrix::row(
			-1, 0, -2
		),
		fcppt::math::matrix::row(
			3, -4, 1
		)
	);

	BOOST_CHECK_EQUAL(
		t[0],
		vector_type(
			-3,
			2,
			-5
		)
	);

	vector_type vec(
		t[0]
	);

	vec = t[1];

	BOOST_CHECK_EQUAL(
		vec,
		vector_type(
			-1,
			0,
			-2
		)
	);
}


#ifndef MI_OPEN_GL_UTILITY_HPP
#define MI_OPEN_GL_UTILITY_HPP 1
#include <Eigen/Dense>
namespace mi
{
        class OpenGlUtility
        {
        public:
                static void lookAt( const  Eigen::Vector3d& eye, const Eigen::Vector3d& center, Eigen::Vector3d& up );
                static void perspective ( const double fov, const double aspect,const double znear, const double zfar );
                static void ortho ( const double left, const double  right, const double  bottom, const double  top, const double znear, const double zfar );
                static void ortho2d ( const double  left, const double  right, const double  bottom, const double  top );
        private:
                static void setZero ( double* m );
        };
}
#endif//MI_OPEN_GL_UTILITY_HPP


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

#ifndef BOOST_HASH_ADLER_HPP
#define BOOST_HASH_ADLER_HPP

#include <boost/hash/detail/primes.hpp>
#include <boost/hash/digest.hpp>
#include <boost/hash/pack.hpp>
#include <boost/static_assert.hpp>

#ifdef BOOST_HASH_SHOW_PROGRESS
#include <cstdio>
#endif

namespace boost {
namespace hashes {

template <unsigned Bits>
class basic_adler {
  public:
    static unsigned const value_bits = 8;
    typedef uint_t<value_bits>::least value_type;

    BOOST_STATIC_ASSERT(Bits % 2 == 0);
    BOOST_STATIC_ASSERT(Bits >= value_bits);

    static unsigned const digest_bits = Bits;

    static unsigned const word_bits = Bits;
    typedef typename uint_t<word_bits>::least word_type;

    typedef boost::array<word_type, 2> state_type;

    typedef hashes::digest<digest_bits> digest_type;

    static word_type const modulo = detail::largest_prime<Bits/2>::value;

  public:
    basic_adler() { reset(); }
    void reset() { state_[0] = 0; state_[1] = 1; }

    digest_type digest() const {
        word_type x = state_[0] << (Bits/2) | state_[1];
        digest_type d;
        // RFC 1950, Section 2.2 stores the ADLER-32 in big-endian
        pack_n<stream_endian::big_bit,
               digest_bits,
               octet_bits>(&x, 1,
                           d.data(), digest_bits/octet_bits);
        return d;
    }

    digest_type end_message() {
        digest_type d = digest();
        reset();
        return d;
    }

  public:

    basic_adler &
    update_one(value_type x) {
        if (Bits < 16) x %= modulo; // avoid overflow
#ifdef BOOST_HASH_SHOW_PROGRESS
printf("(%.4x, %.4x) + %.2x ==> ", (int)state_[0], (int)state_[1], (int)x);
#endif
        state_[1] = (state_[1] + x) % modulo;
        state_[0] = (state_[0] + state_[1]) % modulo;
#ifdef BOOST_HASH_SHOW_PROGRESS
printf("(%.4x, %.4x) mod %.4x\n", (int)state_[0], (int)state_[1], (int)modulo);
#endif
        return *this;
    }

    template <typename IterT>
    basic_adler &
    update_n(IterT p, size_t n) {
#ifndef BOOST_HASH_NO_OPTIMIZATION

        unsigned const fast_word_bits = (word_bits < 16 ? 16 : word_bits);
        typedef typename uint_t<fast_word_bits>::least/*fast*/ fast_word_type;
/*

Worst-case behaviour for delaying the modulo:
- every input is 255
- s1 and s0 start out at modulo-1

So after k inputs, we have:
- s1 = (modulo-1) + k*255
- s0 = (modulo-1) + Sigma(i = 1 to k)[ (modulo-1) + i*255 ]
     = (modulo-1) + k*(modulo-1) + Sigma(i = 1 to k)[ i*255 ]
     = (k+1)*(modulo-1) + 255 * Sigma(i = 1 to k)[i]
     = (k+1)*(modulo-1) + 255 * k*(k+1)/2

And to avoid overflow we need s1, s0 <= 2**fast_word_bits - 1

s1 = (modulo-1) + k*255 <= 2**fast_word_bits - 1
     k*255 <= 2**fast_word_bits - 1 - (modulo-1)
     k <= (2**fast_word_bits - modulo)/255

Then use an overestimate for s0 to make the numbers nicer
s0 < (k+1)*modulo + 256/2(k+1)**2 < 2**fast_word_bits

Which solves as
k < ( sqrt(512*2**fast_word_bits + modulo**2) - m - 256 )/256

So then overestimating m as 2**(word_bits/2) and other safe approximations gives
k < 2**((fast_word_bits-7)/2) - 2**((word_bits-16)/2) - 1

Bits    Limit
----    -----
8       16
16      16
24      240
32      3840
40      61440
48      983040
56      15728640
64      251658240

*/

        unsigned const less = (1 << (fast_word_bits/2 - 8));
        unsigned const limit = (1 << (fast_word_bits/2 - 4))
                             - (word_bits < 16 ? 0 : less);

#define BOOST_HASH_ADLER_STEP \
        { value_type x = *p++; s1 += x; s0 += s1; }

#define BOOST_HASH_ADLER_8_STEPS \
        { \
            BOOST_HASH_ADLER_STEP BOOST_HASH_ADLER_STEP \
            BOOST_HASH_ADLER_STEP BOOST_HASH_ADLER_STEP \
            BOOST_HASH_ADLER_STEP BOOST_HASH_ADLER_STEP \
            BOOST_HASH_ADLER_STEP BOOST_HASH_ADLER_STEP \
        }

        fast_word_type s0 = state_[0];
        fast_word_type s1 = state_[1];

        for ( ; n >= limit; n -= limit) {
            unsigned m = limit;
            for ( ; m >= 8; m -=8) {
                BOOST_HASH_ADLER_8_STEPS
            }
            while (m--) {
                BOOST_HASH_ADLER_STEP
            }
            s1 %= modulo;
            s0 %= modulo;
        }
        for ( ; n >= 8; n -=8) {
            BOOST_HASH_ADLER_8_STEPS
        }
        while (n--) {
            BOOST_HASH_ADLER_STEP
        }
        s1 %= modulo;
        s0 %= modulo;

        state_[0] = s0;
        state_[1] = s1;

#else
        while (n--) update_one(*p++);
#endif
        return *this;
    }

    template <typename IterT>
    basic_adler &
    update(IterT b, IterT e, std::random_access_iterator_tag) {
        return update_n(b, e-b);
    }
    template <typename IterT, typename Category>
    basic_adler &
    update(IterT b, IterT e, Category) {
        while (b != e) update_one(*b++);
        return *this;
    }
    template <typename IterT>
    basic_adler &
    update(IterT b, IterT e) {
        typedef typename std::iterator_traits<IterT>::iterator_category cat;
        return update(b, e, cat());
    }

  private:
    state_type state_;
};

template <unsigned Bits>
struct adler {
  private:
    typedef basic_adler<Bits> octet_hash_type;
  public:
    template <unsigned value_bits>
    struct stream_hash {
        BOOST_STATIC_ASSERT(value_bits == 8);
        typedef octet_hash_type type_;
#ifdef BOOST_HASH_NO_HIDE_INTERNAL_TYPES
        typedef type_ type;
#else
        struct type : type_ {};
#endif
    };
    typedef typename octet_hash_type::digest_type digest_type;
};

} // namespace hashes
} // namespace boost

#endif // BOOST_HASH_ADLER_HPP


// __BEGIN_LICENSE__
//  Copyright (c) 2009-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NGT platform is licensed under the Apache License, Version 2.0 (the
//  "License"); you may not use this file except in compliance with the
//  License. You may obtain a copy of the License at
//  http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
// __END_LICENSE__

#include <test/Helpers.h>
#include <vw/Math/Functors.h>
#include <vw/Math/Vector.h>
#include <vw/Image/ImageView.h>
#include <vw/Image/ImageMath.h>
#include <vw/Image/ImageViewRef.h>
#include <vw/InterestPoint/InterestData.h>
#include <asp/Core/IntegralAutoGainDetector.h>

#include <boost/foreach.hpp>
#include <boost/random/normal_distribution.hpp>

using namespace vw;
using namespace asp;

TEST( IntegralAutoGainDetector, VerifyMaxima ) {

  ImageView<float> test_image(101,101);

  const float SIGMA = 3;

  // Drawing a DoG signal
  Vector2i location, center(50,50);
  for ( ; location.y() < test_image.rows(); location.y()++ ) {
    for ( location.x() = 0; location.x() < test_image.cols(); location.x()++ ) {
      float distance = norm_2( Vector2f(location - center) );
      test_image( location.x(), location.y() ) =
        40 * (
              1/(2*M_PI*SIGMA*SIGMA)*exp(-pow(distance,2)/(2*SIGMA*SIGMA)) -
              1/(2*M_PI*4*SIGMA*SIGMA)*exp(-pow(distance,2)/(2*4*SIGMA*SIGMA))
              ) + 0.5;
    }
  }

  // Detect interest points
  IntegralAutoGainDetector detector;
  ip::InterestPointList list = detector.process_image( test_image );
  EXPECT_EQ( list.size(), 9 ); // Digitization error

  // Find the best IP
  const ip::InterestPoint* best_ip = &list.front();
  float best_ip_value = list.begin()->interest;
  BOOST_FOREACH( ip::InterestPoint const& ip, list ) {
    if ( ip.interest > best_ip_value ) {
      best_ip_value = ip.interest;
      best_ip = &ip;
    }
  }

  // The best IP is the one centered on the circle feature we
  // drew. There are more due to digitization errors and float point
  // errors in the the integral image.
  EXPECT_EQ( best_ip->x, 50 );
  EXPECT_EQ( best_ip->y, 50 );
  EXPECT_EQ( best_ip->scale, 1.875 );

  // This is odd ... but I'm just verifying the impl calls the same
  // code. This was a bug that elluded me for a very long time.
  ASSERT_EQ( typeid(detector),
             typeid(detector.impl()) );
}


/*! \file sabrRbsSmile.hpp
    \brief swaption smile which uses sabr interpolation "near" atm and an rbs parametric smile for extrapolation
	atm of underlying cube is always exactly reproduced
	vol spreads are interpolated linearly in option and swap maturity direction, flat extrapolation is used
	these spreads are added to base cube atm vol
	Peter Caspers
*/

#include <ql/quantlib.hpp>
#include <sabrModel.hpp>
#include <RbsSmile.hpp>
#include <iostream>
#include <fstream>
#include <boost/property_tree/detail/rapidxml.hpp>
#include <map>

#ifndef quantlib_sabrRbsSmile_hpp
#define quantlib_sabrRbsSmile_hpp

#define MINSTRIKE 0.0010   // left from this strike, this strike is used always
//#define MINDISTATM 0.00001 // below this difference strike is treated as atm

using namespace boost;
using namespace std;

namespace QuantLib {

	/*! sabr/parametric swaption smile with the purpose to match to cms market */

	class SabrRbsSmile {
		public:
			/*! smile is set up by
			- swaption vol cube (as the basis)
			- left and right end point (relative to atm) defining the interpolation area, outside extrapolation is used
			- option tenors, swap tenors w.r.t. sabr / parametric smiles are calibrated
			- start values for sabr calibration (alpha, nu, rho. beta is taken from time termstructure for beta below)
			- option, underlying cms calibration termstructure (given as pillars) for parameters mu, nu (parametric) and beta (sabr)
			- matrix for mu, nu, beta corresponding to cms calibration termstructure
			- strike spreads (relative to atm) defining the strikes on which SABR models are calibrated
			- vegaWeighted = true then difference in implied vols is weighted by vega
			- acceptRmse rmse for sabr calibration below which sabr smile is accepted
			- rejectRmse rmse for sabr calibration above which fall back smile is used insted. between these two, model is accepted (but more start values are tried to find a better fit)
			- haltonIterations the give start values plus a halton sequence of this length is tried to achieve acceptRmse
			- optimizer, endcriteria are the optimizer and the end criteria for optimization
			- BLACKACCURACY = accuracy to convert price to vol
			- hDiff is used to compute numerically first and second derivatives
			*/
			SabrRbsSmile(boost::shared_ptr<SwaptionVolatilityCube>& volCube,
				const vector<Period>& optionTenors, const vector<Period>& swapTenors,
				const Matrix& alpha, const Matrix& nu, const Matrix& rho,
				double leftBound, double rightBound,
				const vector<Period>& optionPillars, const vector<Period>& underlyingPillars,
				const Matrix& pMu, const Matrix& pNu, const Matrix& beta,
				vector<double>& strikeSpreads, bool vegaWeighted=true, double acceptRmse=0.0020, double rejectRmse=0.02,
				int haltonIterations=150,
				const boost::shared_ptr<EndCriteria>& endCriteria=boost::shared_ptr<EndCriteria>(),
                const boost::shared_ptr<OptimizationMethod>& optMethod=boost::shared_ptr<OptimizationMethod>(),
				double BLACKACCURACY=1.0E-5,
				double hDiff=1.0E-4);

			/*! get volatility from sabr / parametric smile.
				if spread true then strike=atm+spread is calculated, otherwise strike must be  given
				if market is true then the vol from the base cube is returned
				if sabr calibration is rejected (rmse>rejectRmse) also the vol from the base cube is returned as fall back
				bilinear interpolation (with flat extrapolation) is used if option / swap tenor does not fall on pillar */
			double volatility(const Date& fixing, const Period& tenor, const double& strike0, bool spread=false, bool market=false);
			double volatility(const Period& option, const Period& tenor, const double& strike0, bool spread=false, bool market=false);

			/*! preset sabr / parametric model to get volatility more efficiently */
			bool setFastVolatility(const Date& fixing, const Period& tenor);
			/*! get volatility from preset sabr / parametric smile model
			    note that spread mode = true and market = true is not available here*/
			double getFastVolatility(const double& strike);

			/*! get option price (not discounted) from sabr / parametric smile (only on defined pillars)
				if forceSabr is true the sabr model (or fall back cube) is always used (never rbs), needed for rbs smile setup */
			double optionPrice(const Date& fixing, const Period& tenor, const double& strike0, const Option::Type type, bool spread=false, bool market=false, bool forceSabr=false);
			double optionPrice(const Period& option, const Period& tenor, const double& strike0, const Option::Type type, bool spread=false, bool market=false, bool forceSabr=false);

			/*! get matrix of sabr parameters and rmse
			    rmse is -1.0 if calibration failed
				best rmse is returned even if calibration is rejected
				in both cases (=no valid sabr model) the start values for alpha, nu, rho are returned */
			Matrix sabrAlpha() { return alpha_; }
			Matrix sabrNu() { return nu_; }
			Matrix sabrRho() { return rho_; }
			Matrix sabrRmse() { return sabrRmse_; }

			/*! get matrix of sabr beta, rbs smile mu and nu on option / swap pillars
			    if a sabr model is not valid, -1 is returned for beta*/
			Matrix sabrBeta();
			Matrix rbsMu();
			Matrix rbsNu();

			/*! get matrix of sabr beta, rbs smile mu and nu on cms calibration termstructure */
			Matrix sabrBetaTs() { return beta_; }
			Matrix rbsMuTs() { return pMu_; }
			Matrix rbsNuTs() { return pNu_; }

			/*! get underlying vol cube */
			boost::shared_ptr<SwaptionVolatilityCube> volCube() { return volCube_; }

			/*! get option pillar times */
			vector<double> optionPillarTimes()  { return optTimes_; }

			/*! set parameter mu at a pillar */
			bool setPillarMu(int optPillar, int undPillar, double mu);
			/*! set parameter nu at a pillar */
			bool setPillarNu(int optPillar, int undPillar, double nu);
			/*! set parameter beta at a pillar */
			bool setPillarBeta(int optPillar, int undPillar, double beta);
			/*! get pillar mu */
			double pillarMu(int optPillar, int undPillar) { return pMu_[undPillar][optPillar]; }
			/*! get pillar nu */
			double pillarNu(int optPillar, int undPillar) { return pNu_[undPillar][optPillar]; }
			/*! get pillar beta */
			double pillarBeta(int optPillar, int undPillar) { return beta_[undPillar][optPillar]; }

			/*! get mu, nu, beta for given option time and swap time */
			double mu(double optMaturity, double undMaturity) { return muInterpol_(optMaturity,undMaturity,true); }
			double nu(double optMaturity, double undMaturity) { return nuInterpol_(optMaturity,undMaturity,true); }
			double beta(double optMaturity, double undMaturity) { return betaInterpol_(optMaturity,undMaturity,true); }

			/*! get SABR parameters for (fixing, tenor) */
			vector<double> sabrParameters(const Date& fixing, const Period& tenor);
			vector<double> sabrParameters(const Period& option, const Period& tenor);

			/*! get RBS Smile parameters for (fixing, tenor) */
			vector<double> rbsParameters(const Date& fixing, const Period& tenor);
			vector<double> rbsParameters(const Period& option, const Period& tenor);

			/*! write murex xml file. if inPath is given, atms in this xml file are used */
			bool writeMurexFile(string outPath, string nickname, string date, string swap, vector<string>& optionNames, vector<string>& swapNames,
				vector<Period>& optionTenors,vector<Period>& swapTenors,
				vector<double> strikeSpreads, string inPath="");

			/*! recalibrate models on pillar pair (sabr and rbs smile) w.r.t. given
				termstructure of beta, mu, nu (as set by methods setPillarMu ...)
			    returns true if succesful, false otherwise (no changes are made then in the model, see sabrBeta(), rbsMu(), rbsNu(),
				though given termstructure of mu, nu, beta returned by rbsMuTs(), rbsNuTs(), sabrBetaTs() ... remains same)
				if calSabr false only the rbs smiles are recalibrated (e.g. if only mu, nu was changed, not beta */
			bool recalibrate(const Period& option, const Period& tenor, bool calSabr);

			/*! recalibrate all models, returns false if one recalibration is not succesfull
				if calSabr false only the rbs smiles are recalibrated (e.g. if only mu, nu was changed, not beta */
			bool recalibrate(bool calSabr);

			/*! recalibrate all models with option tenor leq given option pillar and underlying equal to given underlying pillar */
			bool recalibrate(const int maxOptPillar, const int undPillar, bool calSabr);

			/*! return fixing dates of cube */
			vector<Date> fixingDates() { return fixings_; }

		private:

			/*! set up all sabr models */
			void setupSabrModels();

			/*! set up all rbs smiles */
			void setupRbsSmiles();

			/*! get time from referenceDate w.r.t. day counter of input cube */
			double time(const Date& date);

			/*! get sabr model for given (fixing date, swap tenor) */
			boost::shared_ptr<SabrModel> sabrModel(const Period& option, const Period& swap);
			boost::shared_ptr<SabrModel> sabrModel(const Date& fixing, const Period& swap);

			/*! get rbs smile for given (fixing date, swap Tenor) */
			boost::shared_ptr<RbsSmile> rbsSmile(const Period& option, const Period& swap);
			boost::shared_ptr<RbsSmile> rbsSmile(const Date& fixing, const Period& swap);

			boost::shared_ptr<SwaptionVolatilityCube> volCube_; // base cube
			vector<Period> optionTenors_,swapTenors_; // model pillars (sabr / rbs smile)
			vector<Date> fixings_; // fixing dates corresponding to option tenors
			Matrix alpha_,nu_,rho_; // matrix of sabr parameters
			Matrix sabrRmse_; // matrix of sabr rmse (calibration error)

			vector<Period> optPillars_, undPillars_; // term structure of beta, mu, nu
			vector<double> optTimes_, undTimes_; // corresponding times (for interpolation)
			Matrix pMu_,pNu_,beta_; // corresponding pillar parameters

			double leftBound_, rightBound_,h_; // sabr region, step for numerical differentation
			vector<double> strikeSpreads_; // smile spreads (relative to atm) on which sabr is calibrated

			bool vegaWeighted_;
			double acceptRmse_,rejectRmse_; // sabr optimization parameters
			int haltonIterations_;
			const boost::shared_ptr<EndCriteria>& endCriteria_;
            const boost::shared_ptr<OptimizationMethod>& optMethod_;

			Interpolation2D muInterpol_,nuInterpol_,betaInterpol_; // linear interpolation for beta, mu, nu

			Date referenceDate_; // underlying cube data
			Calendar calendar_;
			BusinessDayConvention bdc_;
			DayCounter dc_;

			map<pair<Date,Period>,bool> sabrValid_; // maps fixing, underlying tenor to true (sabr is well calibrated) or false (sabr could not be calibrated)
			map<pair<Date,Period>,bool>::iterator sabrValidIter_; // iterator
			map<pair<Date,Period>,boost::shared_ptr<SabrModel>> sabrModels_; // maps fixing, underlying tenor to sabr model
			map<pair<Date,Period>,boost::shared_ptr<SabrModel>>::iterator sabrModelsIter_; // iterator

			map<pair<Date,Period>,boost::shared_ptr<RbsSmile>> rbsSmiles_; // maps fixing, underlying tenor to rbs parametric smiles
			map<pair<Date,Period>,boost::shared_ptr<RbsSmile>>::iterator rbsSmilesIter_; // iterator

			double BLACKACCURACY, OPTACCURACY;

			// member variable for setFastVolatility(), getFastVolatility()
			double fastAtm11_,fastAtm12_,fastAtm21_,fastAtm22_,fastAtm_;
			double fastAtmVol11_,fastAtmVol12_,fastAtmVol21_,fastAtmVol22_;
			double fastOptionTime_,fastOptionTime1_,fastOptionTime2_;
			double fastSwapTime_,fastSwapTime1_,fastSwapTime2_;
			boost::shared_ptr<SabrModel> fastSabrModel11_,fastSabrModel12_,fastSabrModel21_,fastSabrModel22_;
			boost::shared_ptr<RbsSmile> fastRbsModel11_,fastRbsModel12_,fastRbsModel21_,fastRbsModel22_;
			bool fastOptEq_,fastSwapEq_;
			Date fastFixing_;
			Period fastTenor_;
			bool fastValid_;

	};

}

#endif


#include <blitz/array.h>
#include <blitz/tinyvec-et.h>

BZ_USING_NAMESPACE(blitz)

/*
 * The current implementation of stencil objects forces these variables
 * to be placed in global scope.  Ugh.  This restriction will be removed
 * eventually.
 */
double rho;                        // Density of fluid
double recip_rho;                  // 1/rho
double eta;                        // Kinematic viscosity
double time_now;                   // Elapsed seconds
double delta_t;                    // Time step
double volume;                     // Volume of a cell
double airPressure;                // Air pressure (Pa)
double spatialStep;                // Grid element size
double gravity;                    // Acceleration due to gravity
double gravityPressureGradient;    // Pressure gradient due to gravity
/*
 * The "geometry" object specifies how an array is mapped into real-world
 * space.  In this case, "UniformCubicGeometry" is used, which means that
 * the real-world grid is orthogonal, regularly spaced, with the same spatial
 * step in each dimension.
 */

UniformCubicGeometry<3> geom;      // Geometry
/*
 * Some typedefs to make life easier.
 */

typedef TinyVector<double,3> vector3d;
typedef Array<vector3d,3> vectorField;
typedef Array<double,3>   scalarField;

/***********          Timestep the velocity field           ************
 * This is a 63-point stencil.  For example, Laplacian3DVec4 turns into
 * a 45-point stencil: each 2nd derivative is a 5-point stencil, and
 * there are 9 of these derivatives to take the Laplacian of a 3D vector
 * field.
 */

BZ_DECLARE_STENCIL5(timestep, V, nextV, P, advect, force)

    nextV = *V + delta_t * ( recip_rho * (
      eta * Laplacian3DVec4(V,geom) - grad3D4(P, geom) + *force) - *advect);

BZ_END_STENCIL
/*
 * Allocate arrays and set their initial state
 */
void setup(const int N, vectorField& V, vectorField& nextV, scalarField& P,
    scalarField& P_rhs, vectorField& advect, vectorField& force)
{
    // A 1m x 1m x 1m domain
    spatialStep = 1.0 / (N - 1);
    geom = UniformCubicGeometry<3>(spatialStep);

    // Allocate arrays
    allocateArrays(shape(N,N,N), advect, V, nextV, force);  // vector fields
    allocateArrays(shape(N,N,N), P, P_rhs);                 // scalar fields

    // Since incompressibility is assumed, pressure only shows up as
    // derivative terms in the equations.  We choose airPressure = 0
    // as an arbitrary datum.

    airPressure = 0;             // Pa
    rho = 1000;                  // density of fluid, kg/m^3
    recip_rho = 1.0 / rho;       // inverse of density
    eta = 1.0e-6;                // kinematic viscosity of fluid, m^2/s
    gravity = 9.81;              // m/s^2
    delta_t = 0.001;             // initial time step, in seconds
    volume = pow3(spatialStep);  // cubic volume associated with grid point

    // Kludge: Set eta high, so that the flow will spread faster.
    // This means the cube is filled with molasses, rather than water.
    eta *= 1000;

    // Initial conditions: quiescent
    V = 0.0;
    P_rhs = 0.0;
    advect = 0.0;
    nextV = 0.0;
    P = 0.0;
    force = 0.0;
}

// Calculate a simple check on a vector field
void record(vectorField& V)
{
    // Calculate the magnitude of a field
    const int x=0, y=1, z=2;
    double magx = sum(pow2(V[x])) / V.numElements();
    double magy = sum(pow2(V[y])) / V.numElements();
    double magz = sum(pow2(V[z])) / V.numElements();

    cout << "norm = [" << magx
        << " " << magy << " " << magz << " ]" << endl;
}

void iterate(vectorField& V, vectorField& nextV, scalarField& P,
    scalarField& P_rhs, vectorField& advect, vectorField& force)
{
    // Time step
    applyStencil(timestep(), V, nextV, P, advect, force);
}

int main()
{
    vectorField V, nextV;        // Velocity fields
    scalarField P, P_rhs;        // Pressure fields
    vectorField advect;          // Advection field
    vectorField force;           // Forcing function

    const int N = 50;            // Arrays are NxNxN

    setup(N, V, nextV, P, P_rhs, advect, force);

    const int nIters = 10;

    for (int i=0; i < nIters; ++i)
    {
        iterate(V, nextV, P, P_rhs, advect, force);
    }

    return 0;
}


#include <iostream>
#include <vector>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>

// Graph Type with nested interior edge properties for flow algorithms
typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> traits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
    boost::property<boost::edge_capacity_t, long,
        boost::property<boost::edge_residual_capacity_t, long,
            boost::property<boost::edge_reverse_t, traits::edge_descriptor>>>> graph;

typedef traits::vertex_descriptor vertex_desc;
typedef traits::edge_descriptor edge_desc;

class edge_adder {
  graph &G;

 public:
  explicit edge_adder(graph &G) : G(G) {}

  void add_edge(int from, int to, long capacity) {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    const auto e = boost::add_edge(from, to, G).first;
    const auto rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0; // reverse edge has no capacity!
    r_map[e] = rev_e;
    r_map[rev_e] = e;
  }
};

using namespace std;

int index(int row, int col, bool in, int cols) {
  return 2 * (row * cols + col) + in;
}

void solve() {
  int cols; cin >> cols;
  int rows; cin >> rows;
  int k; cin >> k;
  int c; cin >> c;

  graph G(2 * rows * cols);
  edge_adder adder(G);

  if (cols == 0 || rows == 0 || k == 0 || c == 0) {
    cout << 0 << endl;
    return;
  }

  // Set up graph
  for (int row = 0; row < rows; ++row) {
    for (int col = 0; col < cols; ++col) {
      // In to out vertex
      adder.add_edge(index(row, col, true, cols), index(row, col, false, cols), c);

      // Neighbors
      if (row > 0)
        adder.add_edge(index(row - 1, col, false, cols), index(row, col, true, cols), 1);
      if (row < rows - 1)
        adder.add_edge(index(row + 1, col, false, cols), index(row, col, true, cols), 1);
      if (col > 0)
        adder.add_edge(index(row, col - 1, false, cols), index(row, col, true, cols), 1);
      if (col < cols - 1)
        adder.add_edge(index(row, col + 1, false, cols), index(row, col, true, cols), 1);

    }
  }

  // Set up target
  vertex_desc target = boost::add_vertex(G);
  for (int row = 0; row < rows; ++row) {
    adder.add_edge(index(row, 0, false, cols), target, 1);
    adder.add_edge(index(row, cols - 1, false, cols), target, 1);
  }
  for (int col = 0; col < cols; ++col) {
    adder.add_edge(index(0, col, false, cols), target, 1);
    adder.add_edge(index(rows - 1, col, false, cols), target, 1);
  }

  // Set up source
  vertex_desc source = boost::add_vertex(G);
  int row, col;
  for (int i = 0; i < k; ++i) {
    cin >> col;
    cin >> row;
    adder.add_edge(source, index(row, col, true, cols), 1);
  }

  long flow = boost::push_relabel_max_flow(G, source, target);
  cout << flow << endl;
}

int main() {
  ios_base::sync_with_stdio(false);
  int t; cin >> t;
  for (int i = 0; i < t; ++i) {
    solve();
  }
}


/*
   Copyright (C) 2014 - 2015 by Chris Beck <render787@gmail.com>
   Part of the Battle for Wesnoth Project http://www.wesnoth.org/

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2 of the License, or
   (at your option) any later version.
   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY.

   See the COPYING file for more details.
*/

#pragma once

#include <boost/cstdint.hpp>
#include <boost/random/mersenne_twister.hpp>

using boost::uint32_t;

//class config;

namespace wesnoth
{

/*
   This class provides an interface, similar to simple_rng, to the
   boost mt19937 generator.
*/

class mt_rng
{
public:
	mt_rng();
	//explicit mt_rng(const config& cfg);
	explicit mt_rng(boost::uint32_t seed);
	/** Get a new random number. */
	uint32_t get_next_random();

	/**
	 *  Same as uint32_t version, but uses a stringstream to convert given
         *  hex string.
         *  @param seed         A hex string. Should not have 0x leading.
         *  @param call_count   Value to set internal call counter to after seeding.
         */
	void seed_random(const std::string & seed, const unsigned int call_count = 0);

	/**
	 * Resets the random to the 0 calls and the seed to the random
	 *  this way we stay in the same sequence but don't have a lot
	 *  calls. Used when moving to the next scenario.
	 */
	void rotate_random();

	uint32_t get_random_seed() const { return random_seed_; }
	std::string get_random_seed_str() const;
	unsigned int get_random_calls() const { return random_calls_; }

	//Comparisons, mainly used for testing
	bool operator== (const mt_rng &other) const;
	bool operator!= (const mt_rng &other) const
	{ return !operator==(other); }

private:
	/** Initial seed for the pool. */
	uint32_t random_seed_;

	/** State for the random pool (boost mersenne twister random generator). */
	boost::mt19937 mt_;

	/** Number of time a random number is generated. */
	unsigned int random_calls_;

	/** On my local version of boost::random, I can use mt_.discard to discard a number of rng results.
	In older versions this seems to be unavailable. I'm implementing as a private method of mt_rng,
	following description here: http://www.boost.org/doc/libs/1_51_0/doc/html/boost/random/mersenne_twister_engine.html#id1408119-bb
	*/
	void discard(const unsigned int call_count);

	/**
	 *  Seeds the random pool. This is the old version, I would like to mark this private.
	 *
	 *  @param seed         The initial value for the random engine.
	 *  @param call_count   Upon loading we need to restore the state at saving
	 *                      so set the number of times a random number is
	 *                      generated for replays the orginal value is
	 *                      required.
	 */
	void seed_random(const uint32_t seed, const unsigned int call_count = 0);
};

} // ends wesnoth namespace


#include <iostream>
#include <cassert>
#include <vector>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/successive_shortest_path_nonnegative_weights.hpp>
#include <boost/graph/find_flow_cost.hpp>

typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> GraphTraits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
                              boost::property<boost::edge_capacity_t, long,
                                              boost::property<boost::edge_residual_capacity_t, long,
                                                              boost::property<boost::edge_reverse_t, GraphTraits::edge_descriptor,
                                                                              boost::property<boost::edge_weight_t, long>>>>>
    Graph;

const int debug_level = 0;

#define DEBUG(min_level, x)      \
  if (debug_level >= min_level)  \
  {                              \
    std::cerr << x << std::endl; \
  }

struct BoatSailorPair
{
  int b, s, c;
};

const int max_possible_spectacle = 50;

void testcase()
{
  int b, s, p;
  std::cin >> b >> s >> p;
  assert(b >= 1 && b <= 500 && s >= 1 && s <= 500 && p >= 1 && p <= b * s && p <= 5000);

  std::vector<BoatSailorPair> boat_sailor_pairs(p);
  for (BoatSailorPair &pair : boat_sailor_pairs)
  {
    std::cin >> pair.b >> pair.s >> pair.c;
    assert(pair.b >= 0 && pair.b < b && pair.s >= 0 && pair.s < s && pair.c >= 1 && pair.c <= max_possible_spectacle);
  }

  int next_node_index = 0;
  const int node_source = next_node_index++;
  const int node_target = next_node_index++;
  const auto get_node_boat = [next_node_index, b](int i) {
    assert(i >= 0 && i < b);
    return next_node_index + i;
  };
  next_node_index += b;
  const auto get_node_sailor = [next_node_index, s](int i) {
    assert(i >= 0 && i < s);
    return next_node_index + i;
  };
  next_node_index += s;
  const int num_nodes = next_node_index;
  Graph G(num_nodes);

  const auto add_edge = [&G](int from, int to, long capacity, long cost) {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    auto w_map = boost::get(boost::edge_weight, G);
    const Graph::edge_descriptor e = boost::add_edge(from, to, G).first;
    const Graph::edge_descriptor rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0;
    r_map[e] = rev_e;
    r_map[rev_e] = e;
    w_map[e] = cost;
    w_map[rev_e] = -cost;
  };

  for (int i = 0; i < b; i++)
  {
    add_edge(node_source, get_node_boat(i), 1, 0);
    add_edge(get_node_boat(i), node_target, 1, max_possible_spectacle);
  }

  for (int i = 0; i < s; i++)
  {
    add_edge(get_node_sailor(i), node_target, 1, 0);
  }

  for (BoatSailorPair &pair : boat_sailor_pairs)
  {
    add_edge(get_node_boat(pair.b), get_node_sailor(pair.s), 1, max_possible_spectacle - pair.c);
  }

  boost::successive_shortest_path_nonnegative_weights(G, node_source, node_target);
  const int flow_cost = boost::find_flow_cost(G);
  const int spectacle_sum = b * max_possible_spectacle - flow_cost;
  assert(spectacle_sum > 0);
  std::cout << spectacle_sum << "\n";
}

int main()
{
  std::ios_base::sync_with_stdio(false);

  int t;
  std::cin >> t;
  for (int i = 0; i < t; i++)
  {
    testcase();
    DEBUG(1, "");
  }

  return 0;
}