//=============================================================================================================
/**
 * @file     main.cpp
 * @author   Daniel Strohmeier <Daniel.Strohmeier@tu-ilmenau.de>;
 *           Lorenz Esch <lesch@mgh.harvard.edu>
 * @since    0.1.0
 * @date     March, 2018
 *
 * @section  LICENSE
 *
 * Copyright (C) 2018, Daniel Strohmeier, Lorenz Esch. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that
 * the following conditions are met:
 *     * Redistributions of source code must retain the above copyright notice, this list of conditions and the
 *       following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and
 *       the following disclaimer in the documentation and/or other materials provided with the distribution.
 *     * Neither the name of MNE-CPP authors nor the names of its contributors may be used
 *       to endorse or promote products derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 *
 * @brief    Example of the computation of spectra, PSD and CSD
 *
 */

//=============================================================================================================
// INCLUDES
//=============================================================================================================

#include <math.h>
#include <disp/plots/plot.h>
#include <utils/spectral.h>
#include <utils/generics/applicationlogger.h>
#include <mne/mne.h>

//=============================================================================================================
// Eigen
//=============================================================================================================

#include <Eigen/Core>

//=============================================================================================================
// QT INCLUDES
//=============================================================================================================

#include <QApplication>
#include <QtMath>

//=============================================================================================================
// USED NAMESPACES
//=============================================================================================================

using namespace Eigen;
using namespace DISPLIB;
using namespace UTILSLIB;

//=============================================================================================================
// MAIN
//=============================================================================================================

//=============================================================================================================
/**
 * The function main marks the entry point of the program.
 * By default, main has the storage class extern.
 *
 * @param [in] argc (argument count) is an integer that indicates how many arguments were entered on the command line when the program was started.
 * @param [in] argv (argument vector) is an array of pointers to arrays of character objects. The array objects are null-terminated strings, representing the arguments that were entered on the command line when the program was started.
 * @return the value that was set to exit() (which is 0 if exit() is called via quit()).
 */
int main(int argc, char *argv[])
{
    qInstallMessageHandler(ApplicationLogger::customLogWriter);
    QApplication a(argc, argv);

    // Generate input data
    int iNSamples = 500;
    double dSampFreq = 23.0;
    MatrixXd inputData = MatrixXd::Random(2, iNSamples);
    for (int n = 0; n < iNSamples; n++) {
        inputData(0, n) += 10.0 * sin(2.0 * M_PI *  10. * n / iNSamples );
        inputData(0, n) += 10.0 * sin(2.0 * M_PI * 100. * n / iNSamples);
        inputData(0, n) += 10.0 * sin(2.0 * M_PI * 200. * n / iNSamples);

        inputData(1, n) += 10.0 * sin(2.0 * M_PI *  50. * n / iNSamples);
        inputData(1, n) += 10.0 * sin(2.0 * M_PI * 100. * n / iNSamples);
        inputData(1, n) += 10.0 * sin(2.0 * M_PI * 150. * n / iNSamples);
    }

    //Generate hanning window
    QPair<MatrixXd, VectorXd> tapers = Spectral::generateTapers(iNSamples, "hanning");
    MatrixXd matTaps = tapers.first;
    VectorXd vecTapWeights = tapers.second;

    //Plot hanning window
    VectorXd hann = matTaps.row(0).transpose();
    Plot plotWindow(hann);
    plotWindow.setTitle("Hanning window");
    plotWindow.setXLabel("X Axes");
    plotWindow.setYLabel("Y Axes");
    plotWindow.setWindowTitle("Corresponding function to MATLABs plot");
    plotWindow.show();

    //Compute Spectrum of first row of input data
    int iNfft = iNSamples;
    MatrixXcd matTapSpectrumSeed = Spectral::computeTaperedSpectraRow(inputData.row(0), matTaps, iNfft);

    //Compute PSD
    RowVectorXd vecPsd = Spectral::psdFromTaperedSpectra(matTapSpectrumSeed, vecTapWeights, iNfft, dSampFreq);

    //Plot PSD
    VectorXd psd = vecPsd.transpose();
    Plot plotPsd(psd);
    plotPsd.setTitle("PSD of Row 0");
    plotPsd.setXLabel("X Axes");
    plotPsd.setYLabel("Y Axes");
    plotPsd.setWindowTitle("Corresponding function to MATLABs plot");
    plotPsd.show();

    //Check PSD
    VectorXd psdTest = matTapSpectrumSeed.row(0).cwiseAbs2().transpose();
    psdTest *= 2.0;
    psdTest(0) /= 2.0;
    if (iNfft % 2 == 0){
        psdTest.tail(1) /= 2.0;
    }
    //Normalization
    psdTest /= dSampFreq;

    //Plot PSDTest
    Plot plotPsdTest(psdTest);
    plotPsdTest.setTitle("PSD of Row 0");
    plotPsdTest.setXLabel("X Axes");
    plotPsdTest.setYLabel("Y Axes");
    plotPsdTest.setWindowTitle("Corresponding function to MATLABs plot");
    plotPsdTest.show();

    //Compute CSD of matTapSpectrumSeed and matTapSpectrumSeed
    //The real part should be equivalent to the PSD of matTapSpectrumSeed)
    RowVectorXcd vecCsdSeed = Spectral::csdFromTaperedSpectra(matTapSpectrumSeed, matTapSpectrumSeed, vecTapWeights, vecTapWeights, iNfft, dSampFreq);
    VectorXd psdTest2 = vecCsdSeed.real();

    //Plot PSDTest2
    Plot plotPsdTest2(psdTest2);
    plotPsdTest2.setTitle("PSD of Row 0");
    plotPsdTest2.setXLabel("X Axes");
    plotPsdTest2.setYLabel("Y Axes");
    plotPsdTest2.setWindowTitle("Corresponding function to MATLABs plot");
    plotPsdTest2.show();

    //Check that sums of different psds of the same signal are equal
    qDebug()<<psd.sum();
    qDebug()<<psdTest.sum();
    qDebug()<<psdTest2.sum();
    RowVectorXd data_hann = inputData.row(0).cwiseProduct(matTaps.row(0));
    qDebug()<<data_hann.row(0).cwiseAbs2().sum() * double(iNSamples) / dSampFreq;

    //Compute Spectrum of second row of input data
    MatrixXcd matTapSpectrumTarget = Spectral::computeTaperedSpectraRow(inputData.row(1), matTaps, iNfft);

    //Compute CSD between seed and target
    RowVectorXcd vecCsd = Spectral::csdFromTaperedSpectra(matTapSpectrumSeed, matTapSpectrumTarget, vecTapWeights, vecTapWeights, iNfft, dSampFreq);

    //Plot real part of CSD
    VectorXd csd = vecCsd.transpose().real();
    Plot plotCsd(csd);
    plotCsd.setTitle("Real part of the CSD of Row 0 and 1");
    plotCsd.setXLabel("X Axes");
    plotCsd.setYLabel("Y Axes");
    plotCsd.setWindowTitle("Corresponding function to MATLABs plot");
    plotCsd.show();

    return a.exec();
}


/*
 * drakeUtil.cpp
 *
 *  Created on: Jun 19, 2013
 *      Author: russt
 */

#include "drakeUtil.h"
#include <string.h>
#include <string>
#include <math.h>
#include <limits>
#include <Eigen/Dense>
#include <stdexcept>

using namespace std;
using namespace Eigen;

bool isa(const mxArray* mxa, const char* class_str)
// mxIsClass seems to not be able to handle derived classes. so i'll implement what I need by calling back to matlab
{
  mxArray* plhs;
  mxArray* prhs[2];
  prhs[0] = const_cast<mxArray*>(mxa);
  prhs[1] = mxCreateString(class_str);
  mexCallMATLAB(1, &plhs, 2, prhs, "isa");
  bool tf = *mxGetLogicals(plhs);
  mxDestroyArray(plhs);
  mxDestroyArray(prhs[1]);
  return tf;
}

bool mexCallMATLABsafe(int nlhs, mxArray* plhs[], int nrhs, mxArray* prhs[], const char* filename)
{
  int i;
  mxArray* ex = mexCallMATLABWithTrap(nlhs, plhs, nrhs, prhs, filename);
  if (ex) {
    mexPrintf("DrakeSystem S-Function: error when calling ''%s'' with the following arguments:\n", filename);
    for (i = 0; i < nrhs; i++)
      mexCallMATLAB(0, NULL, 1, &prhs[i], "disp");
    mxArray *report;
    mexCallMATLAB(1, &report, 1, &ex, "getReport");
    char *errmsg = mxArrayToString(report);
    mexPrintf(errmsg);
    mxFree(errmsg);
    mxDestroyArray(report);
    mexErrMsgIdAndTxt("Drake:mexCallMATLABsafe:CallbackError", "Error in MATLAB callback.\nSee additional debugging information above");
    mxDestroyArray(ex);
    return true;
  }
  for (i = 0; i < nlhs; i++)
    if (!plhs[i]) {
      mexPrintf("Drake mexCallMATLABsafe: error when calling ''%s'' with the following arguments:\n", filename);
      for (i = 0; i < nrhs; i++)
        mexCallMATLAB(0, NULL, 1, &prhs[i], "disp");
      mexErrMsgIdAndTxt("Drake:mexCallMATLABsafe:NotEnoughOutputs", "Asked for %d outputs, but function only returned %d\n", nrhs, i);
      return true;
    }
  return false;
}

/*
 * @param subclass_name (optional) if you want to call a class that derives from
 * DrakeMexPointer (e.g. so that you can refer to it as something more specific in
 * your matlab code), then you can pass in the alternative name here.  The constructor
 * for this class must take the same inputs as the DrakeMexPointer constructor.
 */
mxArray* createDrakeMexPointer(void* ptr, const char* name, int num_additional_inputs, mxArray* delete_fcn_additional_inputs[], const char* subclass_name)
{
  mxClassID cid;
  if (sizeof(ptr) == 4) cid = mxUINT32_CLASS;
  else if (sizeof(ptr) == 8) cid = mxUINT64_CLASS;
  else mexErrMsgIdAndTxt("Drake:constructDrakeMexPointer:PointerSize", "Are you on a 32-bit machine or 64-bit machine??");

  int nrhs = 3 + num_additional_inputs;
  mxArray *plhs[1];
  mxArray **prhs;
  prhs = new mxArray*[nrhs];

  prhs[0] = mxCreateNumericMatrix(1, 1, cid, mxREAL);
  memcpy(mxGetData(prhs[0]), &ptr, sizeof(ptr));

  prhs[1] = mxCreateString(mexFunctionName());

  prhs[2] = mxCreateString(name);

  for (int i = 0; i < num_additional_inputs; i++)
    prhs[3+i] = delete_fcn_additional_inputs[i];

//  mexPrintf("deleteMethod = %s\n name =%s\n", deleteMethod,name);

  // call matlab to construct mex pointer object
  if (subclass_name) {
    mexCallMATLABsafe(1, plhs, nrhs, prhs, subclass_name);
    if (!isa(plhs[0], "DrakeMexPointer")) {
      mxDestroyArray(plhs[0]);
      mexErrMsgIdAndTxt("Drake:createDrakeMexPointer:InvalidSubclass", "subclass_name is not a valid subclass of DrakeMexPointer");
    }
  }
  else
    mexCallMATLABsafe(1, plhs, nrhs, prhs, "DrakeMexPointer");

  mexLock();

//  mexPrintf("incrementing lock count\n");

  delete[] prhs;
  return plhs[0];
}

void* getDrakeMexPointer(const mxArray* mx)
{
  void* ptr = NULL;

  // todo: optimize this by caching the pointer values, as described in
  // http://groups.csail.mit.edu/locomotion/bugs/show_bug.cgi?id=1590
  mxArray* ptrArray = mxGetProperty(mx, 0, "ptr");

  if (!ptrArray)
    mexErrMsgIdAndTxt("Drake:getDrakeMexPointer:BadInputs", "cannot retrieve 'ptr' field from this mxArray.  are you sure it's a valid DrakeMexPointer object?");

  switch (sizeof(void*)) {
    case 4:
      if (!mxIsUint32(ptrArray))
        mexErrMsgIdAndTxt("Drake:getDrakeMexPointer:BadPointerSize", "DrakeMexPointer expected a 32-bit ptr field but got something else");
      break;
    case 8:
      if (!mxIsUint64(ptrArray))
        mexErrMsgIdAndTxt("Drake:getDrakeMexPointer:BadPointerSize", "DrakeMexPointer expected a 64-bit ptr field but got something else");
      break;
    default:
      mexErrMsgIdAndTxt("Drake:getDrakeMexPointer:BadPointerSize", "DrakeMexPointer got a pointer that was neither 32-bit nor 64-bit.");
  }

  if (!mxIsNumeric(ptrArray) || mxGetNumberOfElements(ptrArray) != 1)
    mexErrMsgIdAndTxt("Drake:getDrakeMexPointer:BadInputs","the ptr property of this DrakeMexPointer does not appear to contain a valid pointer");
  memcpy(&ptr, mxGetData(ptrArray), sizeof(ptr));     // note: could use a reinterpret_cast here instead

  return ptr;
}

double angleAverage(double theta1, double theta2) {
  // Computes the average between two angles by averaging points on the unit
  // circle and taking the arctan of the result.
  //   see: http://en.wikipedia.org/wiki/Mean_of_circular_quantities
  // theta1 is a scalar or column vector of angles (rad)
  // theta2 is a scalar or column vector of angles (rad)

  double x_mean = 0.5 * (cos(theta1) + cos(theta2));
  double y_mean = 0.5 * (sin(theta1) + sin(theta2));

  double angle_mean = atan2(y_mean, x_mean);

  return angle_mean;
}

std::pair<Eigen::Vector3d, double> resolveCenterOfPressure(Eigen::Vector3d torque, Eigen::Vector3d force, Eigen::Vector3d normal, Eigen::Vector3d point_on_contact_plane)
{
  // TODO: implement multi-column version
  using namespace Eigen;

  if (abs(normal.squaredNorm() - 1.0) > 1e-12) {
    mexErrMsgIdAndTxt("Drake:resolveCenterOfPressure:BadInputs", "normal should be a unit vector");
  }

  Vector3d cop;
  double normal_torque_at_cop;

  double fz = normal.dot(force);
  bool cop_exists = abs(fz) > 1e-12;

  if (cop_exists) {
    auto torque_at_point_on_contact_plane = torque - point_on_contact_plane.cross(force);
    double normal_torque_at_point_on_contact_plane = normal.dot(torque_at_point_on_contact_plane);
    auto tangential_torque = torque_at_point_on_contact_plane - normal * normal_torque_at_point_on_contact_plane;
    cop = normal.cross(tangential_torque) / fz + point_on_contact_plane;
    auto torque_at_cop = torque - cop.cross(force);
    normal_torque_at_cop = normal.dot(torque_at_cop);
  }
  else {
    cop.setConstant(std::numeric_limits<double>::quiet_NaN());
    normal_torque_at_cop = std::numeric_limits<double>::quiet_NaN();
  }
  return std::pair<Vector3d, double>(cop, normal_torque_at_cop);
}

double * mxGetPrSafe(const mxArray *pobj) {
  if (!mxIsDouble(pobj)) mexErrMsgIdAndTxt("Drake:mxGetPrSafe:BadInputs", "mxGetPr can only be called on arguments which correspond to Matlab doubles");
  return mxGetPr(pobj);
}

mxArray* mxGetPropertySafe(const mxArray* array, std::string const& field_name) {
  return mxGetPropertySafe(array, 0, field_name);
}

mxArray* mxGetPropertySafe(const mxArray* array, size_t index, std::string const& field_name)
{
  mxArray* ret = mxGetProperty(array, index, field_name.c_str());
  if (!ret)
  {
    mexErrMsgIdAndTxt("Drake:mxGetPropertySafe", ("Field not found: " + field_name).c_str());
  }
  return ret;
}

mxArray* mxGetFieldSafe(const mxArray* array, std::string const& field_name) {
  return mxGetFieldSafe(array, 0, field_name);
}

mxArray* mxGetFieldSafe(const mxArray* array, size_t index, std::string const& field_name)
{
  mxArray* ret = mxGetField(array, index, field_name.c_str());
  if (!ret)
  {
    mexErrMsgIdAndTxt("Drake:mxGetFieldSafe", ("Field not found: " + field_name).c_str());
  }
  return ret;
}

void mxSetFieldSafe(mxArray* array, size_t index, std::string const & fieldname, mxArray* data)
{
  int fieldnum;
  fieldnum = mxGetFieldNumber(array, fieldname.c_str());
  if (fieldnum < 0) {
    fieldnum = mxAddField(array, fieldname.c_str());
  }
  mxSetFieldByNumber(array, index, fieldnum, data);
}

const std::vector<double> matlabToStdVector(const mxArray* in) {
  // works for both row vectors and column vectors
  if (mxGetM(in) != 1 && mxGetN(in) != 1)
    throw runtime_error("Not a vector");
  double* data = mxGetPrSafe(in);
  return std::vector<double>(data, data + mxGetNumberOfElements(in));
}

int sub2ind(mwSize ndims, const mwSize* dims, const mwSize* sub) {
  int stride = 1;
  int ret = 0;
  for (int i = 0; i < ndims; i++) {
    ret += sub[i] * stride;
    stride *= dims[i];
  }
  return ret;
}

void sizecheck(const mxArray* mat, int M, int N) {
  if (mxGetM(mat) != M) {
    mexErrMsgIdAndTxt("Drake:WrongSize", "wrong number of rows. Expected: %d but got: %d", M, mxGetM(mat));
  }
  if (mxGetN(mat) != N) {
    mexErrMsgIdAndTxt("Drake:WrongSize", "wrong number of columns. Expected: %d but got: %d", N, mxGetN(mat));
  }
  return;
}

//builds a matlab sparse matrix in mex from a given eigen matrix
//the caller is responsible for destroying the resulting array
template <typename Derived>
mxArray* eigenToMatlabSparse(MatrixBase<Derived> const & M, int & num_non_zero)
{
  const mwSize rows = M.rows();
  const mwSize cols = M.cols();

  vector<mwIndex> ir;
  vector<mwIndex> jc;
  vector<double> pr;

  mwSize cumulative_nonzero = 0;
  jc.push_back(cumulative_nonzero);
  double eps = std::numeric_limits<double>::epsilon();
  for (mwIndex j = 0; j < cols; j++) {
    for (mwIndex i = 0; i < rows; i++)  {
      double value = M(i, j);

      if (value > eps || value < -eps) {
        ir.push_back(i);
        pr.push_back(value);
        cumulative_nonzero++;
      }
    }
    jc.push_back(cumulative_nonzero);
  }

  mxArray* sparse_mex = mxCreateSparse(rows, cols, cumulative_nonzero, mxREAL);

  memcpy((double*)mxGetPr(sparse_mex), pr.data(), cumulative_nonzero * sizeof(double));
  memcpy((int*)mxGetIr(sparse_mex), ir.data(), cumulative_nonzero * sizeof(mwIndex));
  memcpy((int *)mxGetJc(sparse_mex), jc.data(), (cols + 1) * sizeof(mwIndex));

  num_non_zero = cumulative_nonzero;
  return sparse_mex;
}

template DLLEXPORT mxArray* eigenToMatlabSparse(MatrixBase< MatrixXd > const &, int &) ;
template DLLEXPORT mxArray* eigenToMatlabSparse(MatrixBase< Map< MatrixXd> > const &, int &) ;


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

#ifndef BN_HPP
#define BN_HPP

#include "common.hpp"

#include <stdio.h>

#include <arpa/inet.h>

#include <exception>
#include <iostream>
#include <string>

#include <boost/functional/hash.hpp>

namespace libcage {
// this is a sizeof(T) * N bytes integer class
// T should be unsigned
// T should not be long long
        template <typename T, int N>
        class bn
        {
        public:
                bn();
                bn(const bn<T, N> &rhs);
                bn(T rhs);

                ~bn(){};

                // for assign
                bn<T, N>        operator =(const bn<T, N> &rhs);
                bn<T, N>        operator =(T rhs);

                // for addition
                bn<T, N>        operator +(const bn<T, N> &rhs) const;
                bn<T, N>        operator +(T rhs) const;
                bn<T, N>        operator +=(const bn<T, N> &rhs);
                bn<T, N>        operator +=(T rhs);
                // friend bn<T, N> operator +<T, N>(T lhs, const bn<T, N> &rhs);

                // for substraction
                bn<T, N>        operator -(const bn<T, N> &rhs) const;
                bn<T, N>        operator -(T rhs) const;
                bn<T, N>        operator -=(const bn<T, N> &rhs);
                bn<T, N>        operator -=(T rhs);
                // friend bn<T, N> operator -<T, N>(T lhs, const bn<T, N> &rhs);

                // for multiplication
                bn<T, N>        operator *(const bn<T, N> &rhs) const;
                bn<T, N>        operator *(T rhs) const;
                bn<T, N>        operator *=(const bn<T, N> &rhs);
                bn<T, N>        operator *=(T rhs);
                // friend bn<T, N> operator *<T, N>(T lhs, const bn<T, N> &rhs);

                // for exclusive or
                bn<T, N>        operator ^(const bn<T, N> &rhs) const;
                bn<T, N>        operator ^(T rhs) const;
                bn<T, N>        operator ^=(const bn<T, N> &rhs);
                bn<T, N>        operator ^=(T rhs);

                // for and
                bn<T, N>        operator &(const bn<T, N> &rhs) const;
                bn<T, N>        operator &(T rhs) const;
                bn<T, N>        operator &=(const bn<T, N> &rhs);
                bn<T, N>        operator &=(T rhs);

                // for not
                bn<T, N>        operator ~();

                // for multiplication with double
                bn<T, N>        operator *(double rhs) const;
                bn<T, N>        operator *=(double rhs);
                // friend bn<T, N> operator *<T, N>(double lhs, const bn<T, N> &rhs);

                // for shift
                bn<T, N>        operator <<(int rhs) const;
                bn<T, N>        operator >>(int rhs) const;
                bn<T, N>        operator <<=(int rhs);
                bn<T, N>        operator >>=(int rhs);

                // for comparison
                bool            operator ==(const bn<T, N> &rhs) const;
                bool            operator !=(const bn<T, N> &rhs) const;
                bool            operator <=(const bn<T, N> &rhs) const;
                bool            operator >=(const bn<T, N> &rhs) const;
                bool            operator <(const bn<T, N> &rhs) const;
                bool            operator >(const bn<T, N> &rhs) const;

                                operator T() const { return m_num[N - 1]; }
                                operator T&() { return m_num[N - 1]; }

                void            fill_zero();
                void            fill_max();
                void            to_binary(void *buf, int len) const;
                void            from_binary(const void *buf, int len);
                bool            is_zero() const;

                std::string     to_string() const;

                size_t          hash_value() const;

//  operator -(const bn &rhs);
//  operator *(const bn &rhs);

#ifdef DEBUG
                void            dump() const;
#endif

        private:
                T               m_num[N];

                static uint64_t m_exp_mask;

                void            shift_right(int bits, T arr[], int size) const;
                void            shift_left(int bits, T arr[], int size) const;

        };

// using namespace std;

        template <typename T, int N>
        uint64_t
        bn<T, N>::m_exp_mask = (((uint64_t)1 << 11) - 1) << 52;

        template <typename T, int N>
        bn<T, N>::bn()
        {

        }

        template <typename T, int N>
        bn<T, N>::bn(const bn<T, N> &rhs)
        {
                for (int i = 0; i < N; i++)
                        m_num[i] = rhs.m_num[i];
        }

        template <typename T, int N>
        bn<T, N>::bn(T rhs)
        {
                *this = rhs;
        }

        template <typename T, int N>
        bn<T, N>
        bn<T, N>::operator =(const bn<T, N> &rhs)
        {
                for (int i = 0; i < N; i++)
                        m_num[i] = rhs.m_num[i];

                return *this;
        }

        template <typename T, int N>
        bn<T, N>
        bn<T, N>::operator =(T rhs)
        {
                for (int i = 0; i < N - 1; i++)
                        m_num[i] = 0;

                m_num[N - 1] = rhs;

                return *this;
        }

#define PLUS_BN()                                       \
        do {                                            \
                T               carry = 0;              \
                T               tmp;                    \
                                                        \
                for (int i = N - 1; i >= 0; i--) {      \
                        tmp = m_num[i] + rhs.m_num[i];  \
                        if (tmp < m_num[i]) {           \
                                tmp = tmp + carry;      \
                                carry = 1;              \
                        } else {                        \
                                tmp = tmp + carry;      \
                                if (tmp < carry)        \
                                        carry = 1;      \
                                else                    \
                                        carry = 0;      \
                        }                               \
                        n.m_num[i] = tmp;               \
                }                                       \
                                                        \
                return n;                               \
        } while (0);

        template <typename T, int N>
        bn<T, N>
        bn<T, N>::operator +(const bn<T, N> &rhs) const
        {
                bn<T, N>        n;

                PLUS_BN();
        }

        template <typename T, int N>
        bn<T, N>
        bn<T, N>::operator +=(const bn<T, N> &rhs)
        {
                bn<T, N>       &n = *this;

                PLUS_BN();
        }

#undef PLUS_BN

#define PLUS_T()                                        \
        do{                                             \
                T               carry;                  \
                T               tmp;                    \
                                                        \
                tmp = m_num[N - 1] + rhs;               \
                if (tmp < m_num[N - 1])                 \
                        carry = 1;                      \
                else                                    \
                        carry = 0;                      \
                                                        \
                n.m_num[N - 1] = tmp;                   \
                                                        \
                for (int i = N - 2; i >= 0; i--) {      \
                        tmp = m_num[i] + carry;         \
                        if (tmp < m_num[i])             \
                                carry = 1;              \
                        else                            \
                                carry = 0;              \
                        n.m_num[i] = tmp;               \
                }                                       \
                                                        \
                return n;                               \
        } while (0);

        template <typename T, int N>
        bn<T, N>
        bn<T, N>::operator +(T rhs) const
        {
                bn<T, N>        n;

                PLUS_T();
        }

        template <typename T, int N>
        bn<T, N>
        bn<T, N>::operator +=(T rhs)
        {
                bn<T, N>       &n = *this;

                PLUS_T();
        }

#undef PLUS_T

/*
  template <typename T, int N>
  bn<T, N>
  operator +(T lhs, const bn<T, N> &rhs)
  {
  return rhs + lhs;
  }
*/

#define MINUS_BN()                                      \
        do {                                            \
                T               borrow = 0;             \
                T               tmp;                    \
                                                        \
                for (int i = N - 1; i >= 0; i--) {      \
                        tmp = m_num[i] - rhs.m_num[i];  \
                        if (tmp > m_num[i]) {           \
                                tmp = tmp - borrow;     \
                                borrow = 1;             \
                        } else {                        \
                                tmp = tmp - borrow;     \
                                if (tmp == (T)-1)       \
                                        borrow = 1;     \
                                else                    \
                                        borrow = 0;     \
                        }                               \
                        n.m_num[i] = tmp;               \
                }                                       \
                                                        \
                return n;                               \
        } while (0);

        template <typename T, int N>
        bn<T, N>
        bn<T, N>::operator -(const bn<T, N> &rhs) const
        {
                bn<T, N>        n;

                MINUS_BN();
        }

        template <typename T, int N>
        bn<T, N>
        bn<T, N>::operator -=(const bn<T, N> &rhs)
        {
                bn<T, N>       &n = *this;

                MINUS_BN();
        }

#undef MINUS_BN

#define MINUS_T()                                       \
        do {                                            \
                T               borrow;                 \
                T               tmp;                    \
                                                        \
                tmp = m_num[N - 1] - rhs;               \
                if (tmp > m_num[N - 1])                 \
                        borrow = 1;                     \
                else                                    \
                        borrow = 0;                     \
                                                        \
                n.m_num[N - 1] = tmp;                   \
                                                        \
                for (int i = N - 2; i >= 0; i--) {      \
                        tmp = m_num[i] - borrow;        \
                        if (tmp > m_num[i])             \
                                borrow = 1;             \
                        else                            \
                                borrow = 0;             \
                        n.m_num[i] = tmp;               \
                }                                       \
                                                        \
                return n;                               \
                                                        \
        } while (0);

        template <typename T, int N>
        bn<T, N>
        bn<T, N>::operator -(T rhs) const
        {
                bn<T, N>        n;

                MINUS_T();
        }

        template <typename T, int N>
        bn<T, N>
        bn<T, N>::operator -=(T rhs)
        {
                bn<T, N>       &n = *this;

                MINUS_T();
        }

#undef MINUS_T

/*
  template <typename T, int N>
  bn<T, N>
  operator -(T lhs, const bn<T, N> &rhs)
  {
  return rhs - lhs;
  }
*/

        template <typename T, int N>
        bn<T, N>
        bn<T, N>::operator *(const bn<T, N> &rhs) const
        {
                bn<T, N>        n;
                T               carry;
                uint64_t        u;
                int             i, j;

                for (i = 0; i < N; i++)
                        n.m_num[i] = 0;

                for (i = N - 1; i >= 0; i--) {
                        carry = 0;
                        for (j = N - 1; j >= 0; j--) {
                                int ij = N - 1 - (N - 1 - i + N - 1 - j);
                                if (ij >= 0) {
                                        u = (uint64_t)n.m_num[ij] +
                                                (uint64_t)rhs.m_num[j] *
                                                (uint64_t)m_num[i] +
                                                (uint64_t)carry;
                                        n.m_num[ij] = u &
                                                (((uint64_t)1 <<
                                                  sizeof(m_num[0]) * 8) - 1);
                                        carry = u >> sizeof(m_num[0]) * 8;
                                }
                        }
                }

                return n;
        }

        template <typename T, int N>
        bn<T, N>
        bn<T, N>::operator *=(const bn<T, N> &rhs)
        {
                *this = *this * rhs;

                return *this;
        }

        template <typename T, int N>
        bn<T, N>
        bn<T, N>::operator *(T rhs) const
        {
                bn<T, N>        n;
                T               carry;
                uint64_t        u;
                int             i;

                for (i = 0; i < N; i++)
                        n.m_num[i] = 0;

                for (i = N - 1; i >= 0; i--) {
                        carry = 0;
                        u = (uint64_t)n.m_num[i] +
                                (uint64_t)rhs *
                                (uint64_t)m_num[i] +
                                (uint64_t)carry;
                        n.m_num[i] = u & (((uint64_t)1 <<
                                           sizeof(m_num[0]) * 8) - 1);
                        carry = u >> sizeof(m_num[0]) * 8;
                }

                return n;
        }

        template <typename T, int N>
        bn<T, N>
        bn<T, N>::operator *=(T rhs)
        {
                *this = *this * rhs;

                return *this;
        }

/*
  template <typename T, int N>
  bn<T, N>
  operator *(T lhs, const bn<T, N> &rhs)
  {
  bn<T, N>        n;

  n = rhs * lhs;

  return n;
  }
*/
        template <typename T, int N>
        bn<T, N>
        bn<T, N>::operator ^(const bn<T, N> &rhs) const
        {
                bn<T, N>        n;
                int             i;

                for (i = 0; i < N; i++) {
                        n.m_num[i] = m_num[i] ^ rhs.m_num[i];
                }

                return n;
        }

        template <typename T, int N>
        bn<T, N>
        bn<T, N>::operator ^=(const bn<T, N> &rhs)
        {
                int i;
                for (i = 0; i < N; i++) {
                        m_num[i] ^= rhs.m_num[i];
                }

                return *this;
        }

        template <typename T, int N>
        bn<T, N>
        bn<T, N>::operator ^(T rhs) const
        {
                bn<T, N>        n;

                n = *this;
                n.m_rhs[0] ^= rhs;

                return n;
        }

        template <typename T, int N>
        bn<T, N>
        bn<T, N>::operator ^=(T rhs)
        {
                m_num[0] ^= rhs;

                return *this;
        }

        template <typename T, int N>
        bn<T, N>
        bn<T, N>::operator &(const bn<T, N> &rhs) const
        {
                bn<T, N>        n;
                int             i;

                for (i = 0; i < N; i++) {
                        n.m_num[i] = m_num[i] & rhs.m_num[i];
                }

                return n;
        }

        template <typename T, int N>
        bn<T, N>
        bn<T, N>::operator &=(const bn<T, N> &rhs)
        {
                int i;
                for (i = 0; i < N; i++) {
                        m_num[i] &= rhs.m_num[i];
                }

                return *this;
        }

        template <typename T, int N>
        bn<T, N>
        bn<T, N>::operator &(T rhs) const
        {
                bn<T, N>        n;

                n = *this;
                n.m_rhs[0] &= rhs;

                return n;
        }

        template <typename T, int N>
        bn<T, N>
        bn<T, N>::operator &=(T rhs)
        {
                m_num[0] &= rhs;

                return *this;
        }

        template <typename T, int N>
        bn<T, N>
        bn<T, N>::operator ~()
        {
                bn<T, N>        n;

                for (int i = 0; i < N; i++)
                        n.m_num[i] = ~m_num[i];

                return n;
        }

        template <typename T, int N>
        bn<T, N>
        bn<T, N>::operator *(double rhs) const
        {
                bn<T, N>        n;
                T               flo[2 * N];
                T               result[2 * N];
                T               carry;
                uint64_t      bits;
                uint64_t      decimal;
                uint64_t      mask;
                uint64_t      u;
                long long       exp_part;
                int             shift;
                int             i, j;

                memcpy(&bits, &rhs, sizeof(bits));

                // bits = *((uint64_t*)&rhs);

                decimal = bits << 12;
                exp_part = bits & m_exp_mask;
                exp_part >>= 52;
                exp_part -= 1023;

                memset(flo, 0, sizeof(flo));

                flo[N - 1] = 1;

                shift = sizeof(decimal) / sizeof(flo[0]);
                shift = (shift - 1) * 8 * sizeof(flo[0]);

                mask = ~(T)0;

                for (i = N; i < 2 * N && shift >= 0;
                     shift -= sizeof(flo[0]) * 8, i++)
                        flo[i] = (T)((decimal >> shift) & mask);

                shift_left((int)exp_part, flo, sizeof(flo) / sizeof(flo[0]));

                for (i = 0; i < N; i++)
                        result[i] = 0;

                for (i = N - 1; i >= 0; i--) {
                        carry = 0;
                        for (j = 2 * N - 1; j >= 0; j--) {
                                int ij = 2 * N - 1 -
                                        (N - 1 - i + 2 * N - 1 - j);
                                if (ij >= 0) {
                                        u = (uint64_t)result[ij] +
                                                (uint64_t)flo[j] *
                                                (uint64_t)m_num[i] +
                                                (uint64_t)carry;
                                        result[ij] = (T)(u &
                                                         (((uint64_t)1 <<
                                                           sizeof(result[0]) *
                                                           8) - 1));
                                        carry = (T)(u >> sizeof(result[0]) * 8);
                                }
                        }
                }

                for (int i = 0; i < N; i++)
                        n.m_num[i] = result[i];

                return n;
        }

        template <typename T, int N>
        bn<T, N>
        bn<T, N>::operator *=(double rhs)
        {
                *this = *this * rhs;

                return *this;
        }

/*
  template <typename T, int N>
  bn<T, N>
  operator *(double lhs, const bn<T, N> &rhs)
  {
  bn<T, N>        n;

  n = rhs * lhs;

  return n;
  }
*/

        template <typename T, int N>
        bn<T, N>
        bn<T, N>::operator <<(int rhs) const
        {
                bn<T, N>        n(*this);

                shift_left(rhs, n.m_num, N);

                return n;
        }

        template <typename T, int N>
        bn<T, N>
        bn<T, N>::operator >>(int rhs) const
        {
                bn<T, N>        n(*this);

                shift_right(rhs, n.m_num, N);

                return n;
        }

        template <typename T, int N>
        bn<T, N>
        bn<T, N>::operator <<=(int rhs)
        {
                shift_left(rhs, m_num, N);

                return *this;
        }

        template <typename T, int N>
        bn<T, N>
        bn<T, N>::operator >>=(int rhs)
        {
                shift_right(rhs, m_num, N);

                return *this;
        }

        template <typename T, int N>
        bool
        bn<T, N>::operator ==(const bn<T, N> &rhs) const
        {
                for (int i = 0; i < N; i++)
                        if (m_num[i] != rhs.m_num[i])
                                return false;

                return true;
        }

        template <typename T, int N>
        bool
        bn<T, N>::operator !=(const bn<T, N> &rhs) const
        {
                return !(*this == rhs);
        }

        template <typename T, int N>
        bool
        bn<T, N>::operator <=(const bn<T, N> &rhs) const
        {
                return !(*this > rhs);
        }

        template <typename T, int N>
        bool
        bn<T, N>::operator >=(const bn<T, N> &rhs) const
        {
                return !(*this < rhs);
        }

        template <typename T, int N>
        bool
        bn<T, N>::operator <(const bn<T, N> &rhs) const
        {
                for (int i = 0; i < N; i++)
                        if (m_num[i] < rhs.m_num[i])
                                return true;
                        else if (m_num[i] > rhs.m_num[i])
                                return false;

                return false;
        }

        template <typename T, int N>
        bool
        bn<T, N>::operator >(const bn<T, N> &rhs) const
        {
                for (int i = 0; i < N; i++)
                        if (m_num[i] > rhs.m_num[i])
                                return true;
                        else if (m_num[i] < rhs.m_num[i])
                                return false;

                return false;
        }

        template <typename T, int N>
        void
        bn<T, N>::shift_right(int bits, T arr[], int size) const
        {
                T               mask;
                int             n, m;
                int             i;

                if (bits < 0) {
                        shift_left(abs(bits), arr, size);
                        return;
                }

                n = bits / (sizeof(arr[0]) * 8);
                m = bits % (sizeof(arr[0]) * 8);

                for (i = size - 1; i >= n; i--)
                        arr[i] = arr[i - n];

                for (i = 0; i < n; i++)
                        arr[i] = 0;

                if (m == 0 || n >= size)
                        return;

                for (i = size - 1; i >= n + 1; i--) {
                        arr[i] >>= m;

                        mask = 1;
                        mask <<= m;
                        mask--;

                        arr[i] |= (mask & arr[i - 1]) <<
                                (sizeof(arr[0]) * 8 - m);
                }

                arr[n] >>= m;
        }

        template <typename T, int N>
        void
        bn<T, N>::shift_left(int bits, T arr[], int size) const
        {
                int             n, m;
                int             i;

                if (bits < 0) {
                        shift_right(abs(bits), arr, size);
                        return;
                }

                n = bits / (sizeof(arr[0]) * 8);
                m = bits % (sizeof(arr[0]) * 8);

                for (i = 0; i < size - n; i++)
                        arr[i] = arr[i + n];

                for (i = size - n; i < size; i++)
                        arr[i] = 0;

                if (m == 0 || n >= size)
                        return;

                for (i = 0; i < size - n; i++) {
                        T msb;

                        msb = arr[i + 1];
                        msb >>= sizeof(T) * 8 - m;

                        arr[i] <<= m;
                        arr[i] |= msb;
                }

                arr[i] <<= m;
        }

        template <typename T, int N>
        void
        bn<T, N>::fill_zero()
        {
                memset(m_num, 0, N * sizeof(T));
        }

        template <typename T, int N>
        void
        bn<T, N>::fill_max()
        {
                for (int i = 0; i < N; i++)
                        m_num[i] = ~(T)0;
        }

        template <typename T, int N>
        void
        bn<T, N>::to_binary(void *buf, int len) const
        {
                T              *num;
                int             i;

                num = (T*)buf;

                for (i = 0; len >= (int)sizeof(T); i++, len -= (int)sizeof(T)) {
                        if (sizeof(T) == 4) {
                                uint32_t n = (uint32_t)m_num[i];
                                num[i] = htonl(n);
                        } else if (sizeof(T) == 2) {
                                uint16_t n = (uint16_t)m_num[i];
                                num[i] = htons(n);
                        } else if (sizeof(T) == 1) {
                                num[i] = m_num[i];
                        }
                }
        }

        template <typename T, int N>
        void
        bn<T, N>::from_binary(const void *buf, int len)
        {
                const T        *num;
                int             i;

                num = (T*)buf;

                memset(m_num, 0, sizeof(m_num));

                for (i = 0; i < N && len >= (int)sizeof(T);
                     i++, len -= (int)sizeof(T))
                {
                        if (sizeof(T) == 4) {
                                uint32_t n = (uint32_t)num[i];
                                m_num[i] = ntohl(n);
                        } else if (sizeof(T) == 2) {
                                uint16_t n = (uint16_t)num[i];
                                m_num[i] = ntohs(n);
                        }else if (sizeof(T) == 1) {
                                m_num[i] = num[i];
                        }
                }
        }

        template <typename T, int N>
        bool
        bn<T, N>::is_zero() const
        {
                T               zero = 0;

                for (int i = 0; i < N; i++)
                        if (m_num[i] != zero)
                                return false;

                return true;
        }

        static const char *hexstr[16] = {"0", "1", "2", "3",
                                         "4", "5", "6", "7",
                                         "8", "9", "a", "b",
                                         "c", "d", "e", "f"};

        template <typename T, int N>
        std::string
        bn<T, N>::to_string() const
        {
                T               num[N];
                unsigned int    i;
                char           *p;
                std::string     str;

                to_binary((char*)num, sizeof(num));

                try {
                        str = "";
                        p = (char*)num;

                        for (i = 0; i < sizeof(T) * N; i++) {
                                uint8_t t1, t2;
                                t1 = (0x00f0 & (p[i])) >> 4;
                                t2 = 0x000f & (p[i]);
                                str += hexstr[t1];
                                str += hexstr[t2];
                        }
                } catch(std::exception &e) {
                        std::cerr << "get_string(): " << e.what() << std::endl;
                        return str;
                }

                return str;
        }

        template <typename T, int N>
        size_t
        bn<T, N>::hash_value() const
        {
                size_t h = 0;

                for (int i = 0; i < N; i++) {
                        boost::hash_combine(h, m_num[i]);
                }

                return h;

        }

#ifdef DEBUG
        template <typename T, int N>
        void
        bn<T, N>::dump() const
        {
                std::string     str;
                int             i;
                char           *p;

                str = "";
                p = (char*)m_num;

                std::cout << "0x";
                for (i = 0; i < sizeof(T) * N; i++) {
                        uint8_t t1, t2;
                        t1 = (0x00f0 & (p[i])) >> 4;
                        t2 = 0x000f & (p[i]);
                        str += hexstr[t1];
                        str += hexstr[t2];
                }
                std::cout << str << std::endl;
        }
#endif

        typedef bn<uint32_t, 4> uint128_t;
        typedef bn<uint32_t, 5> uint160_t;

        inline
        size_t
        hash_value(const uint128_t &num)
        {
                return num.hash_value();
        }

        inline
        size_t
        hash_value(const uint160_t &num)
        {
                return num.hash_value();
        }
}

#endif // BN_HPP