#define BOOST_TEST_MODULE SparseMatrices
#include <boost/test/unit_test.hpp>
#include <vexcl/vector.hpp>
#include <vexcl/sparse/csr.hpp>
#include <vexcl/sparse/ell.hpp>
#include <vexcl/sparse/matrix.hpp>
#include <vexcl/sparse/distributed.hpp>

typedef std::array<std::array<double, 2>, 2> matrix_value;
typedef std::array<double, 2> vector_value;

matrix_value mconst(double c) {
    matrix_value a = {c, c, c, c};
    return a;
}

vector_value vconst(double c) {
    vector_value v = {c, c};
    return v;
}

namespace vex {

template <> struct is_cl_native< matrix_value > : std::true_type {};
template <> struct is_cl_native< vector_value > : std::true_type {};

template <> struct type_name_impl<matrix_value> {
    static std::string get() { return "double4"; }
};

template <> struct type_name_impl<vector_value> {
    static std::string get() { return "double2"; }
};

namespace sparse {

template <>
struct rhs_of< matrix_value > {
    typedef vector_value type;
};

template <>
struct spmv_ops_impl<matrix_value, vector_value> {
    static void decl_accum_var(backend::source_generator &src, const std::string &name)
    {
        src.new_line() << type_name<vector_value>() << " " << name << " = {0,0};";
    }

    static void append_product(backend::source_generator &src,
            const std::string &sum, const std::string &mat_val, const std::string &vec_val)
    {
        src.open("{");
        src.new_line() << type_name<vector_value>() << " v = " << vec_val << ";";
        src.new_line() << sum << ".x += " << mat_val << ".x * v.x + " << mat_val << ".y * v.y;";
        src.new_line() << sum << ".y += " << mat_val << ".z * v.x + " << mat_val << ".w * v.y;";
        src.close("}");
    }
};

} // namespace sparse
} // namespace vex

#include "context_setup.hpp"
#include "random_matrix.hpp"

BOOST_AUTO_TEST_CASE(csr)
{
    const size_t n = 1024;

    std::vector<vex::command_queue> q(1, ctx.queue(0));

    std::vector<int>    row;
    std::vector<int>    col;
    std::vector<double> val;

    random_matrix(n, n, 16, row, col, val);

    std::vector<double> x = random_vector<double>(n);

    vex::sparse::csr<double> A(q, n, n, row, col, val);
    vex::vector<double> X(q, x);
    vex::vector<double> Y(q, n);

    Y = A * X;

    check_sample(Y, [&](size_t idx, double a) {
            double sum = 0;
            for(int j = row[idx]; j < row[idx + 1]; j++)
                sum += val[j] * x[col[j]];

            BOOST_CHECK_CLOSE(a, sum, 1e-8);
            });
}

BOOST_AUTO_TEST_CASE(ell)
{
    const size_t n = 1024;

    std::vector<vex::command_queue> q(1, ctx.queue(0));

    std::vector<int>    row;
    std::vector<int>    col;
    std::vector<double> val;

    random_matrix(n, n, 16, row, col, val);

    std::vector<double> x = random_vector<double>(n);

    vex::sparse::ell<double> A(q, n, n, row, col, val);
    vex::vector<double> X(q, x);
    vex::vector<double> Y(q, n);

    Y = A * X;

    check_sample(Y, [&](size_t idx, double a) {
            double sum = 0;
            for(int j = row[idx]; j < row[idx + 1]; j++)
                sum += val[j] * x[col[j]];

            BOOST_CHECK_CLOSE(a, sum, 1e-8);
            });
}

BOOST_AUTO_TEST_CASE(matrix)
{
    const size_t n = 1024;

    std::vector<vex::command_queue> q(1, ctx.queue(0));

    std::vector<int>    row;
    std::vector<int>    col;
    std::vector<double> val;

    random_matrix(n, n, 16, row, col, val);

    std::vector<double> x = random_vector<double>(n);

    vex::sparse::matrix<double> A(q, n, n, row, col, val);
    vex::vector<double> X(q, x);
    vex::vector<double> Y(q, n);

    Y = A * X;

    check_sample(Y, [&](size_t idx, double a) {
            double sum = 0;
            for(int j = row[idx]; j < row[idx + 1]; j++)
                sum += val[j] * x[col[j]];

            BOOST_CHECK_CLOSE(a, sum, 1e-8);
            });
}

BOOST_AUTO_TEST_CASE(distributed)
{
    const int n = 1024;

    std::vector<int>    ptr;
    std::vector<int>    col;
    std::vector<double> val;

    ptr.push_back(0);
    for(int i = 0; i < n; ++i) {
        if (i > 0) {
            col.push_back(i-1);
            val.push_back(-1);
        }
        col.push_back(i);
        val.push_back(2);
        if (i + 1 < n) {
            col.push_back(i+1);
            val.push_back(-1);
        }

        ptr.push_back(static_cast<int>(col.size()));
    }

    vex::sparse::distributed<vex::sparse::ell<double>> A(ctx, n, n, ptr, col, val);

    std::vector<double> x = random_vector<double>(n);
    vex::vector<double> X(ctx, x);
    vex::vector<double> Y(ctx, n);

    Y = A * X;

    for(int i = 0; i < n; ++i) {
        double y = Y[i];
        double sum = 0;
        for(int j = ptr[i]; j < ptr[i + 1]; j++)
            sum += val[j] * x[col[j]];

        BOOST_CHECK_CLOSE(y, sum, 1e-8);
    }
}

BOOST_AUTO_TEST_CASE(distributed_single)
{
    std::vector<vex::command_queue> q(1, ctx.queue(0));

    const int n = 1024;

    std::vector<int>    ptr;
    std::vector<int>    col;
    std::vector<double> val;

    ptr.push_back(0);
    for(int i = 0; i < n; ++i) {
        if (i > 0) {
            col.push_back(i-1);
            val.push_back(-1);
        }
        col.push_back(i);
        val.push_back(2);
        if (i + 1 < n) {
            col.push_back(i+1);
            val.push_back(-1);
        }

        ptr.push_back(static_cast<int>(col.size()));
    }

    vex::sparse::distributed<vex::sparse::ell<double>> A(q, n, n, ptr, col, val);

    std::vector<double> x = random_vector<double>(n);
    vex::vector<double> X(q, x);
    vex::vector<double> Y(q, n);

    Y = A * X;

    for(int i = 0; i < n; ++i) {
        double y = Y[i];
        double sum = 0;
        for(int j = ptr[i]; j < ptr[i + 1]; j++)
            sum += val[j] * x[col[j]];

        BOOST_CHECK_CLOSE(y, sum, 1e-8);
    }
}

BOOST_AUTO_TEST_CASE(custom_values)
{
    const int n = 1024;
    std::vector<vex::command_queue> q(1, ctx.queue(0));

    std::vector<int> ptr, col;
    std::vector<matrix_value> val;

    ptr.push_back(0);
    for(int i = 0; i < n; ++i) {
        if (i > 0) {
            col.push_back(i-1);
            val.push_back(mconst(-1));
        }
        col.push_back(i);
        val.push_back(mconst(2));
        if (i + 1 < n) {
            col.push_back(i+1);
            val.push_back(mconst(-1));
        }

        ptr.push_back(static_cast<int>(col.size()));
    }

    vex::sparse::matrix<matrix_value> A(q, n, n, ptr, col, val);

    std::vector<vector_value> x(n, vconst(1));
    vex::vector<vector_value> X(q, x);
    vex::vector<vector_value> Y(q, n);

    Y = A * X;

    for(int i = 0; i < n; ++i) {
        vector_value y = Y[i];
        vector_value sum = {0,0};
        for(int j = ptr[i]; j < ptr[i + 1]; j++) {
            sum[0] += val[j][0][0] * x[col[j]][0] + val[j][0][1] * x[col[j]][1];
            sum[1] += val[j][1][0] * x[col[j]][0] + val[j][1][1] * x[col[j]][1];
        }

        BOOST_CHECK_CLOSE(y[0], sum[0], 1e-8);
        BOOST_CHECK_CLOSE(y[1], sum[1], 1e-8);
    }
}

BOOST_AUTO_TEST_SUITE_END()


// solve_graphsensing.cpp
// Copyright 2019 Mikko Lauri
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <boost/program_options.hpp>
#include <chrono>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <sstream>
#include "BackwardPassParticle.hpp"
#include "GraphSensingProblem.h"
#include "JointPolicyUtilities.h"
#include "Particle.hpp"
#include "PolicyInitialization.h"

int main(int argc, char** argv) {
  unsigned int rng_seed;
  unsigned int horizon;
  unsigned int width;
  int improvement_steps;
  int num_particles_fwd;
  int num_rollouts;
  int num_particle_rollout;
  std::size_t blind_policy_initial_joint_action;
  pgi::PolicyInitialization policy_initialization;

  double mx, my, sx, sy;  // optional gaussian initial belief params

  namespace po = boost::program_options;
  po::options_description config("NPGI planner for (Dec)POMDPs \nUsage: " +
                                 std::string(argv[0]) +
                                 " [OPTION]...\nOptions");
  config.add_options()("help", "produce help message")(
      "horizon,h", po::value<unsigned int>(&horizon)->default_value(3),
      "problem horizon")("policy-width,w",
                         po::value<unsigned int>(&width)->default_value(3),
                         "policy width")(
      "policy-initialization,p",
      po::value<pgi::PolicyInitialization>(&policy_initialization)
          ->default_value(pgi::PolicyInitialization::random),
      "policy initialization type: (random | greedy | blind)")(
      "blind-action,b",
      po::value<std::size_t>(&blind_policy_initial_joint_action)
          ->default_value(0),
      "action for blind policy initialization")(
      "improvement-steps,i",
      po::value<int>(&improvement_steps)->default_value(9),
      "number of policy improvement steps")(
      "num-rollouts,r",
      po::value<int>(&num_rollouts)->default_value(100),
      "number of rollouts for value estimation")
      (
      "num-particles-rollout,k", po::value<int>(&num_particle_rollout)->default_value(100),
      "number of particles per rollout")(
      "num-particles,n", po::value<int>(&num_particles_fwd)->default_value(1000),
      "number of particles in P(s,q) forward pass")(
      "seed,s", po::value<unsigned int>(&rng_seed)->default_value(1234567890),
      "random number seed")(
      "output-prefix,o", po::value<std::string>()->default_value("./"),
      "output prefix")("gaussian", po::bool_switch()->default_value(false),
                       "toggle for Gaussian initial belief")(
      "mx", po::value<double>(&mx)->default_value(0.1),
      "initial belief mean x value")("my",
                                     po::value<double>(&my)->default_value(0.1),
                                     "initial belief mean y value")(
      "sx", po::value<double>(&sx)->default_value(0.1),
      "initial belief x value standard deviation")(
      "sy", po::value<double>(&sy)->default_value(0.1),
      "initial belief y value standard deviation");

  po::options_description cmdline_options;
  cmdline_options.add(config);

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).options(cmdline_options).run(),
            vm);
  po::notify(vm);
  if (vm.count("help")) {
    std::cout << config << std::endl;
    return 1;
  }

  pgi::PRNG rng(rng_seed);

  // Create the GraphSensing Dec-POMDP problem
  pgi::JointActionSpace jas = pgi::GraphSensing::joint_action_space;
  pgi::GraphSensing::StateTransitionModel t;
  pgi::GraphSensing::RewardModel r;

  // RSS
  pgi::JointObservationSpace jos =
      pgi::GraphSensing::rss_joint_observation_space;
  pgi::GraphSensing::RSSObservationModel o;

  // Initialize policy
  std::vector<pgi::PolicyGraph> local_policy_graphs;
  for (std::size_t agent = 0; agent < jas.num_local_spaces(); ++agent) {
    local_policy_graphs.emplace_back(pgi::fixed_width(
        vm["horizon"].as<unsigned int>(), width, jas.num_local_indices(agent),
        jos.num_local_indices(agent)));
  }

  if (policy_initialization == pgi::PolicyInitialization::random) {
    pgi::set_random(local_policy_graphs, rng);
  } else if (policy_initialization == pgi::PolicyInitialization::greedy) {
    throw std::runtime_error(
        "Greedy open loop initialization not implemented for continuous-state "
        "problems!");
  } else if (policy_initialization == pgi::PolicyInitialization::blind) {
    pgi::set_random(local_policy_graphs,
                    rng);  // to randomize the edges in the policy
    pgi::set_blind(local_policy_graphs, blind_policy_initial_joint_action, jas);
  }
  pgi::JointPolicy jp(local_policy_graphs);

  // Create initial belief
  pgi::ParticleSet<pgi::GraphSensing::state_t> init_particles;
  if (vm["gaussian"].as<bool>()) {
    pgi::GraphSensing::sample_initial_states_gaussian(
        init_particles.states_, num_particles_fwd,
        pgi::GraphSensing::location_t{mx, my}, sx, sy, rng);
  } else {
    pgi::GraphSensing::sample_initial_states(init_particles.states_,
                                             num_particles_fwd, rng);
  }
  init_particles.nodes_ =
      std::vector<pgi::JointPolicy::joint_vertex_t>(num_particles_fwd, jp.root());
  init_particles.weights_ = std::vector<double>(
      num_particles_fwd, 1.0 / static_cast<double>(num_particles_fwd));

  // Set up output streams
  std::ofstream fvalue(vm["output-prefix"].as<std::string>() +
                       "policy_values.txt");
  std::ofstream ftime(vm["output-prefix"].as<std::string>() +
                      "duration_microseconds.txt");

  // Get value of initial policy
  double best_value = estimate_value(num_rollouts, init_particles.states_,
                                     init_particles.weights_, jp, jp.root(), t,
                                     o, r, jas, jos, rng);

  fvalue << best_value << std::endl;
  std::cout << "Policy value: " << best_value << std::endl;

  // write initial best policy and value
  for (std::size_t agent = 0; agent < jas.num_local_spaces(); ++agent) {
    std::ofstream fs(vm["output-prefix"].as<std::string>() +
                     "best_policy_agent" + std::to_string(agent) + ".dot");
    print(fs, jp.local_policy(agent), pgi::element_names(jas.get(agent)),
          pgi::element_names(jos.get(agent)));
  }

  std::ofstream fs(vm["output-prefix"].as<std::string>() + "best_value.txt");
  fs << best_value << std::endl;

  // Backward pass setup
  pgi::backpass::BackPassProperties props;
  props.prob_random_history_in_heuristic_improvement = 0.05;

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

    // sample new set of particles for improvement
    if (vm["gaussian"].as<bool>()) {
      pgi::GraphSensing::sample_initial_states_gaussian(
          init_particles.states_, num_particles_fwd,
          pgi::GraphSensing::location_t{mx, my}, sx, sy, rng);
    } else {
      pgi::GraphSensing::sample_initial_states(init_particles.states_,
                                               num_particles_fwd, rng);
    }
    init_particles.nodes_ =
        std::vector<pgi::JointPolicy::joint_vertex_t>(num_particles_fwd, jp.root());
    init_particles.weights_ = std::vector<double>(
        num_particles_fwd, 1.0 / static_cast<double>(num_particles_fwd));

    std::chrono::high_resolution_clock::time_point t1 =
        std::chrono::high_resolution_clock::now();
    auto bp = pgi::backpass::improve_particle(num_rollouts, num_particle_rollout, init_particles, jp, t, o, r, jas,
                                              jos, rng, props);
    std::chrono::high_resolution_clock::time_point t3 =
        std::chrono::high_resolution_clock::now();
    auto duration =
        std::chrono::duration_cast<std::chrono::microseconds>(t3 - t1).count();
    ftime << duration << std::endl;

    // check
    double improved_policy_value = estimate_value(
        num_rollouts, init_particles.states_, init_particles.weights_, bp.improved_policy,
        bp.improved_policy.root(), t, o, r, jas, jos, rng);

    fvalue << improved_policy_value << std::endl;

    if (improved_policy_value > best_value) {
      best_value = improved_policy_value;
      jp = bp.improved_policy;

      // Update best value and policy!
      for (std::size_t agent = 0; agent < jas.num_local_spaces(); ++agent) {
        std::ofstream fs(vm["output-prefix"].as<std::string>() +
                         "best_policy_agent" + std::to_string(agent) + ".dot");
        print(fs, jp.local_policy(agent), pgi::element_names(jas.get(agent)),
              pgi::element_names(jos.get(agent)));
      }

      std::ofstream fs(vm["output-prefix"].as<std::string>() + "best_value.txt");
      fs << best_value << std::endl;
    }

    std::cout << "Step " << i << " of " << improvement_steps << ": "
              << improved_policy_value << " (best: " << best_value << ")"
              << ", " << duration << " microseconds" << std::endl;

    std::ostringstream ss;
    ss << vm["output-prefix"].as<std::string>() << "/step" << std::setw(3)
       << std::setfill('0') << i << "_";

    for (std::size_t agent = 0; agent < jas.num_local_spaces(); ++agent) {
      std::ofstream fs(ss.str() + "agent" + std::to_string(agent) + ".dot");
      print(fs, bp.improved_policy.local_policy(agent),
            pgi::element_names(jas.get(agent)),
            pgi::element_names(jos.get(agent)));
    }
  }

  pgi::ForwardPassParticle<pgi::GraphSensing::state_t> fwd(init_particles, jp,
                                                           t, o, jas, jos, rng);
  std::cout << "Forward pass results: \n";
  for (unsigned int s = 1; s <= jp.max_steps(); ++s) {
  for (auto& q : jp.joint_vertices_with_steps_remaining(s))
  {
    auto ql = jp.to_local(q);
    for (auto& qq : ql)
      std::cout << qq << " ";

    const pgi::ParticleSet<pgi::GraphSensing::state_t>& p = fwd.particles_at(q);
    std::cout << ": " << p.weights_.size() << " particles\n";

  }
  }

  return 0;
}


/* The MIT License

Copyright (c) 2011 Sahab Yazdani

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

#include "stippler_impl.h"

#include <fstream>
#include <limits>
#include <cstring>

#include <boost/random.hpp>

#include "VoronoiDiagramGenerator.h"

Stippler::Stippler( const StipplingParameters &parameters )
: IStippler(),
vertsX(new float[parameters.points]), vertsY(new float[parameters.points]), radii(new float[parameters.points]),
displacement(std::numeric_limits<float>::max()),
image(parameters.inputFile),
parameters(parameters) {
	createInitialDistribution();
}

Stippler::~Stippler() {
	delete[] radii;
	delete[] vertsX;
	delete[] vertsY;
}

void Stippler::distribute() {
	createVoronoiDiagram();
	redistributeStipples();
}

float Stippler::getAverageDisplacement() {
	return displacement;
}

void Stippler::createInitialDistribution() {
	using std::ceil;

	// find initial distribution
	boost::mt19937 rng;
	boost::uniform_01<boost::mt19937, float> generator( rng );

	float w = (float)(image.getWidth() - 1), h = (float)(image.getHeight() - 1);
	float xC, yC;

	for ( unsigned int i = 0; i < parameters.points; ) {
		xC = generator() * w;
		yC = generator() * h;

		// do a nearest neighbour search on the vertices
		if ( ceil(generator() * 255.0f) <= image.getIntensity( xC, yC ) ) {
			vertsX[i] = xC;
			vertsY[i] = yC;
			radii[i] = 0.0f;

			i++;
		}
	}
}

void Stippler::getStipples( StipplePoint *dst ) {
	StipplePoint *workingPtr;

	for (unsigned int i = 0; i < parameters.points; i++ ) {
		workingPtr = &(dst[i]);

		workingPtr->x = vertsX[i];
		workingPtr->y = vertsY[i];
		workingPtr->radius = radii[i];

		image.getColour(vertsX[i], vertsY[i], workingPtr->r, workingPtr->g, workingPtr->b);
	}
}

void Stippler::createVoronoiDiagram() {
	VoronoiDiagramGenerator generator;

	generator.generateVoronoi( vertsX, vertsY, parameters.points,
		0.0f, (float)(image.getWidth() - 1), 0.0f, (float)(image.getHeight() - 1) );

	edges.clear();

	Point< float > p1, p2;
	Edge< float > edge;

	generator.resetIterator();
	while ( generator.getNext(
		edge.begin.x, edge.begin.y, edge.end.x, edge.end.y,
		p1.x, p1.y, p2.x, p2.y ) ) {

		if ( edge.begin == edge.end ) {
			continue;
		}

		if ( edges.find( p1 ) == edges.end() ) {
			edges[p1] = EdgeList();
		}
		if ( edges.find( p2 ) == edges.end() ) {
			edges[p2] = EdgeList();
		}
		edges[p1].push_back( edge );
		edges[p2].push_back( edge );
	}
}

void Stippler::redistributeStipples() {
	using std::pow;
	using std::sqrt;
	using std::pair;
	using std::make_pair;
	using std::vector;

	vector< pair< Point< float >, EdgeList > > vectorized;
	for ( EdgeMap::iterator key_iter = edges.begin(); key_iter != edges.end(); ++key_iter ) {
		vectorized.push_back(make_pair(key_iter->first, key_iter->second));
	}

	float local_displacement = 0.0f;

	#pragma omp parallel for reduction(+:local_displacement)
	for (int i = 0; i < (int)vectorized.size(); i++) {
		pair< Point< float >, EdgeList > item = vectorized[i];
		pair< Point<float>, float > centroid = calculateCellCentroid( item.first, item.second );

		radii[i] = centroid.second;
		vertsX[i] = centroid.first.x;
		vertsY[i] = centroid.first.y;

		local_displacement += sqrt( pow( item.first.x - centroid.first.x, 2.0f ) + pow( item.first.y - centroid.first.y, 2.0f ) );
	}

	displacement = local_displacement / vectorized.size(); // average out the displacement
}

inline Line<float> Stippler::createClipLine( float insideX, float insideY, float x1, float y1, float x2, float y2 ) {
	using std::abs;
	using std::numeric_limits;

	Line<float> l;

	// if the floating point version of the line collapsed down to one
	// point, then just ignore it all
	if (abs(x1 - x2) < numeric_limits<float>::epsilon() && abs(y1 - y2) < numeric_limits<float>::epsilon()) {
		l.a = .0f;
		l.b = .0f;
		l.c = .0f;

		return l;
	}

	l.a = -(y1 - y2);
	l.b = x1 - x2;
	l.c = (y1 - y2) * x1 - (x1 - x2) * y1;

	// make sure the known inside point falls on the correct side of the clipping plane
	if ( insideX * l.a + insideY * l.b + l.c > 0.0f ) {
		l.a *= -1;
		l.b *= -1;
		l.c *= -1;
	}

	return l;
}

std::pair< Point<float>, float > Stippler::calculateCellCentroid( Point<float> &inside, EdgeList &edgeList ) {
	using std::make_pair;
	using std::numeric_limits;
	using std::vector;
	using std::floor;
	using std::ceil;
	using std::abs;
	using std::sqrt;
	using std::pow;

	vector< Line<float> > clipLines;
	Extents<float> extent = getCellExtents(edgeList);

	unsigned int x, y;

	float xDiff = ( extent.maxX - extent.minX );
	float yDiff = ( extent.maxY - extent.minY );

	unsigned int tileWidth = (unsigned int)ceil(xDiff) * parameters.subpixels;
	unsigned int tileHeight = (unsigned int)ceil(yDiff) * parameters.subpixels;

	float xStep = xDiff / (float)tileWidth;
	float yStep = yDiff / (float)tileHeight;

	float spotDensity, areaDensity = 0.0f, maxAreaDensity = 0.0f;
	float xSum = 0.0f;
	float ySum = 0.0f;

	float xCurrent;
	float yCurrent;

	// compute the clip lines
	for ( EdgeList::iterator value_iter = edgeList.begin(); value_iter != edgeList.end(); ++value_iter ) {
		Line<float> l = createClipLine( inside.x, inside.y,
			value_iter->begin.x, value_iter->begin.y,
			value_iter->end.x, value_iter->end.y );

		if (l.a < numeric_limits<float>::epsilon() && abs(l.b) < numeric_limits<float>::epsilon()) {
			continue;
		}

		clipLines.push_back(l);
	}

	for ( y = 0, yCurrent = extent.minY; y < tileHeight; ++y, yCurrent += yStep ) {
		for ( x = 0, xCurrent = extent.minX; x < tileWidth; ++x, xCurrent += xStep ) {
			// a point is outside of the polygon if it is outside of all clipping planes
			bool outside = false;
			for ( vector< Line<float> >::iterator iter = clipLines.begin(); iter != clipLines.end(); iter++ ) {
				if ( xCurrent * iter->a + yCurrent * iter->b + iter->c >= 0.0f ) {
					outside = true;
					break;
				}
			}

			if (!outside) {
				spotDensity = image.getIntensity(xCurrent, yCurrent);

				areaDensity += spotDensity;
				maxAreaDensity += 255.0f;
				xSum += spotDensity * xCurrent;
				ySum += spotDensity * yCurrent;
			}
		}
	}

	float area = areaDensity * xStep * yStep / 255.0f;
	float maxArea = maxAreaDensity * xStep * yStep / 255.0f;

	Point<float> pt;
	if (areaDensity > numeric_limits<float>::epsilon()) {
		pt.x = xSum / areaDensity;
		pt.y = ySum / areaDensity;
	} else {
		// if for some reason, the cell is completely white, then the centroid does not move
		pt.x = inside.x;
		pt.y = inside.y;
	}

	float closest = numeric_limits<float>::max(),
		  farthest = numeric_limits<float>::min(),
		  distance;
	float x0 = pt.x, y0 = pt.y,
	      x1, x2, y1, y2;

	for ( EdgeList::iterator value_iter = edgeList.begin(); value_iter != edgeList.end(); ++value_iter ) {
		x1 = value_iter->begin.x; x2 = value_iter->end.x;
		y1 = value_iter->begin.y; y2 = value_iter->end.y;

		distance = abs( ( x2 - x1 ) * ( y1 - y0 ) - ( x1 - x0 ) * ( y2 - y1 ) ) / sqrt( pow( x2 - x1, 2.0f ) + pow( y2 - y1, 2.0f ) );
		if ( closest > distance ) {
			closest = distance;
		}
		if ( farthest < distance ) {
			farthest = distance;
		}
	}

	float radius;
	if ( parameters.noOverlap ) {
		radius = closest;
	} else {
		radius = farthest;
	}
	radius *= area / maxArea;

	return make_pair( pt, radius );
}

Extents<float> Stippler::getCellExtents( Stippler::EdgeList &edgeList ) {
	using std::numeric_limits;

	Extents<float> extent;

	extent.minX = extent.minY = numeric_limits<float>::max();
	extent.maxX = extent.maxY = numeric_limits<float>::min();

	for ( EdgeList::iterator value_iter = edgeList.begin(); value_iter != edgeList.end(); ++value_iter ) {
		if ( value_iter->begin.x < extent.minX ) extent.minX = value_iter->begin.x;
		if ( value_iter->end.x < extent.minX ) extent.minX = value_iter->end.x;
		if ( value_iter->begin.y < extent.minY ) extent.minY = value_iter->begin.y;
		if ( value_iter->end.y < extent.minY ) extent.minY = value_iter->end.y;

		if ( value_iter->begin.x > extent.maxX ) extent.maxX = value_iter->begin.x;
		if ( value_iter->end.x > extent.maxX ) extent.maxX = value_iter->end.x;
		if ( value_iter->begin.y > extent.maxY ) extent.maxY = value_iter->begin.y;
		if ( value_iter->end.y > extent.maxY ) extent.maxY = value_iter->end.y;
	}

	return extent;
}

bool operator==(Point<float> const& p1, Point<float> const& p2)
{
	using std::abs;
	using std::numeric_limits;

	return abs( p1.x - p2.x ) < numeric_limits<float>::epsilon() &&
		abs( p1.y - p2.y ) < numeric_limits<float>::epsilon();
}

std::size_t hash_value(Point<float> const& p) {
	using std::size_t;
	using boost::hash_combine;

    size_t seed = 0;

    hash_combine(seed, p.x);
    hash_combine(seed, p.y);

    return seed;
}


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2016 INRIA.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
*/

#include "SiconosConfig.h"
#include <boost/numeric/bindings/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/trans.hpp>
#include <boost/numeric/bindings/blas/level3.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/std/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
// Note Franck : sounds useless. It seems it's defined in bindings
// (to be checked, especially on windows)

// #define BIND_FORTRAN_LOWERCASE_UNDERSCORE

// needed for blas3
#include <assert.h>

namespace siconosBindings = boost::numeric::bindings;

// for ublas::axpy_prod, ...
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/operation_sparse.hpp>

// require for matrix stuff like value_type
//#include <boost/numeric/bindings/traits/ublas_matrix.hpp>

#include "SimpleMatrix.hpp"
#include "BlockMatrixIterators.hpp"
#include "BlockMatrix.hpp"

#include "SiconosAlgebra.hpp"

using namespace Siconos;

//======================
// Product of matrices
//======================

const SimpleMatrix prod(const SiconosMatrix &A, const SiconosMatrix& B)
{
  // To compute C = A * B
  assert(!(B.isPLUFactorized()) && "B is PLUFactorized in prod !!");
  assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!");

  if ((A.size(1) != B.size(0)))
    SiconosMatrixException::selfThrow("Matrix function C=prod(A,B): inconsistent sizes");

  unsigned int numA = A.num();
  unsigned int numB = B.num();

  // == TODO: implement block product ==
  if (numA == 0 || numB == 0)
    SiconosMatrixException::selfThrow("Matrix product ( C=prod(A,B) ): not yet implemented for BlockMatrix objects.");

  if (numA == 7 || numB == 6) // A = identity or B = 0
    return SimpleMatrix(B);

  else if (numB == 7 || numA == 6) // B = identity or A = 0
    return SimpleMatrix(A);

  else // neither A or B is equal to identity or zero.
  {
    if (numB == 1)
    {
      if (numA == 1)
      {
        DenseMat p(A.size(0), B.size(1));
        siconosBindings::blas::gemm(1.0, *A.dense(), *B.dense(), 1.0, p);
        //      return (DenseMat)(prod(*A.dense(),*B.dense()));
        return p;
      }
      else if (numA == 2)
        return (DenseMat)(prod(*A.triang(), *B.dense()));
      else if (numA == 3)
        return (DenseMat)(prod(*A.sym(), *B.dense()));
      else if (numA == 4)
        return (DenseMat)(prod(*A.sparse(), *B.dense()));
      else// if(numA==5)
        return (DenseMat)(prod(*A.banded(), *B.dense()));
    }
    else if (numB == 2)
    {
      if (numA == 1)
        return (DenseMat)(prod(*A.dense(), *B.triang()));
      else if (numA == 2)
        return (TriangMat)(prod(*A.triang(), *B.triang()));
      else if (numA == 3)
        return (DenseMat)(prod(*A.sym(), *B.triang()));
      else if (numA == 4)
        return (DenseMat)(prod(*A.sparse(), *B.triang()));
      else //if(numA==5)
        return (DenseMat)(prod(*A.banded(), *B.triang()));
    }
    else if (numB == 3)
    {
      if (numA == 1)
        return (DenseMat)(prod(*A.dense(), *B.sym()));
      else if (numA == 2)
        return (DenseMat)(prod(*A.triang(), *B.sym()));
      else if (numA == 3)
        return (SymMat)(prod(*A.sym(), *B.sym()));
      else if (numA == 4)
        return (DenseMat)(prod(*A.sparse(), *B.sym()));
      else // if (numA == 5)
        return (DenseMat)(prod(*A.banded(), *B.sym()));
    }
    else if (numB == 4)
    {
      if (numA == 1)
        return (DenseMat)(prod(*A.dense(), *B.sparse()));
      else if (numA == 2)
        return (DenseMat)(prod(*A.triang(), *B.sparse()));
      else if (numA == 3)
        return (DenseMat)(prod(*A.sym(), *B.sparse()));
      else if (numA == 4)
        return (SparseMat)(prod(*A.sparse(), *B.sparse()));
      else //if(numA==5){
        return (DenseMat)(prod(*A.banded(), *B.sparse()));
    }
    else //if(numB==5)
    {
      if (numA == 1)
        return (DenseMat)(prod(*A.dense(), *B.banded()));
      else if (numA == 2)
        return (DenseMat)(prod(*A.triang(), *B.banded()));
      else if (numA == 3)
        return (DenseMat)(prod(*A.sym(), *B.banded()));
      else if (numA == 4)
        return (DenseMat)(prod(*A.sparse(), *B.banded()));
      else //if(numA==5)
        return (DenseMat)(prod(*A.banded(), *B.banded()));
    }
  }
}
/**

indexStart : indexStart[0] is the first raw, indexStart[1] is the first col
dim : dim[0] number of raw, dim[1] number of col
*/
// void zeroBlock(const SiconosMatrix& A, index indexStart, index dim){
//   ;
// }
// void prod(const SiconosMatrix& A, const SiconosMatrix& B, SiconosMatrix& C, int indexACol, bool init){
//   // To compute C[indexAcol::] = A * B

//   unsigned int numA = A.num();
//   unsigned int numB = B.num();
//   unsigned int numC = C.num();
//   if (numA == 0 || numB == 0 || numC == 0)
//     SiconosMatrixException::selfThrow("Matrix function prod(A,B,C,index): inconsistent sizes");
//   // === if C is zero or identity => read-only ===
//   if (numC == 6 || numC == 7)
//     SiconosMatrixException::selfThrow("Matrix product ( prod(A,B,C,index) ): wrong type for resulting matrix C (read-only: zero or identity).");

//   if (numA == 7 || numC == 6) // A = identity or 0
//     SiconosMatrixException::selfThrow("Matrix function prod(A,B,C,index): numA == 7 || numC == 6 not yet implemented");

//   int rawB = B.size(0);
//   int colB = B.size(1);

// }
void prod(const SiconosMatrix& A, const SiconosMatrix& B, SiconosMatrix& C, bool init)
{
  // To compute C = A * B
  assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!" );
  assert(!(B.isPLUFactorized()) && "B is PLUFactorized in prod !!" );
  if(!C.isBlock())
    C.resetLU();

  if ((A.size(1) != B.size(0)))
    SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): inconsistent sizes");

  if (A.size(0) != C.size(0) || B.size(1) != C.size(1))
    SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): inconsistent sizes");

  unsigned int numA = A.num();
  unsigned int numB = B.num();
  unsigned int numC = C.num();

  // == TODO: implement block product ==
  if (numA == 0 || numB == 0)
    SiconosMatrixException::selfThrow("Matrix product ( prod(A,B,C) ): not yet implemented for BlockMatrix objects.");

  // === if C is zero or identity => read-only ===
  if (numC == 6 || numC == 7)
    SiconosMatrixException::selfThrow("Matrix product ( prod(A,B,C) ): wrong type for resulting matrix C (read-only: zero or identity).");

  if (numA == 7) // A = identity ...
  {
    if (init)
    {
      if (&C != &B) C = B; // if C and B are two different objects.
      // else nothing
    }
    else
      C += B;
  }

  else if (numB == 7) // B = identity
  {
    if (init)
    {
      if (&C != &A) C = A; // if C and A are two different objects.
      // else nothing
    }
    else
      C += A;
  }

  else if (numA == 6 || numB == 6) // if A or B = 0
  {
    if (init)
      C.zero();
    //else nothing
  }
  else if (numC == 0) // if C is Block - Temp. solution
  {
    SimpleMatrix tmp(C);
    prod(A, B, tmp, init);
    C = tmp;
  }
  else // neither A or B is equal to identity or zero.
  {
    if (init)
    {
      if (&C == &A) // if common memory between A and C
      {
        switch (numA)
        {
        case 1:
          if (numB == 1)
          {
            *C.dense()  = prod(*A.dense(), *B.dense());
            //siconosBindings::blas::gemm(1.0, *A.dense(), *B.dense(), 0.0, *C.dense());
          }
          else if (numB == 2)
            *C.dense()  = prod(*A.dense(), *B.triang());
          else if (numB == 3)
            *C.dense()  = prod(*A.dense(), *B.sym());
          else if (numB == 4)
            *C.dense()  = prod(*A.dense(), *B.sparse());
          else //if(numB==5)
            *C.dense() = prod(*A.dense(), *B.banded());
          break;
        case 2:
          if (numB != 2)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          *C.triang() = prod(*A.triang(), *B.triang());
          break;
        case 3:
          if (numB != 3)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          *C.sym() = prod(*A.sym(), *B.sym());
          break;
        case 4:
          if (numB != 4)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          *C.sparse() = prod(*A.sparse(), *B.sparse());
          break;
        default:
          SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
        }
      }
      else if (&C == &B)
      {
        switch (numB)
        {
        case 1:
          if (numA == 1)
            *C.dense() = prod(*A.dense(), *B.dense());
          else if (numA == 2)
            *C.dense()  = prod(*A.triang(), *B.dense());
          else if (numA == 3)
            *C.dense()  = prod(*A.sym(), *B.dense());
          else if (numA == 4)
            *C.dense()  = prod(*A.sparse(), *B.dense());
          else //if(numB==5)
            *C.dense() = prod(*A.banded(), *B.dense());
          break;
        case 2:
          if (numA != 2)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          *C.triang() = prod(*A.triang(), *B.triang());
          break;
        case 3:
          if (numA != 3)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          *C.sym() = prod(*A.sym(), *B.sym());
          break;
        case 4:
          if (numA != 4)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          *C.sparse() = prod(*A.sparse(), *B.sparse());
          break;
        default:
          SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
        }
      }
      else // if no alias between C and A or B.
      {
        switch (numC)
        {
        case 1:
          if (numB == 1)
          {
            if (numA == 1)
              noalias(*C.dense()) = prod(*A.dense(), *B.dense());
            else if (numA == 2)
              noalias(*C.dense()) = prod(*A.triang(), *B.dense());
            else if (numA == 3)
              noalias(*C.dense())  = prod(*A.sym(), *B.dense());
            else if (numA == 4)
              noalias(*C.dense()) = prod(*A.sparse(), *B.dense());
            else// if(numA==5)
              noalias(*C.dense())  = prod(*A.banded(), *B.dense());
          }
          else if (numB == 2)
          {
            if (numA == 1)
              noalias(*C.dense())  = prod(*A.dense(), *B.triang());
            else if (numA == 2)
              noalias(*C.dense())  = prod(*A.triang(), *B.triang());
            else if (numA == 3)
              noalias(*C.dense())  = prod(*A.sym(), *B.triang());
            else if (numA == 4)
              noalias(*C.dense())  = prod(*A.sparse(), *B.triang());
            else //if(numA==5)
              noalias(*C.dense())  = prod(*A.banded(), *B.triang());
          }
          else if (numB == 3)
          {
            if (numA == 1)
              noalias(*C.dense())  = prod(*A.dense(), *B.sym());
            else if (numA == 2)
              noalias(*C.dense())  = prod(*A.triang(), *B.sym());
            else if (numA == 3)
              noalias(*C.dense())  = prod(*A.sym(), *B.sym());
            else if (numA == 4)
              noalias(*C.dense())  = prod(*A.sparse(), *B.sym());
            else // if (numA == 5)
              noalias(*C.dense())  = prod(*A.banded(), *B.sym());
          }
          else if (numB == 4)
          {
            if (numA == 1)
              noalias(*C.dense()) = prod(*A.dense(), *B.sparse());
            else if (numA == 2)
              noalias(*C.dense()) = prod(*A.triang(), *B.sparse());
            else if (numA == 3)
              noalias(*C.dense()) = prod(*A.sym(), *B.sparse());
            else if (numA == 4)
              noalias(*C.dense()) = prod(*A.sparse(), *B.sparse());
            else //if(numA==5){
              noalias(*C.dense()) = prod(*A.banded(), *B.sparse());
          }
          else //if(numB==5)
          {
            if (numA == 1)
              noalias(*C.dense()) = prod(*A.dense(), *B.banded());
            else if (numA == 2)
              noalias(*C.dense()) = prod(*A.triang(), *B.banded());
            else if (numA == 3)
              noalias(*C.dense()) = prod(*A.sym(), *B.banded());
            else if (numA == 4)
              noalias(*C.dense()) = prod(*A.sparse(), *B.banded());
            else //if(numA==5)
              noalias(*C.dense()) = prod(*A.banded(), *B.banded());
          }
          break;
        case 2:
          if (numA != 2 || numB != 2)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          noalias(*C.triang()) = prod(*A.triang(), *B.triang());
          break;
        case 3:
          if (numA != 3 || numB != 3)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          noalias(*C.sym()) = prod(*A.sym(), *B.sym());
          break;
        case 4:
          if (numA != 4 || numB != 4)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          noalias(*C.sparse()) = prod(*A.sparse(), *B.sparse());
          break;
        default:
          SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
        }
      }
    }
    else // += case
    {
      if (&C == &A) // if common memory between A and C
      {
        switch (numA)
        {
        case 1:
          if (numB == 1)
            *C.dense() += prod(*A.dense(), *B.dense());
          else if (numB == 2)
            *C.dense()  += prod(*A.dense(), *B.triang());
          else if (numB == 3)
            *C.dense()  += prod(*A.dense(), *B.sym());
          else if (numB == 4)
            *C.dense()  += prod(*A.dense(), *B.sparse());
          else //if(numB==5)
            *C.dense() += prod(*A.dense(), *B.banded());
          break;
        case 2:
          if (numB != 2)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          *C.triang() += prod(*A.triang(), *B.triang());
          break;
        case 3:
          if (numB != 3)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          *C.sym() += prod(*A.sym(), *B.sym());
          break;
        case 4:
          if (numB != 4)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          *C.sparse() += prod(*A.sparse(), *B.sparse());
          break;
        default:
          SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
        }
      }
      else if (&C == &B)
      {
        switch (numB)
        {
        case 1:
          if (numA == 1)
            *C.dense() += prod(*A.dense(), *B.dense());
          else if (numA == 2)
            *C.dense()  += prod(*A.triang(), *B.dense());
          else if (numA == 3)
            *C.dense()  += prod(*A.sym(), *B.dense());
          else if (numA == 4)
            *C.dense()  += prod(*A.sparse(), *B.dense());
          else //if(numB==5)
            *C.dense() += prod(*A.banded(), *B.dense());
          break;
        case 2:
          if (numA != 2)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          *C.triang() += prod(*A.triang(), *B.triang());
          break;
        case 3:
          if (numA != 3)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          *C.sym() += prod(*A.sym(), *B.sym());
          break;
        case 4:
          if (numA != 4)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          *C.sparse() += prod(*A.sparse(), *B.sparse());
          break;
        default:
          SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
        }
      }
      else // if no alias between C and A or B.
      {
        switch (numC)
        {
        case 1:
          if (numB == 1)
          {
            if (numA == 1)
              noalias(*C.dense()) += prod(*A.dense(), *B.dense());
            else if (numA == 2)
              noalias(*C.dense()) += prod(*A.triang(), *B.dense());
            else if (numA == 3)
              noalias(*C.dense())  += prod(*A.sym(), *B.dense());
            else if (numA == 4)
              noalias(*C.dense()) += prod(*A.sparse(), *B.dense());
            else// if(numA==5)
              noalias(*C.dense())  += prod(*A.banded(), *B.dense());
          }
          else if (numB == 2)
          {
            if (numA == 1)
              noalias(*C.dense())  += prod(*A.dense(), *B.triang());
            else if (numA == 2)
              noalias(*C.dense())  += prod(*A.triang(), *B.triang());
            else if (numA == 3)
              noalias(*C.dense())  += prod(*A.sym(), *B.triang());
            else if (numA == 4)
              noalias(*C.dense())  += prod(*A.sparse(), *B.triang());
            else //if(numA==5)
              noalias(*C.dense())  += prod(*A.banded(), *B.triang());
          }
          else if (numB == 3)
          {
            if (numA == 1)
              noalias(*C.dense())  += prod(*A.dense(), *B.sym());
            else if (numA == 2)
              noalias(*C.dense())  += prod(*A.triang(), *B.sym());
            else if (numA == 3)
              noalias(*C.dense())  += prod(*A.sym(), *B.sym());
            else if (numA == 4)
              noalias(*C.dense())  += prod(*A.sparse(), *B.sym());
            else // if (numA == 5)
              noalias(*C.dense())  += prod(*A.banded(), *B.sym());
          }
          else if (numB == 4)
          {
            if (numA == 1)
              noalias(*C.dense()) += prod(*A.dense(), *B.sparse());
            else if (numA == 2)
              noalias(*C.dense()) += prod(*A.triang(), *B.sparse());
            else if (numA == 3)
              noalias(*C.dense()) += prod(*A.sym(), *B.sparse());
            else if (numA == 4)
              noalias(*C.dense()) += prod(*A.sparse(), *B.sparse());
            else //if(numA==5){
              noalias(*C.dense()) += prod(*A.banded(), *B.sparse());
          }
          else //if(numB==5)
          {
            if (numA == 1)
              noalias(*C.dense()) += prod(*A.dense(), *B.banded());
            else if (numA == 2)
              noalias(*C.dense()) += prod(*A.triang(), *B.banded());
            else if (numA == 3)
              noalias(*C.dense()) += prod(*A.sym(), *B.banded());
            else if (numA == 4)
              noalias(*C.dense()) += prod(*A.sparse(), *B.banded());
            else //if(numA==5)
              noalias(*C.dense()) += prod(*A.banded(), *B.banded());
          }
          break;
        case 2:
          if (numA != 2 || numB != 2)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          noalias(*C.triang()) += prod(*A.triang(), *B.triang());
          break;
        case 3:
          if (numA != 3 || numB != 3)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          noalias(*C.sym()) += prod(*A.sym(), *B.sym());
          break;
        case 4:
          if (numA != 4 || numB != 4)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          noalias(*C.sparse()) += prod(*A.sparse(), *B.sparse());
          break;
        default:
          SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
        }
      }
    }
  if(!C.isBlock())
    C.resetLU();
  }
}

void axpy_prod(const SiconosMatrix& A, const SiconosMatrix& B, SiconosMatrix& C, bool init)
{
  // To compute C = A * B (init = true) or C += A * B (init = false) using ublas axpy_prod.
  if ((A.size(1) != B.size(0)))
    SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): inconsistent sizes");

  if (A.size(0) != C.size(0) || B.size(1) != C.size(1))
    SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): inconsistent sizes");

  assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!" );
  assert(!(B.isPLUFactorized()) && "B is PLUFactorized in prod !!" );
  if(!C.isBlock())
    C.resetLU();
  unsigned int numA = A.num();
  unsigned int numB = B.num();
  unsigned int numC = C.num();
  // == TODO: implement block product ==
  if (numA == 0 || numB == 0)
    SiconosMatrixException::selfThrow("Matrix product ( prod(A,B,C) ): not yet implemented for BlockMatrix objects.");

  // === if C is zero or identity => read-only ===
  if (numC == 6 || numC == 7)
    SiconosMatrixException::selfThrow("Matrix product ( prod(A,B,C) ): wrong type for resulting matrix C (read-only: zero or identity).");

  if (numA == 7) // A = identity ...
  {
    if (!init) C += B;
    else
    {
      if (&C != &B)
        C = B; // if C and B are two different objects.
      // else nothing
    }
  }

  else if (numB == 7) // B = identity
  {
    if (!init) C += A;
    else
    {
      if (&C != &A) C = A; // if C and A are two different objects.
      // else nothing
    }
  }

  else if (numA == 6 || numB == 6) // if A or B = 0
  {
    if (init) C.zero(); // else nothing
  }
  else if (numC == 0) // if C is Block - Temp. solution
  {
    SimpleMatrix tmp(C);
    axpy_prod(A, B, tmp, init);
    C = tmp;
  }
  else // neither A or B is equal to identity or zero.
  {
    if (&C == &A) // if common memory between A and C
    {
      switch (numA)
      {
      case 1:
        if (numB == 1)
          ublas::axpy_prod(*A.dense(), *B.dense(), *A.dense(), init);
        else if (numB == 2)
          ublas::axpy_prod(*A.dense(), *B.triang(), *A.dense(), init);
        else if (numB == 3)
          ublas::axpy_prod(*A.dense(), *B.sym(), *A.dense(), init);
        else if (numB == 4)
          ublas::axpy_prod(*A.dense(), *B.sparse(), *A.dense(), init);
        else //if(numB==5)
          ublas::axpy_prod(*A.dense(), *B.banded(), *A.dense(), init);
        break;
      case 2:
        //        if(numB != 2)
        SiconosMatrixException::selfThrow("Matrix function axpy_prod(A,B,C): wrong type for C (according to A and B types).");
        //        ublas::axpy_prod(*A.triang(), *B.triang(), *A.triang(), init);
        break;
      case 3:
        //if(numB != 3)
        SiconosMatrixException::selfThrow("Matrix function axpy_prod(A,B,C): wrong type for C (according to A and B types).");
        //ublas::axpy_prod(*A.sym(), *B.sym(), *A.sym(), init);
        break;
      case 4:
        //        if(numB != 4)
        SiconosMatrixException::selfThrow("Matrix function axpy_prod(A,B,C): wrong type for C (according to A and B types).");
        //ublas::axpy_prod(*A.sparse(), *B.sparse(), *A.sparse(),init);
        break;
      default:
        SiconosMatrixException::selfThrow("Matrix function axpy_prod(A,B,C): wrong type for C (according to A and B types).");
      }
    }
    else if (&C == &B)
    {
      switch (numB)
      {
      case 1:
        if (numA == 1)
          ublas::axpy_prod(*A.dense(), *B.dense(), *B.dense(), init);
        else if (numA == 2)
          ublas::axpy_prod(*A.triang(), *B.dense(), *B.dense(), init);
        else if (numA == 3)
          ublas::axpy_prod(*A.sym(), *B.dense(), *B.dense(), init);
        else if (numA == 4)
          ublas::axpy_prod(*A.sparse(), *B.dense(), *B.dense(), init);
        else //if(numB==5)
          ublas::axpy_prod(*A.banded(), *B.dense(), *B.dense(), init);
        break;
      case 2:
        //if(numA != 2)
        SiconosMatrixException::selfThrow("Matrix function axpy_prod(A,B,C): wrong type for C (according to A and B types).");
        //        ublas::axpy_prod(*A.triang(), *B.triang(),*B.triang(), init);
        break;
      case 3:
        //        if(numA != 3)
        SiconosMatrixException::selfThrow("Matrix function axpy_prod(A,B,C): wrong type for C (according to A and B types).");
        //ublas::axpy_prod(*A.sym(), *B.sym(), *B.sym(), init);
        break;
      case 4:
        //        if(numA != 4)
        SiconosMatrixException::selfThrow("Matrix function axpy_prod(A,B,C): wrong type for C (according to A and B types).");
        //ublas::axpy_prod(*A.sparse(), *B.sparse(), *B.sparse(), init);
        break;
      default:
        SiconosMatrixException::selfThrow("Matrix function axpy_prod(A,B,C): wrong type for C (according to A and B types).");
      }
    }
    else // if no alias between C and A or B.
    {
      switch (numC)
      {
      case 1:
        if (numB == 1)
        {
          if (numA == 1)
            ublas::axpy_prod(*A.dense(), *B.dense(), *C.dense(), init);
          else if (numA == 2)
            ublas::axpy_prod(*A.triang(), *B.dense(), *C.dense(), init);
          else if (numA == 3)
            ublas::axpy_prod(*A.sym(), *B.dense(), *C.dense(), init);
          else if (numA == 4)
            ublas::axpy_prod(*A.sparse(), *B.dense(), *C.dense(), init);
          else// if(numA==5)
            ublas::axpy_prod(*A.banded(), *B.dense(), *C.dense(), init);
        }
        else if (numB == 2)
        {
          if (numA == 1)
            ublas::axpy_prod(*A.dense(), *B.triang(), *C.dense(), init);
          else if (numA == 2)
            ublas::axpy_prod(*A.triang(), *B.triang(), *C.dense(), init);
          else if (numA == 3)
            ublas::axpy_prod(*A.sym(), *B.triang(), *C.dense(), init);
          else if (numA == 4)
            ublas::axpy_prod(*A.sparse(), *B.triang(), *C.dense(), init);
          else //if(numA==5)
            ublas::axpy_prod(*A.banded(), *B.triang(), *C.dense(), init);
        }
        else if (numB == 3)
        {
          if (numA == 1)
            ublas::axpy_prod(*A.dense(), *B.sym(), *C.dense(), init);
          else if (numA == 2)
            ublas::axpy_prod(*A.triang(), *B.sym(), *C.dense(), init);
          else if (numA == 3)
            ublas::axpy_prod(*A.sym(), *B.sym(), *C.dense(), init);
          else if (numA == 4)
            ublas::axpy_prod(*A.sparse(), *B.sym(), *C.dense(), init);
          else // if (numA == 5)
            ublas::axpy_prod(*A.banded(), *B.sym(), *C.dense(), init);
        }
        else if (numB == 4)
        {
          if (numA == 1)
            ublas::axpy_prod(*A.dense(), *B.sparse(), *C.dense(), init);
          else if (numA == 2)
            ublas::axpy_prod(*A.triang(), *B.sparse(), *C.dense(), init);
          else if (numA == 3)
            ublas::axpy_prod(*A.sym(), *B.sparse(), *C.dense(), init);
          else if (numA == 4)
            ublas::axpy_prod(*A.sparse(), *B.sparse(), *C.dense(), init);
          else //if(numA==5){
            ublas::axpy_prod(*A.banded(), *B.sparse(), *C.dense(), init);
        }
        else //if(numB==5)
        {
          if (numA == 1)
            ublas::axpy_prod(*A.dense(), *B.banded(), *C.dense(), init);
          else if (numA == 2)
            ublas::axpy_prod(*A.triang(), *B.banded(), *C.dense(), init);
          else if (numA == 3)
            ublas::axpy_prod(*A.sym(), *B.banded(), *C.dense(), init);
          else if (numA == 4)
            ublas::axpy_prod(*A.sparse(), *B.banded(), *C.dense(), init);
          else //if(numA==5)
            ublas::axpy_prod(*A.banded(), *B.banded(), *C.dense(), init);
        }
        break;
      case 2:
        // if(numA!= 2 || numB != 2)
        SiconosMatrixException::selfThrow("Matrix function axpy_prod(A,B,C): wrong type for C (according to A and B types).");
        //ublas::axpy_prod(*A.triang(), *B.triang(),*C.triang(), init);
        break;
      case 3:
        //        if(numA!= 3 || numB != 3)
        SiconosMatrixException::selfThrow("Matrix function axpy_prod(A,B,C): wrong type for C (according to A and B types).");
        //ublas::axpy_prod(*A.sym(), *B.sym(),*C.sym(),init);
        break;
      case 4:
        if (numA != 4 || numB != 4)
          SiconosMatrixException::selfThrow("Matrix function axpy_prod(A,B,C): wrong type for C (according to A and B types).");
        ublas::sparse_prod(*A.sparse(), *B.sparse(), *C.sparse(), init);
        break;
      default:
        SiconosMatrixException::selfThrow("Matrix function axpy_prod(A,B,C): wrong type for C (according to A and B types).");
      }
    }
  if(!C.isBlock())
    C.resetLU();
  }
}

void gemmtranspose(double a, const SiconosMatrix& A, const SiconosMatrix& B, double b, SiconosMatrix& C)
{
  if (A.isBlock() || B.isBlock() || C.isBlock())
    SiconosMatrixException::selfThrow("gemm(...) not yet implemented for block matrices.");
  unsigned int numA = A.num();
  unsigned int numB = B.num();
  unsigned int numC = C.num();
  if (numA != 1 || numB != 1 || numC != 1)
    SiconosMatrixException::selfThrow("gemm(...) failed: reserved to dense matrices.");

  assert(!(B.isPLUFactorized()) && "B is PLUFactorized in prod !!");
  assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!");

  siconosBindings::blas::gemm(a, siconosBindings::trans(*A.dense()), siconosBindings::trans(*B.dense()), b, *C.dense());

  C.resetLU();
}

void gemm(double a, const SiconosMatrix& A, const SiconosMatrix& B, double b, SiconosMatrix& C)
{
  unsigned int numA = A.num();
  unsigned int numB = B.num();
  unsigned int numC = C.num();
  assert(!(B.isPLUFactorized()) && "B is PLUFactorized in prod !!");
  assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!");
  C.resetLU();

  // At the time, only dense output allowed
  DenseMat * tmpC = NULL;
  if (numA == 0 || numB == 0 || numC == 0)
    SiconosMatrixException::selfThrow("gemm(...) not yet implemented for block matrices.");

  if (numA == 1 && numB == 1 && numC == 1)
    siconosBindings::blas::gemm(a, *A.dense(), *B.dense(), b, *C.dense());
  else if (numA == 1 && numB == 1 && numC != 1)
  {
    // Copy C into tmpC ...
    tmpC = new DenseMat(*C.dense());
    siconosBindings::blas::gemm(a, *A.dense(), *B.dense(), b, *tmpC);
    std::cout << *tmpC << std::endl;
    noalias(*C.dense()) = *tmpC;
    delete tmpC;
  }
  else
    SiconosMatrixException::selfThrow("gemm(...) not yet implemented for these kinds of matrices.");
  C.resetLU();
}

void scal(double a, const SiconosMatrix& A, SiconosMatrix& B, bool init)
{
  // To compute B = a * A (init = true) or B += a*A (init = false).
  assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!" );
  if(!B.isBlock())
    B.resetLU();

  if (&A == &B)
  {
    if (init) B *= a;
    else B *= (1.0 + a);
  }
  else
  {
    unsigned int numA = A.num();
    unsigned int numB = B.num();

    if (numB == 6 || numB == 7) // B = 0 or identity.
      SiconosMatrixException::selfThrow("scal(a,A,B) : forbidden for B being a zero or identity matrix.");

    if (numA == 6)
    {
      if (init) B.zero(); // else nothing
    }
    else if (numA == 7)
    {
      if (init)
      {
        B.eye();
        B *= a;
      }
      else
      {
        // Assuming B is square ...
        for (unsigned int i = 0; i < B.size(0); ++i)
          B(i, i) += a;
      }
    }
    else
    {
      if (numA == numB) // if A and B are of the same type ...
      {
        switch (numA)
        {

        case 0: // A and B are block
          if (isComparableTo(A, B))
          {
            const BlockMatrix& Aref = static_cast<const BlockMatrix&>(A);
            BlockMatrix& Bref = static_cast<BlockMatrix&>(B);
            BlocksIterator1 itB1;
            BlocksIterator2 itB2;
            ConstBlocksIterator1 itA1 = Aref._mat->begin1();
            ConstBlocksIterator2 itA2;
            for (itB1 = Bref._mat->begin1(); itB1 != Bref._mat->end1(); ++itB1)
            {
              itA2 = itA1.begin();
              for (itB2 = itB1.begin(); itB2 != itB1.end(); ++itB2)
              {
                scal(a, **itA2++, **itB2, init);
              }
              itA1++;
            }
          }
          else // if A and B are not "block-consistent"
          {
            if (init)
            {
              for (unsigned int i = 0; i < A.size(0); ++i)
                for (unsigned int j = 0; j < A.size(1); ++j)
                  B(i, j) = a * A(i, j);
            }
            else
            {
              for (unsigned int i = 0; i < A.size(0); ++i)
                for (unsigned int j = 0; j < A.size(1); ++j)
                  B(i, j) += a * A(i, j);
            }
          }
          break;

        case 1: // if both are dense
          if (init)
            noalias(*B.dense()) = a ** A.dense();
          else
            noalias(*B.dense()) += a ** A.dense();
          break;
        case 2:
          if (init)
            noalias(*B.triang()) = a ** A.triang();
          else
            noalias(*B.triang()) += a ** A.triang();
          break;
        case 3:
          if (init)
            noalias(*B.sym()) = a ** A.sym();
          else
            noalias(*B.sym()) += a ** A.sym();
          break;
        case 4:
          if (init)
            noalias(*B.sparse()) = a ** A.sparse();
          else
            noalias(*B.sparse()) += a ** A.sparse();
          break;
        case 5:
          if (init)
            noalias(*B.banded()) = a ** A.banded();
          else
            noalias(*B.banded()) += a ** A.banded();
          break;
        }
      }
      else // if A and B are of different types.
      {
        if (numA == 0 || numB == 0) // if A or B is block
        {
          if (init)
          {
            B = A;
            B *= a;
          }
          else
          {
            SimpleMatrix tmp(A);
            tmp *= a;
            B += tmp; // bof bof ...
          }
        }
        else
        {
          if (numB != 1)
            SiconosMatrixException::selfThrow("scal(a,A,B) failed. A and B types do not fit together.");

          if (init)
          {
            switch (numB)
            {
            case 1:
              noalias(*B.dense()) = a ** A.dense();
              break;
            case 2:
              noalias(*B.dense()) = a ** A.triang();
              break;
            case 3:
              noalias(*B.dense()) = a ** A.sym();
              break;
            case 4:
              noalias(*B.dense()) = a ** A.sparse();
              break;
            case 5:
              noalias(*B.dense()) = a ** A.banded();
              break;
            }
          }
          else

          {
            switch (numB)
            {
            case 1:
              noalias(*B.dense()) += a ** A.dense();
              break;
            case 2:
              noalias(*B.dense()) += a ** A.triang();
              break;
            case 3:
              noalias(*B.dense()) += a ** A.sym();
              break;
            case 4:
              noalias(*B.dense()) += a ** A.sparse();
              break;
            case 5:
              noalias(*B.dense()) += a ** A.banded();
              break;
            }
          }
        }
      }
    }
  }
}


#ifndef PYTHONIC_INCLUDE_NUMPY_SIN_HPP
#define PYTHONIC_INCLUDE_NUMPY_SIN_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"
#include <boost/simd/function/sin.hpp>

namespace pythonic
{

  namespace numpy
  {
#define NUMPY_NARY_FUNC_NAME sin
#define NUMPY_NARY_FUNC_SYM boost::simd::sin
#include "pythonic/include/types/numpy_nary_expr.hpp"
  }
}

#endif


#include <boost/format.hpp>
#include <iostream>
#include <cmath>

struct show_resolution_save
{
  bool first;
  float ass, bss, css;
  show_resolution_save() : first(true) {}
};

struct common
{
  float a, b, c;
  show_resolution_save show_resolution_sve;
  std::ostream & out_stream;
  common(std::ostream& out_stream_) : out_stream(out_stream_) {}
};

void
show_resolution(common& cmn, int h, int k, int l)
{
  show_resolution_save& sve = cmn.show_resolution_sve;
  if (sve.first) {
    sve.first = false;
    if (cmn.a <= 0 || cmn.b <= 0 || cmn.c <= 0) {
      throw std::runtime_error(
        "invalid unit cell constants.");
    }
    sve.ass = 1/(cmn.a*cmn.a);
    sve.bss = 1/(cmn.b*cmn.b);
    sve.css = 1/(cmn.c*cmn.c);
  }
  float dss = h*h*sve.ass + k*k*sve.bss + l*l*sve.css;
  std::ostream& cout = cmn.out_stream;
  if (dss == 0) {
    cout << boost::format(" %3d %3d %3d     infinity\n")
      % h % k % l;
  }
  else {
    cout << boost::format(" %3d %3d %3d %12.6f\n")
      % h % k % l % std::sqrt(1/dss);
  }
}

void
conv_recipe(common& cmn)
{
  cmn.a = 11.0;
  cmn.b = 12.0;
  cmn.c = 13.0;
  show_resolution(cmn, 0, 0, 0);
  show_resolution(cmn, 1, 2, 3);
}

int
main()
{
  common cmn(std::cout);
  conv_recipe(cmn);
  return 0;
}


#ifndef INCLUDED_chemical_HBondRay_HH
#define INCLUDED_chemical_HBondRay_HH

#include <Eigen/Dense>

namespace scheme { namespace chemical {

const float ORBLEN = 0.61; // don't change this number!!!!!!!!!!

struct HBondRay {
	::Eigen::Vector3f horb_cen, direction;
	int32_t id=1, group=-1;
	bool operator==(HBondRay const & other) const {
		float d1 = (horb_cen-other.horb_cen).norm();
		float d2 = (direction-other.direction).norm();
		return d1 < 0.0001 && d2 < 0.0001;
	}

	template< class Xform >
	void apply_xform( Xform const & xform ) {
		Eigen::Vector3f dirpos = horb_cen + direction;
		horb_cen = xform * horb_cen;
		direction = xform * dirpos - horb_cen;
	}
};

}}

#endif


#ifndef STAN_MATH_PRIM_MAT_PROB_BERNOULLI_LOGIT_GLM_LPMF_HPP
#define STAN_MATH_PRIM_MAT_PROB_BERNOULLI_LOGIT_GLM_LPMF_HPP

#include <stan/math/prim/scal/meta/is_constant_struct.hpp>
#include <stan/math/prim/scal/meta/partials_return_type.hpp>
#include <stan/math/prim/scal/meta/broadcast_array.hpp>
#include <stan/math/prim/scal/meta/operands_and_partials.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_consistent_size.hpp>
#include <stan/math/prim/scal/err/check_bounded.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/mat/fun/value_of.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <stan/math/prim/mat/meta/is_vector.hpp>
#include <stan/math/prim/scal/meta/scalar_seq_view.hpp>
#include <stan/math/prim/scal/fun/size_zero.hpp>
#include <boost/random/variate_generator.hpp>
#include <cmath>

namespace stan {
namespace math {

/**
 * Returns the log PMF of the Generalized Linear Model (GLM)
 * with Bernoulli distribution and logit link function.
 * The idea is that bernoulli_logit_glm_lpmf(y, x, alpha, beta) should
 * compute a more efficient version of bernoulli_logit_lpmf(y, alpha + x * beta)
 * by using analytically simplified gradients.
 * If containers are supplied, returns the log sum of the probabilities.
 * @tparam T_y type of binary vector of dependent variables (labels);
 * this can also be a single binary value;
 * @tparam T_x type of the matrix of independent variables (features); this
 * should be an Eigen::Matrix type whose number of rows should match the
 * length of y and whose number of columns should match the length of beta
 * @tparam T_alpha type of the intercept(s);
 * this can be a vector (of the same length as y) of intercepts or a single
 * value (for models with constant intercept);
 * @tparam T_beta type of the weight vector;
 * this can also be a single value;
 * @param y binary vector parameter
 * @param x design matrix
 * @param alpha intercept (in log odds)
 * @param beta weight vector
 * @return log probability or log sum of probabilities
 * @throw std::domain_error if x, beta or alpha is infinite.
 * @throw std::domain_error if y is not binary.
 * @throw std::invalid_argument if container sizes mismatch.
 */

template <bool propto, typename T_y, typename T_x, typename T_alpha,
          typename T_beta>
typename return_type<T_x, T_alpha, T_beta>::type bernoulli_logit_glm_lpmf(
    const T_y &y, const T_x &x, const T_alpha &alpha, const T_beta &beta) {
  static const char *function = "bernoulli_logit_glm_lpmf";
  typedef typename stan::partials_return_type<T_y, T_x, T_alpha, T_beta>::type
      T_partials_return;

  using Eigen::Dynamic;
  using Eigen::Matrix;
  using std::exp;

  if (size_zero(y, x, beta))
    return 0.0;

  T_partials_return logp(0.0);

  const size_t N = x.col(0).size();
  const size_t M = x.row(0).size();

  check_bounded(function, "Vector of dependent variables", y, 0, 1);
  check_finite(function, "Matrix of independent variables", x);
  check_finite(function, "Weight vector", beta);
  check_finite(function, "Intercept", alpha);
  check_consistent_size(function, "Vector of dependent variables", y, N);
  check_consistent_size(function, "Weight vector", beta, M);
  if (is_vector<T_alpha>::value)
    check_consistent_sizes(function, "Vector of intercepts", alpha,
                           "Vector of dependent variables", y);

  if (!include_summand<propto, T_x, T_alpha, T_beta>::value)
    return 0.0;

  Matrix<T_partials_return, Dynamic, 1> signs(N, 1);
  {
    scalar_seq_view<T_y> y_vec(y);
    for (size_t n = 0; n < N; ++n) {
      signs[n] = 2 * y_vec[n] - 1;
    }
  }
  Matrix<T_partials_return, Dynamic, 1> beta_dbl(M, 1);
  {
    scalar_seq_view<T_beta> beta_vec(beta);
    for (size_t m = 0; m < M; ++m) {
      beta_dbl[m] = value_of(beta_vec[m]);
    }
  }
  Eigen::Array<T_partials_return, Dynamic, 1> ytheta
      = signs.array() * (value_of(x) * beta_dbl).array();
  scalar_seq_view<T_alpha> alpha_vec(alpha);

  // Compute the log-density and handle extreme values gracefully
  // using Taylor approximations.
  // And compute the derivatives wrt theta.
  static const double cutoff = 20.0;
  Matrix<T_partials_return, Dynamic, 1> theta_derivative(N, 1);
  T_partials_return theta_derivative_sum = 0;
  T_partials_return exp_m_ythetan;
  for (size_t n = 0; n < N; ++n) {
    ytheta[n] += signs[n] * value_of(alpha_vec[n]);
    exp_m_ythetan = exp(-ytheta[n]);
    if (ytheta[n] > cutoff) {
      logp -= exp_m_ythetan;
      theta_derivative[n] = -exp_m_ythetan;
    } else if (ytheta[n] < -cutoff) {
      logp += ytheta[n];
      theta_derivative[n] = signs[n];
    } else {
      logp -= log1p(exp_m_ythetan);
      theta_derivative[n] = signs[n] * exp_m_ythetan / (exp_m_ythetan + 1);
    }
    if (!is_vector<T_alpha>::value)
      theta_derivative_sum += theta_derivative[n];
  }

  // Compute the necessary derivatives.
  operands_and_partials<T_x, T_alpha, T_beta> ops_partials(x, alpha, beta);
  if (!is_constant_struct<T_beta>::value) {
    ops_partials.edge3_.partials_ = value_of(x).transpose() * theta_derivative;
  }
  if (!is_constant_struct<T_x>::value) {
    ops_partials.edge1_.partials_ = theta_derivative * beta_dbl.transpose();
  }
  if (!is_constant_struct<T_alpha>::value) {
    if (is_vector<T_alpha>::value)
      ops_partials.edge2_.partials_ = theta_derivative;
    else
      ops_partials.edge2_.partials_[0] = theta_derivative_sum;
  }

  return ops_partials.build(logp);
}

template <typename T_y, typename T_x, typename T_alpha, typename T_beta>
inline typename return_type<T_x, T_beta, T_alpha>::type
bernoulli_logit_glm_lpmf(const T_y &y, const T_x &x, const T_alpha &alpha,
                         const T_beta &beta) {
  return bernoulli_logit_glm_lpmf<false>(y, x, alpha, beta);
}
}  // namespace math
}  // namespace stan
#endif


/*
 * Copyright 2016 Erik Crevel
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *    http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "ArithmeticEncoder.hpp"

#include <cassert>
#include <algorithm>
#include <boost/range/adaptor/reversed.hpp>

ArithmeticEncoder::Range ArithmeticEncoder::scaleDown(Range _rMax, Range _r)
{
	return (static_cast<DoubleRange>(_r) * _rMax)>>std::numeric_limits<Range>::digits;
}

ArithmeticEncoder::Range ArithmeticEncoder::scaleUp(Range _rMax, Range _r)
{
	return ((static_cast<DoubleRange>(_r)<<std::numeric_limits<Range>::digits) + RANGE_MAX - 1)/_rMax;
}

void ArithmeticEncoder::scaleRange()
{
	assert(this->range.second > 0);

	// Scale up range if it has become too small
	while(this->range.second <= RANGE_MAX>>8)
	{
		this->writeBytes.push_back(this->range.first>>LAST_BYTE_SHIFT);

		this->range.first &= ~(static_cast<Range>(255)<<LAST_BYTE_SHIFT);

		this->range.first <<= 8;
		this->range.second <<= 8;
	}
}

void ArithmeticEncoder::writeFixedBytes()
{
	if(!this->writeBytes.empty())
	{
		std::deque<unsigned char>::iterator writeIterEnd;
		if(this->writeBytes.back() <= 253)
			writeIterEnd = this->writeBytes.end()-1;
		else
		{
			auto rIterWriteEnd = std::find_if(this->writeBytes.rbegin()+1, this->writeBytes.rend(),
				[](unsigned char _c) { return _c <= 254; });
			writeIterEnd = (rIterWriteEnd != this->writeBytes.rend())
				? (rIterWriteEnd+1).base()
				: this->writeBytes.begin();
		}

		for(auto i = this->writeBytes.begin(); i != writeIterEnd; ++i)
			this->output.put(*i);
		this->writeBytes.erase(this->writeBytes.begin(), writeIterEnd);
	}
}

ArithmeticEncoder::Range ArithmeticEncoder::removeOverflow(DoubleRange _pos)
{
	if(_pos > RANGE_MAX)
	{
		// Add one, accounting for any overflow
		for(unsigned char& c : boost::adaptors::reverse(this->writeBytes))
			if(++c != 0)
				break;

		_pos -= RANGE_MAX+1;
	}

	return _pos;
}

void ArithmeticEncoder::encode(RangePair _range)
{
	assert(this->active);
	assert(_range.second - _range.first >= MIN_RANGE_SIZE);

	this->scaleRange();

	DoubleRange start = scaleDown(this->range.second, _range.first);
	Range size = scaleDown(this->range.second, _range.second) - start;

	assert(scaleUp(this->range.second, start) >= _range.first);
	assert(scaleUp(this->range.second, start+size-1) < _range.second);

	start += this->range.first;

	this->range = {this->removeOverflow(start), size};

	this->writeFixedBytes();

	++this->size_;
}

void ArithmeticEncoder::close()
{
	std::size_t writeBytesPreSize = this->writeBytes.size();

	this->scaleRange();

	// Scale up range so that the range encompassses the max range
	if(this->range.second < RANGE_MAX)
	{
		DoubleRange writePos = static_cast<DoubleRange>(this->range.first) + (1<<LAST_BYTE_SHIFT) - 1;

		this->writeBytes.push_back(this->removeOverflow(writePos)>>LAST_BYTE_SHIFT);
	}

	// Write padding bytes, these are important to make sure
	// encoder write sizes and decoder read sizes match
	this->writeBytes.insert(
			this->writeBytes.end(),
			std::numeric_limits<ArithmeticEncoder::Range>::digits/8
			- (this->writeBytes.size() - writeBytesPreSize),
			0);

	for(unsigned char c : this->writeBytes)
		this->output.put(c);
	this->writeBytes.clear();

	this->active = false;
}


//  ratio.hpp  ---------------------------------------------------------------//

//  Copyright 2008 Howard Hinnant
//  Copyright 2008 Beman Dawes
//  Copyright 2009 Vicente J. Botet Escriba

//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

/*

This code was derived by Beman Dawes from Howard Hinnant's time2_demo prototype.
Many thanks to Howard for making his code available under the Boost license.
The original code was modified to conform to Boost conventions and to section
20.4 Compile-time rational arithmetic [ratio], of the C++ committee working
paper N2798.
See http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2008/n2798.pdf.

time2_demo contained this comment:

    Much thanks to Andrei Alexandrescu,
                   Walter Brown,
                   Peter Dimov,
                   Jeff Garland,
                   Terry Golubiewski,
                   Daniel Krugler,
                   Anthony Williams.
*/

// The way overflow is managed for ratio_less is taken from llvm/libcxx/include/ratio

#ifndef BOOST_RATIO_DETAIL_RATIO_OPERATIONS_HPP
#define BOOST_RATIO_DETAIL_RATIO_OPERATIONS_HPP

#include <boost/ratio/config.hpp>
#include <boost/ratio/detail/mpl/abs.hpp>
#include <boost/ratio/detail/mpl/sign.hpp>
#include <cstdlib>
#include <climits>
#include <limits>
#include <boost/cstdint.hpp>
#include <boost/type_traits/integral_constant.hpp>
#include <boost/core/enable_if.hpp>
#include <boost/integer_traits.hpp>

//
// We simply cannot include this header on gcc without getting copious warnings of the kind:
//
// boost/integer.hpp:77:30: warning: use of C99 long long integer constant
//
// And yet there is no other reasonable implementation, so we declare this a system header
// to suppress these warnings.
//
#if defined(__GNUC__) && (__GNUC__ >= 4)
#pragma GCC system_header
#endif

namespace mars_boost {} namespace boost = mars_boost; namespace mars_boost
{

//----------------------------------------------------------------------------//
//                                 helpers                                    //
//----------------------------------------------------------------------------//

namespace ratio_detail
{

  template <mars_boost::intmax_t X, mars_boost::intmax_t Y, mars_boost::intmax_t = mpl::sign_c<mars_boost::intmax_t, Y>::value>
  class br_add;

  template <mars_boost::intmax_t X, mars_boost::intmax_t Y>
  class br_add<X, Y, 1>
  {
      static const mars_boost::intmax_t min = mars_boost::integer_traits<mars_boost::intmax_t>::const_min;
      static const mars_boost::intmax_t max = mars_boost::integer_traits<mars_boost::intmax_t>::const_max;

      BOOST_RATIO_STATIC_ASSERT(X <= max - Y , BOOST_RATIO_OVERFLOW_IN_ADD, ());
  public:
      static const mars_boost::intmax_t value = X + Y;
  };

  template <mars_boost::intmax_t X, mars_boost::intmax_t Y>
  class br_add<X, Y, 0>
  {
  public:
      static const mars_boost::intmax_t value = X;
  };

  template <mars_boost::intmax_t X, mars_boost::intmax_t Y>
  class br_add<X, Y, -1>
  {
      static const mars_boost::intmax_t min = mars_boost::integer_traits<mars_boost::intmax_t>::const_min;
      static const mars_boost::intmax_t max = mars_boost::integer_traits<mars_boost::intmax_t>::const_max;

      BOOST_RATIO_STATIC_ASSERT(min - Y <= X, BOOST_RATIO_OVERFLOW_IN_ADD, ());
  public:
      static const mars_boost::intmax_t value = X + Y;
  };

  template <mars_boost::intmax_t X, mars_boost::intmax_t Y, mars_boost::intmax_t = mpl::sign_c<mars_boost::intmax_t, Y>::value>
  class br_sub;

  template <mars_boost::intmax_t X, mars_boost::intmax_t Y>
  class br_sub<X, Y, 1>
  {
      static const mars_boost::intmax_t min = mars_boost::integer_traits<mars_boost::intmax_t>::const_min;
      static const mars_boost::intmax_t max = mars_boost::integer_traits<mars_boost::intmax_t>::const_max;

      BOOST_RATIO_STATIC_ASSERT(min + Y <= X, BOOST_RATIO_OVERFLOW_IN_SUB, ());
  public:
      static const mars_boost::intmax_t value = X - Y;
  };

  template <mars_boost::intmax_t X, mars_boost::intmax_t Y>
  class br_sub<X, Y, 0>
  {
  public:
      static const mars_boost::intmax_t value = X;
  };

  template <mars_boost::intmax_t X, mars_boost::intmax_t Y>
  class br_sub<X, Y, -1>
  {
      static const mars_boost::intmax_t min = mars_boost::integer_traits<mars_boost::intmax_t>::const_min;
      static const mars_boost::intmax_t max = mars_boost::integer_traits<mars_boost::intmax_t>::const_max;

      BOOST_RATIO_STATIC_ASSERT(X <= max + Y, BOOST_RATIO_OVERFLOW_IN_SUB, ());
  public:
      static const mars_boost::intmax_t value = X - Y;
  };

  template <mars_boost::intmax_t X, mars_boost::intmax_t Y>
  class br_mul
  {
      static const mars_boost::intmax_t nan =
          mars_boost::intmax_t(BOOST_RATIO_UINTMAX_C(1) << (sizeof(mars_boost::intmax_t) * CHAR_BIT - 1));
      static const mars_boost::intmax_t min = mars_boost::integer_traits<mars_boost::intmax_t>::const_min;
      static const mars_boost::intmax_t max = mars_boost::integer_traits<mars_boost::intmax_t>::const_max;

      static const mars_boost::intmax_t a_x = mpl::abs_c<mars_boost::intmax_t, X>::value;
      static const mars_boost::intmax_t a_y = mpl::abs_c<mars_boost::intmax_t, Y>::value;

      BOOST_RATIO_STATIC_ASSERT(X != nan, BOOST_RATIO_OVERFLOW_IN_MUL, ());
      BOOST_RATIO_STATIC_ASSERT(Y != nan, BOOST_RATIO_OVERFLOW_IN_MUL, ());
      BOOST_RATIO_STATIC_ASSERT(a_x <= max / a_y, BOOST_RATIO_OVERFLOW_IN_MUL, ());
  public:
      static const mars_boost::intmax_t value = X * Y;
  };

  template <mars_boost::intmax_t Y>
  class br_mul<0, Y>
  {
  public:
      static const mars_boost::intmax_t value = 0;
  };

  template <mars_boost::intmax_t X>
  class br_mul<X, 0>
  {
  public:
      static const mars_boost::intmax_t value = 0;
  };

  template <>
  class br_mul<0, 0>
  {
  public:
      static const mars_boost::intmax_t value = 0;
  };

  // Not actually used but left here in case needed in future maintenance
  template <mars_boost::intmax_t X, mars_boost::intmax_t Y>
  class br_div
  {
      static const mars_boost::intmax_t nan = mars_boost::intmax_t(BOOST_RATIO_UINTMAX_C(1) << (sizeof(mars_boost::intmax_t) * CHAR_BIT - 1));
      static const mars_boost::intmax_t min = mars_boost::integer_traits<mars_boost::intmax_t>::const_min;
      static const mars_boost::intmax_t max = mars_boost::integer_traits<mars_boost::intmax_t>::const_max;

      BOOST_RATIO_STATIC_ASSERT(X != nan, BOOST_RATIO_OVERFLOW_IN_DIV, ());
      BOOST_RATIO_STATIC_ASSERT(Y != nan, BOOST_RATIO_OVERFLOW_IN_DIV, ());
      BOOST_RATIO_STATIC_ASSERT(Y != 0, BOOST_RATIO_DIVIDE_BY_0, ());
  public:
      static const mars_boost::intmax_t value = X / Y;
  };

  // ratio arithmetic
  template <class R1, class R2> struct ratio_add;
  template <class R1, class R2> struct ratio_subtract;
  template <class R1, class R2> struct ratio_multiply;
  template <class R1, class R2> struct ratio_divide;

  template <class R1, class R2>
  struct ratio_add
  {
      //The nested typedef type shall be a synonym for ratio<T1, T2>::type where T1 has the value R1::num *
      //R2::den + R2::num * R1::den and T2 has the value R1::den * R2::den.
      // As the preceding doesn't works because of overflow on mars_boost::intmax_t we need something more elaborated.
  private:
      static const mars_boost::intmax_t gcd_n1_n2 = mpl::gcd_c<mars_boost::intmax_t, R1::num, R2::num>::value;
      static const mars_boost::intmax_t gcd_d1_d2 = mpl::gcd_c<mars_boost::intmax_t, R1::den, R2::den>::value;
  public:
      // No need to normalize as ratio_multiply is already normalized
      typedef typename ratio_multiply
         <
             ratio<gcd_n1_n2, R1::den / gcd_d1_d2>,
             ratio
             <
                 mars_boost::ratio_detail::br_add
                 <
                     mars_boost::ratio_detail::br_mul<R1::num / gcd_n1_n2, R2::den / gcd_d1_d2>::value,
                     mars_boost::ratio_detail::br_mul<R2::num / gcd_n1_n2, R1::den / gcd_d1_d2>::value
                 >::value,
                 R2::den
             >
         >::type type;
  };
  template <class R, mars_boost::intmax_t D>
  struct ratio_add<R, ratio<0,D> >
  {
    typedef R type;
  };

  template <class R1, class R2>
  struct ratio_subtract
  {
      //The nested typedef type shall be a synonym for ratio<T1, T2>::type where T1 has the value
      // R1::num *R2::den - R2::num * R1::den and T2 has the value R1::den * R2::den.
      // As the preceding doesn't works because of overflow on mars_boost::intmax_t we need something more elaborated.
  private:
      static const mars_boost::intmax_t gcd_n1_n2 = mpl::gcd_c<mars_boost::intmax_t, R1::num, R2::num>::value;
      static const mars_boost::intmax_t gcd_d1_d2 = mpl::gcd_c<mars_boost::intmax_t, R1::den, R2::den>::value;
  public:
      // No need to normalize as ratio_multiply is already normalized
      typedef typename ratio_multiply
         <
             ratio<gcd_n1_n2, R1::den / gcd_d1_d2>,
             ratio
             <
                 mars_boost::ratio_detail::br_sub
                 <
                     mars_boost::ratio_detail::br_mul<R1::num / gcd_n1_n2, R2::den / gcd_d1_d2>::value,
                     mars_boost::ratio_detail::br_mul<R2::num / gcd_n1_n2, R1::den / gcd_d1_d2>::value
                 >::value,
                 R2::den
             >
         >::type type;
  };

  template <class R, mars_boost::intmax_t D>
  struct ratio_subtract<R, ratio<0,D> >
  {
    typedef R type;
  };

  template <class R1, class R2>
  struct ratio_multiply
  {
      // The nested typedef type  shall be a synonym for ratio<R1::num * R2::den - R2::num * R1::den, R1::den * R2::den>::type.
      // As the preceding doesn't works because of overflow on mars_boost::intmax_t we need something more elaborated.
  private:
     static const mars_boost::intmax_t gcd_n1_d2 = mpl::gcd_c<mars_boost::intmax_t, R1::num, R2::den>::value;
     static const mars_boost::intmax_t gcd_d1_n2 = mpl::gcd_c<mars_boost::intmax_t, R1::den, R2::num>::value;
  public:
      typedef typename ratio
         <
             mars_boost::ratio_detail::br_mul<R1::num / gcd_n1_d2, R2::num / gcd_d1_n2>::value,
             mars_boost::ratio_detail::br_mul<R2::den / gcd_n1_d2, R1::den / gcd_d1_n2>::value
         >::type type;
  };

  template <class R1, class R2>
  struct ratio_divide
  {
      // The nested typedef type  shall be a synonym for ratio<R1::num * R2::den, R2::num * R1::den>::type.
      // As the preceding doesn't works because of overflow on mars_boost::intmax_t we need something more elaborated.
  private:
      static const mars_boost::intmax_t gcd_n1_n2 = mpl::gcd_c<mars_boost::intmax_t, R1::num, R2::num>::value;
      static const mars_boost::intmax_t gcd_d1_d2 = mpl::gcd_c<mars_boost::intmax_t, R1::den, R2::den>::value;
  public:
      typedef typename ratio
         <
             mars_boost::ratio_detail::br_mul<R1::num / gcd_n1_n2, R2::den / gcd_d1_d2>::value,
             mars_boost::ratio_detail::br_mul<R2::num / gcd_n1_n2, R1::den / gcd_d1_d2>::value
         >::type type;
  };
  template <class R1, class R2>
  struct is_evenly_divisible_by
  {
  private:
      static const mars_boost::intmax_t gcd_n1_n2 = mpl::gcd_c<mars_boost::intmax_t, R1::num, R2::num>::value;
      static const mars_boost::intmax_t gcd_d1_d2 = mpl::gcd_c<mars_boost::intmax_t, R1::den, R2::den>::value;
  public:
      typedef integral_constant<bool,
             ((R2::num / gcd_n1_n2 ==1) && (R1::den / gcd_d1_d2)==1)
      > type;
  };

  template <class T>
  struct is_ratio : public mars_boost::false_type
  {};
  template <mars_boost::intmax_t N, mars_boost::intmax_t D>
  struct is_ratio<ratio<N, D> > : public mars_boost::true_type
  {};

  template <class R1, class R2,
            mars_boost::intmax_t Q1 = R1::num / R1::den, mars_boost::intmax_t M1 = R1::num % R1::den,
            mars_boost::intmax_t Q2 = R2::num / R2::den, mars_boost::intmax_t M2 = R2::num % R2::den>
  struct ratio_less1
  {
    static const bool value = Q1 < Q2;
  };

  template <class R1, class R2, mars_boost::intmax_t Q>
  struct ratio_less1<R1, R2, Q, 0, Q, 0>
  {
    static const bool value = false;
  };

  template <class R1, class R2, mars_boost::intmax_t Q, mars_boost::intmax_t M2>
  struct ratio_less1<R1, R2, Q, 0, Q, M2>
  {
    static const bool value = true;
  };

  template <class R1, class R2, mars_boost::intmax_t Q, mars_boost::intmax_t M1>
  struct ratio_less1<R1, R2, Q, M1, Q, 0>
  {
    static const bool value = false;
  };

  template <class R1, class R2, mars_boost::intmax_t Q, mars_boost::intmax_t M1, mars_boost::intmax_t M2>
  struct ratio_less1<R1, R2, Q, M1, Q, M2>
  {
    static const bool value = ratio_less1<ratio<R2::den, M2>, ratio<R1::den, M1>
                                            >::value;
  };

  template <
      class R1,
      class R2,
      mars_boost::intmax_t S1 = mpl::sign_c<mars_boost::intmax_t, R1::num>::value,
    mars_boost::intmax_t S2 = mpl::sign_c<mars_boost::intmax_t, R2::num>::value
>
  struct ratio_less
  {
      static const bool value = S1 < S2;
  };

  template <class R1, class R2>
  struct ratio_less<R1, R2, 1LL, 1LL>
  {
      static const bool value = ratio_less1<R1, R2>::value;
  };

  template <class R1, class R2>
  struct ratio_less<R1, R2, -1LL, -1LL>
  {
      static const bool value = ratio_less1<ratio<-R2::num, R2::den>,
                                            ratio<-R1::num, R1::den> >::value;
  };

}  // namespace ratio_detail

}  // namespace mars_boost

#endif  // BOOST_RATIO_HPP