//Authors: Dario Cattaruzza, Alessandro Abate, Peter Schrammel, Daniel Kroening
//University of Oxford 2016
//This code is supplied under the BSD license agreement (see license.txt)

#include <math.h>

#include <Eigen/Eigenvalues>

#include <boost/timer.hpp>

#include "AbstractMatrix.h"

namespace abstract{

/// Constructs an empty buffer
template <class scalar>
AbstractMatrix<scalar>::AbstractMatrix(int dimension) : AccelMatrix<scalar>(dimension),
  //m_roundDimension(dimension),
  m_abstractDynamics(dimension),
  m_abstractInputDynamics(dimension),
  m_abstractRoundDynamics(dimension)
  {
    m_abstractDynamics.setName("Abstract Dynamics");
    m_abstractRoundDynamics.setName("Round Dynamics");
  }

/// Changes the default dimension of the system
template <class scalar>
void AbstractMatrix<scalar>::changeDimensions(const int dimension)
{
  AccelMatrix<scalar>::changeDimensions(dimension);
  m_abstractDynamics.changeDimension(dimension);
  m_abstractInputDynamics.changeDimension(dimension);
  m_abstractRoundDynamics.changeDimension(dimension);
}

template <class scalar>
void AbstractMatrix<scalar>::swapValues(scalar &low,scalar &high)
{
  scalar temp=low;
  low=high;
  high=temp;
}

template <class scalar>
inline void AbstractMatrix<scalar>::checkRange(const int row,const powerS iteration,const scalar mag,scalar &min,scalar &max)
{
  if (func::isPositive(min-max)) {
    scalar temp=min;
    min=max;
    max=temp;
  }
  if ((m_zeniths[row]>0) && (m_zeniths[row]<=iteration)) {
    scalar mid=condPow(mag,m_zeniths[row],row);
    if (func::isPositive(mid-max)) max=mid;
    if (m_zeniths[row]<iteration) {
      mid=condPow(mag,m_zeniths[row]+1,row);
      if (func::toLower(mid)>func::toUpper(max)) max=mid;
    }
  }
  if (m_freq[row]>0) {
    if (m_freq[row]<=iteration) {
    }
    else if (row>m_dimension) {
      if (func::toUpper(min)<-func::toLower(max)) min=-max;
      else max=-min;
    }
  }
}

template <class scalar>
typename JordanMatrix<scalar>::complexS AbstractMatrix<scalar>::condPow(const complexS &coef,const powerS n,int row)
{
  if (row>=m_dimension) {
    if (m_inputType==eParametricInputs) row-=m_dimension;
    else {
      scalar mag=ms_one-func::pow(func::norm2(coef),n);
      if (func::isPositive(mag)) {
        return complexS(mag*this->m_foldedBinomialMultipliers.coeff(row-m_dimension),0);
      }
    }
    if (m_jordanIndex[row]==0) {
      if (func::isZero(func::norm2(coef-ms_complexOne))) return complexS(n,0);
      return this->ms_complexOne-func::c_pow(coef,n);
    }
    if (n<m_jordanIndex[row]) return complexS(0,0);
    if (func::isZero(func::norm2(coef-ms_complexOne))) {
      if (n<=m_jordanIndex[row]) return complexS(0,0);
      complexS mult(binomial(n,m_jordanIndex[row]+1),0);
      return mult;
    }
    complexS mult(binomial(n,m_jordanIndex[row]),0);
    return -mult*func::c_pow(coef,n-m_jordanIndex[row]);
  }
  if (m_jordanIndex[row]==0) {
    if (n==1) return coef;
    return func::c_pow(coef,n);
  }
  if (n<m_jordanIndex[row]) return complexS(0,0);
  complexS mult(binomial(n,m_jordanIndex[row]),0);
  return mult*func::c_pow(coef,n-m_jordanIndex[row]);
}

template <class scalar>
scalar AbstractMatrix<scalar>::condPow(const scalar &coef,const powerS n,int row)
{
  if (row>=m_dimension) {
    if (m_inputType==eParametricInputs) row-=m_dimension;
    else {
      scalar mag=ms_one-func::pow(coef,n);
      if (func::isPositive(mag)) {
        return mag*this->m_foldedBinomialMultipliers.coeff(row-m_dimension);
      }
    }
    if (m_jordanIndex[row]==0) {
      if (abs(coef-ms_one)<m_zero) return scalar(n);
      return ms_one-func::pow(coef,n);
    }
    if (n<m_jordanIndex[row])  return 0;
    if (abs(coef-ms_one)<m_zero) {
      if (n<=m_jordanIndex[row])  return 0;
      return binomial(n,m_jordanIndex[row]+1);
    }
    return -binomial(n,m_jordanIndex[row])*func::pow(coef,n-m_jordanIndex[row]);
  }

  if (m_jordanIndex[row]==0) return func::pow(coef,n);
  if (n<m_jordanIndex[row])  return 0;
  return binomial(n,m_jordanIndex[row])*func::pow(coef,n-m_jordanIndex[row]);
}

template <class scalar>
scalar AbstractMatrix<scalar>::diffPow(const scalar &coef,const powerS n,int row)
{
  if (row>=m_dimension) {
    if (m_inputType==eParametricInputs) row-=m_dimension;
    else {
      scalar mag=ms_one-func::pow(coef,n);
      if (func::isPositive(mag)) {
        mag=func::pow(coef,n)-func::pow(coef,n+1);
        return mag*this->m_foldedBinomialMultipliers.coeff(row-m_dimension);
      }
    }
    if (m_jordanIndex[row]==0) {
      if (abs(coef-ms_one)<m_zero) return ms_one;
      return func::pow(coef,n)-func::pow(coef,n+1);
    }
    if (n<m_jordanIndex[row])  return 0;
    if (abs(coef-ms_one)<m_zero) {
      if (n<m_jordanIndex[row])  return 0;
      return binomial(n,m_jordanIndex[row]);
    }
    return (binomial(n,m_jordanIndex[row])-binomial(n+1,m_jordanIndex[row])*coef)*func::pow(coef,n-m_jordanIndex[row]);
  }
  if (m_jordanIndex[row]==0) return (coef-ms_one)*func::pow(coef,n);
  if (n<m_jordanIndex[row])  return 0;
  return (binomial(n+1,m_jordanIndex[row])*coef-binomial(n,m_jordanIndex[row]))*func::pow(coef,n-m_jordanIndex[row]);
}

/// Calculates the expected iteration of coefficient to obtain value
template <class scalar>
typename AbstractMatrix<scalar>::powerS AbstractMatrix<scalar>::condLog(const refScalar &coef,refScalar value,int row,powerS &range)
{
  range=0;
  if (row>=m_dimension) {
    if (m_inputType==eParametricInputs) row-=m_dimension;
    else {
      if (func::isPositive(value)) value/=func::toCentre(this->m_foldedBinomialMultipliers.coeff(row-m_dimension));
      value=func::ms_1-value;
      if (func::isNegative(value)) return 0;
      return func::toInt(func::log(coef,value));
    }
    if (m_jordanIndex[row]==0) {
      if (abs(coef-ms_one)<m_zero) return func::toInt(value);
      value=func::ms_1-value;
      if (func::isNegative(value)) return 0;
      return func::toInt(func::log(coef,value));
    }
    range=m_jordanIndex[row]-1;
    if (func::isZero(value)) return 1;
    if (abs(coef-ms_one)<m_zero) {
      for (int i=2;i<=m_jordanIndex[row]+1;i++) value*=i;
      refScalar expected=pow(value,1/m_jordanIndex[row]+1);
      return func::toInt(expected);
    }
    value=-value;
    return binomialCondLog(coef,value,row,range);
  }
  if (m_jordanIndex[row]==0) return func::toInt(func::log(coef,value));
  range=m_jordanIndex[row]-1;
  if (func::isZero(value)) return 1;
  return binomialCondLog(coef,value,row,range);
}

/// Calculates the expected iteration of coefficient to obtain value for a jordan diagonal
template <class scalar>
typename AbstractMatrix<scalar>::powerS AbstractMatrix<scalar>::binomialCondLog(const refScalar &coef,refScalar value,int row,powerS &range)
{
  for (int i=2;i<=m_jordanIndex[row];i++) value*=i;
  value/=func::pow(coef,m_jordanIndex[row]);
  powerS low,high;
  if (coef>func::ms_1) {
    high=func::toInt(func::log(coef,value));
    refScalar bin=func::toCentre(binomial(high,m_jordanIndex[row]));
    low=func::toInt(func::log(coef,value/bin));
  }
  else {
    low=func::toInt(pow(value,1/m_jordanIndex[row]));
    refScalar bin=func::toCentre(binomial(low,m_jordanIndex[row]));
    high=func::toInt(func::log(coef,value/bin));
  }
  range=high-low;
  return low;
}

template <class scalar>
void AbstractMatrix<scalar>::fillDirection(const int row,const scalar &min,const scalar &max)
{
  m_directions.coeffRef(row,m_pos)=1;
  m_supports.coeffRef(m_pos++,0)=max;
  m_directions.coeffRef(row,m_pos)=-1;
  m_supports.coeffRef(m_pos++,0)=-min;
}

template <class scalar>
void AbstractMatrix<scalar>::fillDirections(const int row1,const int row2,const scalar &dir1,const scalar &dir2,const scalar &min,const scalar &max)
{
  m_supports.coeffRef(m_pos,0)=-min;
  m_directions.coeffRef(row1,m_pos)=-dir1;
  m_directions.coeffRef(row2,m_pos++)=-dir2;
  m_supports.coeffRef(m_pos,0)=max;
  m_directions.coeffRef(row1,m_pos)=dir1;
  m_directions.coeffRef(row2,m_pos++)=dir2;
}

template <class scalar>
void AbstractMatrix<scalar>::fillConjugateSupport(const int row1,const int row2,const scalar &angle,const scalar &max)
{
  m_directions.coeffRef(row1,m_pos)=func::cosine(angle);
  m_directions.coeffRef(row2,m_pos)=func::sine(angle);
  m_supports.coeffRef(m_pos++)=max;
}

template <class scalar>
void AbstractMatrix<scalar>::fillSupportFromPoints(const int row1,const int row2,const scalar &x1,const scalar &y1,const scalar &x2,const scalar &y2,const scalar &xRef,const scalar &yRef)
{
  scalar angle=func::invtan(y2-y1,x2-x1)+func::const_pi(this->ms_half);
  scalar max=y1*func::sine(angle)+x1*func::cosine(angle);
  scalar out=yRef*func::sine(angle)+xRef*func::cosine(angle);
  out-=max;
  if (func::isPositive(out)) {
    angle+=func::const_pi(ms_one);
    max=-max;
  }
  fillConjugateSupport(row1,row2,angle,max);
}

template <class scalar>
void AbstractMatrix<scalar>::fillTangentSupport(const int row1,const int row2,const scalar &mag1,const scalar &mag2,powerS iter)
{
  scalar angle=func::const_pi(this->ms_half);
  scalar quotient=mag2/mag1;
  scalar slope=iter;
  slope*=func::pow(quotient,iter-1);
  char sign=func::hardSign(mag1);
  if (sign==0) return;
  if (sign<0)  angle+=atan(slope)+func::ms_pi;
  else         angle+=atan(slope);
  scalar max=func::pow(mag2,iter)*func::sine(angle)+func::pow(mag1,iter)*func::cosine(angle);
  fillConjugateSupport(row1,row2,angle,max);
}

template <class scalar>
void AbstractMatrix<scalar>::fillSupportFromIter(const int row1,const int row2,const scalar &mag1,const scalar &mag2,powerS iter)
{
  scalar xdif=diffPow(mag1,iter,row1);
  scalar ydif=diffPow(mag2,iter,row2);
  scalar angle=func::invtan(ydif,xdif)+func::const_pi(this->ms_half);
  scalar x1=condPow(mag1,iter,row1);
  scalar y1=condPow(mag2,iter,row2);
  scalar max=y1*func::sine(angle)+x1*func::cosine(angle);
  scalar out=mag2*func::sine(angle)+mag1*func::cosine(angle);
  out-=max;
  if (func::isPositive(out)) {
    angle+=func::const_pi(ms_one);
    max=-max;
  }
  fillConjugateSupport(row1,row2,angle,max);
}

template <class scalar>
void AbstractMatrix<scalar>::fillSemiConjugateSupportFromIter(const int row1,const int row2,const scalar &mag1,const scalar &mag2,powerS iter)
{
  powerS iter2=iter+1;
  scalar x1=condPow(mag1,iter,row1);
  scalar y1=condPow(mag2,iter,row2);
  scalar x2=condPow(mag1,iter2,row1);
  scalar y2=condPow(mag2,iter2,row2);
  fillSupportFromPoints(row1,row2,x1,y1,x2,y2,mag1,mag2);
  fillSupportFromPoints(row1,row2,-x1,y1,-x2,y2,-mag1,mag2);
}

template <class scalar>
bool AbstractMatrix<scalar>::fillLastConjugateSupportFromPoints(int row1,int row2,scalar x1,scalar y1,scalar x2,scalar y2,scalar xRef,scalar yRef,bool hasLastQuarter)
{
  scalar angle=func::invtan(y2-y1,x2-x1)+func::const_pi(this->ms_half);
  scalar max=y1*func::sine(angle)+x1*func::cosine(angle);
  scalar out=yRef*func::sine(angle)+xRef*func::cosine(angle);
  char sign=func::hardSign(max);
  if (sign<0) {//the vector is in the wrong direction rotate 180deg
    angle+=func::const_pi(ms_one);
    max=-max;
    out=-out;
  }
  else if (sign==0) {// Undecided, overapproximate by error.
    max+=m_zero;
    out+=m_zero;
  }
  out-=max;
  sign=func::hardSign(out);
  if (hasLastQuarter) {
    //If the next to last point would be excluded by the restriction, ignore
    if (sign>0) return false;
    //If the next to last point is undecided around the restriction, make sure it remains inside
    if (sign==0) max+=m_zero;
  }
  else if (sign>0) {
    //Revert the bound to close the half-circle
    angle+=func::const_pi(ms_one);
    max=y1*func::sine(angle)+x1*func::cosine(angle);
  }
  else if (sign==0) {
    //should never happen
    func::imprecise(out,func::ms_hardZero);
    return false;
  }
  else {
    //should never happen but no error
    return false;
  }
  fillConjugateSupport(row1,row2,angle,max);
  return true;
}

template <class scalar>
bool AbstractMatrix<scalar>::testConjugateSupportFromPoints(const scalar &x1,const scalar &y1,const scalar &x2,const scalar &y2,const scalar &xExt,const scalar &yExt)
{
  scalar angle=func::invtan(y2-y1,x2-x1)+func::const_pi(this->ms_half);
  scalar max=y1*func::sine(angle)+x1*func::cosine(angle);
  scalar ext=yExt*func::sine(angle)+xExt*func::cosine(angle);
  char sign=func::hardSign(max);
  if (sign<0) {
    angle+=func::const_pi(ms_one);
    max=-max;
    ext=-ext;
  }
  else if (sign==0) {
    //should never happen
    func::imprecise(max,func::ms_hardZero);
  }
  ext-=max;
  sign=func::hardSign(ext);
  if (sign>0) return false;
  //if ext is zero, the case is dealt when calculating the support function
  return true;
}

template <class scalar>
bool AbstractMatrix<scalar>::fillConjugateSupportFromPoints(const int row1,const int row2,const scalar &x1,const scalar &y1,const scalar &x2,const scalar &y2,const scalar &xExt,const scalar &yExt)
{
  scalar angle=func::invtan(y2-y1,x2-x1)+func::const_pi(this->ms_half);
  scalar max=y1*func::sine(angle)+x1*func::cosine(angle);
  scalar ext=yExt*func::sine(angle)+xExt*func::cosine(angle);
  char sign=func::hardSign(max);
  if (sign<0) {
    angle+=func::const_pi(ms_one);
    max=-max;
    ext=-ext;
  }
  else if (sign==0) {
    //should never happen
    func::imprecise(max,func::ms_hardZero);
  }
  ext-=max;
  sign=func::hardSign(ext);
  if (sign>0) return false;
  if (sign==0) max+=m_zero;
  fillConjugateSupport(row1,row2,angle,max);
  return true;
}

template <class scalar>//template <>
void AbstractMatrix<scalar>::fillLinearSupportFromPoints(const int row1,const int row2,const scalar &min1,const scalar &max1,const scalar &min2,const scalar &max2)
{
  scalar angle=func::invtan(max2-min2,max1-min1)+func::const_pi(this->ms_half);
  scalar min=min2*func::sine(angle)+min1*func::cosine(angle);
  scalar max=max2*func::sine(angle)+max1*func::cosine(angle);
  max=func::getHull(min,max);
  min=-func::toLower(max)+this->m_largeZero;
  max=func::toUpper(max)+this->m_largeZero;
  fillConjugateSupport(row1,row2,angle,max);
  angle+=func::const_pi(ms_one);
  fillConjugateSupport(row1,row2,angle,min);
}

template <class scalar>
void AbstractMatrix<scalar>::fillQuadraticSupport(int row1,int row2,scalar mag1,scalar mag2,powerS iteration,int precision)
{
  precision=(1<<precision)-2;//Number of faces grows geometrically
  if (precision<1) return;
  refScalar iterationStep=iteration/precision;
  if (iterationStep<1) {
    iterationStep=1;
    precision=iteration;
  }
  int firstIter=1;
  if (m_jordanIndex[row1%m_dimension]>0) firstIter=m_jordanIndex[row1%m_dimension];
  if (m_jordanIndex[row2%m_dimension]>0) firstIter=m_jordanIndex[row2%m_dimension];
  for (int i=firstIter;i<precision;i++) {
    powerS iter1=func::toInt(floor(i*iterationStep));
    if (iter1==1) {
      powerS iter2=iter1+1;
      scalar x1=condPow(mag1,iter1,row1);
      scalar y1=condPow(mag2,iter1,row2);
      scalar x2=condPow(mag1,iter2,row1);
      scalar y2=condPow(mag2,iter2,row2);
      scalar x3=condPow(mag1,3,row1);
      scalar y3=condPow(mag2,3,row2);
      fillSupportFromPoints(row1,row2,x1,y1,x2,y2,x3,y3);
    }
    else fillSupportFromIter(row1,row2,mag1,mag2,iter1);
  //    else fillSupportFromPoints(row1,row2,x1,y1,x2,y2,mag1,mag2);
  }
}

template <class scalar>
void AbstractMatrix<scalar>::fillQuadraticConjugateSupport(int row1,int row2,scalar mag1,scalar mag2,powerS iteration,int precision)
{
  scalar x1=condPow(mag1,1,row1);
  scalar y1=condPow(mag2,1,row2);
  scalar x2=condPow(mag1,iteration,row1);
  scalar y2=condPow(mag2,iteration,row2);
  scalar x3=condPow(mag1,2,row1);
  scalar y3=condPow(mag2,2,row2);
  scalar angle=func::invtan(y2-y1,x2-x1)-func::const_pi(this->ms_half);//invtan returns positive values (0-pi)
  scalar max=y1*func::sine(angle)+x1*func::cosine(angle);
  scalar out=y3*func::sine(angle)+x3*func::cosine(angle);
  max-=out;
  if (func::isNegative(max)) {//TODO: need to split for jordan blocks
    precision=(1<<precision)-2;//Number of faces grows geometrically
    if (precision<1) return;
    scalar mag=condPow(mag1,iteration,row1);
    while(func::isZero(mag)) {
      iteration>>=2;
      mag=condPow(mag1,iteration,row1);
    }
    refScalar iterationStep=iteration/precision;
    if (iterationStep<1) {
      iterationStep=1;
      precision=iteration;
    }
    for (int i=1;i<precision;i++) {
      powerS iter1=func::toInt(floor(i*iterationStep));
      powerS iter2=iter1+1;
      scalar x1=condPow(mag1,iter1,row1);
      scalar y1=condPow(mag2,iter1,row2);
      scalar x2=condPow(mag1,iter2,row1);
      scalar y2=condPow(mag2,iter2,row2);
      if (iter1==1) {
        scalar x3=condPow(mag1,3,row1);
        scalar y3=condPow(mag2,3,row2);
        fillSupportFromPoints(row1,row2,x1,y1,x2,y2,x3,y3);
        fillSupportFromPoints(row1,row2,-x1,y1,-x2,y2,-x3,y3);
      }
      else {
        fillSupportFromPoints(row1,row2,x1,y1,x2,y2,mag1,mag2);
        fillSupportFromPoints(row1,row2,-x1,y1,-x2,y2,-mag1,mag2);
      }
    }
    return;
  }
  fillSupportFromPoints(row1,row2,x1,y1,x2,y2,0,y1);
  fillSupportFromPoints(row1,row2,-x1,y1,-x2,y2,0,y1);
}

template <class scalar>
void AbstractMatrix<scalar>::findConjugateSupports(const int row,const complexS &coef,const powerS iteration,int precision)
{
  if (m_conjugatePair[row]<row) return;
  precision=(1<<precision);//Number of faces grows geometrically
  int row2=row+1;
  if (iteration<=2) {
    complexS p1=condPow(coef,1,row);
    scalar minR=p1.real();
    scalar minI=p1.imag();
    if (iteration==2) {
      complexS p2=condPow(coef,2,row);
      scalar maxR=p2.real();
      scalar maxI=p2.imag();
      fillLinearSupportFromPoints(row,row2,minR,maxR,minI,maxI);
      if (func::isPositive(minR-maxR)) swapValues(minR,maxR);
      if (func::isPositive(minI-maxI)) swapValues(minI,maxI);
      fillDirection(row,minR,maxR);
      fillDirection(row2,minI,maxI);
    }
    else {
      fillDirection(row,minR,minR);
      fillDirection(row2,minI,minI);
    }
    return;
  }
  scalar mag=func::norm2(coef);
  scalar angle=func::invtan(coef.imag(),coef.real());
  scalar quartFreq=abs(func::const_pi(this->ms_half)/angle);
  powerS offset=func::toInt(trunc(4*func::toLower(quartFreq)));
  offset+=m_jordanIndex[row]+1;
  powerS start=(func::isPositive(mag-ms_one)) ? iteration : 1;
  powerS end=func::isPositive(mag-ms_one) ? ((iteration>offset) ? iteration-offset : 0) : ((iteration>offset) ? offset : iteration);
  int dir=((end-start)>0) ? 1 : -1;
  powerS step=(end-start)/precision;
  if (step==0) step=dir;
  complexS ext=condPow(coef,start+2*dir,row);
  complexS p1=condPow(coef,start,row);
  complexS p2=condPow(coef,start+dir,row);
  fillConjugateSupportFromPoints(row,row2,p1.real(),p1.imag(),p2.real(),p2.imag(),ext.real(),ext.imag());
  start+=step;
  ext=condPow(coef,func::isPositive(mag-ms_one) ? iteration : 1,row2);
  while (abs(end-start)>=abs(step)) {
    p1=condPow(coef,start,row);
    p2=condPow(coef,start+dir,row);
    if (!fillConjugateSupportFromPoints(row,row2,p1.real(),p1.imag(),p2.real(),p2.imag(),ext.real(),ext.imag())) break;
    start+=step;
  }
  if (abs(end-start)<abs(step)) {
    p1=condPow(coef,end,row);
    p2=condPow(coef,end-dir,row);
    if ((abs(end-start)<=abs(dir)) || fillConjugateSupportFromPoints(row,row2,p1.real(),p1.imag(),p2.real(),p2.imag(),ext.real(),ext.imag())) {
      start=end;
      p1=condPow(coef,start,row);
      p2=ext;
      ext=condPow(coef,start-dir,row);
      if (fillLastConjugateSupportFromPoints(row,row2,p1.real(),p1.imag(),p2.real(),p2.imag(),ext.real(),ext.imag(),iteration>func::toInt(trunc(3*func::toLower(quartFreq))))) return;
      ext=p2;
    }
  }
  powerS base=start-step;
  end=start;
  while (abs(end-base)>1) {
    start=(base+end)/2;
    p1=condPow(coef,start,row);
    p2=condPow(coef,start+dir,row);
    if (testConjugateSupportFromPoints(p1.real(),p1.imag(),p2.real(),p2.imag(),ext.real(),ext.imag())) {
      base=start;
    }
    else {
      end=start;
    }
  }
  p1=condPow(coef,base,row);
  p2=condPow(coef,base-dir,row);
  if (testConjugateSupportFromPoints(p1.real(),p1.imag(),p2.real(),p2.imag(),ext.real(),ext.imag())) base+=dir;
  p1=condPow(coef,base,row);
  fillSupportFromPoints(row,row2,p1.real(),p1.imag(),ext.real(),ext.imag(),0,0);
}

template <class scalar>
void AbstractMatrix<scalar>::wrapUp()
{
  m_directions.conservativeResize(m_directions.rows(),m_pos);
  m_supports.conservativeResize(m_pos,1);
  m_pos=0;
}

/// Finds the point at which each jordan column is maximum
template <class scalar>
void AbstractMatrix<scalar>::findZeniths()
{
  //y_n=binomial(n,k)x^n-k = y_{n-1}*nx/(n-k) -> zenith=k/(1-x)
  m_zeniths.resize(2*m_dimension);
  for (int row=0;row<m_dimension;row++) {
    scalar mag=func::norm2(m_eigenValues.coeff(row,row));
    if ((m_jordanIndex[row]>0) && (func::isNegative(mag-ms_one))) {
      scalar zenith=m_jordanIndex[row];
      zenith/=(ms_one-mag);
      m_zeniths[row]=func::toInt(func::toLower(zenith));
    }
    else m_zeniths[row]=0;
  }
  for (int row=m_dimension;row<2*m_dimension;row++) m_zeniths[row]=0;
}

/// lists the first source row for a folded set of dimensions
template <class scalar>
void AbstractMatrix<scalar>::findUnfolded()
{
  m_unfolded.resize(m_dimension);
  int pos=0;
  for (int i=0;i<m_dimension;i++) {
    if (m_jordanIndex[i]>0) continue;
    if (m_conjugatePair[i]>=0) {
      m_unfolded[pos++]=i;
      i++;
    }
    else if (m_jordanIndex[i+1]>0) m_unfolded[pos++]=i;
  }
  m_unfolded.resize(pos);
}

/// Marks the round indices in a rounded vector array
template <class scalar>
void AbstractMatrix<scalar>::findRoundIndices(std::vector<bool> &isRoundIndex)
{
  int pos=0,mult;
  isRoundIndex.resize(m_dimension);
  for (int row=0;row<m_dimension;row+=mult,pos++) {
    mult=(m_conjugatePair[row]<0) ? 1 : 2;
    if (m_jordanIndex[row+mult]==0) isRoundIndex[pos]=(m_conjugatePair[row]>=0);
    else isRoundIndex[pos]=(m_jordanIndex[row+mult]!=0);
    while (m_jordanIndex[row+mult]!=0) row+=mult;
  }
  isRoundIndex.resize(pos);
}

/// Finds the frequency of rotation of each conjugate pair
template <class scalar>
void AbstractMatrix<scalar>::findFrequencies()
{
  m_freq.assign(2*m_dimension,0);
  for (int row=0;row<m_dimension;row++) {
    if (m_conjugatePair[row]>row) {
      complexS coef=m_eigenValues.coeff(row,row);
      scalar angle=func::invtan(coef.imag(),coef.real());
      m_freq[row++]=func::toInt(2*func::toUpper(abs(func::const_pi(ms_one)/angle)));
      m_freq[row]=m_freq[row-1];
    }
  }
}

/// Indicates if the matrix dynamics are divergent
/// @param strict if true returns only true if no eigenvalues are convergent
template <class scalar>
bool AbstractMatrix<scalar>::isDivergent(const bool strict)
{
  for (int i=0;i<m_dimension;i++) {
    scalar eigenNorm=func::norm2(m_eigenValues.coeff(i,i));
    char sign=func::hardSign(eigenNorm-ms_one);
    if (strict && (sign<0)) return false;
    else if (!strict && (sign>=0)) return true;
  }
  return strict;
}

/// Finds the coefficient, magnitude, maximum, and minimum for the abstract vector at row
template <class scalar>
void AbstractMatrix<scalar>::findCoeffBounds(int row,powerS iteration,complexS &coef,scalar &mag,scalar &min,scalar &max)
{
  if (row<m_dimension) coef=m_eigenValues.coeff(row,row);
  else {
    int transRow=(m_inputType==eVariableInputs) ? m_unfolded[row-m_dimension] : row-m_dimension;
    coef=m_eigenValues.coeff(transRow,transRow);
    if ((m_inputType==eVariableInputs) && (func::isPositive(func::norm2(coef)-ms_one))) {
      coef=m_foldedEigenValues(transRow,transRow);
    }
  }
  if ((row>=m_dimension) && (m_inputType==eVariableInputs)) {
    scalar rad=func::norm2(coef);
    mag=condPow(rad,1,row);
    max=condPow(rad,iteration,row);
  }
  else if (m_conjugatePair[row]<0) {
    mag=condPow(coef.real(),1,row);
    max=condPow(coef.real(),iteration,row);
  }
  else if (iteration>2) {
    mag=func::norm2(condPow(coef,1,row));
    max=func::norm2(condPow(coef,iteration,row));
  }
  else if (m_conjugatePair[row]>row) {
    mag=condPow(coef,1,row).real();
    max=condPow(coef,iteration,row).real();
  }
  else {
    mag=-condPow(coef,1,row).imag();
    max=-condPow(coef,iteration,row).imag();
  }
  min=mag;
  if ((m_conjugatePair[row]<0) && ((row<m_dimension) || (m_inputType!=eVariableInputs)) && (func::isNegative(coef.real()))) {
    scalar sum=min+max;
    char sign=func::hardSign(sum);
    if (sign>0) min=-max;
    else if (sign<0) max=-min;
  }
}

/// Retrieves the last calculated dynamics
template <class scalar>
AbstractPolyhedra<scalar>& AbstractMatrix<scalar>::getAbstractDynamics(const inputType_t inputType)
{
  if (inputType==eVariableInputs) return m_abstractRoundDynamics;
  if (this->m_hasOnes && inputType==eParametricInputs) return m_abstractInputDynamics;
  return m_abstractDynamics;
}

/// retrieves the abstract dynamics matrix for a given iteration (n)
template <class scalar>
AbstractPolyhedra<scalar>& AbstractMatrix<scalar>::getAbstractDynamics(const powerS iteration,int precision,const inputType_t inputType,const bool normalised)
{
  boost::timer timer;
  if (precision<1) precision=1;
  int dimension=m_dimension;
  m_inputType=inputType;
  if (inputType==eVariableInputs) dimension+=m_foldedEigenValues.rows();
  else if (this->m_hasOnes && (inputType==eParametricInputs)) dimension+=m_dimension;
  m_maxIterations=iteration;
  if (m_hasMultiplicities) this->calculateBoundedEigenError(calculateMaxIterations(iteration));
  findUnfolded();
  findZeniths();
  findFrequencies();
  AbstractPolyhedra<scalar>& result=getAbstractDynamics(inputType);
  result.changeDimension(dimension);
  m_supports.resize((1<<precision)*dimension*dimension,1);
  m_directions=MatrixS::Zero(dimension,(1<<precision)*dimension*dimension);
  m_pos=0;
  complexS coef,coef2;
  for (int row=0;row<dimension;row++)
  {
    scalar mag,min1,max1;
    findCoeffBounds(row,iteration,coef,mag,min1,max1);
    scalar min=min1;
    scalar max=max1;
    checkRange(row,iteration,func::norm2(coef),min,max);
    if (m_conjugatePair[row]>=0) findConjugateSupports(row,coef,iteration,precision);
    else {
      if (func::isNegative(coef.real())) {
        scalar sum=min+max;
        char sign=func::hardSign(sum);
        if (sign>0) min=-max;
        else if (sign<0) max=-min;
      }
      fillDirection(row,min,max);
    }
    if ((iteration<2) || (precision<2)) continue;
    for (int row2=row+((m_conjugatePair[row]>row) ? 2 : 1);row2<dimension;row2++) {
      if (row2<m_dimension) coef2=m_eigenValues.coeff(row2,row2);
      else {
        int transRow2=(inputType==eVariableInputs) ? m_unfolded[row2-m_dimension]: row2-m_dimension;
        coef2=m_eigenValues.coeff(transRow2,transRow2);//TODO: the rotations and dilations are wrong at the row value
        if ((inputType==eVariableInputs) && (func::isPositive(func::norm2(coef2)-ms_one))) {
          coef=m_foldedEigenValues(transRow2,transRow2);
        }
      }
      scalar mag2,min2,max2;
      findCoeffBounds(row2,iteration,coef2,mag2,min2,max2);
      if (m_conjugatePair[row]>=0) {
        if (m_conjugatePair[row2]<0) {
          fillQuadraticConjugateSupport(row,row2,func::norm2(coef),func::norm2(coef2),iteration,precision);
        }
      }
      else if (m_conjugatePair[row2]>=0) {
        fillQuadraticConjugateSupport(row2,row,func::norm2(coef2),func::norm2(coef),iteration,precision);
      }
      else if ((iteration==2) || func::isZero(max1-max2)) {
        fillLinearSupportFromPoints(row,row2,min1,max1,min2,max2);
      }
      else {
        scalar out1=condPow(min1,2,row);
        scalar out2=condPow(min2,2,row2);
        fillSupportFromPoints(row,row2,min1,min2,max1,max2,out1,out2);
        fillQuadraticSupport(row,row2,func::norm2(coef),func::norm2(coef2),iteration,precision);
      }
    }
  }
  wrapUp();
  if (m_hasMultiplicities) {
    scalar high=func::pow(ms_one+m_error,iteration);
    scalar low=func::pow(ms_one-m_error,iteration);
    scalar relaxation=func::getHull(high,low);
    m_supports*=relaxation;
  }
  result.load(m_directions,m_supports,true);
  if (normalised) result.normalise();
  result.setCalculationTime(timer.elapsed()*1000);
  if (ms_trace_dynamics>=eTraceAbstraction) {
    std::stringstream stream;
    stream << "s=" << iteration << ",l=" << precision;
    result.logTableau(stream.str());
  }
  return result;
}

/// Adds a set of supports at the given iteration in order to refine the abstraction
template <class scalar>
bool AbstractMatrix<scalar>::addSupportsAtIteration(AbstractPolyhedra<scalar>& dynamics,powerS iteration,powerS max)
{
  boost::timer timer;
  complexS coef,coef2;
  int dimension=m_dimension;
  if (m_inputType==eVariableInputs) dimension+=m_foldedEigenValues.rows();
  else if (this->m_hasOnes && (m_inputType==eParametricInputs)) dimension+=m_dimension;

  m_supports.resize(dimension*dimension,1);
  m_directions=MatrixS::Zero(dimension,dimension*dimension);
  m_pos=0;
  for (int row=0;row<dimension;row++)
  {
    if (row<m_dimension) coef=m_eigenValues.coeff(row,row);
    else {
      int transRow=(m_inputType==eVariableInputs) ? m_unfolded[row-m_dimension] : row-m_dimension;
      coef=m_eigenValues.coeff(transRow,transRow);
      if ((m_inputType==eVariableInputs) && (func::isPositive(func::norm2(coef)-ms_one))) {
        coef=m_foldedEigenValues(transRow,transRow);
      }
    }
    if (m_conjugatePair[row]>row) {
      scalar mag=func::norm2(coef);
      int dir=(func::isPositive(mag-ms_one)) ? -1 : 1;
      complexS ext=condPow(coef,func::isPositive(mag-ms_one) ? max : 1,row+1);
      complexS p1=condPow(coef,iteration,row);
      complexS p2=condPow(coef,iteration+dir,row);
      fillConjugateSupportFromPoints(row,row+1,p1.real(),p1.imag(),p2.real(),p2.imag(),ext.real(),ext.imag());
    }
    for (int row2=row+((m_conjugatePair[row]>row) ? 2 : 1);row2<dimension;row2++) {
        if (row2<m_dimension) coef2=m_eigenValues.coeff(row2,row2);
        else {
          int transRow2=(m_inputType==eVariableInputs) ? m_unfolded[row2-m_dimension]: row2-m_dimension;
          coef2=m_eigenValues.coeff(transRow2,transRow2);//TODO: the rotations and dilations are wrong at the row value
          if ((m_inputType==eVariableInputs) && (func::isPositive(func::norm2(coef2)-ms_one))) {
            coef2=m_foldedEigenValues(transRow2,transRow2);
          }
        }
        if (m_conjugatePair[row]>=0) {
          if (m_conjugatePair[row2]<0) {
            fillSemiConjugateSupportFromIter(row,row2,func::norm2(coef),func::norm2(coef2),iteration);
          }
        }
        else if (m_conjugatePair[row2]>=0) {
          fillSemiConjugateSupportFromIter(row2,row,func::norm2(coef2),func::norm2(coef),iteration);
        }
        else {
          fillSupportFromIter(row,row2,func::norm2(coef),func::norm2(coef2),iteration);
        }
      }
  }
  wrapUp();
  if (m_hasMultiplicities) {
    scalar high=func::pow(ms_one+m_error,iteration);
    scalar low=func::pow(ms_one-m_error,iteration);
    scalar relaxation=func::getHull(high,low);
    m_supports*=relaxation;
  }
  dynamics.addDirection(m_directions,m_supports);
  dynamics.addCalculationTime(timer.elapsed()*1000);
  if (ms_trace_dynamics>=eTraceAbstraction) {
    std::stringstream stream;
    stream << "abs supports n=" << iteration;
    dynamics.logTableau(stream.str());
  }
  return true;
}

/// Calculates the number of iterations necessary to reacha fixpoint
template <class scalar>
typename AbstractMatrix<scalar>::powerS AbstractMatrix<scalar>::calculateMaxIterations(powerS max)
{
  powerS result=0;
  findZeniths();
  findFrequencies();
  for (int i=0;i<m_dimension;i++) {
    complexS coef=m_eigenValues.coeff(i,i);
    if (func::isPositive(func::norm2(coef)-ms_one)) return (max<func::ms_infPower) ? max: func::ms_infPower;
    if (m_zeniths[i]+m_freq[i]>result) result=m_zeniths[i]+m_freq[i];
  }
  return (max<result) ? max : result;
}

/// Finds the corresponding iteration that generates dynamics close to the given point
template <class scalar>
void AbstractMatrix<scalar>::findIterations(MatrixS &point,powerList &iterations)
{
  complexS coef;
  powerS range;
  for (int col=0;col<point.cols();col++) {
    if (col<m_dimension) coef=m_eigenValues.coeff(col,col);
    else {
      int transCol=(m_inputType==eVariableInputs) ? m_unfolded[col-m_dimension] : col-m_dimension;
      coef=m_eigenValues.coeff(transCol,transCol);
      if ((m_inputType==eVariableInputs) && (func::isPositive(func::norm2(coef)-ms_one))) {
        coef=m_foldedEigenValues(transCol,transCol);
      }
    }
    refScalar rad=func::toCentre(func::norm2(coef));
    powerS iteration;
    if (m_conjugatePair[col]>=0) {
      scalar coeffAngle=func::invtan(coef.imag(),coef.real());
      scalar angle=func::invtan(point.coeff(0,col+1),point.coeff(0,col));
      char dir=func::hardSign(coeffAngle);
      char sign=func::hardSign(angle);
      if (sign!=dir) {
        if (sign<0) angle+=func::ms_2_pi;
        else        angle-=func::ms_2_pi;
      }
      refScalar finalSteps=func::toCentre(angle/coeffAngle);
      iteration=func::toInt(finalSteps);
      if (rad>func::ms_1) {
        refScalar tau=func::toCentre(coeffAngle/func::ms_2_pi);
        tau*=m_maxIterations;
        powerS mult=func::toInt(tau);
        mult*=m_freq[col];
        iteration+=mult;
      }
      col++;
    }
    else if ((rad>func::ms_1) || (!func::isZero(point.coeff(0,col)))) {
      iteration=condLog(rad,func::toCentre(point.coeff(0,col)),col,range);
    }
    else {
      iteration=0;
      range=0;
    }
    while ((iteration<2) && (range>0)) {
      range--;
      iteration++;
    }
    if (iteration>1) iterations[iteration]=range;//TODO: should merge when there is equality
  }
}

#ifdef USE_LDOUBLE
  #ifdef USE_SINGLES
    template class AbstractMatrix<long double>;
  #endif
  #ifdef USE_INTERVALS
    template class AbstractMatrix<ldinterval>;
  #endif
#endif
#ifdef USE_MPREAL
  #ifdef USE_SINGLES
    template class AbstractMatrix<mpfr::mpreal>;
  #endif
  #ifdef USE_INTERVALS
    template class AbstractMatrix<mpinterval>;
  #endif
#endif

}


/*
agnanowire_modeling_v1.cpp
(Based on agnanowire_modeling_v3.py)
Random resitor network model for Ag nanowire network in oxide matrix
Using data from Will Scheideler

Created by Jeremy Smith on 2015-07-17
University of California, Berkeley
j-smith@eecs.berkeley.edu

Version 1.0
*/

#include <iostream>
#include <fstream>
#include <cstdlib>
#include <unistd.h>
#include <string>
#include <vector>
#include <thread>
#include <ctime>
#include <Eigen/Dense>
#include "nwnet.h"

using namespace std;
using namespace Eigen;

void stats(std::vector<double>& v, double& average, double& stderr, double& median){
	double sum = 0;
	for(int i = 0; i < v.size(); i++){
		sum += v[i];
	}
	average = sum/v.size();
	double dev = 0;
	for(int i = 0; i < v.size(); i++){
		dev += (v[i] - average)*(v[i] - average);
	}
	stderr = std::sqrt(dev/(v.size()*(v.size()-1)));
	std::sort(v.begin(), v.end());
	median = v[v.size()/2];
}

int main(){
	/*
	Setup parameters
	Change values here for different nanowire networks
	*/
	std::srand(time(NULL) | (getpid() << 4));
	double substratesize = 50.0;
	// Points to test resistance as fraction of substrate size
	ArrayXXd testpoints(12, 2);
	testpoints << 0.250, 0.750, 0.750, 0.250,
	              0.250, 0.250, 0.750, 0.750,
	              0.375, 0.625, 0.625, 0.375,
	              0.375, 0.375, 0.625, 0.625,
	              0.323, 0.500, 0.677, 0.500,
	              0.500, 0.323, 0.500, 0.677;
	testpoints *= substratesize;
	double nwlength = 14.0;                                              // Nanowire length
	double nwlength_sd = 4.0;                                            // Standard deviation of wire lengths
	double nwdiameter = 0.033;                                           // Nanowire diameter
	double agresistivity = 1.59e-2;                                      // Ag resistivity

	double nwdensity[] = {0.02, 0.04, 0.05};                             // Nanowires per sq micron
	double nwinterres[] = {1.0, 10.0, 1000.0};                           // Resistance between wires

	double matrixrsheet = 1.0e8;                                         // Sheet resistance of matrix
	int runs = 4;                                                        // Number of runs per condition

	double nwresistance = 4*agresistivity/(M_PI*nwdiameter*nwdiameter);  // Nanowire resistance per unit length

	std::vector<std::string> summaryList;
	std::ofstream outfile;

	cout << "\n=================\n";
	cout << "Ag Nanowire Model\n";
	cout << "Jeremy Smith\n";
	cout << "=================\n\n";

	for(int j = 0; j < 3; j++){
		for(int k = 0; k < 3; k++){
			std::vector<WireNet> nets;
			std::vector<std::thread> procs;
			int nwnumber = nwdensity[k]*substratesize*substratesize;

			for(int i = 0; i < runs; i++){
				unsigned seed = rand()%10000;
				WireNet n(nwnumber, nwlength, nwlength_sd, substratesize, nwresistance, nwinterres[j], matrixrsheet, seed, false);
				nets.push_back(n);
			}
			for(int i = 0; i < runs; i++){
				procs.push_back(std::thread(&WireNet::solve, &nets[i]));
			}
			for(int i = 0; i < runs; i++){
				procs[i].join();
			}
			cout << endl;

			std::vector<double> rList;
			std::vector<double> tList;
			for(int i = 0; i < runs; i++){
				nets[i].parameters();
				for(int p = 0; p < testpoints.rows()/2 - 1; p++){
					Vector2d xy1 = testpoints.row(2*p);
					Vector2d xy2 = testpoints.row(2*p+1);
					int node1 = findnode(nets[i].nodeCoords, xy1);
					int node2 = findnode(nets[i].nodeCoords, xy2);
					double r = two_point_resistance(nets[i].eigenvalues, nets[i].eigenvectors, node1, node2);
					cout << "  R: " << r << "    Between nodes: " << node1 << "," << node2 << endl;
					rList.push_back(std::log10(r));
				}
				double t = 100*(1 - nets[i].areal_coverage(nwdiameter));
				cout << "  T: " << t << " " << char(37) << endl;
				tList.push_back(t);

				nets[i].output_files("data_" + std::to_string(j) + std::to_string(k) + char(65+i));    // Test conditions j and k and run number i
				cout << endl;
			}
			double r_average;
			double r_stderr;
			double r_median;
			double t_average;
			double t_stderr;
			double t_median;
			stats(rList, r_average, r_stderr, r_median);
			stats(tList, t_average, t_stderr, t_median);
			summaryList.push_back("data_" + std::to_string(j) + std::to_string(k) + '\t'
				                  + to_string(nwlength) + '\t' + to_string(nwlength_sd) + '\t' + to_string(nwdiameter) + '\t'
				                  + to_string(agresistivity) + '\t' + to_string(nwresistance) + '\t'
				                  + to_string(nwdensity[k]) + '\t' + to_string(nwnumber) + '\t'
				                  + to_string(nwinterres[j]) + '\t' + to_string(matrixrsheet) + '\t'
				                  + to_string(r_average) + '\t' + to_string(r_median) + '\t' + to_string(r_stderr) + '\t'
				                  + to_string(t_average) + '\t' + to_string(t_stderr) + '\n');
		}
	}

	cout << "Writing out summary file..." << endl;
    outfile.open("summary.txt");
    outfile << "filename\tnwlength\tnwstd\tnwdiameter\tagresistivity\tnwresistance\t"
            << "nwdensity\tnwnumber\tnwinterres\tmatrixrsheet\t"
            << "resistance\tmedresistance\tresistanceerr\ttransmission\ttransmissionerr\n";
    for(int i = 0; i < summaryList.size(); i++){
        outfile << summaryList[i];
    }
    outfile.close();
    cout << "DONE" << endl;
}


#include <Eigen/Geometry>
#include <sbs/physics/simulation.h>
#include <sbs/physics/xpbd/collision_constraint.h>

namespace sbs {
namespace physics {
namespace xpbd {

collision_constraint_t::collision_constraint_t(
    scalar_type alpha,
    scalar_type beta,
    simulation_t const& simulation,
    index_type bi,
    index_type vi,
    Eigen::Vector3d const& p,
    Eigen::Vector3d const& n)
    : constraint_t{alpha, beta}, bi_(bi), vi_(vi), qs_(p), n_(n)
{
}

void collision_constraint_t::project_positions(simulation_t& simulation, scalar_type dt)
{
    particle_t& p       = simulation.particles()[bi_][vi_];
    scalar_type const w = p.invmass();
    scalar_type const C = evaluate(p.xi());

    if (C >= static_cast<scalar_type>(0.))
        return;

    scalar_type const alpha_tilde    = alpha_ / (dt * dt);
    scalar_type const delta_lagrange = -(C + alpha_tilde * lagrange_) / (w + alpha_tilde);

    lagrange_ += delta_lagrange;

    /**
     * grad(C) = n
     * ||n||^2 = 1,
     * so w*||n||^2 = w
     */
    p.xi() += w * n_ * delta_lagrange;
}

scalar_type collision_constraint_t::evaluate(Eigen::Vector3d const& p) const
{
    Eigen::Vector3d const qp = p - qs_;
    double const C           = qp.dot(n_);
    return C;
}

} // namespace xpbd
} // namespace physics
} // namespace sbs


#ifndef LSHFUNC_HPP
#define LSHFUNC_HPP

#ifndef EIGEN_CONFIG_H_
#define EIGEN_CONFIG_H_

#include <boost/serialization/array.hpp>
// w.r.t Eigen_3.2.4/Eigen/Core
#define EIGEN_DENSEBASE_PLUGIN "../../../../EigenDenseBaseAddons.hpp"
#include <Eigen/Core>
#endif // EIGEN_CONFIG_H_

#include <Eigen/Dense>
#include <boost/random.hpp>
#include <boost/filesystem.hpp>
#include <cmath>
#include <functional>
#include "config.hpp"

using namespace std;
namespace fs = boost::filesystem;

class LSHFunc {
  float w;
  int k; // number of bits in a function (length of key)
  int dim; // dimension of features
  Eigen::MatrixXf A;
  Eigen::MatrixXf b;

public:
  LSHFunc(int _k, int _dim): k(_k), dim(_dim) {
    genLSHfunc();
  }
  LSHFunc() {} // used while serializing

  void genLSHfunc() {
    w = 24; // default value
    A = Eigen::MatrixXf::Random(dim, k); // TODO: Use normal distribution to sample (as in GS code)
    typedef boost::mt19937 RNGType;
    RNGType rng;
    boost::uniform_real<> generator(0, w);
    boost::variate_generator<RNGType, boost::uniform_real<>> dice(rng, generator);

    b = Eigen::MatrixXf::Random(1, k);
    for (int i = 0; i < k; i++) {
      b(0, i) = dice();
    }
  }

  void computeHash(const vector<float>& _feat, vector<int>& hash) const {
    if (_feat.size() == 0) {
      return;
    }
    hash.clear();
    Eigen::MatrixXf feat = Eigen::VectorXf::Map(&_feat[0], _feat.size());
    #if NORMALIZE_FEATS == 1
      feat = feat / feat.norm(); // normalize the feature
    #endif
    Eigen::MatrixXf res = (feat.transpose() * A - b.replicate(feat.cols(), 1)) / w;
    for (int i = 0; i < res.size(); i++) {
      hash.push_back((int) floor(res(i)));
    }
  }

  template<class Archive>
  void serialize(Archive &ar, const unsigned int version) {
    ar & w;
    ar & k;
    ar & dim;
    ar & A;
    ar & b;
  }
};

#endif


#include <stan/math/mix/scal.hpp>
#include <gtest/gtest.h>
#include <boost/math/special_functions/digamma.hpp>
#include <test/unit/math/rev/scal/fun/util.hpp>
#include <test/unit/math/mix/scal/fun/nan_util.hpp>

TEST(AgradFwdLogRisingFactorial,FvarVar_FvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::log_rising_factorial;
  using boost::math::digamma;

  fvar<var> a(4.0,1.0);
  fvar<var> b(3.0,1.0);
  fvar<var> c = log_rising_factorial(a,b);

  EXPECT_FLOAT_EQ(std::log(120.0), c.val_.val());
  EXPECT_FLOAT_EQ(2.4894509, c.d_.val());

  AVEC y = createAVEC(a.val_,b.val_);
  VEC g;
  c.val_.grad(y,g);
  EXPECT_FLOAT_EQ(0.61666667, g[0]);
  EXPECT_FLOAT_EQ(1.8727844, g[1]);
}
TEST(AgradFwdLogRisingFactorial,FvarVar_Double_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::log_rising_factorial;
  using boost::math::digamma;

  fvar<var> a(4.0,1.0);
  double b(3.0);
  fvar<var> c = log_rising_factorial(a,b);

  EXPECT_FLOAT_EQ(std::log(120.0), c.val_.val());
  EXPECT_FLOAT_EQ(0.61666667, c.d_.val());

  AVEC y = createAVEC(a.val_);
  VEC g;
  c.val_.grad(y,g);
  EXPECT_FLOAT_EQ(0.61666667, g[0]);
}
TEST(AgradFwdLogRisingFactorial,Double_FvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::log_rising_factorial;
  using boost::math::digamma;

  double a(4.0);
  fvar<var> b(3.0,1.0);
  fvar<var> c = log_rising_factorial(a,b);

  EXPECT_FLOAT_EQ(std::log(120.0), c.val_.val());
  EXPECT_FLOAT_EQ(1.8727844, c.d_.val());

  AVEC y = createAVEC(b.val_);
  VEC g;
  c.val_.grad(y,g);
  EXPECT_FLOAT_EQ(1.8727844, g[0]);
}

TEST(AgradFwdLogRisingFactorial,FvarVar_FvarVar_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::log_rising_factorial;
  using boost::math::digamma;

  fvar<var> a(4.0,1.0);
  fvar<var> b(3.0,1.0);
  fvar<var> c = log_rising_factorial(a,b);

  AVEC y = createAVEC(a.val_,b.val_);
  VEC g;
  c.d_.grad(y,g);
  EXPECT_FLOAT_EQ(0.023267401, g[0]);
  EXPECT_FLOAT_EQ(0.30709034, g[1]);
}
TEST(AgradFwdLogRisingFactorial,FvarVar_Double_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::log_rising_factorial;
  using boost::math::digamma;

  fvar<var> a(4.0,1.0);
  double b(3.0);
  fvar<var> c = log_rising_factorial(a,b);

  AVEC y = createAVEC(a.val_);
  VEC g;
  c.d_.grad(y,g);
  EXPECT_FLOAT_EQ(-0.13027778, g[0]);
}
TEST(AgradFwdLogRisingFactorial,Double_FvarVar_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::log_rising_factorial;
  using boost::math::digamma;

  double a(4.0);
  fvar<var> b(3.0,1.0);
  fvar<var> c = log_rising_factorial(a,b);

  AVEC y = createAVEC(b.val_);
  VEC g;
  c.d_.grad(y,g);
  EXPECT_FLOAT_EQ(0.15354517, g[0]);
}
TEST(AgradFwdLogRisingFactorial,FvarFvarVar_FvarFvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::log_rising_factorial;
  using boost::math::digamma;

  fvar<fvar<var> > x;
  x.val_.val_ = 4.0;
  x.val_.d_ = 1.0;

  fvar<fvar<var> > y;
  y.val_.val_ = 3.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = log_rising_factorial(x,y);

  EXPECT_FLOAT_EQ(std::log(120.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0.61666667, a.val_.d_.val());
  EXPECT_FLOAT_EQ(1.8727844, a.d_.val_.val());
  EXPECT_FLOAT_EQ(0.15354517, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_,y.val_.val_);
  VEC g;
  a.val_.val_.grad(p,g);
  EXPECT_FLOAT_EQ(0.61666667, g[0]);
  EXPECT_FLOAT_EQ(1.8727844, g[1]);
}
TEST(AgradFwdLogRisingFactorial,FvarFvarVar_Double_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::log_rising_factorial;
  using boost::math::digamma;

  fvar<fvar<var> > x;
  x.val_.val_ = 4.0;
  x.val_.d_ = 1.0;
  double y(3.0);

  fvar<fvar<var> > a = log_rising_factorial(x,y);

  EXPECT_FLOAT_EQ(std::log(120.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0.61666667, a.val_.d_.val());
  EXPECT_FLOAT_EQ(0, a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.val_.val_.grad(p,g);
  EXPECT_FLOAT_EQ(0.61666667, g[0]);
}
TEST(AgradFwdLogRisingFactorial,Double_FvarFvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::log_rising_factorial;
  using boost::math::digamma;

  double x(4.0);
  fvar<fvar<var> > y;
  y.val_.val_ = 3.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = log_rising_factorial(x,y);

  EXPECT_FLOAT_EQ(std::log(120.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0, a.val_.d_.val());
  EXPECT_FLOAT_EQ(1.8727844, a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(y.val_.val_);
  VEC g;
  a.val_.val_.grad(p,g);
  EXPECT_FLOAT_EQ(1.8727844, g[0]);
}
TEST(AgradFwdLogRisingFactorial,FvarFvarVar_FvarFvarVar_2ndDeriv_x) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::log_rising_factorial;
  using boost::math::digamma;

  fvar<fvar<var> > x;
  x.val_.val_ = 4.0;
  x.val_.d_ = 1.0;

  fvar<fvar<var> > y;
  y.val_.val_ = 3.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = log_rising_factorial(x,y);

  AVEC p = createAVEC(x.val_.val_,y.val_.val_);
  VEC g;
  a.val_.d_.grad(p,g);
  EXPECT_FLOAT_EQ(-0.13027778, g[0]);
  EXPECT_FLOAT_EQ(0.15354517, g[1]);
}
TEST(AgradFwdLogRisingFactorial,FvarFvarVar_FvarFvarVar_2ndDeriv_y) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::log_rising_factorial;
  using boost::math::digamma;

  fvar<fvar<var> > x;
  x.val_.val_ = 4.0;
  x.val_.d_ = 1.0;

  fvar<fvar<var> > y;
  y.val_.val_ = 3.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = log_rising_factorial(x,y);

  AVEC p = createAVEC(x.val_.val_,y.val_.val_);
  VEC g;
  a.d_.val_.grad(p,g);
  EXPECT_FLOAT_EQ(0.15354517, g[0]);
  EXPECT_FLOAT_EQ(0.15354517, g[1]);
}
TEST(AgradFwdLogRisingFactorial,FvarFvarVar_Double_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::log_rising_factorial;
  using boost::math::digamma;

  fvar<fvar<var> > x;
  x.val_.val_ = 4.0;
  x.val_.d_ = 1.0;
  double y(3.0);

  fvar<fvar<var> > a = log_rising_factorial(x,y);

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.val_.d_.grad(p,g);
  EXPECT_FLOAT_EQ(-0.13027778, g[0]);
}
TEST(AgradFwdLogRisingFactorial,Double_FvarFvarVar_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::log_rising_factorial;
  using boost::math::digamma;

  double x(4.0);
  fvar<fvar<var> > y;
  y.val_.val_ = 3.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = log_rising_factorial(x,y);

  AVEC p = createAVEC(y.val_.val_);
  VEC g;
  a.d_.val_.grad(p,g);
  EXPECT_FLOAT_EQ(0.15354517, g[0]);
}
TEST(AgradFwdLogRisingFactorial,FvarFvarVar_FvarFvarVar_3rdDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::log_rising_factorial;
  using boost::math::digamma;

  fvar<fvar<var> > x;
  x.val_.val_ = 4.0;
  x.val_.d_ = 1.0;

  fvar<fvar<var> > y;
  y.val_.val_ = 3.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = log_rising_factorial(x,y);

  AVEC p = createAVEC(x.val_.val_,y.val_.val_);
  VEC g;
  a.d_.d_.grad(p,g);
  EXPECT_FLOAT_EQ(-0.023530472, g[0]);
  EXPECT_FLOAT_EQ(-0.023530472, g[1]);
}
TEST(AgradFwdLogRisingFactorial,FvarFvarVar_Double_3rdDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::log_rising_factorial;
  using boost::math::digamma;

  fvar<fvar<var> > x;
  x.val_.val_ = 4.0;
  x.val_.d_ = 1.0;
  x.d_.val_ = 1.0;
  double y(3.0);

  fvar<fvar<var> > a = log_rising_factorial(x,y);

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.d_.d_.grad(p,g);
  EXPECT_FLOAT_EQ(0.056509256, g[0]);
}
TEST(AgradFwdLogRisingFactorial,Double_FvarFvarVar_3rdDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::log_rising_factorial;
  using boost::math::digamma;

  double x(4.0);
  fvar<fvar<var> > y;
  y.val_.val_ = 3.0;
  y.d_.val_ = 1.0;
  y.val_.d_ = 1.0;

  fvar<fvar<var> > a = log_rising_factorial(x,y);

  AVEC p = createAVEC(y.val_.val_);
  VEC g;
  a.d_.d_.grad(p,g);
  EXPECT_FLOAT_EQ(-0.023530472, g[0]);
}

struct log_rising_factorial_fun {
  template <typename T0, typename T1>
  inline
  typename boost::math::tools::promote_args<T0,T1>::type
  operator()(const T0 arg1,
             const T1 arg2) const {
    return log_rising_factorial(arg1,arg2);
  }
};

TEST(AgradFwdLogRisingFactorial, nan) {
  log_rising_factorial_fun log_rising_factorial_;
  test_nan_mix(log_rising_factorial_,3.0,5.0,false);
}


// Copyright (C) 2011  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <sstream>
#include <string>
#include <cstdlib>
#include <ctime>
#include <dlib/svm_threaded.h>

#include "tester.h"

namespace
{
    using namespace test;
    using namespace dlib;
    using namespace std;

    logger dlog("test.svm_struct");

    template <
        typename matrix_type,
        typename sample_type,
        typename label_type
        >
    class test_multiclass_svm_problem : public structural_svm_problem_threaded<matrix_type,
                                                                 std::vector<std::pair<unsigned long,typename matrix_type::type> > >
    {

    public:
        typedef typename matrix_type::type scalar_type;
        typedef std::vector<std::pair<unsigned long,scalar_type> > feature_vector_type;

        test_multiclass_svm_problem (
            const std::vector<sample_type>& samples_,
            const std::vector<label_type>& labels_
        ) :
            structural_svm_problem_threaded<matrix_type,
                std::vector<std::pair<unsigned long,typename matrix_type::type> > >(2),
            samples(samples_),
            labels(labels_),
            dims(10+1) // +1 for the bias
        {
            for (int i = 0; i < 10; ++i)
            {
                distinct_labels.push_back(i);
            }
        }

        virtual long get_num_dimensions (
        ) const
        {
            return dims*10;
        }

        virtual long get_num_samples (
        ) const
        {
            return static_cast<long>(samples.size());
        }

        virtual void get_truth_joint_feature_vector (
            long idx,
            feature_vector_type& psi
        ) const
        {
            assign(psi, samples[idx]);
            // Add a constant -1 to account for the bias term.
            psi.push_back(std::make_pair(dims-1,static_cast<scalar_type>(-1)));

            // Find which distinct label goes with this psi.
            const long label_idx = index_of_max(mat(distinct_labels) == labels[idx]);

            offset_feature_vector(psi, dims*label_idx);
        }

        virtual void separation_oracle (
            const long idx,
            const matrix_type& current_solution,
            scalar_type& loss,
            feature_vector_type& psi
        ) const
        {
            scalar_type best_val = -std::numeric_limits<scalar_type>::infinity();
            unsigned long best_idx = 0;

            // Figure out which label is the best.  That is, what label maximizes
            // LOSS(idx,y) + F(x,y).  Note that y in this case is given by distinct_labels[i].
            for (unsigned long i = 0; i < distinct_labels.size(); ++i)
            {
                // Compute the F(x,y) part:
                // perform: temp == dot(relevant part of current solution, samples[idx]) - current_bias
                scalar_type temp = dot(rowm(current_solution, range(i*dims, (i+1)*dims-2)), samples[idx]) - current_solution((i+1)*dims-1);

                // Add the LOSS(idx,y) part:
                if (labels[idx] != distinct_labels[i])
                    temp += 1;

                // Now temp == LOSS(idx,y) + F(x,y).  Check if it is the biggest we have seen.
                if (temp > best_val)
                {
                    best_val = temp;
                    best_idx = i;
                }
            }

            assign(psi, samples[idx]);
            // add a constant -1 to account for the bias term
            psi.push_back(std::make_pair(dims-1,static_cast<scalar_type>(-1)));

            offset_feature_vector(psi, dims*best_idx);

            if (distinct_labels[best_idx] == labels[idx])
                loss = 0;
            else
                loss = 1;
        }

    private:

        void offset_feature_vector (
            feature_vector_type& sample,
            const unsigned long val
        ) const
        {
            if (val != 0)
            {
                for (typename feature_vector_type::iterator i = sample.begin(); i != sample.end(); ++i)
                {
                    i->first += val;
                }
            }
        }

        const std::vector<sample_type>& samples;
        const std::vector<label_type>& labels;
        std::vector<label_type> distinct_labels;
        const long dims;
    };

// ----------------------------------------------------------------------------------------

    template <
        typename K,
        typename label_type_ = typename K::scalar_type
        >
    class test_svm_multiclass_linear_trainer2
    {
    public:
        typedef label_type_ label_type;
        typedef K kernel_type;
        typedef typename kernel_type::scalar_type scalar_type;
        typedef typename kernel_type::sample_type sample_type;
        typedef typename kernel_type::mem_manager_type mem_manager_type;

        typedef multiclass_linear_decision_function<kernel_type, label_type> trained_function_type;

        test_svm_multiclass_linear_trainer2 (
        ) :
            C(10),
            eps(1e-4),
            verbose(false)
        {
        }

        trained_function_type train (
            const std::vector<sample_type>& all_samples,
            const std::vector<label_type>& all_labels
        ) const
        {
            scalar_type svm_objective = 0;
            return train(all_samples, all_labels, svm_objective);
        }

        trained_function_type train (
            const std::vector<sample_type>& all_samples,
            const std::vector<label_type>& all_labels,
            scalar_type& svm_objective
        ) const
        {
            // make sure requires clause is not broken
            DLIB_ASSERT(is_learning_problem(all_samples,all_labels),
                "\t trained_function_type test_svm_multiclass_linear_trainer2::train(all_samples,all_labels)"
                << "\n\t invalid inputs were given to this function"
                << "\n\t all_samples.size():     " << all_samples.size()
                << "\n\t all_labels.size():      " << all_labels.size()
                );

            typedef matrix<scalar_type,0,1> w_type;
            w_type weights;
            std::vector<sample_type> samples1(all_samples.begin(), all_samples.begin()+all_samples.size()/2);
            std::vector<sample_type> samples2(all_samples.begin()+all_samples.size()/2, all_samples.end());

            std::vector<label_type> labels1(all_labels.begin(), all_labels.begin()+all_labels.size()/2);
            std::vector<label_type> labels2(all_labels.begin()+all_labels.size()/2, all_labels.end());
            test_multiclass_svm_problem<w_type, sample_type, label_type> problem1(samples1, labels1);
            test_multiclass_svm_problem<w_type, sample_type, label_type> problem2(samples2, labels2);
            problem1.set_max_cache_size(3);
            problem2.set_max_cache_size(0);

            svm_struct_processing_node node1(problem1, 12345, 3);
            svm_struct_processing_node node2(problem2, 12346, 0);

            solver.set_inactive_plane_threshold(50);
            solver.set_subproblem_epsilon(1e-4);

            svm_struct_controller_node controller;
            controller.set_c(C);
            controller.set_epsilon(eps);
            if (verbose)
                controller.be_verbose();
            controller.add_processing_node("127.0.0.1", 12345);
            controller.add_processing_node("localhost:12346");
            svm_objective = controller(solver, weights);

            trained_function_type df;

            const long dims = max_index_plus_one(all_samples);
            df.labels  = select_all_distinct_labels(all_labels);
            df.weights = colm(reshape(weights, df.labels.size(), dims+1), range(0,dims-1));
            df.b       = colm(reshape(weights, df.labels.size(), dims+1), dims);
            return df;
        }

    private:
        scalar_type C;
        scalar_type eps;
        bool verbose;
        mutable oca solver;
    };

// ----------------------------------------------------------------------------------------

    template <
        typename K,
        typename label_type_ = typename K::scalar_type
        >
    class test_svm_multiclass_linear_trainer3
    {
    public:
        typedef label_type_ label_type;
        typedef K kernel_type;
        typedef typename kernel_type::scalar_type scalar_type;
        typedef typename kernel_type::sample_type sample_type;
        typedef typename kernel_type::mem_manager_type mem_manager_type;

        typedef multiclass_linear_decision_function<kernel_type, label_type> trained_function_type;

        test_svm_multiclass_linear_trainer3 (
        ) :
            C(10),
            eps(1e-4),
            verbose(false)
        {
        }

        trained_function_type train (
            const std::vector<sample_type>& all_samples,
            const std::vector<label_type>& all_labels
        ) const
        {
            scalar_type svm_objective = 0;
            return train(all_samples, all_labels, svm_objective);
        }

        trained_function_type train (
            const std::vector<sample_type>& all_samples,
            const std::vector<label_type>& all_labels,
            scalar_type& svm_objective
        ) const
        {
            // make sure requires clause is not broken
            DLIB_ASSERT(is_learning_problem(all_samples,all_labels),
                "\t trained_function_type test_svm_multiclass_linear_trainer3::train(all_samples,all_labels)"
                << "\n\t invalid inputs were given to this function"
                << "\n\t all_samples.size():     " << all_samples.size()
                << "\n\t all_labels.size():      " << all_labels.size()
                );

            typedef matrix<scalar_type,0,1> w_type;
            w_type weights;
            test_multiclass_svm_problem<w_type, sample_type, label_type> problem(all_samples, all_labels);
            problem.set_max_cache_size(0);

            problem.set_c(C);
            problem.set_epsilon(eps);

            if (verbose)
                problem.be_verbose();

            solver.set_inactive_plane_threshold(50);
            solver.set_subproblem_epsilon(1e-4);
            svm_objective = solver(problem, weights);

            trained_function_type df;

            const long dims = max_index_plus_one(all_samples);
            df.labels  = select_all_distinct_labels(all_labels);
            df.weights = colm(reshape(weights, df.labels.size(), dims+1), range(0,dims-1));
            df.b       = colm(reshape(weights, df.labels.size(), dims+1), dims);
            return df;
        }

    private:
        scalar_type C;
        scalar_type eps;
        bool verbose;
        mutable oca solver;
    };

// ----------------------------------------------------------------------------------------

    template <
        typename K,
        typename label_type_ = typename K::scalar_type
        >
    class test_svm_multiclass_linear_trainer4
    {
    public:
        typedef label_type_ label_type;
        typedef K kernel_type;
        typedef typename kernel_type::scalar_type scalar_type;
        typedef typename kernel_type::sample_type sample_type;
        typedef typename kernel_type::mem_manager_type mem_manager_type;

        typedef multiclass_linear_decision_function<kernel_type, label_type> trained_function_type;

        test_svm_multiclass_linear_trainer4 (
        ) :
            C(10),
            eps(1e-4),
            verbose(false)
        {
        }

        trained_function_type train (
            const std::vector<sample_type>& all_samples,
            const std::vector<label_type>& all_labels
        ) const
        {
            scalar_type svm_objective = 0;
            return train(all_samples, all_labels, svm_objective);
        }

        trained_function_type train (
            const std::vector<sample_type>& all_samples,
            const std::vector<label_type>& all_labels,
            scalar_type& svm_objective
        ) const
        {
            // make sure requires clause is not broken
            DLIB_ASSERT(is_learning_problem(all_samples,all_labels),
                "\t trained_function_type test_svm_multiclass_linear_trainer4::train(all_samples,all_labels)"
                << "\n\t invalid inputs were given to this function"
                << "\n\t all_samples.size():     " << all_samples.size()
                << "\n\t all_labels.size():      " << all_labels.size()
                );

            typedef matrix<scalar_type,0,1> w_type;
            w_type weights;
            test_multiclass_svm_problem<w_type, sample_type, label_type> problem(all_samples, all_labels);
            problem.set_max_cache_size(3);

            problem.set_c(C);
            problem.set_epsilon(eps);

            if (verbose)
                problem.be_verbose();

            solver.set_inactive_plane_threshold(50);
            solver.set_subproblem_epsilon(1e-4);
            svm_objective = solver(problem, weights);

            trained_function_type df;

            const long dims = max_index_plus_one(all_samples);
            df.labels  = select_all_distinct_labels(all_labels);
            df.weights = colm(reshape(weights, df.labels.size(), dims+1), range(0,dims-1));
            df.b       = colm(reshape(weights, df.labels.size(), dims+1), dims);
            return df;
        }

    private:
        scalar_type C;
        scalar_type eps;
        bool verbose;
        mutable oca solver;
    };

// ----------------------------------------------------------------------------------------

    template <
        typename K,
        typename label_type_ = typename K::scalar_type
        >
    class test_svm_multiclass_linear_trainer5
    {
    public:
        typedef label_type_ label_type;
        typedef K kernel_type;
        typedef typename kernel_type::scalar_type scalar_type;
        typedef typename kernel_type::sample_type sample_type;
        typedef typename kernel_type::mem_manager_type mem_manager_type;

        typedef multiclass_linear_decision_function<kernel_type, label_type> trained_function_type;

        test_svm_multiclass_linear_trainer5 (
        ) :
            C(10),
            eps(1e-4),
            verbose(false)
        {
        }

        trained_function_type train (
            const std::vector<sample_type>& all_samples,
            const std::vector<label_type>& all_labels
        ) const
        {
            scalar_type svm_objective = 0;
            return train(all_samples, all_labels, svm_objective);
        }

        trained_function_type train (
            const std::vector<sample_type>& all_samples,
            const std::vector<label_type>& all_labels,
            scalar_type& svm_objective
        ) const
        {
            // make sure requires clause is not broken
            DLIB_ASSERT(is_learning_problem(all_samples,all_labels),
                "\t trained_function_type test_svm_multiclass_linear_trainer5::train(all_samples,all_labels)"
                << "\n\t invalid inputs were given to this function"
                << "\n\t all_samples.size():     " << all_samples.size()
                << "\n\t all_labels.size():      " << all_labels.size()
                );

            typedef matrix<scalar_type,0,1> w_type;
            w_type weights;
            const long dims = max_index_plus_one(all_samples);
            trained_function_type df;
            df.labels  = select_all_distinct_labels(all_labels);
            multiclass_svm_problem<w_type, sample_type, label_type> problem(all_samples, all_labels, df.labels, dims, 4);
            problem.set_max_cache_size(3);

            problem.set_c(C);
            problem.set_epsilon(eps);

            if (verbose)
                problem.be_verbose();

            solver.set_inactive_plane_threshold(50);
            solver.set_subproblem_epsilon(1e-4);
            svm_objective = solver(problem, weights);

            df.weights = colm(reshape(weights, df.labels.size(), dims+1), range(0,dims-1));
            df.b       = colm(reshape(weights, df.labels.size(), dims+1), dims);
            return df;
        }

    private:
        scalar_type C;
        scalar_type eps;
        bool verbose;
        mutable oca solver;
    };

// ----------------------------------------------------------------------------------------

    typedef matrix<double,10,1> sample_type;
    typedef double scalar_type;

    void make_dataset (
        std::vector<sample_type>& samples,
        std::vector<scalar_type>& labels,
        int num,
        dlib::rand& rnd
    )
    {
        samples.clear();
        labels.clear();
        for (int i = 0; i < 10; ++i)
        {
            for (int j = 0; j < num; ++j)
            {
                sample_type samp;
                samp = 0;
                samp(i) = 10*rnd.get_random_double()+1;

                samples.push_back(samp);
                labels.push_back(i);
            }
        }
    }

// ----------------------------------------------------------------------------------------

    class test_svm_struct : public tester
    {
    public:
        test_svm_struct (
        ) :
            tester ("test_svm_struct",
                    "Runs tests on the structural svm components.")
        {}

        void run_test (
            const std::vector<sample_type>& samples,
            const std::vector<scalar_type>& labels,
            const double true_obj
        )
        {
            typedef linear_kernel<sample_type> kernel_type;
            svm_multiclass_linear_trainer<kernel_type> trainer1;
            test_svm_multiclass_linear_trainer2<kernel_type> trainer2;
            test_svm_multiclass_linear_trainer3<kernel_type> trainer3;
            test_svm_multiclass_linear_trainer4<kernel_type> trainer4;
            test_svm_multiclass_linear_trainer5<kernel_type> trainer5;

            trainer1.set_epsilon(1e-4);
            trainer1.set_c(10);

            multiclass_linear_decision_function<kernel_type,double> df1, df2, df3, df4, df5;
            double obj1, obj2, obj3, obj4, obj5;

            // Solve a multiclass SVM a whole bunch of different ways and make sure
            // they all give the same answer.
            print_spinner();
            df1 = trainer1.train(samples, labels, obj1);
            print_spinner();
            df2 = trainer2.train(samples, labels, obj2);
            print_spinner();
            df3 = trainer3.train(samples, labels, obj3);
            print_spinner();
            df4 = trainer4.train(samples, labels, obj4);
            print_spinner();
            df5 = trainer5.train(samples, labels, obj5);
            print_spinner();

            dlog << LINFO << "obj1: "<< obj1;
            dlog << LINFO << "obj2: "<< obj2;
            dlog << LINFO << "obj3: "<< obj3;
            dlog << LINFO << "obj4: "<< obj4;
            dlog << LINFO << "obj5: "<< obj5;
            DLIB_TEST(std::abs(obj1 - obj2) < 1e-2);
            DLIB_TEST(std::abs(obj1 - obj3) < 1e-2);
            DLIB_TEST(std::abs(obj1 - obj4) < 1e-2);
            DLIB_TEST(std::abs(obj1 - obj5) < 1e-2);
            DLIB_TEST(std::abs(obj1 - true_obj) < 1e-2);
            DLIB_TEST(std::abs(obj2 - true_obj) < 1e-2);
            DLIB_TEST(std::abs(obj3 - true_obj) < 1e-2);
            DLIB_TEST(std::abs(obj4 - true_obj) < 1e-2);
            DLIB_TEST(std::abs(obj5 - true_obj) < 1e-2);

            dlog << LINFO << "weight error: "<< max(abs(df1.weights - df2.weights));
            dlog << LINFO << "weight error: "<< max(abs(df1.weights - df3.weights));
            dlog << LINFO << "weight error: "<< max(abs(df1.weights - df4.weights));
            dlog << LINFO << "weight error: "<< max(abs(df1.weights - df5.weights));

            DLIB_TEST(max(abs(df1.weights - df2.weights)) < 1e-2);
            DLIB_TEST(max(abs(df1.weights - df3.weights)) < 1e-2);
            DLIB_TEST(max(abs(df1.weights - df4.weights)) < 1e-2);
            DLIB_TEST(max(abs(df1.weights - df5.weights)) < 1e-2);

            dlog << LINFO << "b error: "<< max(abs(df1.b - df2.b));
            dlog << LINFO << "b error: "<< max(abs(df1.b - df3.b));
            dlog << LINFO << "b error: "<< max(abs(df1.b - df4.b));
            dlog << LINFO << "b error: "<< max(abs(df1.b - df5.b));
            DLIB_TEST(max(abs(df1.b - df2.b)) < 1e-2);
            DLIB_TEST(max(abs(df1.b - df3.b)) < 1e-2);
            DLIB_TEST(max(abs(df1.b - df4.b)) < 1e-2);
            DLIB_TEST(max(abs(df1.b - df5.b)) < 1e-2);

            matrix<double> res = test_multiclass_decision_function(df1, samples, labels);
            dlog << LINFO << res;
            dlog << LINFO << "accuracy: " << sum(diag(res))/sum(res);
            DLIB_TEST(sum(diag(res)) == samples.size());

            res = test_multiclass_decision_function(df2, samples, labels);
            dlog << LINFO << res;
            dlog << LINFO << "accuracy: " << sum(diag(res))/sum(res);
            DLIB_TEST(sum(diag(res)) == samples.size());

            res = test_multiclass_decision_function(df3, samples, labels);
            dlog << LINFO << res;
            dlog << LINFO << "accuracy: " << sum(diag(res))/sum(res);
            DLIB_TEST(sum(diag(res)) == samples.size());

            res = test_multiclass_decision_function(df4, samples, labels);
            dlog << LINFO << res;
            dlog << LINFO << "accuracy: " << sum(diag(res))/sum(res);
            DLIB_TEST(sum(diag(res)) == samples.size());

            res = test_multiclass_decision_function(df5, samples, labels);
            dlog << LINFO << res;
            dlog << LINFO << "accuracy: " << sum(diag(res))/sum(res);
            DLIB_TEST(sum(diag(res)) == samples.size());
        }

        void perform_test (
        )
        {
            std::vector<sample_type> samples;
            std::vector<scalar_type> labels;

            dlib::rand rnd;

            dlog << LINFO << "test with 100 samples per class";
            make_dataset(samples, labels, 100, rnd);
            run_test(samples, labels, 1.155);

            dlog << LINFO << "test with 1 sample per class";
            make_dataset(samples, labels, 1, rnd);
            run_test(samples, labels, 0.251);

            dlog << LINFO << "test with 2 sample per class";
            make_dataset(samples, labels, 2, rnd);
            run_test(samples, labels, 0.444);
        }
    } a;

}


/*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2014-2015, Timm Linder, Social Robotics Lab, University of Freiburg
*  Copyright (c) 2006-2012, Matthias Luber, Luciano Spinello and Kai O. Arras, Social Robotics Laboratory and
*    Oscar Martinez, Autonomous Intelligent Systems, University of Freiburg
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions are met:
*
*  * Redistributions of source code must retain the above copyright notice, this
*    list of conditions and the following disclaimer.
*  * Redistributions in binary form must reproduce the above copyright notice,
*    this list of conditions and the following disclaimer in the documentation
*    and/or other materials provided with the distribution.
*  * Neither the name of the copyright holder 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 HOLDER 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.
*/

#include <srl_laser_features/features/feature06.h>

#include <Eigen/LU>
#include <Eigen/QR>
#include <float.h>

#define MATRIX_LN_EPS -1e8

namespace srl_laser_features {

Feature06::Feature06(bool extended) : Feature(), m_extended(extended)
{
}

void Feature06::evaluate(const Segment& segment, Eigen::VectorXd& result) const
{
	result = Eigen::VectorXd::Zero(getNDimensions());

	const size_t numPoints = segment.points.size();
	if (numPoints > 2) {

		double px[numPoints];
		double py[numPoints];

		// copy
		for (size_t pIndex = 0; pIndex < numPoints; ++pIndex) {
			px[pIndex] = segment.points[pIndex](0);
			py[pIndex] = segment.points[pIndex](1);
		}

		double xc, yc, rc;
		fitCircle(numPoints, px, py, xc, yc, rc);

		result(1) = rc;
		if (m_extended) {
			result(3) = DBL_MAX;
			result(4) = 0.0;
		}

		// residual sum
		Eigen::Vector2d centerOfCircle(xc, yc);

		double residual;
		for (size_t pIndex = 0; pIndex < numPoints; ++pIndex) {
			double dist = (centerOfCircle - segment.points[pIndex]).norm();

			if (m_extended) {
				if (dist < result(3)) {
					result(3) = dist;
				}
				if (dist > result(4)) {
					result(4) = dist;
				}
			}

			residual = rc - dist;
			result(0) += residual * residual;
		}

		if (m_extended) {
			result(2) = result(0) / (numPoints - 1);
			if (result(4) != 0.0) {
				result(5) = result(3) / result(4);
			}
		}
	}
}

int Feature06::fitCircle(int n, double *x_vec, double *y_vec, double& xc, double& yc, double& r)
{
	int i;

	if (n > 3) {
		Eigen::MatrixXd A(n, 3);
		Eigen::MatrixXd X(3, 1);
		Eigen::MatrixXd B(n, 3);
		Eigen::MatrixXd P(3, n);
		Eigen::MatrixXd T(3, 3);

		// We want the overdetermined linear equation system Ax = b
		// Fill in matrix A
		for (i = 0; i < n; i++) {
			A(i, 0) = -2 * x_vec[i];
			A(i, 1) = -2 * y_vec[i];
			A(i, 2) = 1.0;
		}

		// Fill in vector b
		for (i = 0; i < n; i++) {
			B(i, 0) = -(x_vec[i] * x_vec[i] + y_vec[i] * y_vec[i]);
		}

		// Now we have the equation system with n equations and 3 unknowns.
		// The LSQ-solution is given by the pseudo-inverse:
		P = A.transpose();
		T = P * A;
		Eigen::FullPivLU<Eigen::MatrixXd> lu(T);
		if (lu.isInvertible()) {
			if (log(lu.determinant()) > MATRIX_LN_EPS) {
				X = lu.inverse() * P * B;
				// Extract circle center and radius
				xc = X(0, 0);
				yc = X(1, 0);
				r = sqrt(-X(2, 0) + xc * xc + yc * yc);
				return 1;
			}
			else {
				// Points badly conditioned (n > 3)
				return -3;
			}
		}
		else {
			// try QR
			Eigen::FullPivHouseholderQR<Eigen::MatrixXd> QR(A);
			X = QR.solve(B);

			// Extract circle center and radius
			xc = X(0, 0);
			yc = X(1, 0);
			r = sqrt(-X(2, 0) + xc * xc + yc * yc);
			return 1;
		}
	}
	else if (n == 3) {
		double a, b, c, d, e, f, g;

		a = x_vec[1] - x_vec[0];
		b = y_vec[1] - y_vec[0];
		c = x_vec[2] - x_vec[0];
		d = y_vec[2] - y_vec[0];
		e = a * (x_vec[0] + x_vec[1]) + b * (y_vec[0] + y_vec[1]);
		f = c * (x_vec[0] + x_vec[2]) + d * (y_vec[0] + y_vec[2]);
		g = 2 * (a * (y_vec[2] - y_vec[1]) - b * (x_vec[2] - x_vec[1]));

		if (g != 0.0) {
			xc = (d * e - b * f) / g;
			yc = (a * f - c * e) / g;
			r = sqrt((x_vec[0] - xc) * (x_vec[0] - xc) + (y_vec[0] - yc) * (y_vec[0] - yc));
			return 2;
		}
		else {
			// Points badly conditioned (n == 3)
			return -2;
		}
	}
	else {
		// Too few points
		xc = 0.0;
		yc = 0.0;
		r = -1.0;
		return -1;
	}
}

} // end of namespace srl_laser_features


/**
 * \file
 * \copyright
 * Copyright (c) 2012-2020, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 */

#include <boost/math/constants/constants.hpp>
#include <cmath>
#include <memory>
#include <numeric>
#include <random>

#include "MeshLib/Elements/Element.h"
#include "MeshLib/Mesh.h"
#include "MeshLib/MeshGenerators/MeshGenerator.h"
#include "MeshLib/MeshQuality/ElementQualityInterface.h"
#include "MeshLib/Node.h"
#include "gtest/gtest.h"

class TriElementQuality : public ::testing::Test
{
public:
    TriElementQuality()
    {
        std::random_device rd;
        std::mt19937 gen(rd());
        std::uniform_int_distribution<> distrib(1, 10);

        lengths = {1, 1};
        n_subdivisions = {distrib(gen), distrib(gen)};
    }

    std::vector<double> getElementQualityVectorFromRegularTriMesh(
        MeshLib::MeshQualityType const type)
    {
        std::vector<std::unique_ptr<BaseLib::ISubdivision>> vec_div;
        for (int i = 0; i < 2; ++i)
        {
            vec_div.emplace_back(
                new BaseLib::UniformSubdivision(lengths[i], n_subdivisions[i]));
        }

        std::unique_ptr<MeshLib::Mesh> mesh(
            MeshLib::MeshGenerator::generateRegularTriMesh(*vec_div[0],
                                                           *vec_div[1]));
        MeshLib::ElementQualityInterface element_quality(*mesh, type);
        return element_quality.getQualityVector();
    }

    std::array<int, 2> lengths;
    std::array<int, 2> n_subdivisions;
};

TEST_F(TriElementQuality, ElementSize)
{
    auto const type = MeshLib::MeshQualityType::ELEMENTSIZE;
    auto const element_quality_vector =
        getElementQualityVectorFromRegularTriMesh(type);
    auto const expected_value =
        0.5 / std::accumulate(n_subdivisions.begin(), n_subdivisions.end(), 1,
                              std::multiplies<int>());
    for (auto const element_quality : element_quality_vector)
    {
        ASSERT_NEAR(expected_value, element_quality,
                    std::numeric_limits<double>::epsilon());
    }
}

TEST_F(TriElementQuality, SizeDifference)
{
    auto const type = MeshLib::MeshQualityType::SIZEDIFFERENCE;
    auto const element_quality_vector =
        getElementQualityVectorFromRegularTriMesh(type);
    // all elements have the same size, the quality value has to be 1.0
    auto constexpr expected_value = 1.0;
    for (auto const element_quality : element_quality_vector)
    {
        ASSERT_NEAR(expected_value, element_quality,
                    10 * std::numeric_limits<double>::epsilon());
    }
}

TEST_F(TriElementQuality, EdgeRatio)
{
    auto const type = MeshLib::MeshQualityType::EDGERATIO;
    auto const element_quality_vector =
        getElementQualityVectorFromRegularTriMesh(type);
    auto const& min_max =
        std::minmax_element(n_subdivisions.begin(), n_subdivisions.end());
    auto const expected_value =
        double(*min_max.first) / std::sqrt(*min_max.first * *min_max.first +
                                      *min_max.second * *min_max.second);
    for (auto const element_quality : element_quality_vector)
    {
        ASSERT_NEAR(expected_value, element_quality,
                    10 * std::numeric_limits<double>::epsilon());
    }
}

TEST_F(TriElementQuality, EquiAngleSkew)
{
    using namespace boost::math::double_constants;
    auto const type = MeshLib::MeshQualityType::EQUIANGLESKEW;
    auto const element_quality_vector =
        getElementQualityVectorFromRegularTriMesh(type);
    // all triangles are right-angled triangles
    auto const hypothenuse = std::sqrt(std::pow(1.0 / n_subdivisions[0], 2) +
                                       std::pow(1.0 / n_subdivisions[1], 2));
    std::array const angles = {
        std::asin((1.0 / n_subdivisions[0]) / hypothenuse),
        std::asin((1.0 / n_subdivisions[1]) / hypothenuse), half_pi};
    auto const& min_max = std::minmax_element(angles.begin(), angles.end());
    auto const expected_value =
        std::max((*min_max.second - third_pi) / two_thirds_pi,
                 (third_pi - *min_max.first) / third_pi);

    for (auto const element_quality : element_quality_vector)
    {
        ASSERT_NEAR(expected_value, element_quality,
                    10 * std::numeric_limits<double>::epsilon());
    }
}