#include <path_follower/controller/robotcontroller_2steer_inputscaling.h>

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <path_follower/utils/pose_tracker.h>
#include <path_follower/utils/visualizer.h>

#include <cslibs_navigation_utilities/MathHelper.h>

#include <visualization_msgs/Marker.h>

#include <deque>
#include <limits>
#include <boost/algorithm/clamp.hpp>

#include <time.h>

#ifdef TEST_OUTPUT
#include <std_msgs/Float64MultiArray.h>
#endif

#include <path_follower/factory/controller_factory.h>

REGISTER_ROBOT_CONTROLLER(RobotController_2Steer_InputScaling, 2steer_inputscaling, default_collision_avoider);

RobotController_2Steer_InputScaling::RobotController_2Steer_InputScaling() :
    RobotController(),
    phi_(0.)
{

    const double k = params_.k_forward();
    setTuningParameters(k);

    ROS_INFO("Parameters: k_forward=%f, k_backward=%f\n"
                "vehicle_length=%f\n"
                "goal_tolerance=%f\n"
                "max_steering_angle=%f\nmax_steering_angle_speed=%f",
                params_.k_forward(), params_.k_backward(),
                params_.vehicle_length(),
                params_.goal_tolerance(),
                params_.max_steering_angle(), params_.max_steering_angle_speed());

#ifdef TEST_OUTPUT
    test_pub_ = nh_.advertise<std_msgs::Float64MultiArray>("test_output", 100);
#endif
}

void RobotController_2Steer_InputScaling::setTuningParameters(const double k) {
    k1_ = 1. * k * k * k;
    k2_ = 3. * k * k;
    k3_ = 3. * k;
}

void RobotController_2Steer_InputScaling::stopMotion() {

    move_cmd_.setVelocity(0.f);
    move_cmd_.setDirection(0.f);

    phi_ = 0.;

    MoveCommand cmd = move_cmd_;
    publishMoveCommand(cmd);
}

void RobotController_2Steer_InputScaling::start() {

}

void RobotController_2Steer_InputScaling::reset() {
    old_time_ = ros::Time::now();

    RobotController::reset();
}

void RobotController_2Steer_InputScaling::setPath(Path::Ptr path) {
    RobotController::setPath(path);

    if (getDirSign() < 0.)
        setTuningParameters(params_.k_backward());
    else
        setTuningParameters(params_.k_forward());
}

RobotController::MoveCommandStatus RobotController_2Steer_InputScaling::computeMoveCommand(
        MoveCommand* cmd) {

    std::clock_t begin = std::clock();

    if(path_interpol.n() <= 2)
        return RobotController::MoveCommandStatus::ERROR;

    const Eigen::Vector3d pose = pose_tracker_->getRobotPose();
    const geometry_msgs::Twist v_meas_twist = pose_tracker_->getVelocity();

    double velocity_measured = dir_sign_ * sqrt(v_meas_twist.linear.x * v_meas_twist.linear.x
            + v_meas_twist.linear.y * v_meas_twist.linear.y);

    RobotController::findOrthogonalProjection();
    double d = orth_proj_;

    if(RobotController::isGoalReached(cmd)){
       return RobotController::MoveCommandStatus::REACHED_GOAL;
    }

    if (getDirSign() < 0.)
        setTuningParameters(params_.k_backward());
    else
        setTuningParameters(params_.k_forward());

    // draw a line to the orthogonal projection
    geometry_msgs::Point from, to;
    from.x = pose[0]; from.y = pose[1];
    to.x = path_interpol.p(proj_ind_); to.y = path_interpol.q(proj_ind_);
    visualizer_->drawLine(12341234, from, to, getFixedFrame(), "kinematic", 1, 0, 0, 1, 0.01);

    // theta_e = theta_vehicle - theta_path (orientation error)
    double theta_e = MathHelper::AngleDelta(path_interpol.theta_p(proj_ind_), pose[2]);

    // if dir_sign is negative we drive backwards and set theta_e to the complementary angle
    if (getDirSign() < 0.)
        theta_e = theta_e > 0.? -M_PI + theta_e : M_PI + theta_e;

    // curvature and first two derivations
    const double c = path_interpol.curvature(proj_ind_);
    const double dc_ds = path_interpol.curvature_prim(proj_ind_);
    const double dc_ds_2 = path_interpol.curvature_sek(proj_ind_);

    // 1 - dc(s)
    const double _1_dc = 1. - d * c;
    const double _1_dc_2 = _1_dc * _1_dc;

    // cos, sin, tan of theta_e and phi
    const double cos_theta_e = cos(theta_e);
    const double cos_theta_e_2 = cos_theta_e * cos_theta_e;
    const double cos_theta_e_3 = cos_theta_e_2 * cos_theta_e;

    const double sin_theta_e = sin(theta_e);
    const double sin_theta_e_2 = sin_theta_e * sin_theta_e;

    const double tan_theta_e = tan(theta_e);
    const double tan_theta_e_2 = tan_theta_e * tan_theta_e;

    const double sin_phi = sin(phi_);

    //
    // actual controller formulas begin here
    //

    // x1 - x4
    //    const double x1 = s;
    const double x2 = -dc_ds * d * tan_theta_e
            - c * _1_dc * (1. + sin_theta_e_2) / cos_theta_e_2
            + 2. * _1_dc_2 * sin_phi / (params_.vehicle_length() * cos_theta_e_3);

    const double x3 = _1_dc * tan_theta_e;
    const double x4 = d;

    // u1, u2

    const double u1 = velocity_ * cos_theta_e / _1_dc;
    const double u2 =
            - k1_ * fabs(u1) * x4
            - k2_ * u1 * x3
            - k3_ * fabs(u1) * x2;

    // derivations of x2 (for alpha1)
    const double dx2_dd = -dc_ds * tan_theta_e
            + c * c * (1. + sin_theta_e_2) / cos_theta_e_2
            - 4. * _1_dc * c * sin_phi / (params_.vehicle_length() * cos_theta_e_3);

    const double dx2_dtheta_e = -dc_ds * d * (1. + tan_theta_e_2)
            - 4. * c * _1_dc * tan_theta_e / cos_theta_e_2
            + 6. * _1_dc_2 * sin_phi * tan_theta_e / (params_.vehicle_length()
                                                                    * cos_theta_e_3);
    const double dx2_ds =
            - dc_ds_2 * d * tan_theta_e
            - (dc_ds * _1_dc - d * dc_ds * c) * (1. + sin_theta_e_2) / cos_theta_e_2
            - 4. * _1_dc * dc_ds * d * sin_phi / (params_.vehicle_length() * cos_theta_e_3);

    // alpha1
    const double alpha1 =
            dx2_ds
            + dx2_dd * _1_dc * tan_theta_e
            + dx2_dtheta_e * (2. * sin_phi * _1_dc / (params_.vehicle_length() * cos_theta_e) - c);

    // alpha2
    const double alpha2 = params_.vehicle_length() * cos_theta_e_3 / (2. * _1_dc_2 * cos(phi_));

    // longitudinal velocity
    const double v1 = velocity_;

    // steering angle velocity
    double v2 = alpha2 * (u2 - alpha1 * u1);

    // limit steering angle velocity
    v2 = boost::algorithm::clamp(v2, -params_.max_steering_angle_speed(),
                                            params_.max_steering_angle_speed());

    // time
    const double time_passed = (ros::Time::now() - old_time_).toSec();
    old_time_ = ros::Time::now();

    // update delta according to the time that has passed since the last update
    phi_ += v2 * time_passed;

    // also limit the steering angle
    phi_ = boost::algorithm::clamp(phi_, -params_.max_steering_angle(), params_.max_steering_angle());

    double exp_factor = RobotController::exponentialSpeedControl();
    move_cmd_.setDirection(getDirSign() * (float) phi_);
    move_cmd_.setVelocity(getDirSign() * (float) v1 * exp_factor);
    *cmd = move_cmd_;

#ifdef TEST_OUTPUT
    publishTestOutput(proj_ind_, d, theta_e, phi_, velocity_measured);
#endif

    ROS_DEBUG("d=%f, theta_e=%f, c=%f, c'=%f, c''=%f", d, theta_e, c, dc_ds, dc_ds_2);
    ROS_DEBUG("1 - dc(s)=%f", _1_dc);
    ROS_DEBUG("dx2dd=%f, dx2dtheta_e=%f, dx2ds=%f", dx2_dd, dx2_dtheta_e, dx2_ds);
    ROS_DEBUG("alpha1=%f, alpha2=%f, u1=%f, u2=%f", alpha1, alpha2, u1, u2);
    ROS_DEBUG("frame time = %f", (((float) (std::clock() - begin)) / CLOCKS_PER_SEC));

    return RobotController::MoveCommandStatus::OKAY;
}

void RobotController_2Steer_InputScaling::publishMoveCommand(
        const MoveCommand& cmd) const {

    geometry_msgs::Twist msg;
    msg.linear.x  = cmd.getVelocity();
    msg.linear.y  = 0;
    msg.angular.z = cmd.getDirectionAngle();

    cmd_pub_.publish(msg);
}

#ifdef TEST_OUTPUT
void RobotController_2Steer_InputScaling::publishTestOutput(const unsigned int waypoint, const double d,
                                                                            const double theta_e,
                                                                            const double phi, const double v) const {
    std_msgs::Float64MultiArray msg;

    msg.data.push_back((double) waypoint);
    msg.data.push_back(d);
    msg.data.push_back(theta_e);
    msg.data.push_back(phi);
    msg.data.push_back(v);

    test_pub_.publish(msg);
}
#endif


#include <bits/stdc++.h>
//#include <boost/multiprecision/cpp_int.hpp>
//#define ll cpp_int
#define ll long long
#define rep(i, n) for (ll i = 0; i < (n); ++i)
#define fi(a, b, c, d) for (int a = b; a < c; a += d)
#define fl(a, b, c, d) for (ll a = b; a < c; a += d)
#define fei(a, b, c, d) for (int a = b; a <= c; a += d)
#define fel(a, b, c, d) for (ll a = b; a <= c; a += d)
#define all(x) (x).begin(), (x).end()
#define vi vector<int>
#define vll vector<ll>
#define viip vector<pair<int, int>>
#define vllp vector<pair<ll, ll>>
#define pb emplace_back
// pairs
#define pi pair<int, int>
#define pll pair<ll, ll>
#define mp make_pair
#define F first
#define S second
// using boost::multiprecision::cpp_int;
using namespace std;
bool iseven(ll n)
{
    if (n % 2 == 0)
        return true;
    else
    {
        return false;
    }
}
ll position(ll a)
{
    // vector<ll>p(30,0);
    // unordered_set<ll>s;
    ll s = 0;
    long double x = a;
    int i = 0;
    while (a > 0)
    {
        if ((a & 1) == 1)
        {
            // s.insert(i);
            s++;
        }

        a = a >> 1;
        i++;
    }

    return s;
}
ll power(ll x, unsigned ll y)
{
    ll res = 1; // Initialize result

    while (y > 0)
    {
        // If y is odd, multiply x with result
        if (y & 1)
            res = res * x;

        // y must be even now
        y = y >> 1; // y = y/2
        x = x * x;  // Change x to x^2
    }
    return res;
}
void print(vector<ll> a, ll n)
{
    for (int i = 0; i < n; i++)
        cout << a[i] << " ";
    cout << "\n";
}
bool sec(const pair<ll, ll> &a, const pair<ll, ll> &b)
{
    if (a.first == b.first)
        return (a.second > b.second);
    else
    {
        return (a.first > b.first);
    }
}
vector<ll> suffixsum(vll &a, ll n)
{
    vll pre(n, 0);
    pre[n - 1] = a[n - 1];
    for (int i = n - 2; i >= 0; i--)
    {
        pre[i] = pre[i + 1] + a[i];
    }
    return pre;
}
vector<ll> preffixsum(vll &a, ll n)
{
    vll pre(n, 0);
    pre[0] = a[0];
    for (int i = 1; i < n; i++)
    {
        pre[i] = pre[i - 1] + a[i];
    }
    return pre;
}

vector<vector<int>> check(
    vector<string> arr)
{
    vector<vector<int>> ans;
    for (auto s : arr)
    {
        int c = 0;
        string temp = "";
        vector<int> v;
        for (int i = 0; i < s.size(); i++)
        {
            if (s[i] == ' ')
            {
                if (c == 0)
                {
                    if (temp == "discount")
                    {
                        v.push_back(0);
                    }
                }
                else
                {
                    v.push_back(stoi(temp));
                }
                temp = "";
                c++;
            }
            else
            {
                temp += s[i];
            }
        }
        v.push_back(stoi(temp));
        ans.push_back(v);
    }
    return ans;
}

void solve()
{

    // cout << (a + b) / 4 << "\n";
}

int main()
{
    ios_base::sync_with_stdio(false);
    cin.tie(NULL);
    ll t = 1;
    cin >> t;
    while (t--)
    {
        // solve();
        vector<vector<int>> ans = check({"discount 20 49", "discount 20 49 60"});
        for (auto v : ans)
        {
            for (auto i : v)
            {
                cout << i << " ";
            }
            cout << "\n";
        }
    }

    return 0;
}


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2013-2013 by Ryo IGARASHI <rigarash@issp.u-tokyo.ac.jp>
*               2014-2014    Synge Todo <wistaria@comp-phys.org>
*
* 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 <boost/foreach.hpp>

#include <rokko/grid.hpp>
#include <rokko/solver.hpp>
#include <rokko/distributed_matrix.hpp>
#include <rokko/utility/frank_matrix.hpp>

#define BOOST_TEST_MODULE test_distributed_matrix
#ifndef BOOST_TEST_DYN_LINK
#include <boost/test/included/unit_test.hpp>
#else
#include <boost/test/unit_test.hpp>
#endif

unsigned int dim_global;

double frank_calculate_matrix_element(int i, int j) {
  return dim_global - std::max(i, j);
}

BOOST_AUTO_TEST_CASE(test_distributed_matrix) {
  int argc = boost::unit_test::framework::master_test_suite().argc;
  char** argv = boost::unit_test::framework::master_test_suite().argv;
  unsigned int dim = 10;
  dim_global = dim;
  int provided;
  MPI_Init_thread(&argc, &argv, MPI_THREAD_MULTIPLE, &provided);
  MPI_Comm comm = MPI_COMM_WORLD;
  rokko::grid g(comm);
  BOOST_FOREACH(std::string name, rokko::parallel_dense_solver::solvers()) {
    rokko::parallel_dense_solver solver(name);
    solver.initialize(argc, argv);
    rokko::distributed_matrix<rokko::matrix_col_major> mat(dim, dim, g, solver);
    mat.generate(frank_calculate_matrix_element);
    mat.print();
    //    BOOST_CHECK_CLOSE(mat.get_global(0, 0), dim, 1e-14);
    solver.finalize();
  }
  MPI_Finalize();
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EULER_FUNCTIONS_SIMD_COMMON_GAMMA_HPP_INCLUDED
#define NT2_EULER_FUNCTIONS_SIMD_COMMON_GAMMA_HPP_INCLUDED

#include <nt2/euler/functions/gamma.hpp>
#include <nt2/euler/functions/details/gamma_kernel.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/pi.hpp>
#include <nt2/include/constants/three.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/functions/simd/abs.hpp>
#include <nt2/include/functions/simd/any.hpp>
#include <nt2/include/functions/simd/divides.hpp>
#include <nt2/include/functions/simd/floor.hpp>
#include <nt2/include/functions/simd/if_allbits_else.hpp>
#include <nt2/include/functions/simd/if_else.hpp>
#include <nt2/include/functions/simd/inbtrue.hpp>
#include <nt2/include/functions/simd/is_even.hpp>
#include <nt2/include/functions/simd/is_flint.hpp>
#include <nt2/include/functions/simd/is_greater_equal.hpp>
#include <nt2/include/functions/simd/is_less.hpp>
#include <nt2/include/functions/simd/is_ltz.hpp>
#include <nt2/include/functions/simd/logical_and.hpp>
#include <nt2/include/functions/simd/minus.hpp>
#include <nt2/include/functions/simd/multiplies.hpp>
#include <nt2/include/functions/simd/negif.hpp>
#include <nt2/include/functions/simd/seladd.hpp>
#include <nt2/include/functions/simd/selsub.hpp>
#include <nt2/include/functions/simd/sinpi.hpp>
#include <nt2/include/functions/simd/splat.hpp>
#include <nt2/include/functions/simd/stirling.hpp>
#include <nt2/include/functions/simd/unary_minus.hpp>
#include <nt2/sdk/meta/as_logical.hpp>
#include <nt2/sdk/meta/cardinal_of.hpp>

#include <boost/simd/sdk/config.hpp>

#ifndef BOOST_SIMD_NO_INVALIDS
#include <nt2/include/functions/simd/is_nan.hpp>
#include <nt2/include/functions/simd/logical_or.hpp>
#endif

#ifndef BOOST_SIMD_NO_INFINITIES
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/functions/simd/is_equal.hpp>
#endif

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  (gamma_, tag::cpu_,
                             (A0)(X),
                             ((simd_<floating_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    typedef typename meta::as_logical<A0>::type bA0;
    NT2_FUNCTOR_CALL(1)
    {
      bA0 nan_result = logical_and(is_ltz(a0), is_flint(a0));
      #ifndef BOOST_SIMD_NO_INVALIDS
      nan_result = logical_or(nt2::is_nan(a0), nan_result);
      #endif
      A0 q = nt2::abs(a0);
      bA0 test = lt(a0, nt2::splat<A0>(-33.0));
      std::size_t nb = nt2::inbtrue(test);
      A0 r =  Nan<A0>();
      if(nb > 0)
      {
        //treat negative large with reflection
        r = large_negative(q);
        if (nb >= meta::cardinal_of<A0>::value)
          return nt2::if_nan_else(nan_result, r);
      }
      A0 r1 = other(a0, test);
      A0 r2 = if_else(test, r, r1);
      return nt2::if_nan_else(nan_result, r2);
    }

  private :
    static inline A0 large_negative(const A0& q)
    {
      A0 st =  nt2::stirling(q);
      A0 p = nt2::floor(q);
      A0 sgngam = nt2::negif(nt2::is_even(p), One<A0>());
      A0 z = q - p;
      bA0 test2 = lt(z, nt2::Half<A0>() );
      z = nt2::selsub(test2, z, nt2::One<A0>());
      z = q*nt2::sinpi(z);
      z =  nt2::abs(z);
      return sgngam*nt2::Pi<A0>()/(z*st);
    }

    static inline A0 other(const A0& q, const bA0& test)
    {
      A0 x =  nt2::if_else(test, Two<A0>(), q);
      #ifndef BOOST_SIMD_NO_INFINITIES
      bA0 inf_result = eq(q, Inf<A0>());
      x = if_else(inf_result, Two<A0>(), x);
      #endif
      A0 z = nt2::One<A0>();
      bA0 test1 = ge(x,Three<A0>());
      while( nt2::any(test1) )
      {
        x = nt2::seladd(test1, x, nt2::Mone<A0>());
        z = nt2::if_else(   test1, z*x, z);
        test1 = ge(x,Three<A0>());
      }
      //all x are less than 3
      test1 = nt2::is_ltz(x);
      while( nt2::any(test1) )
      {
        z = nt2::if_else(test1, z/x, z);
        x = nt2::seladd(test1, x, nt2::One<A0>());
        test1 = nt2::is_ltz(x);
      }
      //all x are greater than 0 and less than 3
      bA0 test2 = lt(x,nt2::Two<A0>());
      while( nt2::any(test2))
      {
        z = nt2::if_else(test2, z/x, z);
        x = nt2::seladd(test2, x, nt2::One<A0>());
        test2 = lt(x,nt2::Two<A0>());
      }
      //all x are greater equal 2 and less than 3
      x = z*details::gamma_kernel<A0>::gamma1(x-nt2::Two<A0>());
      #ifndef BOOST_SIMD_NO_INFINITIES
      return if_else(inf_result, q, x);
      #else
      return x;
      #endif
    }
  };
} }

#endif


/*
 * $Revision: 477 $ $Date: 2010-08-31 11:49:37 -0700 (Tue, 31 Aug 2010) $
 *
 * Copyright by Astos Solutions GmbH, Germany
 *
 * this file is published under the Astos Solutions Free Public License
 * For details on copyright and terms of use see
 * http://www.astos.de/Astos_Solutions_Free_Public_License.html
 */

#include "VectorMapLayer.h"
#include "RenderContext.h"
#include "QuadtreeTile.h"
#include "Units.h"
#include "Debug.h"
#include <Eigen/Geometry>
#include <algorithm>
#include <cmath>

using namespace vesta;
using namespace Eigen;
using namespace std;

enum
{
    OutWest  = 0x1,
    OutEast  = 0x2,
    OutSouth = 0x4,
    OutNorth = 0x8
};

struct SpherePatch
{
    float west;
    float east;
    float south;
    float north;
};

struct GreatCircleArc
{
    float lon0;
    float lat0;
    float lon1;
    float lat1;
};

// Compute outcodes for Cohen-Sutherland line clipping
static unsigned int
computeOutcode(const SpherePatch& box, const Vector2f& p)
{
    unsigned int outcode = 0;

    outcode |= (p.x() < box.west)  ? OutWest   : 0;
    outcode |= (p.x() > box.east)  ? OutEast   : 0;
    outcode |= (p.y() < box.south) ? OutSouth  : 0;
    outcode |= (p.y() > box.north) ? OutNorth  : 0;

    return outcode;
}

static Vector3f sphToCart(float lon, float lat)
{
    return Vector3f(cos(lon) * cos(lat), sin(lon) * cos(lat), sin(lat));
}

static Vector3f spherePoint(float latDeg, float lonDeg)
{
    float lat = float(toRadians(latDeg));
    float lon = float(toRadians(lonDeg));

    return Vector3f(lon, lat, 0.0f);
}

// Compute the extents of a patch that completely contains an arc
// on great circle.
//
// The arc is assumed not to cross the 180 degree meridian. Such arcs
// should be split into two separate segments, one on either side of
// 180 degrees. Given this assumption, the minimum and maximum longitude
// may be calculated trivially as the minimum and maximum of the endpoint
// longitudes.
//
// The minimum and maximum latitudes require some more care. The minimum
// latitude is the minimum of the endpoints latitudes *unless* the arc
// contains the overall minimum latitude of great circle.
static SpherePatch
computeExtrema(const GreatCircleArc& arc)
{
    // Compute the normal to the plane containing the great circle
    // TODO: Be more careful about cases where the arc spans an angle
    // very close to 0 or 180 degrees.
    Vector3f v0 = sphToCart(arc.lon0, arc.lat0);
    Vector3f v1 = sphToCart(arc.lon1, arc.lat1);
    Vector3f N = v0.cross(v1).normalized();

    SpherePatch bounds;
    bounds.west  = min(arc.lon0, arc.lon1);
    bounds.east  = max(arc.lon0, arc.lon1);
    bounds.south = min(arc.lat0, arc.lat1);
    bounds.north = max(arc.lat0, arc.lat1);

    // If the normal is aligned with the z-axis, the great circle is the equator and we're done.
    // Otherwise, proceed with the calculation of extremal latitudes
    if (abs(N.z()) + 1.0e-6f < 1.0f)
    {
        float angleLatMin = atan2(N.y(), N.x());
        float angleLatMax = angleLatMin + float(angleLatMin < 0.0f ? PI : -PI);
        if (N.z() < 0.0f)
        {
            swap(angleLatMin, angleLatMax);
        }

        if (angleLatMin > bounds.west && angleLatMin < bounds.east)
        {
            bounds.south = -acos(N.z());
        }
        else if (angleLatMax > bounds.west && angleLatMax < bounds.east)
        {
            bounds.north = acos(N.z());
        }
    }

    return bounds;
}

VectorMapLayer::VectorMapLayer()
{
}

VectorMapLayer::~VectorMapLayer()
{
}

// Draw an arc of a great circle between two points on the surface
// of a sphere.
//
// TODO: Currently, we draw constant bearing arcs, as these are easier
// to clip against sphere patch boundaries.
static void
drawGreatCircleArc(const Vector3f& v0, const Vector3f& v1, unsigned int subdivision)
{
    float d = 1.0f / subdivision;

    glBegin(GL_LINE_STRIP);

    float cosArc = v0.dot(v1);
    float sinArc = sqrt(1.0f - cosArc * cosArc);
    float invSinArc = 1.0f / sinArc;
    float arc = acos(v0.dot(v1));
    float scale = 1.0f + 1.0e-5f;

    Vector3f v = v0 * scale;
    glVertex3fv(v.data());
    for (unsigned int i = 1; i < subdivision; ++i)
    {
        float t = float(i) * d;
        v = ((1.0f - t) * v0 + t * v1).normalized() * scale;
        glVertex3fv(v.data());
    }
    v = v1 * scale;
    glVertex3fv(v.data());
    glEnd();
}

static void
drawConstantBearingArc(float lon0, float lat0, float lon1, float lat1, unsigned int subdivision)
{
    float d = 1.0f / subdivision;
    float dlat = lat1 - lat0;
    float dlon = lon1 - lon0;

    glBegin(GL_LINE_STRIP);
    for (unsigned int i = 0; i < subdivision; ++i)
    {
        float t = i * d;
        float lat = lat0 + dlat * t;
        float lon = lon0 + dlon * t;
        float cosLat = cos(lat);
        glVertex3f(cos(lon) * cosLat, sin(lon) * cosLat, sin(lat));
    }
    glVertex3f(cos(lon1) * cos(lat1), sin(lon1) * cos(lat1), sin(lat1));
    glEnd();
}

// Draw an arc of a great circle between two points on the surface
// of a sphere. Subdivide the arc into segments of uniform length.
static void
drawGreatCircleArcUniform(const Vector3f& v0, const Vector3f& v1)
{
    glBegin(GL_LINE_STRIP);

    float cosArc = v0.dot(v1);
    float sinArc = sqrt(1.0f - cosArc * cosArc);
    float invSinArc = 1.0f / sinArc;
    float arc = acos(v0.dot(v1));

    glVertex3fv(v0.data());
    for (unsigned int i = 1; i < 32; ++i)
    {
        float t = float(i) / 32.0f;
        //Vector3f v = ((1.0f - t) * v0 + t * v1).normalized();
        Vector3f v = invSinArc * (sin((1.0f - t) * arc) * v0 + sin(t * arc) * v1);
        glVertex3fv(v.data());
    }
    glVertex3fv(v1.data());
    glEnd();
}

static void clippedLine(const SpherePatch& box, const Vector2f& p0, const Vector2f& p1)
{
    bool done = false;
    unsigned int out0 = computeOutcode(box, p0);
    unsigned int out1 = computeOutcode(box, p1);

    // When done, r0 and r1 will contain the clipped line endpoints
    Vector2f r0 = p0;
    Vector2f r1 = p1;

    // Cohen-Sutherland line clipping
    bool rejected = false;
    while (!done)
    {
        bool accept = (out0 | out1) == 0;
        bool reject = (out0 & out1) != 0;
        bool needClip = !accept && !reject;

        if (accept)
        {
            done = true;
        }
        else if (reject)
        {
            rejected = true;
            done = true;
        }
        else if (needClip)
        {
            Vector2f diff = r1 - r0;
            float clipX;
            float clipY;

            unsigned int out_;
            if (out0)
            {
                out_ = out0;
            }
            else
            {
                out_ = out1;
            }

            // Find the intersection point.
            if (out_ & OutNorth)
            {
                clipX = r0.x() + diff.x() * (box.north - r0.y()) / diff.y();
                clipY = box.north;
            }
            else if (out_ & OutSouth)
            {
                clipX = r0.x() + diff.x() * (box.south - r0.y()) / diff.y();
                clipY = box.south;
             }
             else if (out_ & OutEast)
             {
                 clipX = box.east;
                 clipY = r0.y() + diff.y() * (box.east - r0.x()) / diff.x();
             }
             else if (out_ & OutWest)
             {
                 clipX = box.west;
                 clipY = r0.y() + diff.y() * (box.west - r0.x()) / diff.x();
             }

             if (out_ == out0)
             {
                 r0 = Vector2f(clipX, clipY);
                 out0 = computeOutcode(box, r0);
             }
             else
             {
                 r1 = Vector2f(clipX, clipY);
                 out1 = computeOutcode(box, r1);
             }
         }
     }

    if (!rejected)
    {
        // Adjust the subdivision level based on the size of the curve relative to the
        // sphere patch. We subdivide more finely for small patches in order to reduce
        // depth buffer artifacts and maintain a smooth appearance.
        Vector3f v0 = sphToCart(r0.x(), r0.y());
        Vector3f v1 = sphToCart(r1.x(), r1.y());
        float cosArc = v0.dot(v1);
        float arc = acos(max(-1.0f, min(1.0f, cosArc)));
        unsigned int subdivision = (unsigned int) max(1.0f, min(32.0f, 32.0f * arc / (box.north - box.south)));
#if GREAT_CIRCLE
        drawGreatCircleArc(v0, v1);
#else
        drawConstantBearingArc(r0.x(), r0.y(), r1.x(), r1.y(), subdivision);
#endif
    }
}

void
VectorMapLayer::addElement(MapElement* e)
{
    m_elements.push_back(counted_ptr<MapElement>(e));
}

void
VectorMapLayer::renderTile(RenderContext& rc, const WorldGeometry* /* world */, const QuadtreeTile* tile) const
{
    rc.setVertexInfo(VertexSpec::PositionColor);

    Material simpleMaterial;
    simpleMaterial.setDiffuse(Spectrum(1.0f, 1.0f, 1.0f));
    simpleMaterial.setOpacity(1.0f);
    rc.bindMaterial(&simpleMaterial);

    float tileArc = float(PI) * tile->extent();
    Vector2f southwest = tile->southwest();

    SpherePatch box;
    box.west = float(PI) * southwest.x();
    box.east = box.west + tileArc;
    box.south = float(PI) * southwest.y();
    box.north = box.south + tileArc;

    AlignedBox<float, 2> bounds(Vector2f(box.west, box.south), Vector2f(box.east, box.north));

    for (vector<counted_ptr<MapElement> >::const_iterator iter = m_elements.begin(); iter != m_elements.end(); ++iter)
    {
        const MapElement* element = iter->ptr();
        bool tileContainsElement = false;

        if (element)
        {
            AlignedBox<float, 2> elementBox = element->bounds();
            if (!elementBox.isNull())
            {
                if (elementBox.min().x() < bounds.max().x() &&
                    elementBox.max().x() > bounds.min().x() &&
                    elementBox.min().y() < bounds.max().y() &&
                    elementBox.max().y() > bounds.min().y())
                {
                    tileContainsElement = true;
                }
            }
        }

        if (tileContainsElement)
        {
            Spectrum color = element->color();
            glColor4f(color.red(), color.green(), color.blue(), element->opacity());

            if (element->opacity() < 1.0f)
            {
                glEnable(GL_BLEND);
                glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
            }
            else
            {
                glDisable(GL_BLEND);
            }

            element->render(box.west, box.south, box.east, box.north);
        }
    }

    glDisable(GL_BLEND);
}

MapElement::MapElement() :
    m_color(1.0f, 1.0f, 1.0f),
    m_opacity(1.0f)
{
}

MapLineString::MapLineString()
{
}

void
MapLineString::render(float west, float south, float east, float north) const
{
    SpherePatch box;
    box.west = west;
    box.east = east;
    box.south = south;
    box.north = north;

    if (m_points.size() >= 2)
    {
        for (unsigned int i = 1; i < m_points.size(); ++i)
        {
            Vector3f p0 = m_points.at(i - 1);
            Vector3f p1 = m_points.at(i);

            clippedLine(box, p0.start<2>(), p1.start<2>());
        }
    }
}

/** Add a new point to the line string.
  */
void
MapLineString::addPoint(const Eigen::Vector3f& p)
{
    // TODO: Automatically split arcs that cross the 180 degree meridian

    m_points.push_back(p);
    setBounds(bounds().extend(Vector2f(p.x(), p.y())));
}

const std::vector<Eigen::Vector3f>&
MapLineString::points() const
{
    return m_points;
}

MapPolygon::MapPolygon(MapLineString* border) :
    m_border(border)
{
    if (m_border.isValid())
    {
        setBounds(m_border->bounds());
    }
}

void
MapPolygon::render(float west, float south, float east, float north) const
{
    if (m_border.isNull())
    {
        return;
    }

    if (m_border->points().size() >= 3)
    {
        glBegin(GL_POLYGON);
        for (unsigned int i = 0; i < m_border->points().size(); ++i)
        {
            Vector3f p = m_border->points().at(i);
            glVertex3fv(sphToCart(p.x(), p.y()).data());
        }
        glEnd();
    }
}

/** Set the border of the polygon. Setting the border
  * to null will make the polygon invisible.
  */
void
MapPolygon::setBorder(MapLineString* border)
{
    m_border = border;
    if (m_border.isValid())
    {
        setBounds(m_border->bounds());
    }
    else
    {
        setBounds(AlignedBox<float, 2>());
    }
}


#include <CGAL/internal/Surface_mesh_segmentation/Disk_samplers.h>

#include <boost/tuple/tuple.hpp>
#include <vector>

typedef boost::tuple<double, double, double> boost_tuple;

void print(const std::vector<boost_tuple>& samples)
{
  const std::size_t map_size = 31;
  const std::size_t map_size_2 = 45;
  std::vector<std::vector<bool> > sample_map(map_size, std::vector<bool>(map_size_2, false));

  for(std::vector<boost_tuple>::const_iterator sample_it = samples.begin();
    sample_it != samples.end(); ++sample_it)
  {
    double x = (sample_it->get<0>() +1)/2;
    double y = (sample_it->get<1>() +1)/2;
    x *= (map_size-1);
    y *= (map_size_2-1);
    std::size_t x_c  = static_cast<std::size_t>(x + 0.49);
    std::size_t y_c  = static_cast<std::size_t>(y + 0.49);
    sample_map[x_c][y_c] = true;
  }
  for(std::size_t i = 0; i < map_size; ++i)
  {
    for(std::size_t j = 0; j < map_size_2; ++j)
    {
      if(sample_map[i][j]){ std::cout << "*"; }
      else                { std::cout << " "; }
    }
    std::cout << std::endl;
  }
  std::cout << std::endl;
}
/**
 * Uses disk sampling functors to sample points from unit-disk.
 * It also prints sampled points for visual debugging.
 *
 * Note that it always return EXIT_SUCCESS
 */
int main(void)
{
  CGAL::internal::Vogel_disk_sampling<boost_tuple> sampling_1;
  CGAL::internal::Vogel_disk_sampling<boost_tuple, true> sampling_2;
  CGAL::internal::Polar_disk_sampling<boost_tuple> sampling_3;
  CGAL::internal::Concentric_disk_sampling<boost_tuple> sampling_4;

  std::vector<boost_tuple> samples_1;
  std::vector<boost_tuple> samples_2;
  std::vector<boost_tuple> samples_3;
  std::vector<boost_tuple> samples_4;

  sampling_1(64, std::back_inserter(samples_1));
  sampling_2(64, std::back_inserter(samples_2));
  sampling_3(64, std::back_inserter(samples_3));
  sampling_4(64, std::back_inserter(samples_4));

  print(samples_1);
  print(samples_2);
  print(samples_3);
  print(samples_4);
}


/*
* Copyright (c) 2019, Intel Corporation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*     * Redistributions of source code must retain the above copyright
*       notice, this list of conditions and the following disclaimer.
*     * Redistributions in binary form must reproduce the above copyright
*       notice, this list of conditions and the following disclaimer in the
*       documentation and/or other materials provided with the distribution.
*     * Neither the name of the Intel Corporation nor the
*       names of its contributors may be used to endorse or promote products
*       derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#pragma once

#include "Teisko/Algorithm/Bit.hpp"
#include "Teisko/Algorithm/Functors.hpp"
#include "Teisko/Algorithm/NelderMead.hpp"
#include "Teisko/Color.hpp"
#include <Eigen/Dense>
#include <array>
#include <exception>
#include <numeric>
#include <vector>

namespace Teisko
{
    template <int N>
    int operator-(const cyclical_index<N>& a, const cyclical_index<N>& b)
    {
        return a.idx < b.idx ? N - (b.idx - a.idx) : b.idx - a.idx;
    }

    struct color_correction_matrix : Eigen::Matrix3d
    {
        color_correction_matrix()
        {
            setIdentity();
        }

        color_correction_matrix(const Eigen::Matrix3d& v) : Eigen::Matrix3d(v) {}

        // Apply color correction matrix
        template <rgb_cs CS = rgb_cs::sRGB, wp WP = wp::D65>
        rgb<double, CS, WP> apply(const rgb<double, rgb_cs::Sensor, wp::None>& pt) const
        {
            return rgb<double, CS, WP> (
                (*this)(0, 0) * pt.r + (*this)(0, 1) * pt.g + (*this)(0, 2) * pt.b,
                (*this)(1, 0) * pt.r + (*this)(1, 1) * pt.g + (*this)(1, 2) * pt.b,
                (*this)(2, 0) * pt.r + (*this)(2, 1) * pt.g + (*this)(2, 2) * pt.b);
        }
    };

    // Calculate color correction matrix from input and output matrices
    // Either calculate ccm by
    // 1. matrix inverse if matrix is invertible
    // 2. least square method using svd decomposition (high accuracy but slow)
    inline color_correction_matrix calculate_ccm(const Eigen::Matrix3d& in, const Eigen::Matrix3d& out)
    {
        using namespace Eigen;

        const double THRESHOLD = 1e-8;
        if (std::abs(in.determinant()) > THRESHOLD)
        {
            // CCM = M(out) / M(in) = M(out)*inverse(M(in))
            //
            // M(in)  = [RGB(1, in) ^ T, RGB(2, in) ^ T, RGB(grey) ^ T]
            // M(out) = [RGB(1, out) ^ T, RGB(2, out) ^ T, RGB(grey) ^ T]
            return color_correction_matrix(out * in.inverse());
        }
        JacobiSVD<Matrix3d> svd(in.transpose(), ComputeFullU | ComputeFullV);
        return color_correction_matrix(svd.solve(out.transpose()).transpose());
    }

    template <rgb_cs CS, wp WP = wp::D65>
    struct patch
    {
        rgb<double, rgb_cs::Sensor, wp::None> input;
        rgb<double, CS, WP> target;
        double weight;
        bool achromatic;
    };

    template <rgb_cs CS, wp WP = wp::D65>
    struct ccm_input_params
    {
        std::array<patch<CS, WP>, 24> color_checker_classic;
        //std::vector<patch<CS, WP>> munsell; // TODO: Implementation for Munsell

        void white_balance()
        {
            average_f<double> r_per_g;
            average_f<double> b_per_g;

            // Loop over achromatic patches (bottom row), dimmest and brightest omitted
            for (size_t i = 19; i < 23; ++i)
            {
                r_per_g += color_checker_classic[i].input.r / color_checker_classic[i].input.g;
                b_per_g += color_checker_classic[i].input.b / color_checker_classic[i].input.g;
            }

            auto max_gain = std::max({ (double)r_per_g, (double)b_per_g, 1.0 });
            auto gain_r = max_gain / r_per_g;
            auto gain_g = max_gain;
            auto gain_b = max_gain / b_per_g;

            for (auto& patch : color_checker_classic)
            {
                patch.input.r *= gain_r;
                patch.input.g *= gain_g;
                patch.input.b *= gain_b;
            }
        }

        void normalize()
        {
            average_f<double> ratio;
            for (size_t i = 19; i < 23; ++i)
            {
                ratio += color_checker_classic[i].target.mean() / color_checker_classic[i].input.mean();
            }

            for (auto& patch : color_checker_classic)
            {
                patch.input *= ratio;
            }
        }
    };

    template <rgb_cs CS, wp WP = wp::D65>
    struct ccm_optimization_params
    {
        ccm_input_params<CS, WP> input_params;

        ccm_optimization_params() = default;
        ccm_optimization_params(const ccm_input_params<CS, WP>& input) : input_params(input) { }

        void prepare()
        {
            input_params.white_balance();
            input_params.normalize();
        }
    };

    template <rgb_cs CS, wp WP = wp::D65>
    struct ccm_func_optimizer
    {
        ccm_optimization_params<CS, WP> params;

        ccm_func_optimizer(ccm_optimization_params<CS, WP>& p) : params(p) { }

        double operator()(double *p)
        {
            auto sum = 0.0;

            // Create CCM from optimization set. Middle column is constructed from
            // border columns (each row sum up to 1)
            auto ccm = color_correction_matrix();
            ccm <<
                p[0], 1 - p[0] - p[1], p[1],
                p[2], 1 - p[2] - p[3], p[3],
                p[4], 1 - p[4] - p[5], p[5];

            for (const auto& patch : params.input_params.color_checker_classic)
            {
                if (patch.weight > 0)
                {
                    lab<double, WP> pt1(ccm.apply<CS, WP>(patch.input));
                    lab<double, WP> pt2(patch.target); // TODO: Pre calculate

                    sum += color_diff<color_diff_type::DeltaE2000>::calculate(pt1, pt2) * patch.weight;
                }
            }
            return sum;
        }
    };

    template <rgb_cs CS, wp WP = wp::D65>
    class ccm_optimization
    {
    public:
        ccm_optimization(ccm_input_params<CS, WP>& input) : options(input) { }

        color_correction_matrix characterize()
        {
            // Do white balance and adjust input intensity
            options.prepare();

            // Get initial point for nelder mead optimization by using least square CCM
            auto initial_ccm = ls_ccm().data();

            // RinR, BinR, RinG, BinG, RinB, BinB indices
            std::vector<double> initial_pt =
            {
                initial_ccm[0], initial_ccm[2], initial_ccm[3], initial_ccm[5], initial_ccm[6], initial_ccm[8]
            };

            // 1. coarse optimization
            auto solver = ccm_func_optimizer<CS, WP>(options);
            auto result = nelder_mead_simplex(initial_pt, solver, {200, 1e-4, 1e-4});

            // 2. fine optimization
            result = nelder_mead_simplex(result.second, solver, {4000, 1e-12, 1e-4});

            // Create CCM from optimization set. Middle column is constructed from
            // border columns (each row should sum up to 1)
            color_correction_matrix ccm;
            ccm <<
                result.second[0], 1 - result.second[0] - result.second[1], result.second[1],
                result.second[2], 1 - result.second[2] - result.second[3], result.second[3],
                result.second[4], 1 - result.second[4] - result.second[5], result.second[5];
            return ccm;
        }

    private:
        ccm_optimization_params<CS, WP> options;

        // Solve a linear system Ax=b for initial CCM optimization
        // Where A is CCC input matrix and B is CCC target matrix
        // This method does not preserve greys nor does it minimize error in de2000.
        color_correction_matrix ls_ccm()
        {
            Eigen::Matrix<double, 24, 3> A;
            Eigen::Matrix<double, 24, 3> b;
            Eigen::Matrix3d x;

            // Initialize matrices
            size_t n = options.input_params.color_checker_classic.size();
            for (size_t i = 0; i < n; ++i)
            {
                const patch<CS, WP>& patch = options.input_params.color_checker_classic[i];
                A.row(i) << patch.input.r, patch.input.g, patch.input.b;
                b.row(i) << patch.target.r, patch.target.g, patch.target.b;
            }

            // Use HouseholderQR decomposition to solve the linear equation.
            // Prefer speed over accuracy for the decomposition method.
            return color_correction_matrix(A.householderQr().solve(b));
        }
    };
}


/* Author: Guido Kanschat, University of Heidelberg, 2003  */
/*         Baerbel Janssen, University of Heidelberg, 2010 */
/*         Wolfgang Bangerth, Texas A&M University, 2010   */

/*    $Id: step-16.cc 28506 2013-02-21 02:47:24Z bangerth $       */
/*                                                                */
/*    Copyright (C) 2003-2004, 2006-2013 by the deal.II authors                   */
/*                                                                */
/*    This file is subject to QPL and may not be  distributed     */
/*    without copyright and license information. Please refer     */
/*    to the file deal.II/doc/license.html for the  text  and     */
/*    further information on this license.                        */

// As discussed in the introduction, most of this program is copied almost
// verbatim from step-6, which itself is only a slight modification of
// step-5. Consequently, a significant part of this program is not new if
// you've read all the material up to step-6, and we won't comment on that
// part of the functionality that is unchanged. Rather, we will focus on those
// aspects of the program that have to do with the multigrid functionality
// which forms the new aspect of this tutorial program.

// @sect3{Include files}

// Again, the first few include files are already known, so we won't comment
// on them:
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/utilities.h>

#include <deal.II/lac/constraint_matrix.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>

#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_refinement.h>
#include <deal.II/grid/tria_boundary_lib.h>

#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_tools.h>

#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>

#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/error_estimator.h>

// These, now, are the include necessary for the multi-level methods. The
// first two declare classes that allow us to enumerate degrees of freedom not
// only on the finest mesh level, but also on intermediate levels (that's what
// the MGDoFHandler class does) as well as allow to access this information
// (iterators and accessors over these cells).
//
// The rest of the include files deals with the mechanics of multigrid as a
// linear operator (solver or preconditioner).
#include <deal.II/multigrid/mg_dof_handler.h>
#include <deal.II/multigrid/mg_constrained_dofs.h>
#include <deal.II/multigrid/multigrid.h>
#include <deal.II/multigrid/mg_transfer.h>
#include <deal.II/multigrid/mg_tools.h>
#include <deal.II/multigrid/mg_coarse.h>
#include <deal.II/multigrid/mg_smoother.h>
#include <deal.II/multigrid/mg_matrix.h>

// This is C++:
#include <fstream>
#include <sstream>

// The last step is as in all previous programs:
namespace Step16
{
  using namespace dealii;

  // @sect3{The LaplaceProblem class template}

  // This main class is basically the same class as in step-6. As far as
  // member functions is concerned, the only addition is the
  // assemble_multigrid function that assembles the matrices that
  // correspond to the discrete operators on intermediate levels:
  template <int dim>
  class LaplaceProblem
  {
  public:
    LaplaceProblem (const unsigned int deg);
    void run ();

  private:
    void setup_system ();
    void assemble_system ();
    void assemble_multigrid ();
    void solve ();
    void refine_grid ();
    void output_results (const unsigned int cycle) const;

    Triangulation<dim>   triangulation;
    FE_Q<dim>            fe;
    MGDoFHandler<dim>    mg_dof_handler;

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;

    // We need an additional object for the hanging nodes constraints. They
    // are handed to the transfer object in the multigrid. Since we call a
    // compress inside the multigrid these constraints are not allowed to be
    // inhomogeneous so we store them in different ConstraintMatrix objects.
    ConstraintMatrix     hanging_node_constraints;
    ConstraintMatrix     constraints;

    Vector<double>       solution;
    Vector<double>       system_rhs;

    const unsigned int degree;

    // The following four objects are the only additional member variables,
    // compared to step-6. They first three represent the operators that act
    // on individual levels of the multilevel hierarchy, rather than on the
    // finest mesh as do the objects above while the last object stores
    // information about the boundary indices on each level and information
    // about indices lying on a refinement edge between two different
    // refinement levels.
    //
    // To facilitate having objects on each level of a multilevel hierarchy,
    // deal.II has the MGLevelObject class template that provides storage for
    // objects on each level. What we need here are matrices on each level,
    // which implies that we also need sparsity patterns on each level. As
    // outlined in the @ref mg_paper, the operators (matrices) that we need
    // are actually twofold: one on the interior of each level, and one at the
    // interface between each level and that part of the domain where the mesh
    // is coarser. In fact, we will need the latter in two versions: for the
    // direction from coarse to fine mesh and from fine to
    // coarse. Fortunately, however, we here have a self-adjoint problem for
    // which one of these is the transpose of the other, and so we only have
    // to build one; we choose the one from coarse to fine.
    MGLevelObject<SparsityPattern>       mg_sparsity_patterns;
    MGLevelObject<SparseMatrix<double> > mg_matrices;
    MGLevelObject<SparseMatrix<double> > mg_interface_matrices;
    MGConstrainedDoFs                    mg_constrained_dofs;
  };

  // @sect3{Nonconstant coefficients}

  // The implementation of nonconstant coefficients is copied verbatim from
  // step-5 and step-6:

  template <int dim>
  class Coefficient : public Function<dim>
  {
  public:
    Coefficient () : Function<dim>() {}

    virtual double value (const Point<dim>   &p,
                          const unsigned int  component = 0) const;

    virtual void value_list (const std::vector<Point<dim> > &points,
                             std::vector<double>            &values,
                             const unsigned int              component = 0) const;
  };

  template <int dim>
  double Coefficient<dim>::value (const Point<dim> &p,
                                  const unsigned int) const
  {
    if (p.square() < 0.5*0.5)
      return 20;
    else
      return 1;
  }

  template <int dim>
  void Coefficient<dim>::value_list (const std::vector<Point<dim> > &points,
                                     std::vector<double>            &values,
                                     const unsigned int              component) const
  {
    const unsigned int n_points = points.size();

    Assert (values.size() == n_points,
            ExcDimensionMismatch (values.size(), n_points));

    Assert (component == 0,
            ExcIndexRange (component, 0, 1));

    for (unsigned int i=0; i<n_points; ++i)
      values[i] = Coefficient<dim>::value (points[i]);
  }

  // @sect3{The LaplaceProblem class implementation}

  // @sect4{LaplaceProblem::LaplaceProblem}

  // The constructor is left mostly unchanged. We take the polynomial degree
  // of the finite elements to be used as a constructor argument and store it
  // in a member variable.
  //
  // By convention, all adaptively refined triangulations in deal.II never
  // change by more than one level across a face between cells. For our
  // multigrid algorithms, however, we need a slightly stricter guarantee,
  // namely that the mesh also does not change by more than refinement level
  // across vertices that might connect two cells. In other words, we must
  // prevent the following situation:
  //
  // @image html limit_level_difference_at_vertices.png ""
  //
  // This is achieved by passing the
  // Triangulation::limit_level_difference_at_vertices flag to the constructor
  // of the triangulation class.
  template <int dim>
  LaplaceProblem<dim>::LaplaceProblem (const unsigned int degree)
    :
    triangulation (Triangulation<dim>::
                   limit_level_difference_at_vertices),
    fe (degree),
    mg_dof_handler (triangulation),
    degree(degree)
  {}

  // @sect4{LaplaceProblem::setup_system}

  // The following function extends what the corresponding one in step-6
  // did. The top part, apart from the additional output, does the same:
  template <int dim>
  void LaplaceProblem<dim>::setup_system ()
  {
    mg_dof_handler.distribute_dofs (fe);

    // Here we output not only the degrees of freedom on the finest level, but
    // also in the multilevel structure
    deallog << "Number of degrees of freedom: "
            << mg_dof_handler.n_dofs();

    for (unsigned int l=0; l<triangulation.n_levels(); ++l)
      deallog << "   " << 'L' << l << ": "
              << mg_dof_handler.n_dofs(l);
    deallog  << std::endl;

    sparsity_pattern.reinit (mg_dof_handler.n_dofs(),
                             mg_dof_handler.n_dofs(),
                             mg_dof_handler.max_couplings_between_dofs());
    DoFTools::make_sparsity_pattern (mg_dof_handler, sparsity_pattern);

    solution.reinit (mg_dof_handler.n_dofs());
    system_rhs.reinit (mg_dof_handler.n_dofs());

    // But it starts to be a wee bit different here, although this still
    // doesn't have anything to do with multigrid methods. step-6 took care of
    // boundary values and hanging nodes in a separate step after assembling
    // the global matrix from local contributions. This works, but the same
    // can be done in a slightly simpler way if we already take care of these
    // constraints at the time of copying local contributions into the global
    // matrix. To this end, we here do not just compute the constraints do to
    // hanging nodes, but also due to zero boundary conditions. We will use
    // this set of constraints later on to help us copy local contributions
    // correctly into the global linear system right away, without the need
    // for a later clean-up stage:
    constraints.clear ();
    hanging_node_constraints.clear ();
    DoFTools::make_hanging_node_constraints (mg_dof_handler, hanging_node_constraints);
    DoFTools::make_hanging_node_constraints (mg_dof_handler, constraints);

    typename FunctionMap<dim>::type      dirichlet_boundary;
    ZeroFunction<dim>                    homogeneous_dirichlet_bc (1);
    dirichlet_boundary[0] = &homogeneous_dirichlet_bc;
    VectorTools::interpolate_boundary_values (static_cast<const DoFHandler<dim>&>(mg_dof_handler),
                                              dirichlet_boundary,
                                              constraints);
    constraints.close ();
    hanging_node_constraints.close ();
    constraints.condense (sparsity_pattern);
    sparsity_pattern.compress();
    system_matrix.reinit (sparsity_pattern);

    // The multigrid constraints have to be initialized. They need to know
    // about the boundary values as well, so we pass the
    // dirichlet_boundary here as well.
    mg_constrained_dofs.clear();
    mg_constrained_dofs.initialize(mg_dof_handler, dirichlet_boundary);

    // Now for the things that concern the multigrid data structures. First,
    // we resize the multi-level objects to hold matrices and sparsity
    // patterns for every level. The coarse level is zero (this is mandatory
    // right now but may change in a future revision). Note that these
    // functions take a complete, inclusive range here (not a starting index
    // and size), so the finest level is n_levels-1.  We first
    // have to resize the container holding the SparseMatrix classes, since
    // they have to release their SparsityPattern before the can be destroyed
    // upon resizing.
    const unsigned int n_levels = triangulation.n_levels();

    mg_interface_matrices.resize(0, n_levels-1);
    mg_interface_matrices.clear ();
    mg_matrices.resize(0, n_levels-1);
    mg_matrices.clear ();
    mg_sparsity_patterns.resize(0, n_levels-1);

    // Now, we have to provide a matrix on each level. To this end, we first
    // use the MGTools::make_sparsity_pattern function to first generate a
    // preliminary compressed sparsity pattern on each level (see the @ref
    // Sparsity module for more information on this topic) and then copy it
    // over to the one we really want. The next step is to initialize both
    // kinds of level matrices with these sparsity patterns.
    //
    // It may be worth pointing out that the interface matrices only have
    // entries for degrees of freedom that sit at or next to the interface
    // between coarser and finer levels of the mesh. They are therefore even
    // sparser than the matrices on the individual levels of our multigrid
    // hierarchy. If we were more concerned about memory usage (and possibly
    // the speed with which we can multiply with these matrices), we should
    // use separate and different sparsity patterns for these two kinds of
    // matrices.
    for (unsigned int level=0; level<n_levels; ++level)
      {
        CompressedSparsityPattern csp;
        csp.reinit(mg_dof_handler.n_dofs(level),
                   mg_dof_handler.n_dofs(level));
        MGTools::make_sparsity_pattern(mg_dof_handler, csp, level);

        mg_sparsity_patterns[level].copy_from (csp);

        mg_matrices[level].reinit(mg_sparsity_patterns[level]);
        mg_interface_matrices[level].reinit(mg_sparsity_patterns[level]);
      }
  }

  // @sect4{LaplaceProblem::assemble_system}

  // The following function assembles the linear system on the finesh level of
  // the mesh. It is almost exactly the same as in step-6, with the exception
  // that we don't eliminate hanging nodes and boundary values after
  // assembling, but while copying local contributions into the global
  // matrix. This is not only simpler but also more efficient for large
  // problems.
  //
  // This latter trick is something that only found its way into deal.II over
  // time and wasn't used in the initial version of this tutorial
  // program. There is, however, a discussion of this function in the
  // introduction of step-27.
  template <int dim>
  void LaplaceProblem<dim>::assemble_system ()
  {
    const QGauss<dim>  quadrature_formula(degree+1);

    FEValues<dim> fe_values (fe, quadrature_formula,
                             update_values    |  update_gradients |
                             update_quadrature_points  |  update_JxW_values);

    const unsigned int   dofs_per_cell = fe.dofs_per_cell;
    const unsigned int   n_q_points    = quadrature_formula.size();

    FullMatrix<double>   cell_matrix (dofs_per_cell, dofs_per_cell);
    Vector<double>       cell_rhs (dofs_per_cell);

    std::vector<unsigned int> local_dof_indices (dofs_per_cell);

    const Coefficient<dim> coefficient;
    std::vector<double>    coefficient_values (n_q_points);

    typename MGDoFHandler<dim>::active_cell_iterator
    cell = mg_dof_handler.begin_active(),
    endc = mg_dof_handler.end();
    for (; cell!=endc; ++cell)
      {
        cell_matrix = 0;
        cell_rhs = 0;

        fe_values.reinit (cell);

        coefficient.value_list (fe_values.get_quadrature_points(),
                                coefficient_values);

        for (unsigned int q_point=0; q_point<n_q_points; ++q_point)
          for (unsigned int i=0; i<dofs_per_cell; ++i)
            {
              for (unsigned int j=0; j<dofs_per_cell; ++j)
                cell_matrix(i,j) += (coefficient_values[q_point] *
                                     fe_values.shape_grad(i,q_point) *
                                     fe_values.shape_grad(j,q_point) *
                                     fe_values.JxW(q_point));

              cell_rhs(i) += (fe_values.shape_value(i,q_point) *
                              1.0 *
                              fe_values.JxW(q_point));
            }

        cell->get_dof_indices (local_dof_indices);
        constraints.distribute_local_to_global (cell_matrix, cell_rhs,
                                                local_dof_indices,
                                                system_matrix, system_rhs);
      }
  }

  // @sect4{LaplaceProblem::assemble_multigrid}

  // The next function is the one that builds the linear operators (matrices)
  // that define the multigrid method on each level of the mesh. The
  // integration core is the same as above, but the loop below will go over
  // all existing cells instead of just the active ones, and the results must
  // be entered into the correct matrix. Note also that since we only do
  // multi-level preconditioning, no right-hand side needs to be assembled
  // here.
  //
  // Before we go there, however, we have to take care of a significant amount
  // of book keeping:
  template <int dim>
  void LaplaceProblem<dim>::assemble_multigrid ()
  {
    QGauss<dim>  quadrature_formula(1+degree);

    FEValues<dim> fe_values (fe, quadrature_formula,
                             update_values   | update_gradients |
                             update_quadrature_points | update_JxW_values);

    const unsigned int   dofs_per_cell   = fe.dofs_per_cell;
    const unsigned int   n_q_points      = quadrature_formula.size();

    FullMatrix<double>   cell_matrix (dofs_per_cell, dofs_per_cell);

    std::vector<unsigned int> local_dof_indices (dofs_per_cell);

    const Coefficient<dim> coefficient;
    std::vector<double>    coefficient_values (n_q_points);

    // Next a few things that are specific to building the multigrid data
    // structures (since we only need them in the current function, rather
    // than also elsewhere, we build them here instead of the
    // setup_system function). Some of the following may be a bit
    // obscure if you're not familiar with the algorithm actually implemented
    // in deal.II to support multilevel algorithms on adaptive meshes; if some
    // of the things below seem strange, take a look at the @ref mg_paper.
    //
    // Our first job is to identify those degrees of freedom on each level
    // that are located on interfaces between adaptively refined levels, and
    // those that lie on the interface but also on the exterior boundary of
    // the domain. As in many other parts of the library, we do this by using
    // boolean masks, i.e. vectors of booleans each element of which indicates
    // whether the corresponding degree of freedom index is an interface DoF
    // or not. The MGConstraints already computed the information
    // for us when we called initialize in setup_system().
    std::vector<std::vector<bool> > interface_dofs
      = mg_constrained_dofs.get_refinement_edge_indices ();
    std::vector<std::vector<bool> > boundary_interface_dofs
      = mg_constrained_dofs.get_refinement_edge_boundary_indices ();

    // The indices just identified will later be used to decide where the
    // assembled value has to be added into on each level.  On the other hand,
    // we also have to impose zero boundary conditions on the external
    // boundary of each level. But this the MGConstraints knows
    // it. So we simply ask for them by calling get_boundary_indices
    // ().  The third step is to construct constraints on all those
    // degrees of freedom: their value should be zero after each application
    // of the level operators. To this end, we construct ConstraintMatrix
    // objects for each level, and add to each of these constraints for each
    // degree of freedom. Due to the way the ConstraintMatrix stores its data,
    // the function to add a constraint on a single degree of freedom and
    // force it to be zero is called Constraintmatrix::add_line(); doing so
    // for several degrees of freedom at once can be done using
    // Constraintmatrix::add_lines():
    std::vector<ConstraintMatrix> boundary_constraints (triangulation.n_levels());
    std::vector<ConstraintMatrix> boundary_interface_constraints (triangulation.n_levels());
    for (unsigned int level=0; level<triangulation.n_levels(); ++level)
      {
        boundary_constraints[level].add_lines (interface_dofs[level]);
        boundary_constraints[level].add_lines (mg_constrained_dofs.get_boundary_indices()[level]);
        boundary_constraints[level].close ();

        boundary_interface_constraints[level]
        .add_lines (boundary_interface_dofs[level]);
        boundary_interface_constraints[level].close ();
      }

    // Now that we're done with most of our preliminaries, let's start the
    // integration loop. It looks mostly like the loop in
    // assemble_system, with two exceptions: (i) we don't need a
    // right hand side, and more significantly (ii) we don't just loop over
    // all active cells, but in fact all cells, active or not. Consequently,
    // the correct iterator to use is MGDoFHandler::cell_iterator rather than
    // MGDoFHandler::active_cell_iterator. Let's go about it:
    typename MGDoFHandler<dim>::cell_iterator cell = mg_dof_handler.begin(),
                                              endc = mg_dof_handler.end();

    for (; cell!=endc; ++cell)
      {
        cell_matrix = 0;
        fe_values.reinit (cell);

        coefficient.value_list (fe_values.get_quadrature_points(),
                                coefficient_values);

        for (unsigned int q_point=0; q_point<n_q_points; ++q_point)
          for (unsigned int i=0; i<dofs_per_cell; ++i)
            for (unsigned int j=0; j<dofs_per_cell; ++j)
              cell_matrix(i,j) += (coefficient_values[q_point] *
                                   fe_values.shape_grad(i,q_point) *
                                   fe_values.shape_grad(j,q_point) *
                                   fe_values.JxW(q_point));

        // The rest of the assembly is again slightly different. This starts
        // with a gotcha that is easily forgotten: The indices of global
        // degrees of freedom we want here are the ones for current level, not
        // for the global matrix. We therefore need the function
        // MGDoFAccessorLLget_mg_dof_indices, not
        // MGDoFAccessor::get_dof_indices as used in the assembly of the
        // global system:
        cell->get_mg_dof_indices (local_dof_indices);

        // Next, we need to copy local contributions into the level
        // objects. We can do this in the same way as in the global assembly,
        // using a constraint object that takes care of constrained degrees
        // (which here are only boundary nodes, as the individual levels have
        // no hanging node constraints). Note that the
        // boundary_constraints object makes sure that the level
        // matrices contains no contributions from degrees of freedom at the
        // interface between cells of different refinement level.
        boundary_constraints[cell->level()]
        .distribute_local_to_global (cell_matrix,
                                     local_dof_indices,
                                     mg_matrices[cell->level()]);

        // The next step is again slightly more obscure (but explained in the
        // @ref mg_paper): We need the remainder of the operator that we just
        // copied into the mg_matrices object, namely the part on
        // the interface between cells at the current level and cells one
        // level coarser. This matrix exists in two directions: for interior
        // DoFs (index $i$) of the current level to those sitting on the
        // interface (index $j$), and the other way around. Of course, since
        // we have a symmetric operator, one of these matrices is the
        // transpose of the other.
        //
        // The way we assemble these matrices is as follows: since the are
        // formed from parts of the local contributions, we first delete all
        // those parts of the local contributions that we are not interested
        // in, namely all those elements of the local matrix for which not $i$
        // is an interface DoF and $j$ is not. The result is one of the two
        // matrices that we are interested in, and we then copy it into the
        // mg_interface_matrices object. The
        // boundary_interface_constraints object at the same time
        // makes sure that we delete contributions from all degrees of freedom
        // that are not only on the interface but also on the external
        // boundary of the domain.
        //
        // The last part to remember is how to get the other matrix. Since it
        // is only the transpose, we will later (in the solve()
        // function) be able to just pass the transpose matrix where
        // necessary.
        for (unsigned int i=0; i<dofs_per_cell; ++i)
          for (unsigned int j=0; j<dofs_per_cell; ++j)
            if ( !(interface_dofs[cell->level()][local_dof_indices[i]]==true &&
                   interface_dofs[cell->level()][local_dof_indices[j]]==false))
              cell_matrix(i,j) = 0;

        boundary_interface_constraints[cell->level()]
        .distribute_local_to_global (cell_matrix,
                                     local_dof_indices,
                                     mg_interface_matrices[cell->level()]);
      }
  }

  // @sect4{LaplaceProblem::solve}

  // This is the other function that is significantly different in support of
  // the multigrid solver (or, in fact, the preconditioner for which we use
  // the multigrid method).
  //
  // Let us start out by setting up two of the components of multilevel
  // methods: transfer operators between levels, and a solver on the coarsest
  // level. In finite element methods, the transfer operators are derived from
  // the finite element function spaces involved and can often be computed in
  // a generic way independent of the problem under consideration. In that
  // case, we can use the MGTransferPrebuilt class that, given the constraints
  // on the global level and an MGDoFHandler object computes the matrices
  // corresponding to these transfer operators.
  //
  // The second part of the following lines deals with the coarse grid
  // solver. Since our coarse grid is very coarse indeed, we decide for a
  // direct solver (a Householder decomposition of the coarsest level matrix),
  // even if its implementation is not particularly sophisticated. If our
  // coarse mesh had many more cells than the five we have here, something
  // better suited would obviously be necessary here.
  template <int dim>
  void LaplaceProblem<dim>::solve ()
  {

    // Create the object that deals with the transfer between different
    // refinement levels. We need to pass it the hanging node constraints.
    MGTransferPrebuilt<Vector<double> > mg_transfer(hanging_node_constraints, mg_constrained_dofs);
    // Now the prolongation matrix has to be built.  This matrix needs to take
    // the boundary values on each level into account and needs to know about
    // the indices at the refinement egdes. The MGConstraints
    // knows about that so pass it as an argument.
    mg_transfer.build_matrices(mg_dof_handler);

    FullMatrix<double> coarse_matrix;
    coarse_matrix.copy_from (mg_matrices[0]);
    MGCoarseGridHouseholder<> coarse_grid_solver;
    coarse_grid_solver.initialize (coarse_matrix);

    // The next component of a multilevel solver or preconditioner is that we
    // need a smoother on each level. A common choice for this is to use the
    // application of a relaxation method (such as the SOR, Jacobi or
    // Richardson method) or a small number of iterations of a solver method
    // (such as CG or GMRES). The mg::SmootherRelaxation and
    // MGSmootherPrecondition classes provide support for these two kinds of
    // smoothers. Here, we opt for the application of a single SOR
    // iteration. To this end, we define an appropriate typedef
    // and then setup a smoother object.
    //
    // Since this smoother needs temporary vectors to store intermediate
    // results, we need to provide a VectorMemory object. Since these vectors
    // will be reused over and over, the GrowingVectorMemory is more time
    // efficient than the PrimitiveVectorMemory class in the current case.
    //
    // The last step is to initialize the smoother object with our level
    // matrices and to set some smoothing parameters.  The
    // initialize() function can optionally take additional
    // arguments that will be passed to the smoother object on each level. In
    // the current case for the SOR smoother, this could, for example, include
    // a relaxation parameter. However, we here leave these at their default
    // values. The call to set_steps() indicates that we will use
    // two pre- and two post-smoothing steps on each level; to use a variable
    // number of smoother steps on different levels, more options can be set
    // in the constructor call to the mg_smoother object.
    //
    // The last step results from the fact that we use the SOR method as a
    // smoother - which is not symmetric - but we use the conjugate gradient
    // iteration (which requires a symmetric preconditioner) below, we need to
    // let the multilevel preconditioner make sure that we get a symmetric
    // operator even for nonsymmetric smoothers:
    typedef PreconditionSOR<SparseMatrix<double> > Smoother;
    mg::SmootherRelaxation<Smoother, Vector<double> > mg_smoother;
    mg_smoother.initialize(mg_matrices);
    mg_smoother.set_steps(2);
    mg_smoother.set_symmetric(true);

    // The next preparatory step is that we must wrap our level and interface
    // matrices in an object having the required multiplication functions. We
    // will create two objects for the interface objects going from coarse to
    // fine and the other way around; the multigrid algorithm will later use
    // the transpose operator for the latter operation, allowing us to
    // initialize both up and down versions of the operator with the matrices
    // we already built:
    MGMatrix<> mg_matrix(&mg_matrices);
    MGMatrix<> mg_interface_up(&mg_interface_matrices);
    MGMatrix<> mg_interface_down(&mg_interface_matrices);

    // Now, we are ready to set up the V-cycle operator and the multilevel
    // preconditioner.
    Multigrid<Vector<double> > mg(mg_dof_handler,
                                  mg_matrix,
                                  coarse_grid_solver,
                                  mg_transfer,
                                  mg_smoother,
                                  mg_smoother);
    mg.set_edge_matrices(mg_interface_down, mg_interface_up);

    PreconditionMG<dim, Vector<double>, MGTransferPrebuilt<Vector<double> > >
    preconditioner(mg_dof_handler, mg, mg_transfer);

    // With all this together, we can finally get about solving the linear
    // system in the usual way:
    SolverControl solver_control (1000, 1e-12);
    SolverCG<>    cg (solver_control);

    solution = 0;

    cg.solve (system_matrix, solution, system_rhs,
              preconditioner);
    constraints.distribute (solution);

    std::cout << "   " << solver_control.last_step()
              << " CG iterations needed to obtain convergence."
              << std::endl;
  }

  // @sect4{Postprocessing}

  // The following two functions postprocess a solution once it is
  // computed. In particular, the first one refines the mesh at the beginning
  // of each cycle while the second one outputs results at the end of each
  // such cycle. The functions are almost unchanged from those in step-6, with
  // the exception of two minor differences: The KellyErrorEstimator::estimate
  // function wants an argument of type DoFHandler, not MGDoFHandler, and so
  // we have to cast from derived to base class; and we generate output in VTK
  // format, to use the more modern visualization programs available today
  // compared to those that were available when step-6 was written.
  template <int dim>
  void LaplaceProblem<dim>::refine_grid ()
  {
    Vector<float> estimated_error_per_cell (triangulation.n_active_cells());

    KellyErrorEstimator<dim>::estimate (static_cast<DoFHandler<dim>&>(mg_dof_handler),
                                        QGauss<dim-1>(3),
                                        typename FunctionMap<dim>::type(),
                                        solution,
                                        estimated_error_per_cell);
    GridRefinement::refine_and_coarsen_fixed_number (triangulation,
                                                     estimated_error_per_cell,
                                                     0.3, 0.03);
    triangulation.execute_coarsening_and_refinement ();
  }

  template <int dim>
  void LaplaceProblem<dim>::output_results (const unsigned int cycle) const
  {
    DataOut<dim> data_out;

    data_out.attach_dof_handler (mg_dof_handler);
    data_out.add_data_vector (solution, "solution");
    data_out.build_patches ();

    std::ostringstream filename;
    filename << "solution-"
             << cycle
             << ".vtk";

    std::ofstream output (filename.str().c_str());
    data_out.write_vtk (output);
  }

  // @sect4{LaplaceProblem::run}

  // Like several of the functions above, this is almost exactly a copy of of
  // the corresponding function in step-6. The only difference is the call to
  // assemble_multigrid that takes care of forming the matrices
  // on every level that we need in the multigrid method.
  template <int dim>
  void LaplaceProblem<dim>::run ()
  {
    for (unsigned int cycle=0; cycle<8; ++cycle)
      {
        std::cout << "Cycle " << cycle << ':' << std::endl;

        if (cycle == 0)
          {
            GridGenerator::hyper_ball (triangulation);

            static const HyperBallBoundary<dim> boundary;
            triangulation.set_boundary (0, boundary);

            triangulation.refine_global (1);
          }
        else
          refine_grid ();

        std::cout << "   Number of active cells:       "
                  << triangulation.n_active_cells()
                  << std::endl;

        setup_system ();

        std::cout << "   Number of degrees of freedom: "
                  << mg_dof_handler.n_dofs()
                  << " (by level: ";
        for (unsigned int level=0; level<triangulation.n_levels(); ++level)
          std::cout << mg_dof_handler.n_dofs(level)
                    << (level == triangulation.n_levels()-1
                        ? ")" : ", ");
        std::cout << std::endl;

        assemble_system ();
        assemble_multigrid ();

        solve ();
        output_results (cycle);
      }
  }
}

// @sect3{The main() function}
//
// This is again the same function as in step-6:
int main ()
{
  try
    {
      using namespace dealii;
      using namespace Step16;

      deallog.depth_console (0);

      LaplaceProblem<2> laplace_problem(1);
      laplace_problem.run ();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}


/*
Copyright(c) 2014 Akihiro Nishimura

This software is released under the MIT License.
http://opensource.org/licenses/mit-license.php
*/

#include "ctr.h"
#include "SigUtil/lib/calculation/binary_operation.hpp"
#include "SigUtil/lib/tools/convergence.hpp"
#include "SigUtil/lib/functional/filter.hpp"
#include "SigUtil/lib/functional/list_deal.hpp"
#include <Eigen/Dense>

#include "SigUtil/lib/tools/time_watch.hpp"

namespace sigtm
{
//using namespace boost::numeric;

const double projection_z = 1.0;

static double safe_log(double x)
{
	return x > 0 ? std::log(x) : log_lower_limit;
};

template <class C>
static auto row_(C&& src, uint i) ->decltype(ublas::row(src, i))
{
	return ublas::row(src, i);
}
static auto row_(EigenMatrix& src, uint i) ->decltype(src.row(i))
{
	return src.row(i);
}
static auto row_(EigenMatrix const& src, uint i) ->decltype(src.row(i))
{
	return src.row(i);
}

template <class C>
static auto at_(C&& src, uint row, uint col) ->decltype(src(row, col))
{
	return src(row, col);
}
static auto at_(EigenMatrix& src, uint row, uint col) ->decltype(src.coeffRef(col, row))
{
	return src.coeffRef(col, row);
}
static auto at_(EigenMatrix const& src, uint row, uint col) ->decltype(src.coeffRef(col, row))
{
	return src.coeffRef(col, row);
}

template <class V>
void normalize_dist_v(V&& vec)
{
	double sum = vec.sum();
	vec /= sum;
}
template <class F, class V>
auto map_v(F&& func, V&& vec)
{
	using RT = decltype(sig::impl::eval(std::forward<F>(func), std::forward<V>(vec)(0)));

	EigenVector result(vec.size());

	for (uint i = 0, size = vec.size(); i < size; ++i){
		result[i] = std::forward<F>(func)(std::forward<V>(vec)(i));
	}

	return result;
}

template <class F, class M>
auto map_m(F&& func, M&& mat)
{
	using RT = decltype(sig::impl::eval(std::forward<F>(func), std::forward<M>(mat)(0, 0)));

	const uint col_size = mat.cols();
	const uint row_size = mat.rows();

	EigenMatrix result(col_size, row_size);

	for (uint i = 0; i < col_size; ++i){
		for (uint j = 0; j < row_size; ++j){
			result(i, j) = std::forward<F>(func)(std::forward<M>(mat)(i, j));
		}
	}

	return result;
}

template <class V>
bool is_feasible(V const& x)
{
	double val;
	double sum = 0;
	for (uint i = 0, size = x.size()-1; i < size; ++i) {
		val = x[i];
		if (val < 0 || val >1) return false;
		sum += val;
		if (sum > 1) return false;
	}
	return true;
}

// project x on to simplex (using // http://www.cs.berkeley.edu/~jduchi/projects/DuchiShSiCh08.pdf)
template <class V1, class V2>
void simplex_projection(
	V1 const& x,
	V2& x_proj,
	double z)
{
	x_proj = x;
	std::sort(x_proj.data(), x_proj.data() + x_proj.size());
	double cumsum = -z, u;
	int j = 0;

	for (int i = x.size() - 1; i >= 0; --i) {
		u = x_proj[i];
		cumsum += u;
		if (u > cumsum / (j + 1)) j++;
		else break;
	}
	double theta = cumsum / j;
	for (int i = 0, size = x.size(); i < size; ++i) {
		u = x[i] - theta;
		if (u <= 0) u = 0.0;
		x_proj[i] = u;
	}
	normalize_dist_v(x_proj); // fix the normaliztion issue due to numerical errors
}

template <class V1, class V2, class V3>
auto df_simplex(
	V1 const&gamma,
	V2 const& v,
	double lambda,
	V3 const& opt_x)
{
	EigenVector g = -lambda * (opt_x - v);
	EigenVector y = gamma;

	//sig::for_each_v([](double& v1, double v2){ v1 /= v2; }, y, opt_x);
	for(uint i = 0, size = y.size(); i < size; ++i){
		y[i] /= opt_x[i];
	}

	g += y;

	//sig::for_each_v([](double& v1){ v1 *= -1; }, g);
	g.array() *= -1;

	return g;
}

template <class V1, class V2, class V3>
double f_simplex(
	V1 const& gamma,
	V2 const& v,
	double lambda,
	V3 const& opt_x)
{
	auto y = map_v([&](double x){ return safe_log(x); }, opt_x);
	auto z = v - opt_x;

	//double f = ublas::inner_prod(y, gamma);
	double f = y.dot(gamma);

	//double val = ublas::inner_prod(z, z);
	double val = z.dot(z);

	f -= 0.5 * lambda * val;

	return -f;
}

// projection gradient algorithm
template <class V1, class V2, class V3>
void optimize_simplex(
	V1 const& gamma,
	V2 const& v,
	double lambda,
	V3& opt_x)
{
	size_t size = sig::min(gamma.size(), v.size());
	EigenVector x_bar(size);
	EigenVector opt_x_old = opt_x;

	double f_old = f_simplex(gamma, v, lambda, opt_x);

	auto g = df_simplex(gamma, v, lambda, opt_x);

	normalize_dist_v(g);
	//double ab_sum = sig::sum(g);
	//if (ab_sum > 1.0) g *= (1.0 / ab_sum); // rescale the gradient

	opt_x -= g;

	simplex_projection(opt_x, x_bar, projection_z);

	x_bar -= opt_x_old;

	//double r = 0.5 * ublas::inner_prod(g, x_bar);
	double r = 0.5 * g.dot(x_bar);

	const double beta = 0.5;
	double t = beta;
	for (uint iter = 0; iter < 100; ++iter) {
		opt_x = opt_x_old;
		opt_x += t * x_bar;

		double f_new = f_simplex(gamma, v, lambda, opt_x);

		if (f_new > f_old + r * t) t = t * beta;
		else break;
	}

	if (!is_feasible(opt_x))  printf("sth is wrong, not feasible. you've got to check it ...\n");
}

void CTR::init()
{
	sig::SimpleRandom<double> randf(0, 1, FixedRandom);

	beta_ = MatrixKV_(V_, K_); //SIG_INIT_MATRIX(double, K, V, 0);

	if (hparam_->beta_.empty()){
		for(TopicId k = 0; k < K_; ++k){
			auto& beta_v = row_(beta_, k);
			for (uint v = 0; v < V_; ++v) beta_v[v] = randf() + hparam_->beta_smooth_;
			normalize_dist_v(beta_v);
		}
	}
	else{
		std::cout << "beta loading" << std::endl;
		//beta_ = sig::to_matrix_ublas(hparam_->beta_);
		for (uint k = 0; k < K_; ++k){
			for (uint v = 0; v < V_; ++v) beta_(v, k) = hparam_->beta_[k][v];
		}
	}

	theta_ = MatrixIK_::Zero(K_, I_); // SIG_INIT_MATRIX(double, I, K, 0);

	if (hparam_->theta_opt_){
		if (hparam_->theta_.empty()){
			for (ItemId i = 0; i < I_; ++i){
				auto& theta_v = row_(theta_, i);
				for (uint k = 0; k < K_; ++k) theta_v[k] = randf() + hparam_->alpha_smooth_;
				normalize_dist_v(theta_v);
			}
		}
		else{
			std::cout << "theta loading" << std::endl;
			//theta_ = sig::to_matrix_ublas(hparam_->theta_);
			for (uint i = 0; i < I_; ++i){
				for (uint k = 0; k < K_; ++k) beta_(k, i) = hparam_->beta_[i][k];

			}
		}
	}

	user_factor_ = MatrixUK_::Zero(U_, K_);
	item_factor_ = MatrixIK_::Zero(I_, K_);

	if (hparam_->theta_opt_){
		for (ItemId i = 0; i < I_; ++i){
			auto& if_v = row_(item_factor_, i);
			for (uint k = 0; k < K_; ++k) if_v[k] = randf();
		}
	}
	else{
		item_factor_ = theta_;
	}
}

void CTR::printUFactor() const
{
	std::cout << "user_factor" << std::endl;
	for (uint u = 0; u<U_; ++u){
		for (uint k = 0; k<K_; ++k) std::cout << user_factor_(u, k) << ", ";
		std::cout << std::endl;
	}
}
void CTR::printIFactor() const
{
	std::cout << "item_factor" << std::endl;
	for (uint i = 0; i<I_; ++i){
		for (uint k = 0; k<K_; ++k) std::cout << item_factor_(i, k) << ", ";
		std::cout << std::endl;
	}
}

/*
	std::cout << "estimate rating" << std::endl;
	for (uint u = 0; u<ratings_->userSize(); ++u){
		for (uint i = 0; i<ratings_->itemSize(); ++i) std::cout << estimate(u, i) << ", ";
		std::cout << std::endl;
	}
	std::cout << std::endl;
*/

void CTR::saveTmp() const
{
	/*
	sprintf(name, "%s/%04d-U.dat", directory, iter);
      FILE * file_U = fopen(name, "w");
      mtx_fprintf(file_U, user_factor_);
      fclose(file_U);

      sprintf(name, "%s/%04d-V.dat", directory, iter);
      FILE * file_V = fopen(name, "w");
      mtx_fprintf(file_V, item_factor_);
      fclose(file_V);

      if (hparam_->ctr_run) {
        sprintf(name, "%s/%04d-theta.dat", directory, iter);
        FILE * file_theta = fopen(name, "w");
        mtx_fprintf(file_theta, m_theta);
        fclose(file_theta);

        sprintf(name, "%s/%04d-beta.dat", directory, iter);
        FILE * file_beta = fopen(name, "w");
        mtx_fprintf(file_beta, m_beta);
        fclose(file_beta);
	}
	*/
}

void CTR::save() const
{
}

void CTR::load()
{

}

double CTR::docInference(ItemId id,	bool update_word_ss)
{
	double pseudo_count = 1.0;
	double likelihood = 0;
	auto const& theta_v = row_(theta_, id);
	auto log_theta_v = map_v([&](double x){ return safe_log(x); }, theta_v);

	for (auto tid : item_tokens_[id]){
		WordId w = tokens_[tid].word_id;
		auto& phi_v = row_(phi_, tid);

		for (TopicId k = 0; k < K_; ++k){
			phi_v[k] = theta_v[k] * at_(beta_, k, w);
		}
		normalize_dist_v(phi_v);

		for (TopicId k = 0; k < K_; ++k){
			double const& p = phi_v[k];
			if (p > 0){
				likelihood += p * (log_theta_v[k] + log_beta_(k, w) - std::log(p));
			}
		}
	}

	if (pseudo_count > 0) {
		//likelihood += pseudo_count * std::accumulate(std::begin(log_theta_v), std::end(log_theta_v), 0.0);
		likelihood += pseudo_count * log_theta_v.sum();
	}

	// smoothing with small pseudo counts
	//sig::for_each_v([&](double& v){ v = pseudo_count; }, gamma_);
	gamma_.array() += pseudo_count;

	for (auto tid : item_tokens_[id]){
		for (TopicId k = 0; k < K_; ++k) {
			//double x = doc->m_counts[tid] * phi_(tid, k);	// doc_word_ct only
			double const& x = at_(phi_, tid, k);
			gamma_[k] += x;

			if (update_word_ss){
				at_(word_ss_, k, tokens_[tid].word_id) -= x;
			}
		}
	}

	return likelihood;
}

void CTR::updateU()
{
	double delta_ab = hparam_->a_ - hparam_->b_;
	MatrixKK_ XX = MatrixKK_::Zero(K_, K_);

	// calculate VCV^T in equation(8)
	for (uint i = 0; i < I_; i ++){
		if (std::begin(item_ratings_[i]) != std::end(item_ratings_[i])){
			auto const& vec_v = row_(item_factor_, i);

			//XX += outer_prod(vec_v, vec_v);
			XX += vec_v.transpose() * vec_v;
		}
    }

	// negative item weight
	XX *= hparam_->b_;

	//sig::for_diagonal([&](double& v){ v += hparam_->lambda_u_; }, XX);
	XX.diagonal().array() += hparam_->lambda_u_;

	for (uint j = 0; j < U_; ++j){
		auto const& ratings = user_ratings_[j];

		if (std::begin(ratings) != std::end(ratings)){
			EigenMatrix A = XX;
			VectorK_ x(K_, 0);

			for (auto rating : ratings){
				auto const& vec_v = row_(item_factor_, rating->item_id_);

				A += delta_ab * vec_v.transpose() * vec_v;
				x += hparam_->a_ * vec_v;
			}

			auto vec_u = row_(user_factor_, j);
			//vec_u = *sig::matrix_vector_solve(std::move(A), std::move(x));	// update vector u
			auto slv = A.fullPivLu().solve(x);
			for (uint k = 0; k < K_; ++k) vec_u.coeffRef(k) = slv.coeff(k);

			// update the likelihood
			//auto result = inner_prod(vec_u, vec_u);
			auto result = vec_u.dot(vec_u);
			likelihood_ += -0.5 * hparam_->lambda_u_ * result;
		}
	}
}

void CTR::updateV()
{
	double delta_ab = hparam_->a_ - hparam_->b_;
	MatrixKK_ XX = MatrixKK_::Zero(K_, K_);

	for (uint j = 0; j < U_; ++j){
		if (std::begin(user_ratings_[j]) != std::end(user_ratings_[j])){
			auto const& vec_u = row_(user_factor_, j);
			//XX += outer_prod(vec_u, vec_u);
			XX += vec_u.transpose() * vec_u;
		}
	}
	XX.array() *= hparam_->b_;

	for (uint i = 0; i < I_; ++i){
		auto& vec_v = row_(item_factor_, i);
		auto const& theta_v = row_(theta_, i);
		auto const& ratings = item_ratings_[i];

		if (std::begin(ratings) != std::end(ratings)){
			EigenMatrix A = XX;
			VectorK_ x(K_, 0);

			for (auto rating : ratings){
				auto const& vec_u = row_(user_factor_, rating->user_id_);

				//A += delta_ab * outer_prod(vec_u, vec_u);
				A += delta_ab * vec_u.transpose() * vec_u;
				x += hparam_->a_ * vec_u;
			}

			//sig::for_each_v([&](double& x, double t){ x += hparam_->lambda_v_ * t; }, xx, theta_v);
			x += hparam_->lambda_v_ * theta_v;	// adding the topic vector

			EigenMatrix B = A;		// save for computing likelihood

			//sig::for_diagonal([&](double& v){ v += hparam_->lambda_v_; }, A);
			A.diagonal().array() += hparam_->lambda_v_;

			//vec_v = *sig::matrix_vector_solve(A, std::move(x));	// update vector v
			vec_v = A.colPivHouseholderQr().solve(x);

			// update the likelihood for the relevant part
			likelihood_ += -0.5 * item_ratings_[i].size() * hparam_->a_;

			for (auto rating : ratings){
				auto const& vec_u = row_(user_factor_, rating->user_id_);
				//auto result = inner_prod(vec_u, vec_v);
				auto result = vec_u.dot(vec_u);

				likelihood_ += hparam_->a_ * result;
			}
			//likelihood_ += -0.5 * ublas::inner_prod(vec_v, ublas::prod(B, vec_v));
			likelihood_ += -0.5 * vec_v.dot(B * vec_v.transpose());

			// likelihood part of theta, even when theta=0, which is a special case
			EigenVector x2 = vec_v;

			//sig::for_each_v([](double& v1, double v2){ v1 -= v2; }, x2, theta_v);
			x2 -= theta_v;

			//auto result = inner_prod(x2, x2);
			auto result = x2.dot(x2);
			likelihood_ += -0.5 * hparam_->lambda_v_ * result;

			if (hparam_->theta_opt_){
				likelihood_ += docInference(i, true);
				optimize_simplex(gamma_, vec_v, hparam_->lambda_v_, row_(theta_, i));
			}
		}
		else{
			// m=0, this article has never been rated
			if (hparam_->theta_opt_) {
				docInference(i, false);
				normalize_dist_v(gamma_);
				row_(theta_, i) = gamma_;
			}
		}
	}
}

void CTR::updateBeta()
{
	beta_ = word_ss_;

	for (TopicId k = 0; k < K_; ++k){
		auto& beta_v = row_(beta_, k);

		normalize_dist_v(beta_v);
		row_(log_beta_, k) = map_v([&](double x){ return safe_log(x); }, beta_v);
	}
}

auto CTR::recommend_impl(Id id, bool for_user) const->std::vector<EstValueType>
{
	std::vector<EstValueType> result;

	if (for_user){
		uint i = 0;
		for (auto e : user_ratings_[id]){
			for (uint ed = e->item_id_; i < ed; ++i){
				result.push_back(std::make_pair(i, estimate(id, i)));
			}
			i = e->item_id_ + 1;
		}
		for (; i < I_; ++i){
			result.push_back(std::make_pair(i, estimate(id, i)));
		}
	}
	else{
		uint u = 0;
		for (auto e : item_ratings_[id]){
			for (uint ed = e->item_id_; u < ed; ++u){
				result.push_back(std::make_pair(u, estimate(u, id)));
			}
			u = e->user_id_ + 1;
		}
		for (; u < U_; ++u){
			result.push_back(std::make_pair(u, estimate(u, id)));
		}
	}

	sig::sort(result, [](std::pair<Id, double> const& v1, std::pair<Id, double> const& v2){ return v1.second > v2.second; });

	return result;
}

void CTR::train(uint max_iter, uint min_iter, uint save_lag)
{
	uint iter = 0;
	double likelihood_old;
	sig::ManageConvergenceSimple conv(conv_epsilon_);

	auto info_print = [](uint iter, double likelihood, double converge){
		std::cout << "iter=" << iter << ", likelihood=" << likelihood << ", converge=" << converge << std::endl;
		return true;
	};

	if (max_iter < min_iter) std::swap(max_iter, min_iter);

	if (hparam_->theta_opt_){
		gamma_ = VectorK_::Zero(K_);
		log_beta_ = map_m([&](double x){ return safe_log(x); }, beta_);
		word_ss_ = MatrixKV_(V_, K_); // SIG_INIT_MATRIX(double, K, V, 0);
		phi_ = MatrixKV_(K_, T_);  //SIG_INIT_MATRIX(double, T, K, 0);
	}

	 while ((!conv.is_convergence() && iter < max_iter) || iter < min_iter)
	 {
		likelihood_old = likelihood_;
		likelihood_ = 0.0;

		//printUFactor();
		//printIFactor();

		//sig::TimeWatch tw;
		updateU();
		//tw.save();
		//std::cout << tw.get_total_time() << std::endl;

		//if (hparam_->lda_regression_) break; // one iteration is enough for lda-regression

		updateV();

		// update beta if needed
		if (hparam_->theta_opt_) updateBeta();

		if(likelihood_ < likelihood_old) std::cout << "likelihood is decreasing!" << std::endl;

		// save intermediate results
		if (iter % save_lag == 0) {
			saveTmp();
		}

		++iter;
		conv.update( sig::abs_delta(likelihood_, likelihood_old) / likelihood_old);

		info_print(iter, likelihood_, conv.get_value());
	 }

	 std::cout << "train finished" << std::endl;
}

auto CTR::recommend(Id id, bool for_user, sig::Maybe<uint> top_n, sig::Maybe<double> threshold) const->std::vector<EstValueType>
{
	auto result = recommend_impl(id, for_user);

	if (top_n) result = sig::take(*top_n, std::move(result));
	if (threshold) sig::filter([&](std::pair<Id, double> const& e){ return e.second > *threshold; }, std::move(result));

	return result;
}

inline double CTR::estimate(UserId u_id, ItemId i_id) const
{
	//return inner_prod(row_(user_factor_, u_id), row_(item_factor_, i_id));
	return row_(user_factor_, u_id).dot(row_(item_factor_, i_id));
}

/*
void c_ctr::learn_map_estimate(
	const c_data* users,
	const c_data* items,
	const c_corpus* c,
	const ctr_hyperparameter* param,
	const char* directory)
{
  // init model parameters
  printf("\ninitializing the model ...\n");
  init_model(hparam_->ctr_run);

  // filename
  char name[500];

  // start time
  time_t start, current;
  time(&start);
  int elapsed = 0;

  int iter = 0;
  double likelihood = -exp(50), likelihood_old;
  double converge = 1.0;

  /// create the state log file
  sprintf(name, "%s/state.log", directory);
  FILE* file = fopen(name, "w");
  fprintf(file, "iter time likelihood converge\n");

  int i, j, m, n, l, k;
  int* item_ids;
  int* user_ids;

  double result;

  /// confidence parameters
  double a_minus_b = hparam_->a - hparam_->b;

  update();

  save();

  // free memory
  gsl_matrix_free(XX);
  gsl_matrix_free(A);
  gsl_matrix_free(B);
  gsl_vector_free(x);

  if (hparam_->ctr_run && hparam_->theta_opt) {
    gsl_matrix_free(phi);
    gsl_matrix_free(log_beta);
    gsl_matrix_free(word_ss);
    gsl_vector_free(gamma);
  }
}
*/

}	// sigtm