#include <mex.h>
#include <vector>
#include <string>
#include <Eigen/Dense>
#include <iostream>

#include <lcm/lcm-cpp.hpp>
#include "lcmtypes/bot_core/atlas_command_t.hpp"

using namespace Eigen;
using namespace std;

class AtlasCommand {

private:

  static lcm::LCM lcm;

  int m_num_joints;

  vector<int> drake_to_atlas_joint_map;
  bot_core::atlas_command_t msg;

public:
  AtlasCommand(const vector<string>& joint_names, const int atlas_version_number,
    const VectorXd& k_q_p,
    const VectorXd& k_q_i, const VectorXd& k_qd_p, const VectorXd& k_f_p,
    const VectorXd& ff_qd, const VectorXd& ff_qd_d, const VectorXd& ff_f_d,
    const VectorXd& ff_const) {

    if (atlas_version_number == 3 || atlas_version_number == 4) {
      m_num_joints = 28;
    }
    else if (atlas_version_number == 5){
      m_num_joints = 30;
    }
    else {
      mexErrMsgTxt("Unknown atlas_version_number");
    }

    // fixed ordering assumed by drcsim interface AND atlas api
    // see: AtlasControlTypes.h
    vector<string> atlas_joint_names(m_num_joints);
    switch (atlas_version_number) {
      case 3:
        atlas_joint_names[0] = "back_bkz";
        atlas_joint_names[1] = "back_bky";
        atlas_joint_names[2] = "back_bkx";
        atlas_joint_names[3] = "neck_ay";
        atlas_joint_names[4] = "l_leg_hpz";
        atlas_joint_names[5] = "l_leg_hpx";
        atlas_joint_names[6] = "l_leg_hpy";
        atlas_joint_names[7] = "l_leg_kny";
        atlas_joint_names[8] = "l_leg_aky";
        atlas_joint_names[9] = "l_leg_akx";
        atlas_joint_names[10] = "r_leg_hpz";
        atlas_joint_names[11] = "r_leg_hpx";
        atlas_joint_names[12] = "r_leg_hpy";
        atlas_joint_names[13] = "r_leg_kny";
        atlas_joint_names[14] = "r_leg_aky";
        atlas_joint_names[15] = "r_leg_akx";
        atlas_joint_names[16] = "l_arm_usy";
        atlas_joint_names[17] = "l_arm_shx";
        atlas_joint_names[18] = "l_arm_ely";
        atlas_joint_names[19] = "l_arm_elx";
        atlas_joint_names[20] = "l_arm_uwy";
        atlas_joint_names[21] = "l_arm_mwx";
        atlas_joint_names[22] = "r_arm_usy";
        atlas_joint_names[23] = "r_arm_shx";
        atlas_joint_names[24] = "r_arm_ely";
        atlas_joint_names[25] = "r_arm_elx";
        atlas_joint_names[26] = "r_arm_uwy";
        atlas_joint_names[27] = "r_arm_mwx";
        break;
      case 4:
        atlas_joint_names[0] = "back_bkz";
        atlas_joint_names[1] = "back_bky";
        atlas_joint_names[2] = "back_bkx";
        atlas_joint_names[3] = "neck_ay";
        atlas_joint_names[4] = "l_leg_hpz";
        atlas_joint_names[5] = "l_leg_hpx";
        atlas_joint_names[6] = "l_leg_hpy";
        atlas_joint_names[7] = "l_leg_kny";
        atlas_joint_names[8] = "l_leg_aky";
        atlas_joint_names[9] = "l_leg_akx";
        atlas_joint_names[10] = "r_leg_hpz";
        atlas_joint_names[11] = "r_leg_hpx";
        atlas_joint_names[12] = "r_leg_hpy";
        atlas_joint_names[13] = "r_leg_kny";
        atlas_joint_names[14] = "r_leg_aky";
        atlas_joint_names[15] = "r_leg_akx";
        atlas_joint_names[16] = "l_arm_shz";
        atlas_joint_names[17] = "l_arm_shx";
        atlas_joint_names[18] = "l_arm_ely";
        atlas_joint_names[19] = "l_arm_elx";
        atlas_joint_names[20] = "l_arm_uwy";
        atlas_joint_names[21] = "l_arm_mwx";
        atlas_joint_names[22] = "r_arm_shz";
        atlas_joint_names[23] = "r_arm_shx";
        atlas_joint_names[24] = "r_arm_ely";
        atlas_joint_names[25] = "r_arm_elx";
        atlas_joint_names[26] = "r_arm_uwy";
        atlas_joint_names[27] = "r_arm_mwx";
        break;
      case 5:
        atlas_joint_names[0] = "back_bkz";
        atlas_joint_names[1] = "back_bky";
        atlas_joint_names[2] = "back_bkx";
        atlas_joint_names[3] = "neck_ay";
        atlas_joint_names[4] = "l_leg_hpz";
        atlas_joint_names[5] = "l_leg_hpx";
        atlas_joint_names[6] = "l_leg_hpy";
        atlas_joint_names[7] = "l_leg_kny";
        atlas_joint_names[8] = "l_leg_aky";
        atlas_joint_names[9] = "l_leg_akx";
        atlas_joint_names[10] = "r_leg_hpz";
        atlas_joint_names[11] = "r_leg_hpx";
        atlas_joint_names[12] = "r_leg_hpy";
        atlas_joint_names[13] = "r_leg_kny";
        atlas_joint_names[14] = "r_leg_aky";
        atlas_joint_names[15] = "r_leg_akx";
        atlas_joint_names[16] = "l_arm_shz";
        atlas_joint_names[17] = "l_arm_shx";
        atlas_joint_names[18] = "l_arm_ely";
        atlas_joint_names[19] = "l_arm_elx";
        atlas_joint_names[20] = "l_arm_uwy";
        atlas_joint_names[21] = "l_arm_mwx";
        atlas_joint_names[22] = "l_arm_lwy";
        atlas_joint_names[23] = "r_arm_shz";
        atlas_joint_names[24] = "r_arm_shx";
        atlas_joint_names[25] = "r_arm_ely";
        atlas_joint_names[26] = "r_arm_elx";
        atlas_joint_names[27] = "r_arm_uwy";
        atlas_joint_names[28] = "r_arm_mwx";
        atlas_joint_names[29] = "r_arm_lwy";
        break;
      default:
        mexErrMsgTxt("Unknown atlas_version_number");
    }

    if (joint_names.size() != m_num_joints)
      mexErrMsgTxt("Length of joint_names must be equal to m_num_joints");
    if (k_q_p.size() != m_num_joints)
      mexErrMsgTxt("Length of k_q_p must be equal to m_num_joints");
    if (k_q_i.size() != m_num_joints)
      mexErrMsgTxt("Length of k_q_i must be equal to m_num_joints");
    if (k_qd_p.size() != m_num_joints)
      mexErrMsgTxt("Length of k_qd_p must be equal to m_num_joints");
    if (k_f_p.size() != m_num_joints)
      mexErrMsgTxt("Length of k_f_p must be equal to m_num_joints");
    if (ff_qd.size() != m_num_joints)
      mexErrMsgTxt("Length of ff_qd must be equal to m_num_joints");
    if (ff_qd_d.size() != m_num_joints)
      mexErrMsgTxt("Length of ff_qd_d must be equal to m_num_joints");
    if (ff_f_d.size() != m_num_joints)
      mexErrMsgTxt("Length of ff_f_d must be equal to m_num_joints");
    if (ff_const.size() != m_num_joints)
      mexErrMsgTxt("Length of ff_const must be equal to m_num_joints");

    drake_to_atlas_joint_map.reserve(m_num_joints);

    msg.num_joints = m_num_joints;
    msg.joint_names.resize(msg.num_joints);

    msg.position.resize(msg.num_joints);
    msg.velocity.resize(msg.num_joints);
    msg.effort.resize(msg.num_joints);

    msg.k_q_p.resize(msg.num_joints);
    msg.k_q_i.resize(msg.num_joints);
    msg.k_qd_p.resize(msg.num_joints);
    msg.k_f_p.resize(msg.num_joints);
    msg.ff_qd.resize(msg.num_joints);
    msg.ff_qd_d.resize(msg.num_joints);
    msg.ff_f_d.resize(msg.num_joints);
    msg.ff_const.resize(msg.num_joints);

    msg.k_effort.resize(msg.num_joints); // only used in sim
    msg.desired_controller_period_ms = 3; // set desired controller rate (ms), only used in sim

    drake_to_atlas_joint_map.reserve(m_num_joints);
    for (int i=0; i<m_num_joints; i++) {
      for (int j=0; j<m_num_joints; j++) {
        if (joint_names[i].compare(atlas_joint_names[j]) == 0)
          drake_to_atlas_joint_map[i]=j;
      }

      msg.position[i] = 0.0;
      msg.velocity[i] = 0.0;
      msg.effort[i] = 0.0;

      msg.joint_names[drake_to_atlas_joint_map[i]] = joint_names[i];

      msg.k_q_p[drake_to_atlas_joint_map[i]] = k_q_p[i];
      msg.k_q_i[drake_to_atlas_joint_map[i]] = k_q_i[i];;
      msg.k_qd_p[drake_to_atlas_joint_map[i]] = k_qd_p[i];;
      msg.k_f_p[drake_to_atlas_joint_map[i]] = k_f_p[i];;
      msg.ff_qd[drake_to_atlas_joint_map[i]] = ff_qd[i];;
      msg.ff_qd_d[drake_to_atlas_joint_map[i]] = ff_qd_d[i];;
      msg.ff_f_d[drake_to_atlas_joint_map[i]] = ff_f_d[i];;
      msg.ff_const[drake_to_atlas_joint_map[i]] = ff_const[i];;

      msg.k_effort[i] = (uint8_t)255; // take complete control of joints (remove BDI control), sim only
    }
  }

  void updateGains(const VectorXd& k_q_p, const VectorXd& k_q_i,const VectorXd& k_qd_p, const VectorXd& k_f_p,
      const VectorXd& ff_qd, const VectorXd& ff_qd_d, const VectorXd& ff_f_d, const VectorXd& ff_const) {

    if (k_q_p.size() != m_num_joints)
      mexErrMsgTxt("Length of k_q_p must be equal to m_num_joints");
    if (k_q_i.size() != m_num_joints)
      mexErrMsgTxt("Length of k_q_i must be equal to m_num_joints");
    if (k_qd_p.size() != m_num_joints)
      mexErrMsgTxt("Length of k_qd_p must be equal to m_num_joints");
    if (k_f_p.size() != m_num_joints)
      mexErrMsgTxt("Length of k_f_p must be equal to m_num_joints");
    if (ff_qd.size() != m_num_joints)
      mexErrMsgTxt("Length of ff_qd must be equal to m_num_joints");
    if (ff_qd_d.size() != m_num_joints)
      mexErrMsgTxt("Length of ff_qd_d must be equal to m_num_joints");
    if (ff_f_d.size() != m_num_joints)
      mexErrMsgTxt("Length of ff_f_d must be equal to m_num_joints");
    if (ff_const.size() != m_num_joints)
      mexErrMsgTxt("Length of ff_const must be equal to m_num_joints");

    for (int i=0; i<m_num_joints; i++) {
      msg.k_q_p[drake_to_atlas_joint_map[i]] = k_q_p[i];
      msg.k_q_i[drake_to_atlas_joint_map[i]] = k_q_i[i];
      msg.k_qd_p[drake_to_atlas_joint_map[i]] = k_qd_p[i];
      msg.k_f_p[drake_to_atlas_joint_map[i]] = k_f_p[i];
      msg.ff_qd[drake_to_atlas_joint_map[i]] = ff_qd[i];
      msg.ff_qd_d[drake_to_atlas_joint_map[i]] = ff_qd_d[i];
      msg.ff_f_d[drake_to_atlas_joint_map[i]] = ff_f_d[i];
      msg.ff_const[drake_to_atlas_joint_map[i]] = ff_const[i];
    }
  }

  int dim(void) {
  	return 3*m_num_joints;
  }

  void publish(const string& channel, double t, const VectorXd& x) {

    msg.utime = (long)(t*1000000);
    int j;
    for (int i=0; i<m_num_joints; i++) {
      j = drake_to_atlas_joint_map[i];
      msg.position[j] = x(i);
      msg.velocity[j] = x(m_num_joints+i);
      msg.effort[j] = x(2*m_num_joints+i);
    }

    lcm.publish(channel, &msg);
  }
};

// globals: static class members
lcm::LCM AtlasCommand::lcm;

// convert Matlab cell array of strings into a C++ vector of strings
vector<string> get_strings(const mxArray *rhs) {
  int num = mxGetNumberOfElements(rhs);
  vector<string> strings(num);
  for (int i=0; i<num; i++) {
    const mxArray *ptr = mxGetCell(rhs,i);
    int buflen = mxGetN(ptr)*sizeof(mxChar)+1;
    char* str = (char*)mxMalloc(buflen);
    mxGetString(ptr, str, buflen);
    strings[i] = string(str);
    mxFree(str);
  }
  return strings;
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  if (nrhs==10 && mxGetNumberOfElements(prhs[0]) > 1) { // init

    vector<string> joint_names = get_strings(prhs[0]);
    int atlas_version_number = (int) mxGetScalar(prhs[1]);
    Map<VectorXd> k_q_p(mxGetPr(prhs[2]), mxGetNumberOfElements(prhs[2]));
    Map<VectorXd> k_q_i(mxGetPr(prhs[3]), mxGetNumberOfElements(prhs[3]));
    Map<VectorXd> k_qd_p(mxGetPr(prhs[4]), mxGetNumberOfElements(prhs[4]));
    Map<VectorXd> k_f_p(mxGetPr(prhs[5]), mxGetNumberOfElements(prhs[5]));
    Map<VectorXd> ff_qd(mxGetPr(prhs[6]), mxGetNumberOfElements(prhs[6]));
    Map<VectorXd> ff_qd_d(mxGetPr(prhs[7]), mxGetNumberOfElements(prhs[7]));
    Map<VectorXd> ff_f_d(mxGetPr(prhs[8]), mxGetNumberOfElements(prhs[8]));
    Map<VectorXd> ff_const(mxGetPr(prhs[9]), mxGetNumberOfElements(prhs[9]));

    AtlasCommand *ac = new AtlasCommand(joint_names,atlas_version_number,k_q_p,k_q_i,k_qd_p,k_f_p,ff_qd,ff_qd_d,ff_f_d,ff_const);
    mxClassID cid;
    if (sizeof(ac)==4) cid = mxUINT32_CLASS;
    else if (sizeof(ac)==8) cid = mxUINT64_CLASS;
    else mexErrMsgIdAndTxt("Drake:AtlasCommandPublisher:PointerSize error","Are you on a 32-bit machine or 64-bit machine??");
    plhs[0] = mxCreateNumericMatrix(1,1,cid,mxREAL);
    memcpy(mxGetData(plhs[0]),&ac,sizeof(ac));

    return;
  }

  // retrieve object
  AtlasCommand *ac = NULL;
  if (nrhs==0 || !mxIsNumeric(prhs[0]) || mxGetNumberOfElements(prhs[0])!=1) {
    mexErrMsgIdAndTxt("Drake:AtlasCommandPublisher:BadInputs","the first argument should be the mex_ptr");
  }
  memcpy(&ac,mxGetData(prhs[0]),sizeof(ac));

  if (nrhs==1) { // delete()
  	if (ac) delete(ac);
  }
  else if (nrhs==4) { // publish()
    char* str = mxArrayToString(prhs[1]);
    string channel(str);
    mxFree(str);
    double t = mxGetScalar(prhs[2]);
    int n = mxGetNumberOfElements(prhs[3]);
    if (n != ac->dim()) mexErrMsgIdAndTxt("Drake:AtlasCommandPublisher:BadInputs","the dimension of x is wrong");
    Map<VectorXd> x(mxGetPr(prhs[3]), n);
    ac->publish(channel, t, x);
  }
  else if (nrhs==9  && nlhs==1) { // update gains
    Map<VectorXd> k_q_p(mxGetPr(prhs[1]), mxGetNumberOfElements(prhs[1]));
    Map<VectorXd> k_q_i(mxGetPr(prhs[2]), mxGetNumberOfElements(prhs[2]));
    Map<VectorXd> k_qd_p(mxGetPr(prhs[3]), mxGetNumberOfElements(prhs[3]));
    Map<VectorXd> k_f_p(mxGetPr(prhs[4]), mxGetNumberOfElements(prhs[4]));
    Map<VectorXd> ff_qd(mxGetPr(prhs[5]), mxGetNumberOfElements(prhs[5]));
    Map<VectorXd> ff_qd_d(mxGetPr(prhs[6]), mxGetNumberOfElements(prhs[6]));
    Map<VectorXd> ff_f_d(mxGetPr(prhs[7]), mxGetNumberOfElements(prhs[7]));
    Map<VectorXd> ff_const(mxGetPr(prhs[8]), mxGetNumberOfElements(prhs[8]));

    ac->updateGains(k_q_p,k_q_i,k_qd_p,k_f_p,ff_qd,ff_qd_d,ff_f_d,ff_const);

    mxClassID cid;
    if (sizeof(ac)==4) cid = mxUINT32_CLASS;
    else if (sizeof(ac)==8) cid = mxUINT64_CLASS;
    else mexErrMsgIdAndTxt("Drake:AtlasCommandPublisher:PointerSize error","Are you on a 32-bit machine or 64-bit machine??");
    plhs[0] = mxCreateNumericMatrix(1,1,cid,mxREAL);
    memcpy(mxGetData(plhs[0]),&ac,sizeof(ac));
  }
  else {
    mexErrMsgTxt("AtlasCommandPublisher: wrong number of input arguments");
  }

}


// WiiMouse.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include "XimuReceiver.h"
#include "Quaternion.h"
#include "EulerAngles.h"
#include "Utils.h"
#include <boost/asio/serial_port.hpp>
#include <boost/asio.hpp>
#include <windows.h>

using namespace boost;

float DPI = 15.0f;
float SCROLL_SPEED = -1.6f;

int _tmain(int argc, _TCHAR* argv[])
{
	DWORD command = MOUSEEVENTF_MOVE;
	DWORD xOffset = 0;
	DWORD yOffset = 0;
	DWORD scrollAmount = 0;
	float yawDiff = 0.0f;
	float pitchDiff = 0.0f;
	float rollDiff = 0.0f;

	QuaternionPacket quaternionPacket;
	DigitalIOPacket digitalPacket;
	Quaternion quaternion;
	EulerAngles eulerAngles;
	EulerAngles lastEulerAngles;
	XimuReceiver receiver;

	bool leftButtonDown = false;
	bool rightButtonDown = false;

	// Open Serial Port
	asio::io_service io;
	asio::serial_port port(io);

	port.open("COM16");
	port.set_option(asio::serial_port_base::baud_rate(115200));

	// Read buffer character
	char c;

	while (true) {
		// Read 1 character into c, this will block
		// forever if no character arrives.
		asio::read(port, asio::buffer(&c,1));
		receiver.processNewChar(c);

		// Create mouse events based on different data received
		// Quaternion gives orientation and is thus for mouse move and scroll
		if (receiver.isQuaternionGetReady()) {
			quaternionPacket = receiver.getQuaternion();

			quaternion.update(quaternionPacket.getW(), quaternionPacket.getX(),
								quaternionPacket.getY(), quaternionPacket.getZ());
			eulerAngles = quaternion.toEulerAngles();

			yawDiff = angleCorrect(lastEulerAngles.getYaw() - eulerAngles.getYaw());
			pitchDiff = angleCorrect(eulerAngles.getPitch() - lastEulerAngles.getPitch());
			// Introduce symmetry for right-handed users (lefthanded comming in a bit)
			// Because right-handed people tend to tilt the mouse right-wards a bit
			rollDiff = angleCorrect(eulerAngles.getRoll()) - 10.0f;

			xOffset = (DWORD)(yawDiff * DPI);
			yOffset = (DWORD)(pitchDiff * DPI);

			lastEulerAngles = eulerAngles;

			mouse_event(MOUSEEVENTF_MOVE, xOffset, yOffset, scrollAmount, 0);

			if (abs(rollDiff) > 20.0) {
				scrollAmount = (DWORD)(rollDiff * SCROLL_SPEED);
				mouse_event(MOUSEEVENTF_WHEEL, 0, 0, scrollAmount, 0);
				scrollAmount = 0;
			}
		}

		// Push buttons are read as digital inputs and used for left/right clicks
		if (receiver.isDigitalGetReady()) {
			digitalPacket = receiver.getDigitalReading();

			if (digitalPacket.getState(0) == 1) {
				if(!leftButtonDown) {
					mouse_event(MOUSEEVENTF_LEFTDOWN, 0, 0, 0, 0);
					leftButtonDown = true;
				}
			}
			if (digitalPacket.getState(0) == 0) {
				if (leftButtonDown) {
					mouse_event(MOUSEEVENTF_LEFTUP, 0, 0, 0, 0);
					leftButtonDown = false;
				}
			}
			if (digitalPacket.getState(2) == 1) {
				if(!rightButtonDown) {
					mouse_event(MOUSEEVENTF_RIGHTDOWN, 0, 0, 0, 0);
					rightButtonDown = true;
				}
			}
			if (digitalPacket.getState(2) == 0) {
				if (rightButtonDown) {
					mouse_event(MOUSEEVENTF_RIGHTUP, 0, 0, 0, 0);
					rightButtonDown = false;
				}
			}
		}
	}

	return 0;
}


#pragma once

#include <cstdint>
#include <istream>
#include <ostream>
#include <stdexcept>
#include <vector>
#include <iterator>
#include <algorithm>
#include <type_traits>

#include <boost/config.hpp>
#include <boost/random/detail/config.hpp>
#include <boost/random/detail/seed.hpp>
#include <boost/random/detail/seed_impl.hpp>
#include <boost/detail/workaround.hpp>
#include "xoroshiro.hpp"
#include <boost/random/detail/disable_warnings.hpp>

namespace boost {
namespace random {

#if !defined(BOOST_NO_INT64_T) && !defined(BOOST_NO_INTEGRAL_INT64_T)

template<typename IntType, std::size_t w, std::size_t CMWC_CYCLE, std::uint32_t CMWC_C_MAX, std::uint64_t A, bool is_complementary>
class complementary_multiply_with_carry_engine {
	typedef typename std::conditional<is_complementary, std::true_type, std::false_type>::type complementary;
public:
	typedef IntType result_type;

	// Required for old Boost.Random concept.
	static const bool has_fixed_range = true;
	static const IntType default_seed = 1;

	/**
	* Constructs a @c complementary_multiply_with_carry_engine, using the default seed.
	*/
	complementary_multiply_with_carry_engine()
	{
		seed();
	}

	/**
	* Constructs a @c complementary_multiply_with_carry_engine, seeding it with @c value.
	*/
	template<typename IT>
	BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR(complementary_multiply_with_carry_engine,
		IT, value)
	{
		seed(value);
	}

	/**
	* Constructs a @c complementary_multiply_with_carry_engine, seeding it with values
	* produced by a call to @c seq.generate().
	*/
	BOOST_RANDOM_DETAIL_SEED_SEQ_CONSTRUCTOR(complementary_multiply_with_carry_engine,
		SeedSeq, seq)
	{
		seed(seq);
	}

	/**
	* Constructs a @c complementary_multiply_with_carry_engine and seeds it with values
	* taken from the iterator range [first, last) and adjusts
	* first to point to the element after the last one used.
	* If there are not enough elements, throws @c std::invalid_argument.
	*
	* first and last must be input iterators.
	*/
	template<class It>
	complementary_multiply_with_carry_engine(It& first, It last)
	{
		seed(first, last);
	}

	// compiler-generated copy constructor and assignment operator are fine.

	/**
	* Calls seed(default_seed)
	*/
	void seed()
	{
		seed(default_seed);
	}

	/**
	* seeds a @c complementary_multiply_with_carry_engine with splitmix64.
	*/
	template<typename IT>
	BOOST_RANDOM_DETAIL_ARITHMETIC_SEED(complementary_multiply_with_carry_engine, IT, value)
	{
		BOOST_STATIC_ASSERT(std::numeric_limits<IT>::is_integer);

		boost::random::splitmix64 intgen(static_cast<std::uint64_t>(value));
		detail::generator_seed_seq<boost::random::splitmix64> gen(intgen);
		seed(gen);
	}

	/**
	* Seeds a @c complementary_multiply_with_carry_engine using values from a SeedSeq.
	*/
	BOOST_RANDOM_DETAIL_SEED_SEQ_SEED(complementary_multiply_with_carry_engine, SeedSeq, seq)
	{
		detail::seed_array_int<w>(seq, _Q);
		_carry = detail::seed_one_int<IntType, CMWC_C_MAX>(seq);
		_i = CMWC_CYCLE - 1;
		warmup();
	}

	/**
	* Seeds a @c complementary_multiply_with_carry_engine with values taken from the
	* iterator range [first, last) and adjusts @c first to
	* point to the element after the last one used. If there are
	* not enough elements or all the whole input range is zero,
	* throws @c std::invalid_argument.
	*
	* @c first and @c last must be input iterators.
	*/
	template<class It>
	void seed(It& first, It last)
	{
		detail::fill_array_int<w>(first, last, _Q );
		_carry = detail::seed_one_int<IntType, CMWC_C_MAX>(first, last);
		_i = CMWC_CYCLE - 1;
		warmup();
	}

	/**
	* Returns the smallest value that the @c complementary_multiply_with_carry_engine
	* can produce.
	*/
	static result_type min BOOST_PREVENT_MACRO_SUBSTITUTION ( )
	{ return 0; }

	/**
	* Returns the largest value that the @c complementary_multiply_with_carry_engine
	* can produce.
	*/
	static result_type max BOOST_PREVENT_MACRO_SUBSTITUTION ( )
	{ return boost::low_bits_mask_t<w>::sig_bits; }

	/**
	* Returns the next value of the @c
	* complementary_multiply_with_carry_engine,
	* complemented.
	*/
	template<typename U = complementary>
		typename std::enable_if<
			std::is_same<U, std::true_type>::value, result_type>::type
	operator()( )
	{
		_i = (_i + 1) & (CMWC_CYCLE - 1);
		const std::uint64_t t = A * _Q[_i] + _carry;
		_carry = t >> 32;
		std::uint32_t x = t + _carry;
		if (x < _carry) {
			++x, ++_carry;
		}
		return (_Q[_i] = std::uint32_t { 0xFFFFFFFE } - x); // m - x
	}

	/**
	 * Returns the next value of the @c
	 * complementary_multiply_with_carry_engine,
	 * not-complemented.
	 */
	template<typename U = complementary>
		typename std::enable_if<
			std::is_same<U, std::false_type>::value, result_type>::type
	operator()( )
	{
		_i = (_i + 1) & (CMWC_CYCLE - 1);
		const std::uint64_t t = A * _Q[_i] + _carry;
		_carry = t >> 32;
		std::uint32_t x = t + _carry;
		if (x < _carry) {
			++x, ++_carry;
		}
		return (_Q[_i] = x);
	}

	/** Fills a range with random values. */
	template<class Iter>
	void generate(Iter first, Iter last)
	{
		detail::generate_from_int(*this, first, last);
	}

	/** Advances the state of the generator by @c z. */
	void discard(std::uintmax_t z)
	{
		while (z--) {
			_i = (_i + 1) & (CMWC_CYCLE - 1);
			const std::uint64_t t = A * _Q[_i] + _carry;
			_carry = t >> 32;
			_carry += ((t + _carry) < _carry);
		}
	}

	/** Writes a @c complementary_multiply_with_carry_engine to a @c std::ostream. */
	template<class CharT, class Traits>
	friend std::basic_ostream<CharT, Traits>&
		operator<<( std::basic_ostream<CharT, Traits>& os,
			const complementary_multiply_with_carry_engine& cmwc)
	{
		std::size_t i = cmwc._i;
		for (; i < CMWC_CYCLE; ++i) {
			os << cmwc._Q[i] << ' ';
		}
		i = 0;
		for (; i < cmwc._i; ++i) {
			os << cmwc._Q[i] << ' ';
		}
		os << cmwc._carry;
		return os;
	}

	/** Reads a @c complementary_multiply_with_carry_engine from a @c std::istream. */
	template<class CharT, class Traits>
	friend std::basic_istream<CharT, Traits>&
		operator >> ( std::basic_istream<CharT, Traits>& is,
			complementary_multiply_with_carry_engine& cmwc)
	{
		for (std::size_t i = 0; i < CMWC_CYCLE; ++i) {
			is >> cmwc._Q [ i ] >> std::ws;
		}
		is >> cmwc._carry;
		cmwc._i = 0;
		return is;
	}

private:

	// As per http://www0.cs.ucl.ac.uk/staff/D.Jones/GoodPracticeRNG.pdf
	inline void warmup()
	{
		discard(4 * CMWC_CYCLE);
	}

	IntType _Q[CMWC_CYCLE];
	std::size_t _i;
	IntType _carry;
};

using mwc256 = complementary_multiply_with_carry_engine<std::uint32_t, 32, 256, 809430660, 1540315826, false>;

/*
 * Generators below from:
 *
 * Marsaglia, George (2003) "Random Number Generators,"Journal of Modern Applied Statistical Methods: Vol. 2 : Iss. 1 , Article 2, Page 9.
 * DOI: 10.22237/jmasm/1051747320
 * Available at: http://digitalcommons.wayne.edu/jmasm/vol2/iss1/2
 */

using cmwc4 = complementary_multiply_with_carry_engine<std::uint32_t, 32, 4, 123, 987651670, true>;
using cmwc8 = complementary_multiply_with_carry_engine<std::uint32_t, 32, 8, 123, 987651386, true>;
using cmwc16 = complementary_multiply_with_carry_engine<std::uint32_t, 32, 16, 123, 987651178, true>;
using cmwc32 = complementary_multiply_with_carry_engine<std::uint32_t, 32, 32, 123, 987655670, true>;
using cmwc64 = complementary_multiply_with_carry_engine<std::uint32_t, 32, 64, 123, 987651206, true>;
using cmwc128 = complementary_multiply_with_carry_engine<std::uint32_t, 32, 128, 123, 987688302, true>;
using cmwc256 = complementary_multiply_with_carry_engine<std::uint32_t, 32, 256, 123, 987662290, true>;
using cmwc512 = complementary_multiply_with_carry_engine<std::uint32_t, 32, 512, 123, 123462658, true>;
using cmwc1024 = complementary_multiply_with_carry_engine<std::uint32_t, 32, 1024, 123, 5555698, true>;
using cmwc2048 = complementary_multiply_with_carry_engine<std::uint32_t, 32, 2048, 123, 1030770, true>;

using cmwc4096 = complementary_multiply_with_carry_engine<std::uint32_t, 32, 4096, 809430660, 18782, true>;

#endif /* !BOOST_NO_INT64_T && !BOOST_NO_INTEGRAL_INT64_T */

} // namespace random
} // namespace boost