#include "Synthesiser.h"
#include <boost/timer.hpp>
#include <set>
#include <Eigen/QR>
#include <Eigen/Eigenvalues>

namespace abstract{

/// Constructs an empty buffer
template<class scalar>
Synthesiser<scalar>::Synthesiser(int dimension,int idimension) :
  CegarSystem<scalar>(dimension,idimension),
  m_closedLoop(dimension),
  m_synthType(eEigenSynth)
{}

/// Returns the nullSpace vectors of U_1^T(A-\lambda_iI) where B=[U_0 U_1][Z,0]
template<class scalar>
std::vector<typename Synthesiser<scalar>::MatrixS> Synthesiser<scalar>::getNullSpace(const MatrixC &eigenValues)
{
  MatrixS B1=m_sensitivity.block(0,0,m_sensitivity.rows(),m_fdimension);
  SolverMatrixType B1ref;
  interToRef(B1ref,B1);
  Eigen::HouseholderQR<SolverMatrixType> senseQR(B1ref);
  SolverMatrixType U0U1=senseQR.householderQ();
  SolverMatrixType U1Tref=U0U1.block(0,m_fdimension,m_dimension,m_dimension-m_fdimension).transpose();
  MatrixC U1T(m_dimension,m_dimension-m_fdimension);
  MatrixS U1Treal;
  refToInter(U1Treal,U1Tref);
  U1T.real()=U1Treal;
  U1T.imag()=MatrixS::Zero(m_dimension,m_dimension-m_fdimension);
  MatrixC complexDynamics(m_dimension,m_dimension);
  complexDynamics.real()=m_dynamics;
  complexDynamics.imag()=MatrixS::Zero(m_dimension,m_dimension);
  std::vector<MatrixS> result(m_dimension);
  for (int i=0;i<m_dimension;i++) {
    MatrixC nullSpace=U1T*(complexDynamics-eigenValues.coeff(i,i)*MatrixC::Identity(m_dimension,m_dimension));
    this->toRREF(nullSpace);
    std::vector<bool> vars(nullSpace.cols());
    int row=0;
    int freeVars=nullSpace.cols();
    for (int col=0;col<nullSpace.cols();col++) {
      vars[col]=true;
      if (!func::isZero(norm(nullSpace.coeff(row,col)))) {
        vars[col]=false;
        row++;
        freeVars--;
      }
    }
    result[i].resize(m_dimension,freeVars);
    int col=0;
    for (int j=0;j<nullSpace.cols();j++) {
      if (vars[j]) {
        result[i].row(j)=MatrixS::Zero(1,freeVars);
        result[i].coeffRef(j,col)=this->ms_one;
      }
      else {
        int pos=0;
        for (int k=0;k<nullSpace.cols();k++) {
          if (vars[k]) result[i].coeffRef(j,pos++)=-nullSpace.coeff(j,k).real();
        }
      }
    }
    if (m_conjugatePair[i]>i) {
      i++;
      result[i].resize(m_dimension,freeVars);
      int col=0;
      for (int j=0;j<nullSpace.cols();j++) {
        if (vars[j]) {
          result[i].row(j)=MatrixS::Zero(1,freeVars);
          result[i].coeffRef(j,col)=this->ms_one;
        }
        else {
          int pos;
          for (int k=0;k<nullSpace.cols();k++) {
            if (vars[k]) result[i].coeffRef(j,pos++)=-nullSpace.coeff(j,k).imag();
          }
        }
      }
    }
  }
  return result;
}

/// Returns a tranformation of the eigenvector inequalities into their corresponding nullSpace coefficients
template<class scalar>
typename Synthesiser<scalar>::MatrixS Synthesiser<scalar>::getNullSpaceFaces(MatrixS &faces,std::vector<MatrixS> &nullSpace)
{
  nullSpace=getNullSpace(m_closedLoop.getEigenValues());
  int nullSpaceDim=0;
  for (int i=0;i<m_dimension;i++) nullSpaceDim+=nullSpace[i].cols();
  MatrixS result=MatrixS::Zero(faces.rows(),nullSpaceDim);
  int pos=0;
  for (int i=0;i<m_dimension;i++) {
    for (int k=0;k<nullSpace[i].cols();k++,pos++)
    {
      for (int j=0;j<m_dimension;j++) {
        result.col(pos)+=faces.col(i*m_dimension+j)*nullSpace[i].coeff(j,k);
      }
    }
  }
  return result;
}

/// Solves the Sylvester equation AX+XB=C for X
template<class scalar>
typename Synthesiser<scalar>::MatrixS Synthesiser<scalar>::solveSylvester(const MatrixS &A,const MatrixS &B,const MatrixS &C,bool BisDiagonal)
{
  MatrixS U,V;
  SolverMatrixType refA,refB;
  interToRef(refA,A);
  interToRef(refB,B);
  Eigen::RealSchur<SolverMatrixType> realSchur(refA.transpose());
  refToInter(U,realSchur.matrixU());//We want transposed so keep it this way
  refA=realSchur.matrixT().transpose();//Lower triangular
  if (BisDiagonal) {
    V=MatrixS::Identity(C.rows(),refB.rows());
  }
  else {
    interToRef(refB,B);
    realSchur.compute(refB);
    refToInter(V,realSchur.matrixU());
    refB=realSchur.matrixT();
  }
  MatrixS result=U*C*V;
  for (int row=0;row<result.rows();row++) {
    for (int col=0;col<result.cols();col++) {
      for (int i=0;i<row;i++) {
        result.coeffRef(row,col)-=refA.coeff(row,i)*result.coeff(i,col);
      }
      for (int i=0;i<col;i++) {
        result.coeffRef(row,col)-=result.coeff(row,i)*refB.coeff(i,col);
      }
      result.coeffRef(row,col)/=refA.coeff(row,row)+refB.coeff(col,col);
    }
  }
  return result;
}

/// Retrieves a set of viable Left Eigenvectors for given eigenvalues
template<class scalar>
typename Synthesiser<scalar>::MatrixS Synthesiser<scalar>::getValidLeftEigenVectors(const MatrixS &pseudoEigenValues,const MatrixS &desired)
{
  MatrixS S=desired.inverse();//TODO:may be transposed
  MatrixS B=m_sensitivity.leftCols(m_fdimension);
  bool hasInverse=false;
  MatrixS invB=this->getSVDpseudoInverse(B,hasInverse);
  MatrixS C=-B*invB*(m_dynamics*S-S*pseudoEigenValues);//-BP
  S=solveSylvester(m_dynamics,pseudoEigenValues,C,true);
  return S.inverse();//TODO:may be transposed
}

/// Retrieves a set of viable Left Eigenvectors for given eigenvalues
template<class scalar>
typename Synthesiser<scalar>::MatrixS Synthesiser<scalar>::getLeftEigenVectors(const MatrixS &pseudoEigenValues,AbstractPolyhedra<scalar> &eigenVectorSpace)
{
  UNUSED(pseudoEigenValues);
  UNUSED(eigenVectorSpace);
}

/// Retrieves constraints on the controller coefficients based on the I/O constraints
template<class scalar>
AbstractPolyhedra<scalar> Synthesiser<scalar>::getControllerInBounds(AbstractPolyhedra<scalar>& reachTube)
{
  MatrixS &vertices=reachTube.getVertices();
  MatrixS directions=m_inputs.getDirections();
  directions.conservativeResize(m_fdimension,directions.cols());
  MatrixS faces(vertices.rows()*directions.cols(),vertices.cols()*m_fdimension);
  MatrixS supports(vertices.rows()*directions.cols(),1);
  MatrixS inputSupports=m_inputs.getSupports();
  for (int row=0;row<vertices.rows();row++) {
    for (int col=0;col<directions.cols();col++) {
      for (int i=0;i<m_fdimension;i++) {
        for (int j=0;j<vertices.cols();j++) {
          faces.coeffRef(row*directions.cols()+col,i*vertices.cols()+j)=vertices.coeff(row,j)*directions.coeff(i,col);
        }
      }
      supports.coeffRef(row*directions.cols()+col)=inputSupports.coeff(col,0);
    }
  }
  AbstractPolyhedra<scalar> result(vertices.cols()*m_fdimension);
  result.load(faces,supports);
  return result;
}

/// Retrieves constraints on the controller coefficients based on the Dynamic constraints
template<class scalar>
AbstractPolyhedra<scalar> Synthesiser<scalar>::getControllerDynBounds(AbstractPolyhedra<scalar>& reachTube,int &orBlockSize)
{
  //max((I-A-BK)Sv)>max(Rv) -> +p_R+p_S((A-I)^T)<-p_S(BK)
  //(A-I)S-R>-SBK -> -\sum(bji)k_{lj}S_lV_l < (A-I)Sv-B_nR_nv
  MatrixS templates=reachTube.getDirections();

  MatrixS dynamics=m_dynamics;
  for (int i=0;i<m_dimension;i++) dynamics.coeffRef(i,i)-=ms_one;
  MatrixS aDirections=dynamics.transpose()*templates;
  MatrixS inDirections=m_sensitivity.transpose()*templates;
  MatrixS aSupports;
  reachTube.maximiseAll(aDirections,aSupports);

  if (!m_reference.isEmpty()) {
    MatrixS inSupports;
    m_reference.maximiseAll(inDirections,inSupports);
    aSupports-=inSupports;
  }

  MatrixS &vertices=reachTube.getVertices();
  orBlockSize=vertices.rows();
  MatrixS supports(aSupports.rows()*vertices.rows(),1);
  MatrixS faces(supports.rows(),m_feedback.rows()*m_feedback.cols());
  MatrixS coefficients=templates.transpose()*m_sensitivity;
  for (int row=0;row<templates.cols();row++) {
    for (int i=0;i<orBlockSize;i++) {
      for (int j=0;j<m_feedback.rows();j++) {
        faces.block(row*orBlockSize+i,j*m_dimension,1,m_dimension)=coefficients.coeff(row,j)*vertices.row(i);
      }
      supports.coeffRef(row*orBlockSize+i,0)=aSupports.coeff(row,0);
    }
  }
  AbstractPolyhedra<scalar> result(faces.cols());
  result.load(faces,supports);
  return result;
}

/// Synthetises the input that leads to a given reach tube
template<class scalar>
AbstractPolyhedra<scalar> Synthesiser<scalar>::synthesiseInputs(inputType_t inputType,int precision,AbstractPolyhedra<scalar> &init,AbstractPolyhedra<scalar> &end,AbstractPolyhedra<scalar> &dynamics,MatrixS& templates,refScalar tightness)
{
  const MatrixS vectors=end.getDirections();
  MatrixS preSupports=end.getSupports();
  MatrixS abstractVectors,supports;
  dynamics.toInner(true);
  if (inputType==eVariableInputs) {
    MatrixS roundVectors;
    getRoundedDirections(roundVectors,vectors);
    MatrixS combinedVectors(vectors.rows()+roundVectors.rows(),vectors.cols());
    combinedVectors.block(0,0,vectors.rows(),vectors.cols())=vectors;
    combinedVectors.block(vectors.rows(),0,roundVectors.rows(),roundVectors.cols())=roundVectors;
    MatrixS abstractRoundedVectors=dynamics.getSynthVertices(combinedVectors,m_conjugatePair,m_jordanIndex);
    abstractVectors=abstractRoundedVectors.block(0,0,abstractRoundedVectors.rows(),m_dimension);
    init.maximiseAll(abstractVectors.transpose(),supports);
    int factor=supports.rows()/vectors.cols();
    for (int row=0;row<supports.rows();row++) {
      supports.coeffRef(row,0)=preSupports.coeff(row/factor,0)-supports.coeff(row,0);
    }
    AbstractPolyhedra<scalar> tempResult(roundVectors.rows());
    int roundDirs=roundVectors.rows();
    int outRows=abstractRoundedVectors.rows();
    tempResult.m_faces=abstractRoundedVectors.block(0,m_dimension,outRows,roundDirs);
    tempResult.m_faces.conservativeResize(outRows+roundDirs,roundDirs);
    supports.conservativeResize(outRows+roundDirs,1);
    tempResult.m_faces.block(outRows,0,roundDirs,roundDirs)=-MatrixS::Identity(roundDirs,roundDirs);
    supports.block(outRows,0,roundDirs,1)=MatrixS::Zero(roundDirs,1);
    tempResult.load(tempResult.m_faces,supports);
    tempResult.toInner(true);
    tempResult.retemplate(templates,-tightness);//templog?
    MatrixS subTemplates;
    std::vector<bool> isRound;
    this->findRoundIndices(isRound);
    int newRow=0;
    abstractVectors=tempResult.m_faces;
    supports=tempResult.m_supports;
    for (int row=0;row<abstractVectors.rows();row++) {
      bool keep=true;
      for (int col=0;col<isRound.size();col++) {
        if (isRound[col] && (func::hardSign(abstractVectors.coeff(row,col))<0)) keep=false;
      }
      if (keep) {
        abstractVectors.row(newRow)=abstractVectors.row(row);
        supports.row(newRow)=supports.row(row);
        newRow++;
      }
    }
    abstractVectors.conservativeResize(newRow,abstractVectors.cols());
    supports.conservativeResize(newRow,1);

    abstractVectors*=this->m_invIminF.transpose();
    ms_logger.logData(abstractVectors,"deccelerated rounded:");//templog
    for (int col=0;col<m_dimension;col++) {
      int mult=(m_conjugatePair[col]<0) ? 1 : 2;
      int subDim=mult;
      while(m_jordanIndex[col+subDim]>0) subDim+=mult;
      if (subDim>1) {
        this->makeSphericalTemplates(precision,subDim,subTemplates,true);
        int oldRows=abstractVectors.rows();
        int oldCols=abstractVectors.cols();
        abstractVectors.conservativeResize(oldRows*subTemplates.cols(),oldCols+subDim-1);
        supports.conservativeResize(oldRows*subTemplates.cols(),1);
        if (col+1<oldCols) {
          abstractVectors.block(0,col+subDim,oldRows,oldCols-col-1)=abstractVectors.block(0,col+1,oldRows,oldCols-col-1);
        }
        for (int i=1;i<subTemplates.cols();i++) {
          abstractVectors.block(i*oldRows,0,oldRows,col)=abstractVectors.block(0,0,oldRows,col);//All columns before this column stay the same
          if (col+1<oldCols) {
            abstractVectors.block(i*oldRows,col+subDim,oldRows,oldCols-col-1)=abstractVectors.block(0,col+subDim,oldRows,oldCols-col-1);//All columns after this block stay the same
          }
          supports.block(i*oldRows,0,oldRows,1)=supports.block(0,0,oldRows,1);
          for (int j=0;j<oldRows;j++)
          {
            abstractVectors.block(i*oldRows+j,col,1,subDim)=subTemplates.col(i).transpose()*abstractVectors.coeff(j,col);
          }
        }
        for (int j=0;j<oldRows;j++)
        {
          abstractVectors.block(j,col,1,subDim)=subTemplates.col(0).transpose()*abstractVectors.coeff(j,col);
        }
        col+=subDim-1;
      }
    }
  }
  else {
    abstractVectors=dynamics.getSynthVertices(vectors,m_conjugatePair,m_jordanIndex);
    init.maximiseAll(abstractVectors.transpose(),supports);
    int factor=supports.rows()/vectors.cols();
    for (int row=0;row<supports.rows();row++) {
      supports.coeffRef(row,0)=preSupports.coeff(row/factor,0)-supports.coeff(row,0);
    }
    for (int row=0;row<supports.rows();row++) {
      abstractVectors.row(row)=vectors.col(row/factor).transpose()-abstractVectors.row(row);
    }
  }
  AbstractPolyhedra<scalar> result(m_dimension);
  result.load(abstractVectors,supports);
  if (inputType==eParametricInputs) result.transform(this->m_pseudoIminJ,this->m_pseudoInvIminJ);

  //result.retemplate(templates,-tightness);
  return result;
}

/// Synthetises the input that leads to a given reach tube
template<class scalar>
AbstractPolyhedra<scalar> Synthesiser<scalar>::synthesiseInitialState(inputType_t inputType,AbstractPolyhedra<scalar> &input,AbstractPolyhedra<scalar> &end,AbstractPolyhedra<scalar> &dynamics)
{
  MatrixS vectors=end.getDirections();
  MatrixS abstractVectors=dynamics.getSynthVertices(vectors,m_conjugatePair,m_jordanIndex);
  MatrixS supports;
  MatrixS preSupports=end.getSupports();
  if(inputType==eNoInputs) {
    for (int row=0;row<abstractVectors.rows();row++) {
      supports.coeffRef(row,0)=preSupports.coeff(row%preSupports.rows(),0);
    }
  }
  else {
    MatrixS abstractInputVectors=-abstractVectors;
    for (int row=0;row<abstractInputVectors.rows();row++) {
      abstractInputVectors.row(row)+=vectors.col(row%vectors.cols()).transpose();
    }
    input.maximiseAll(abstractVectors,supports);
    for (int row=0;row<supports.rows();row++) {
      supports.coeffRef(row,0)=preSupports.coeff(row%preSupports.rows(),0)-supports.coeff(row,0);
    }
  }
  AbstractPolyhedra<scalar> result(m_dimension);
  result.load(abstractVectors,supports);
  return result;
}

/// Synthetises the eigenstructure of a pole location
template<class scalar>
AbstractPolyhedra<scalar> Synthesiser<scalar>::synthesiseEigenStructure(inputType_t inputType,int precision,int directions,AbstractPolyhedra<scalar> &end,AbstractPolyhedra<scalar> &dynamics)
{
  UNUSED(precision);
  AbstractPolyhedra<scalar>& init=m_initialState.getPolyhedra();
  MatrixS& templates=getTemplates(eNormalSpace,directions);
  const MatrixS& vertices=init.getVertices();
  const MatrixS& lambdas=dynamics.getVertices();
  const MatrixS& final=end.getVertices();
  if ((vertices.rows()<=0) || (lambdas.rows()<0) || (final.rows()<0)) processError(end.getName());
  MatrixS abstractVectors,combinedAbstractVectors;
  MatrixS finalVectors=kronecker(templates,final,true);//templates*final
  if (inputType==eVariableInputs) {
    MatrixS roundVectors;
    getRoundedDirections(roundVectors,templates);
    MatrixS combinedRoundVectors(templates.cols()+roundVectors.rows(),templates.rows());
    combinedRoundVectors.block(0,0,templates.cols(),templates.rows())=templates.transpose();
    combinedRoundVectors.block(templates.cols(),0,roundVectors.rows(),roundVectors.cols())=roundVectors;
    combinedAbstractVectors=dynamics.getSynthVertices(combinedRoundVectors,m_conjugatePair,m_jordanIndex);
    abstractVectors=combinedAbstractVectors.block(0,0,combinedAbstractVectors.rows(),m_dimension);
  }
  else abstractVectors=dynamics.getSynthVertices(templates,m_conjugatePair,m_jordanIndex);//templates*lambdas
  MatrixS combinedVectors=kronecker(abstractVectors,vertices);//templates*lambdas*vertices
  MatrixS combinedInputVectors(0,0);
  int numInputs=1;
  if (inputType>eNoInputs) {
    AbstractPolyhedra<scalar> &inputsSource=m_transformedInputs.getPolyhedra();
    const MatrixS& inputVertices=(inputType==eParametricInputs) ? inputsSource.getVertices() : inputsSource.getCentre();
    numInputs=inputVertices.rows();
    if (inputVertices.rows()<=0) processError(inputsSource.getName());
    //The synth vectors are multiplied by the transpose of the acceleration matrix
    MatrixS accelInputVectors=abstractVectors*this->m_pseudoInvIminJ;
    MatrixS accelInputVertices=inputVertices;
    if (m_hasOnes && (m_inputType==eVariableInputs)) {
      for (int i=0;i<accelInputVertices.cols();i++) {
        if (m_isOne[i]) accelInputVertices.coeffRef(i,0)=0;
      }
    }
    combinedInputVectors=kronecker(accelInputVectors,accelInputVertices);//templates*lambdas*inputVertices
  }
  int maxValues=finalVectors.cols()*finalVectors.rows();
  MatrixS faces(final.rows()*combinedVectors.rows()*numInputs+2*maxValues,finalVectors.cols());
  for (int i=0;i<templates.cols();i++) {
    for (int j=0;j<final.rows();j++) {
      int finalBlock=i*final.rows()+j;
      for (int k=0;k<lambdas.rows();k++) {
        int lambdaBlock=i*lambdas.rows()+k;
        for (int l=0;l<vertices.rows();l++) {
          int rowBlock=(i*lambdas.rows()+k)*vertices.rows()+l;
          rowBlock*=numInputs;
          for (int m=0;m<numInputs;m++) {
            int row=(rowBlock+m)*final.rows()+j;
            faces.row(row)=combinedVectors.row(lambdaBlock*vertices.rows()+l)-finalVectors.row(finalBlock);
          }
        }
      }
    }
  }
  if (inputType==eVariableInputs) {
  }
  MatrixS supports=MatrixS::Zero(faces.rows(),1);
  int ineqSize=final.rows()*combinedVectors.rows()*numInputs;
  faces.block(ineqSize,0,2*maxValues,finalVectors.cols())=MatrixS::Zero(2*maxValues,finalVectors.cols());
  supports.block(ineqSize,0,2*maxValues,1)=MatrixS::Ones(2*maxValues,1);
  for (int i=0;i<finalVectors.cols();i++) {
    faces.block(ineqSize+i*finalVectors.rows(),i,finalVectors.rows(),1)=MatrixS::Ones(finalVectors.rows(),1);
    faces.block(ineqSize+maxValues+i*finalVectors.rows(),i,finalVectors.rows(),1)=-MatrixS::Ones(finalVectors.rows(),1);
  }
  //std::vector<MatrixS> &nullSpace;
  //faces=getNullSpaceFaces(faces,nullSpace);
  AbstractPolyhedra<scalar> result(faces.cols());
  result.load(faces,supports);
  result.logTableau();
  result.FindFeasOrBasis(finalVectors.rows());
  ms_logger.logData(result.m_basisInverse);
  return result;
}

/// Synthetises an input/state polyhedra given a set of conditions
template<class scalar>
bool Synthesiser<scalar>::loadSynthesisedResult(synthesisType_t type, AbstractPolyhedra<scalar> &result,MatrixS& templates,refScalar tightness,int time)
{
    boost::timer timer;
    result.toInner(true);
    if (ms_trace_dynamics>=eTraceDynamics) {
      result.logTableau("Transformed Synth inputs");
    }
    result.retemplate(templates,-tightness);
    m_reachTime=time+timer.elapsed()*1000;;
    result.setCalculationTime(time);
    if (ms_trace_time) ms_logger.logData(m_reachTime,"Synthesis Time: ",true);
    switch(type)
    {
    case eInitSynth:
      m_initialState.load(result,ms_emptyMatrix,ms_emptyMatrix,eEigenSpace);
      break;
    case eInputSynth:
      m_transformedInputs.load(result,ms_emptyMatrix,ms_emptyMatrix,eEigenSpace);
      result.transform(m_pseudoEigenVectors,m_invPseudoEigenVectors);
      this->m_inputs.copy(result);
      this->m_inputs.transform(ms_emptyMatrix,this->m_sensitivity);
      break;
    case eSensitivitySynth:
      break;
    default:
      break;
    }
  return true;
}

/// Synthesises a bound on the dynamics given a known guard and eigenvectors.
template<class scalar>
AbstractPolyhedra<scalar> Synthesiser<scalar>::synthesiseDynamicBounds(inputType_t inputType,AbstractPolyhedra<scalar> &end)
{
  setInputType(inputType);
  MatrixS vectors;
  int numVertices;
  getAbstractVertices(end.getDirections(),vectors,numVertices);
  vectors.transposeInPlace();
  MatrixS supports(vectors.rows(),1);
  MatrixS endSupports=end.getSupports();
  int perTemplate=supports.rows()/endSupports.rows();
  for (int i=0;i<endSupports.rows();i++)
  {
    for (int j=0;j<perTemplate;j++) {
      supports.coeffRef(i*perTemplate+j,0)=endSupports.coeff(i,0);
    }
  }
  if (m_inputType>eNoInputs) {
    MatrixS inSupports=m_accelVertices*end.getDirections();
    demergeAccelInSupports(supports,inSupports,endSupports.rows());
  }
  AbstractPolyhedra<scalar> bounds;
  bounds.load(vectors,supports);
  bounds.removeRedundancies();
  return bounds;
}

/// Corrects the support set by the input offset
template <class scalar>
void Synthesiser<scalar>::demergeAccelInSupports(MatrixS &supports,MatrixS &inSupports,int numTemplates)
{
  if (!m_hasOnes || (m_inputType==eVariableInputs))  {
    for (int row=0;row<numTemplates;row++) {
      int pos=row*m_numVertices;
      supports.coeffRef(pos,0)-=inSupports.coeff(0,row);
      for (int point=1;point<m_numVertices;point++) {
        supports.coeffRef(pos+point,0)-=inSupports.coeff(point%inSupports.rows(),row);
      }
    }
  }
}

/// Retrieves the support set for the inputs
template <class scalar>
typename JordanMatrix<scalar>::MatrixS& Synthesiser<scalar>::getRefinedAccelInSupports()
{
  if (m_hasOnes && (m_inputType==eVariableInputs)) {
    AbstractPolyhedra<scalar>& inputDynamics=getAbstractDynamics(eParametricInputs);
    MatrixS supports;
    inputDynamics.maximiseAll(m_abstractInputVertices,supports);
    m_accelInSupports=supports.transpose();
    if (ms_trace_dynamics>=eTraceAbstraction) ms_logger.logData(m_accelInSupports,"Input Supports",true);
  }
  return m_accelInSupports;
}

/// Retrieves the reach tube at the given iteration
template <class scalar>
AbstractPolyhedra<scalar>& Synthesiser<scalar>::getRefinedAbstractReachTube(space_t space,bool guarded)
{
  boost::timer timer;
  AbstractPolyhedra<scalar>& init=m_initialState.getPolyhedra(eEigenSpace);
  AbstractPolyhedra<scalar>& dynamics=getAbstractDynamics(m_inputType);

  MatrixS& templates=getTemplates(eEigenSpace);
  if (ms_trace_time) ms_logger.logData(timer.elapsed()*1000,"Abstract Vertices: ",true);
  MatrixS supports;
  if (!dynamics.maximiseAll(m_abstractVertices,supports)) processError(dynamics.getName());

  if (m_inputType>eNoInputs) getRefinedAccelInSupports();
  if (ms_trace_dynamics>=eTraceAll) {
    traceSupports(templates,supports,dynamics,m_abstractVertices);
  }
  if (m_inputType>eNoInputs) {
    mergeAccelInSupports(supports,templates.cols());
    if (ms_trace_dynamics>=eTraceAll) {
      ms_logger.logData(m_abstractVertices,supports,"Combined",true);
    }
  }
  mergeAbstractSupports(templates,supports);
  MatrixS faces=templates.transpose();
  m_pAbstractReachTube->mergeLoad(init,faces,supports,eEigenSpace);
  AbstractPolyhedra<scalar>& result=m_pAbstractReachTube->getPolyhedra(space);
  if (guarded) getGuardedReachTube(result,space);
  if (ms_trace_dynamics>=eTraceAbstraction) result.logTableau();
  m_reachTime=timer.elapsed()*1000;
  result.setCalculationTime(m_reachTime);
  if (ms_trace_time) ms_logger.logData(m_reachTime,"Abstract Reach Time: ",true);
  return result;
}

/// Synthetises an input/state polyhedra given a set of conditions
template<class scalar>
bool Synthesiser<scalar>::synthesiseAll(synthesisType_t type,powerS iteration,int precision,int directions,inputType_t inputType,space_t space,refScalar tightness)
{
    /// sup<v.xy>=sup(sum_i(v_ix_iy_i))=sup<vx.y>
    /// (lr1 v1x1 + li1 v1x2) + (lr2 v2x2 + li2 v2x1)

    m_reachTime=-1;
    boost::timer timer;
    this->setInputType(inputType);
    if (iteration<0) iteration=-iteration;
    if (type==eDynamicSynth) {
      AbstractPolyhedra<scalar> &end=getGuardPoly().getPolyhedra();
      std::set<std::string> inequalities;
      for (int i=0;i<m_dimension;i++) {
        std::stringstream buffer;
        buffer << "(lr[" << i << "]*lr[" << i << "]+li[" << i << "]*li[" << i << "])<1";
        inequalities.insert(buffer.str());
        for (int j=i+1;j<m_dimension;j++) {
          std::stringstream buffer;
          buffer << "(w[" << i << "][0]*w[" << j << "][0]";
          for (int k=1;k<m_dimension;k++) {
            buffer << "+w[" << i << "][" << k <<"]*w[" << j << "][" << k << "]";
          }
          buffer << ")==0";
          inequalities.insert(buffer.str());
        }
      }
      for (int i=1;i<m_dimension;i++) {
        std::stringstream buffer;
        buffer << "(li[" << i-1 << "]+li[" << i << "])==0";
        inequalities.insert(buffer.str());
        buffer.str(std::string());
        buffer << "(lr[" << i-1 << "]==lr[" << i << "]) || ((li[" << i-1 << "]==0) && (li[" << i << "]==0))";
        inequalities.insert(buffer.str());
      }
      MatrixS directions=end.getDirections();
      MatrixS safeVertices=end.getVertices();
      MatrixS initVertices=m_initialState.getPolyhedra().getVertices();
      MatrixS inputVertices=m_transformedInputs.getPolyhedra().getVertices();
      MatrixS liveVertices=m_transformedInputs.getPolyhedra().getVertices();
      for (int i=0;i<directions.cols();i++) {
        std::vector<std::string> rv(m_dimension);
        MatrixS row=directions.col(i).transpose();
        for (int k=0;k<m_dimension;k++) {
          rv[k]=ms_logger.MakeWRow(row,k);
        }
        for (int input=0;input<inputVertices.rows();input++) {
          for (int init=0;init<initVertices.rows();init++) {
            std::string centre;
            MatrixS mid=initVertices.row(init)-inputVertices.row(input);
            for (int j=0;j<m_dimension;j++) mid.coeffRef(0,j)*=directions.coeff(j,i);
            std::vector<std::string> rvr(m_dimension);
            std::vector<std::string> rvi(m_dimension);
            for (int k=0;k<m_dimension;k++) {
              rvr[k]=ms_logger.MakeWRow(mid,k);
            }
            mid=initVertices.row(init)-inputVertices.row(input);
            if (i+1<directions.cols()) {
              for (int j=0;j<m_dimension;j++) mid.coeffRef(0,j)*=directions.coeff(j,i+1);
            }
            else mid=MatrixS::Zero(1,m_dimension);

            for (int k=0;k<m_dimension;k++) {
              rvi[k]=ms_logger.MakeWRow(mid,k);
            }
            std::stringstream buffer;
            buffer << "("<< ms_logger.MakeLSTerm("lr",0,rvr[0]);;
            if (rvi[0]!="(0)") buffer << "+" << ms_logger.MakeLSTerm("li",0,rvi[0]);
            for (int k=1;k<m_dimension;k++) {
              if (rvr[k]!="(0)") buffer << "+" << ms_logger.MakeLSTerm("lr",k,rvr[k]);
              if (rvi[k]!="(0)") buffer << "+" << ms_logger.MakeLSTerm("li",k,rvi[k]);
            }
            buffer << ")";
            centre=buffer.str();
            std::string inequality;
            for (int safe=0;safe<safeVertices.rows();safe++) {
              MatrixS max=safeVertices.row(safe)-inputVertices.row(input);
              std::stringstream buffer;
              buffer << "(" << centre << "<(" << ms_logger.MakeSTerm(max.coeff(0,0),rv[0]);
              for (int k=1;k<m_dimension;k++) {
                buffer << "+" << ms_logger.MakeSTerm(max.coeff(0,k),rv[k]);
              }
              buffer << "))";
              if (safe!=0) inequality+="\n  || ";
              inequality+=buffer.str();
            }
            inequalities.insert(inequality);
            for (int live=0;live<liveVertices.rows();live++) {
              MatrixS min=liveVertices.row(live)-inputVertices.row(input);
              std::stringstream buffer;
              buffer << ">(" << ms_logger.MakeSTerm(min.coeff(0,0),rv[0]);
              for (int k=1;k<m_dimension;k++) {
                buffer << "+" << ms_logger.MakeSTerm(min.coeff(0,k),rv[k]);
              }
              buffer << ")";
              inequalities.insert(centre+buffer.str());
            }
          }
        }
      }
      std::stringstream buffer;
      buffer << "extern float lr[" << m_dimension << "];\n";
      buffer << "extern float li[" << m_dimension << "];\n";
      buffer << "extern float w[" << m_dimension << "][" << m_dimension << "];\n";
      buffer << "void main()\n{";
      std::string result=buffer.str();
      result+="if (";
      std::set<std::string>::iterator it=inequalities.begin();
      if (it!=inequalities.end()) {
        result+="("+*it+")\n";
        it++;
      }
      for (;it!=inequalities.end();it++) result+=" && ("+*it+")\n";
      result+=") {\n    assert(0);\n  }\n}\n";
      ms_logger.logData(result);
      return true;
    }
    else if (type==eEigenSynth) {
      m_closedLoop.getAbstractDynamics(iteration,precision,inputType);
      AbstractPolyhedra<scalar> &end=getGuardPoly().getPolyhedra();
      AbstractPolyhedra<scalar> eigenVectors=synthesiseEigenStructure(inputType,precision,directions,end,m_closedLoop.getAbstractDynamics(inputType));
      eigenVectors.logTableau("EigenStructure:");//templog
    }
    else {
      AbstractPolyhedra<scalar> &end=getGuardPoly().getPolyhedra(eEigenSpace);
      AbstractPolyhedra<scalar>& dynamics=getAbstractDynamics(iteration,precision,inputType);
      MatrixS& templates=getTemplates(eEigenSpace,directions);
      AbstractPolyhedra<scalar> &co_space=(type==eInitSynth) ? m_transformedInputs.getPolyhedra(eEigenSpace) : this->getInitialState(eEigenSpace);
      m_reachTime=timer.elapsed()*1000;
      if (tightness==0) tightness=1e-6;
      if (type==eInitSynth) {
        AbstractPolyhedra<scalar> result=synthesiseInitialState(inputType,co_space,end,dynamics);
        return loadSynthesisedResult(type,result,templates,tightness,timer.elapsed()*1000);
      }
      AbstractPolyhedra<scalar> result=synthesiseInputs(inputType,precision,co_space,end,dynamics,templates,tightness);
      return loadSynthesisedResult(type,result,templates,tightness,m_reachTime);
    }
    return false;
  }

template <class scalar>
int Synthesiser<scalar>::loadPoles(const std::string &data,size_t pos)
{
  commands_t command;
  pos=ms_logger.getCommand(command,data,pos);
  MatrixS poles(m_dimension,m_dimension);
  int result=ms_logger.StringToMat(poles,data,pos);
  ms_logger.logData(poles,"cl dynamics");//templog
  m_closedLoop.loadJordan(poles);
  return result;
}

/// Calculates the closed loop dynamics given a plant and a controller
template <class scalar>
bool Synthesiser<scalar>::makeClosedLoop(bool useObserver,bool makeReference,bool makeNoise)
{
  UNUSED(useObserver);
  MatrixS dynamics;
  if (m_outputSensitivity.cols()>0) {
    dynamics=m_dynamics-m_sensitivity.block(0,0,m_dimension,m_fdimension)*m_feedback*m_outputSensitivity;
  }
  else {
    dynamics=m_dynamics-m_sensitivity.block(0,0,m_dimension,m_fdimension)*m_feedback;
  }
  int fdimension= (makeReference || (m_reference.getDimension()>0)) ? 0 : m_fdimension;
  MatrixS sensitivity=m_sensitivity.block(0,fdimension,m_dimension,m_idimension-fdimension);
  m_closedLoop.setInputType(eParametricInputs);
  AbstractPolyhedra<scalar> inputs=generateFeedbackInput(fdimension,makeNoise,sensitivity);
  if (ms_trace_dynamics>=eTraceDynamics) {
    ms_logger.logData(dynamics,"Loading Closed Loop");
  }
  m_closedLoop.setParams(m_paramValues);
  m_closedLoop.changeDimensions(m_dimension,sensitivity.cols(),0,0);
  bool result=m_closedLoop.load(dynamics,sensitivity,m_guard.getPolyhedra(),m_initialState.getPolyhedra(),inputs,m_safeReachTube.getPolyhedra());
  if (result && (ms_trace_dynamics>=eTraceDynamics)) {
    ms_logger.logData(m_closedLoop.getDescription());
  }
  return result;
}

template <class scalar>
void Synthesiser<scalar>::processFiles(stringList &files,displayType_t displayType,space_t space,bool interval,optionList_t &options)
{
  for (stringList::iterator i=files.begin();i!=files.end();i++) {
    int pos=loadFromFile(*i);
    if (pos<0) {
      ms_logger.logData("Error loading file ",false);
      ms_logger.logData(*i);
      continue;
    }
    if ((options.size()>0) && (processOptions(options,displayType,space,interval,false)<0)) continue;
    process(displayType,space,interval);
    while (pos<m_source.length()) {
      pos=m_source.find('|',pos);
      if (pos<0) break;
      pos=load(m_source,pos+1);
      if (pos<0) break;
      process(displayType,space,interval,true);
    }
  }
}

// Processes a problem stated by the inut options
template <class scalar>
int Synthesiser<scalar>::processOptions(optionList_t &options,displayType_t displayType,space_t space,bool interval,bool run)
{
  if (options.size()<=0) return 0;
  if (options[eParamStr].size()>0) {
    if (ms_logger.StringToDim(m_paramValues,options[eParamStr])<0) return -1;
    if (m_paramValues.coeff(eNumBits,0)>0) {
      functions<mpfr::mpreal>::setDefaultPrec(m_paramValues.coeff(eNumBits,0));
    }
    traceDynamics((traceDynamics_t)m_paramValues.coeff(eTraceLevel,0));
    traceSimplex((traceTableau_t)m_paramValues.coeff(eTraceLevel,1),(traceVertices_t)m_paramValues.coeff(eTraceLevel,2));
    if (m_paramValues.coeff(eNumStates,0)>0) changeDimensions(m_paramValues.coeff(eNumStates,0),m_paramValues.coeff(eNumInputs,0)+m_paramValues.coeff(eNumVarInputs,0),m_paramValues.coeff(eNumOutputs,0),m_paramValues.coeff(eNumFeedbacks,0));
    m_sensitivity.conservativeResize(m_paramValues.coeff(eNumStates,0),m_paramValues.coeff(eNumInputs,0)+m_paramValues.coeff(eNumVarInputs,0));
    m_inputType=(m_paramValues.coeff(eNumVarInputs,0)>0) ? eVariableInputs : ((m_paramValues.coeff(eNumInputs,0)>0) ? eParametricInputs : eNoInputs);
  }
  if ((options[eARMAXStr].size()>0) && (loadARMAXModel(options[eARMAXStr])<0))      return -1;
  if ((options[eGuardStr].size()>0) && (loadGuard(options[eGuardStr])<0))           return -1;
  if ((options[sGuardStr].size()>0) && (loadSafeReachTube(options[sGuardStr])<0))   return -1;
  if ((options[oGuardStr].size()>0) && (loadOutputGuard(options[oGuardStr])<0))     return -1;
  if ((options[eDynamicsStr].size()>0) && (loadDynamics(options[eDynamicsStr])<0))  return -1;
  if ((options[eInitStr].size()>0) && (loadInitialState(options[eInitStr])<0))      return -1;
  if ((options[iSenseStr].size()>0) && (loadSensitivities(options[iSenseStr])<0))   return -1;
  if ((options[eInputStr].size()>0) && (loadInputs(options[eInputStr])<0))          return -1;
  if ((options[eTemplateStr].size()>0) && (loadTemplates(options[eTemplateStr])<0)) return -1;
  if ((options[eRefStr].size()>0) && (loadReference(options[eRefStr])<0))           return -1;
  if ((options[eControlStr].size()>0) && (loadController(options[eControlStr])<0))  return -1;
  if (run) process(displayType,space,interval);
  return 0;
}

template <class scalar>
bool Synthesiser<scalar>::process(const displayType_t displayType,const space_t space,const bool interval,const bool append)
{
  try {
    func::ms_isImprecise=false;
    int iter=m_paramValues.coeff(eNumSteps,0);
    int maxIter=m_paramValues.coeff(eNumSteps,1);
    int stepIter=m_paramValues.coeff(eNumSteps,2);
    if (maxIter<=0) maxIter=iter+1;
    if (stepIter<=0) stepIter=1;
    int precision=m_paramValues.coeff(eLogFaces,0);
    int maxPrecision=m_paramValues.coeff(eLogFaces,1);
    int stepPrecision=m_paramValues.coeff(eLogFaces,2);
    if (maxPrecision<=0) maxPrecision=precision;
    if (stepPrecision<=0) stepPrecision=1;
    int directions=m_paramValues.coeff(eLogDirections,0);
    int maxDirections=m_paramValues.coeff(eLogDirections,1);
    int stepDirections=m_paramValues.coeff(eLogDirections,2);
    if (maxDirections<=0) maxDirections=directions;
    if (stepDirections<=0) stepDirections=1;
    int tightness=m_paramValues.coeff(eTightness,0);
    int maxTightness=m_paramValues.coeff(eTightness,1);
    int stepTightness=m_paramValues.coeff(eTightness,2);
    if (maxTightness<=0) maxTightness=tightness;
    if (stepTightness==0) stepTightness=1;
    int width=((maxIter-iter)/stepIter)*((maxPrecision-precision+1)/stepPrecision);
    if (m_synthType!=eReachTubeSynth) {
      m_dynamicParams.resize(eNumFinalParameters,0);
      save(displayType,space,interval);
    }
    for (;directions<=maxDirections;directions+=stepDirections) {
      MatrixS faces=makeLogahedralTemplates(directions,eEigenSpace).transpose();//TODO: the space in makelogahedral looks counterintuitive
      MatrixS supports(faces.rows(),width);
      //MatrixS dynamicSupports(0,width);
      m_dynamicParams.resize(eNumFinalParameters,width);
      int col=0;
      for (iter=m_paramValues.coeff(eNumSteps,0);iter<maxIter;iter+=stepIter) {
        for (tightness=m_paramValues.coeff(eTightness,0);tightness<=maxTightness;tightness+=stepTightness) {
          for (precision=m_paramValues.coeff(eLogFaces,0);precision<=maxPrecision;precision+=stepPrecision) {
            powerS iteration=iter;
            switch(m_synthType) {
            case eReachTubeSynth: {
                if (iter==0) iteration=this->calculateIterations(m_initialState.getPolyhedra(eEigenSpace),m_inputType);
                refScalar longIter=iteration;
                getAbstractReachTube(iteration,precision,directions,m_inputType,space);
                m_dynamicParams.coeffRef(eFinalIterations,col)=longIter;
                m_dynamicParams.coeffRef(eFinalPrecision,col)=precision;
                m_dynamicParams.coeffRef(eFinalLoadTime,col)=scalar(m_loadTime);
                m_dynamicParams.coeffRef(eFinalReachTime,col)=scalar(m_reachTime);
                supports.col(col++)=m_pAbstractReachTube->getPolyhedra(space).getSupports();
                if (!m_safeReachTube.isEmpty()) {
                  AbstractPolyhedra<scalar> bounds=synthesiseDynamicBounds(m_inputType,m_safeReachTube.getPolyhedra(eEigenSpace));
                  for(int i=0;(i<5) && refineAbstractDynamics(bounds);i++) {
                    getRefinedAbstractReachTube(space);
                    m_dynamicParams.conservativeResize(m_dynamicParams.rows(),m_dynamicParams.cols()+1);
                    supports.conservativeResize(supports.rows(),supports.cols()+1);
                    m_dynamicParams.coeffRef(eFinalIterations,col)=longIter;
                    m_dynamicParams.coeffRef(eFinalPrecision,col)=precision;
                    m_dynamicParams.coeffRef(eFinalLoadTime,col)=scalar(m_loadTime);
                    m_dynamicParams.coeffRef(eFinalReachTime,col)=scalar(m_reachTime);
                    supports.col(col++)=m_pAbstractReachTube->getPolyhedra(space).getSupports();
                  }
                }
              }
              break;
            case eCEGISSynth:
              makeCEGISFiles();
              break;
            default: {
                if (iteration==0) iteration=func::ms_infPower;
                refScalar longIter=iteration;
                refScalar tight=tightness;
                tight/=100;
                synthesiseAll(m_synthType,iteration,precision,directions,m_inputType,space,tight);
                m_dynamicParams.resize(eNumFinalParameters,1);
                m_dynamicParams.coeffRef(eFinalIterations,0)=longIter;
                m_dynamicParams.coeffRef(eFinalPrecision,0)=precision;
                m_dynamicParams.coeffRef(eFinalLoadTime,0)=scalar(m_loadTime);
                m_dynamicParams.coeffRef(eFinalReachTime,0)=scalar(m_reachTime);
                save(displayType,space,interval,true);
              }
            }
          }
        }
      }
      if (m_synthType==eReachTubeSynth) {
        for (int row=0;row<supports.rows();row++) {
          for (int col=0;col<supports.cols();col++) {
            if (func::toUpper(supports.coeff(row,col))>func::ms_infPower) supports.coeffRef(row,col)=func::ms_infinity;
          }
        }
        m_pAbstractReachTube->load(faces,supports,space);
        save(displayType,space,interval,(directions>m_paramValues.coeff(eLogDirections,0)) || append);
        if (ms_logger.ms_useConsole) {
          ms_logger.logData(m_pAbstractReachTube->getPolyhedra(space).getDescription(displayType,interval,true));
        }
      }
    }
  }
  catch(std::string &error) {
    ms_logger.logData("Error processing "+this->m_name);
    ms_logger.logData(error);
  }
}

/// Finds the statespace guard given an output guard
template <class scalar>
AbstractPolyhedra<scalar>& Synthesiser<scalar>::calculateGuardFromOutput()
{
  AbstractPolyhedra<scalar>& polyhedra=m_safeReachTube.getPolyhedra();
  m_outputGuard.getTransformedPolyhedra(polyhedra,ms_emptyMatrix,m_outputSensitivity);
  return m_safeReachTube.getPolyhedra(eEigenSpace,true);
}

/// Loads a controller candidate for the system
template <class scalar>
int Synthesiser<scalar>::loadController(const std::string &data,size_t pos)
{
  boost::timer timer;
  commands_t command;
  pos=ms_logger.getCommand(command,data,pos);
  m_feedback.resize(m_idimension,(m_odimension>0) ? m_odimension : m_dimension);
  size_t result=ms_logger.StringToMat(m_feedback,data,pos);
  if (result>0) makeClosedLoop();
  if (ms_trace_time) ms_logger.logData(timer.elapsed()*1000,"Controller time:",true);
  return result;
}

/// Loads a reference input set for the system
template <class scalar>
int Synthesiser<scalar>::loadReference(const std::string &data,size_t pos,bool vertices)
{
  boost::timer timer;
  int result=m_reference.loadData(data,pos,vertices);
  if (ms_trace_time) ms_logger.logData(timer.elapsed()*1000,"Reference time:",true);
  return result;
}

///Creates a c header file for CEGIS
template <class scalar>
std::string Synthesiser<scalar>::makeCEGISHeader(bool intervals)
{
  std::stringstream result;
  result << "#define _DIMENSION " << m_dimension << "\n";
  result << "typedef control_floatt vectort[_DIMENSION];\n"
         << "typedef control_floatt matrixt[_DIMENSION][_DIMENSION];\n";
  result << "struct coefft\n"
         << "{\n"
         << "  vectort coeffs;\n";
  if (intervals) result << "  vectort uncertainty;\n";
  result << "};\n";
  result << "struct transformt\n"
         << "{\n"
         << "  control_floatt coeffs[_DIMENSION][_DIMENSION];\n";
  if (intervals) result << "  control_floatt uncertainty[_DIMENSION][_DIMENSION];\n";
  result << "};\n";
  return result.str();
}

///Creates a c header file for CEGIS
template <class scalar>
std::string Synthesiser<scalar>::makeCEGISDescription(bool intervals)
{
  std::stringstream result;
  MatrixS T=getReachableCanonicalTransformMatrix();
  MatrixS invT=T.inverse();
  MatrixS controllable=T*m_dynamics*invT;
  if (ms_trace_dynamics>=eTraceDynamics) {
    ms_logger.logData(controllable,"Controllable");
  }
  MatrixS coefficients=controllable.row(0);
  int totalbits=m_paramValues.coeff(eNumBits,1);
  int fracbits=m_paramValues.coeff(eNumBits,2);
  int multbits=0;
  if (totalbits<=0) {
    totalbits=m_paramValues.coeff(eNumBits,0);
    if (totalbits<=0) totalbits=func::getDefaultPrec();
    fracbits=totalbits;
  }
  else if (fracbits==0) fracbits=totalbits>>1;
  result << "struct implt impl={ .int_bits=" << (totalbits-fracbits)
         << ", .frac_bits=" << fracbits
         << ", .mult_bits=" << multbits
         << "};\n";
  result << "struct coefft ";
  result << ms_logger.MatToC("plant",coefficients,intervals);
  result << "struct transformt " << ms_logger.MatToC("transform",invT,intervals);
  result << "matrixt dynamics" << ms_logger.MatToC("",m_dynamics,intervals);
  result << "vectort sensitivity" << ms_logger.MatToC("",m_sensitivity.transpose(),intervals);
  result << "#ifdef __CPROVER\n";
  result << "extern vectort controller;\n";
  result << "#else\n";
  result << "vectort controller" << ms_logger.MatToC("",m_feedback,intervals);;
  result << "#endif\n";
  if (m_safeReachTube.isEmpty() && !m_outputGuard.isEmpty()) calculateGuardFromOutput();
  MatrixS vectors=m_safeReachTube.getPolyhedra().getFaceDirections();
  MatrixS support=m_safeReachTube.getPolyhedra().getSupports().transpose();
  AbstractPolyhedra<scalar> K=getControllerInBounds(m_safeReachTube.getPolyhedra());
  K.toOuter(true);
  int controllerOrBlocks;
  AbstractPolyhedra<scalar> K2=getControllerDynBounds(m_safeReachTube.getPolyhedra(),controllerOrBlocks);
  result << "void boundController()\n{\n";
  result << ms_logger.IneToC("verify_assume","(control_floatt)","controller",K.getFaceDirections(),K.getSupports()) << ";\n";
  result << ms_logger.IneToC("verify_assume","(control_floatt)","controller",K2.getFaceDirections(),K2.getSupports(),false,controllerOrBlocks) << ";\n";
  result << "}\n";
  if (m_feedback.rows()>0) {
    MatrixS vertices=m_initialState.getPolyhedra().getVertices();
    makeClosedLoop(false,false,true);
    const MatrixS inputVertices=m_closedLoop.getInputVertices(eNormalSpace,true);
    result << "#define _NUM_VERTICES " << vertices.rows() << "\n";
    result << "#define _NUM_INPUT_VERTICES " << inputVertices.rows() << "\n";
    result << "#define _NUM_VECTORS " << vectors.rows() << "\n";
    if (vertices.rows()>0) {
      result << "control_floatt vertices[_NUM_VERTICES][_DIMENSION]";
      result << ms_logger.MatToC("",vertices);
      result << "control_floatt input_vertices[_NUM_INPUT_VERTICES][_DIMENSION]";
      result << ms_logger.MatToC("",inputVertices);
      result << "control_floatt accel_vertices[_NUM_INPUT_VERTICES][_DIMENSION];\n";
      result << "control_floatt vectors[_NUM_VECTORS][_DIMENSION]";
      result << ms_logger.MatToC("",vectors);
      result << "control_floatt reach_vertices[_NUM_INPUT_VERTICES][_NUM_VERTICES][_DIMENSION];\n";
      result << "control_floatt supports[_NUM_VECTORS]";
      result << ms_logger.MatToC("",support);
      result << "control_floatt accelsupports[_NUM_INPUT_VERTICES][_NUM_VECTORS];\n\n";
    }
    else {
      result << "control_floatt vertices[1][1]={{1}};\n";
    }
  }
  return result.str();
}

///Creates a list of iterations to check with CEGIS
template <class scalar>
std::string Synthesiser<scalar>::makeCEGISIterations(std::string &existing)
{
  std::stringstream result;
  if (func::isZero(m_feedback.norm())) return "#define NO_FEEDBACK\n";
  makeClosedLoop(false,false,true);
  if (m_closedLoop.isDivergent()) return "#define DIVERGENT\n";
  m_closedLoop.getRefinedDynamics(4);
  AbstractPolyhedra<scalar> bounds=m_closedLoop.synthesiseDynamicBounds(m_inputType,m_safeReachTube.getPolyhedra(eEigenSpace));
  powerList counterexamples;
  bool fail=m_closedLoop.findCounterExampleIterations(counterexamples,bounds);
  if (fail && counterexamples.empty()) counterexamples[1]=0;
  if (!counterexamples.empty()) {
    result << "#define POINTS_PER_ITER\n";
    result << "#define _NUM_ITERATIONS " << counterexamples.size() << "\n";
    result << "int iterations[_NUM_ITERATIONS]={";
    for (typename powerList::iterator it=counterexamples.begin();it!=counterexamples.end();it++) {
      if (it!=counterexamples.begin()) result << ",";
      result << it->first;
    }
    result << "};\n";

    result << "int iter_vertices[_NUM_ITERATIONS][2]={";
    AbstractPolyhedra<scalar> &safe=m_safeReachTube.getPolyhedra();
    MatrixS vertices=m_initialState.getPolyhedra().getVertices();
    MatrixS inputVertices=m_closedLoop.getInputVertices();
    for (typename powerList::iterator it=counterexamples.begin();it!=counterexamples.end();it++) {
      if (it!=counterexamples.begin()) result << ",";
      MatrixS dynamics=m_closedLoop.getPseudoDynamics(it->first).transpose();
      if (ms_trace_dynamics>eTraceDynamics) {
        ms_logger.logData(dynamics,"dynamics");
        ms_logger.logData(vertices,"vertices");
        ms_logger.logData(inputVertices,"inputVertices");
      }
      bool found=false;
      for (int i=0;i<vertices.rows();i++) {
        for (int j=0;j<inputVertices.rows();j++) {
          MatrixS point=(vertices.row(i)-inputVertices.row(j))*dynamics+inputVertices.row(j);
          ms_logger.logData(point,"point");
          if (!safe.isInside(point)) {
            result << "{" << i << "," << j << "}";
            i=vertices.rows();
            found=true;
            break;
          }
        }
      }
      if (!found) result << "{-1,-1}";
    }
    result << "};\n";
  }
  else if (fail) {
    return "#define FAILED\n";
  }
  return result.str();
}

///Creates a c header file for CEGIS
template <class scalar>
bool Synthesiser<scalar>::makeCEGISFiles()
{
  std::ofstream headerFile;
  headerFile.open("types.h");
  if (!headerFile.is_open()) return false;
  ms_logger.setPrecision(12);
  std::string data=makeCEGISHeader();
  headerFile.write(data.data(),data.size());
  headerFile.close();

  data=makeCEGISDescription();
  std::string oldIters;
  std::string iters=makeCEGISIterations(oldIters);
  std::ofstream sucess_file;
  sucess_file.open("output.txt");
  if (!sucess_file.is_open()) return false;
  std::string result=iters.empty() ? "SUCCESS" : "FAIL";
  sucess_file.write(result.data(),result.size());
  data+=iters;
  sucess_file.close();
  std::ofstream file;
  file.open("system.h");
  if (!file.is_open()) return false;
  file.write(data.data(),data.size());
  file.close();
  return true;
}

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

}


/**
 * Copyright (C) Omar Thor <omarthoro@gmail.com> - All Rights Reserved
 * Unauthorized copying of this file, via any medium is strictly prohibited
 * Proprietary and confidential
 *
 * Written by Omar Thor <omarthoro@gmail.com>, 2017
 */

#ifndef SP_ALGO_NN_NORMALIZE_HPP
#define SP_ALGO_NN_NORMALIZE_HPP

#include <boost/assert.hpp>
#include "sp/config.hpp"
#include "matrix.hpp"
#include "types.hpp"
#include "layer/detail/layers.hpp"

SP_ALGO_NN_NAMESPACE_BEGIN

/**
 * \file Utility to normalize values for the network specified.
 */

/**
 * \brief Normalize and scale all values and translate to output maximum
 *
 * Note: considers all element at once for min/max of samples
 */
template<typename Network>
void normalize_and_scale(Network& network, sample_vector_type& vec) {
    auto [a, b] = network.out_range();
    float_t min = b;
    float_t max = a;
    for(const auto& v : vec) {
        //find min, max
        std::for_each(v.data(), v.data() + v.size(), [&](auto& x) {
            max = std::max(x, max);
            min = std::min(x, min);
        });
    }

    /**
     * Perform translation and scaling at the same time: First, scale value v_i
     * to [min, max], then rescale to [a, b].
     */
    for(auto& v : vec) {
        v = ((b - a) * (v - min) / ( max - min)) + a;
    }
}

/**
 * \brief Normalize input_type given the network parameters into an input_type
 *
 * @param network
 * @return
 */
template<typename Network>
auto prepare_batch(Network& network, const size_t& batch_size, sample_vector_type& samples) {
    BOOST_ASSERT(batch_size >= 1);
    BOOST_ASSERT(!samples.empty());

    const size_t input_count = samples.size();
    const size_t batch_count = input_count / batch_size;

    const auto& sample_dims = samples[0].dimensions();

    std::vector<tensor_4> vec(
        batch_count,
        tensor_4(
            batch_size,
            sample_dims[0],
            sample_dims[1],
            sample_dims[2]
        )
    );

    /**
     * Merge input sample_type into input_type batch
     */
    for(size_t b = 0; b < batch_count; ++b) {
        for(size_t s = 0; s < batch_size; ++s) {
            BOOST_ASSERT_MSG(
                detail::validate_dimensions<typename Network::input_dims>(samples[b*batch_size+s]),
                "Dimensions of sample matches network input dimension"
            );
            vec[b].chip(s, 0) = samples[b*batch_size + s];
        }
    }

    return vec;
}

/**
 * \brief Prepare samples into batch format of input_type given the network parameters into an input_type
 *
 * @param network
 * @return
 */
template<typename Network>
auto prepare_batch(Network& network, sample_vector_type& samples) {
    return prepare_labels(network, 1, samples);
}

/**
 * \brief Prepare a batch of input_type given the network parameters into an input_type
 *
 * @param network
 * @return
 */
template<typename Network>
auto prepare_batch(Network& network, sample_type& sample) {
    sample_vector_type vec{sample};
    return prepare_labels(network, 1, vec)[0];
}

namespace detail {
    /**
     * \brief Convert class vector type to a a vector of output_type
     */
    template<typename Network>
    auto normalize_label_to_vector_helper(Network& network, const size_t& batch_size, class_vector_type& classes) {

        const size_t class_count = classes.size();
        const size_t batch_count = class_count / batch_size;

        /* Output is a vector of output_type */
        std::vector<tensor_4> vec(
            /* of dimensions (1, 1, 1, the size of the output of the last layer) */
            batch_count, tensor_4(batch_size, Network::output_dims::size, 1, 1)
        );

        auto[min, max] = network.out_target_range();

        for(size_t b = 0; b < batch_count; ++b) {
            /* default to minimum value */
            vec[b].setConstant(min);
            for(size_t s = 0; s < batch_size; ++s) {
                const size_t idx = b * batch_size + s;

                BOOST_ASSERT_MSG(
                    classes[idx] < Network::output_dims::size,
                    "Class value can not exceeds network output size"
                );
                /* set the max value to output layer max */
                vec[b](s, classes[idx], 0, 0) = max;
            }
        }

        return vec;
    }
}

/**
 * \brief Normalize a vector of classes
 */
template<typename Network>
std::vector<tensor_4> prepare_labels(     Network& network,
                                        const size_t& batch_size,
                                        class_vector_type& classes) {
    return detail::normalize_label_to_vector_helper(network, batch_size, classes);
}

/**
 * \brief Normalize a vector of classes
 */
template<typename Network>
std::vector<tensor_4> prepare_batch(     Network& network,
                                        class_vector_type& classes) {
    return prepare_labels(network, 1, classes);
}

/**
 * \brief Converters a vector of a vector to a vector of sample_type
 */
sample_vector_type nested_vector_to_sample_vector(std::vector<std::vector<float_t>> vectors) {
    if(vectors.empty()) {
        return {};
    } else {
        const size_t vector_element_count = vectors[0].size();
        sample_vector_type res(vectors.size(), sample_type(1, 1, vector_element_count));
        size_t idx = 0;
        for(auto& v : vectors) {
            auto& r = res[idx];
            float_t* start = r.data();
            BOOST_ASSERT((static_cast<size_t>(std::distance(start, r.data() + r.size())) == vector_element_count));
            BOOST_ASSERT(vector_element_count == v.size());
            std::copy(v.begin(), v.end(), start);
            ++idx;
        }
        return res;
    }
}
SP_ALGO_NN_NAMESPACE_END

#endif	/* SP_ALGO_NN_NORMALIZE_HPP */


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2013 Nikhar Agrawal
//  Copyright 2013 Christopher Kormanyos
//  Copyright 2014 John Maddock
//  Copyright 2013 Paul Bristow
//  Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef _BOOST_POLYGAMMA_2013_07_30_HPP_
  #define _BOOST_POLYGAMMA_2013_07_30_HPP_

#include <boost/math/special_functions/factorials.hpp>
#include <boost/math/special_functions/detail/polygamma.hpp>
#include <boost/math/special_functions/trigamma.hpp>

namespace boost { namespace math {

  template<class T, class Policy>
  inline typename tools::promote_args<T>::type polygamma(const int n, T x, const Policy& pol)
  {
     //
     // Filter off special cases right at the start:
     //
     if(n == 0)
        return boost::math::digamma(x, pol);
     if(n == 1)
        return boost::math::trigamma(x, pol);
     //
     // We've found some standard library functions to misbehave if any FPU exception flags
     // are set prior to their call, this code will clear those flags, then reset them
     // on exit:
     //
     BOOST_FPU_EXCEPTION_GUARD
     //
     // The type of the result - the common type of T and U after
     // any integer types have been promoted to double:
     //
     typedef typename tools::promote_args<T>::type result_type;
     //
     // The type used for the calculation.  This may be a wider type than
     // the result in order to ensure full precision:
     //
     typedef typename policies::evaluation<result_type, Policy>::type value_type;
     //
     // The type of the policy to forward to the actual implementation.
     // We disable promotion of float and double as that's [possibly]
     // happened already in the line above.  Also reset to the default
     // any policies we don't use (reduces code bloat if we're called
     // multiple times with differing policies we don't actually use).
     // Also normalise the type, again to reduce code bloat in case we're
     // called multiple times with functionally identical policies that happen
     // to be different types.
     //
     typedef typename policies::normalise<
        Policy,
        policies::promote_float<false>,
        policies::promote_double<false>,
        policies::discrete_quantile<>,
        policies::assert_undefined<> >::type forwarding_policy;
     //
     // Whew.  Now we can make the actual call to the implementation.
     // Arguments are explicitly cast to the evaluation type, and the result
     // passed through checked_narrowing_cast which handles things like overflow
     // according to the policy passed:
     //
     return policies::checked_narrowing_cast<result_type, forwarding_policy>(
        detail::polygamma_imp(n, static_cast<value_type>(x), forwarding_policy()),
        "boost::math::polygamma<%1%>(int, %1%)");
  }

  template<class T>
  inline typename tools::promote_args<T>::type polygamma(const int n, T x)
  {
      return boost::math::polygamma(n, x, policies::policy<>());
  }

} } // namespace boost::math

#endif // _BOOST_BERNOULLI_2013_05_30_HPP_


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

/**
 * \file
 *
 * \brief The main file for the BIAS-SGD matrix factorization algorithm.
 *
 * This file contains the main body of the BIAS-SGD matrix factorization
 * algorithm.
 */

#include <graphlab/util/stl_util.hpp>
#include <graphlab.hpp>
#include "eigen_serialization.hpp"
#include <Eigen/Dense>
#include <graphlab/macros_def.hpp>

typedef Eigen::VectorXd vec_type;
typedef Eigen::MatrixXd mat_type;

//when using negative node id range, we are not allowed to use
//0 and 1 so we add 2.
const static int SAFE_NEG_OFFSET=2;
static bool debug;
int iter = 0;
float itmBiasStep = 1e-4;
float itmBiasReg = 1e-4;
float usrBiasStep = 1e-4;
float usrBiasReg = 1e-4;
float usrFctrStep = 1e-4;
float usrFctrReg = 1e-4;
float itmFctrStep = 1e-4;
float itmFctrReg = 1e-4; //gamma7
float itmFctr2Step = 1e-4;
float itmFctr2Reg = 1e-4;
/**
 * \ingroup toolkit_matrix_pvecization
 *
 * \brief the vertex data type which contains the latent pvec.
 *
 * Each row and each column in the matrix corresponds to a different
 * vertex in the BIASSGD graph.  Associated with each vertex is a pvec
 * (vector) of latent parameters that represent that vertex.  The goal
 * of the BIASSGD algorithm is to find the values for these latent
 * parameters such that the non-zero entries in the matrix can be
 * predicted by taking the dot product of the row and column pvecs.
 */
struct vertex_data {
  /**
   * \brief A shared "constant" that specifies the number of latent
   * values to use.
   */
  static size_t NLATENT;
  /** \brief The number of times this vertex has been updated. */
  uint32_t nupdates;
  /** \brief The latent pvec for this vertex */
  vec_type pvec;
  vec_type weight;
  double bias;
  /**
   * \brief Simple default constructor which randomizes the vertex
   *  data
   */
  vertex_data() : nupdates(0) { randomize(); }
  /** \brief Randomizes the latent pvec */
  void randomize() { pvec.resize(NLATENT); pvec.setRandom(); weight.resize(NLATENT); weight.setRandom(); }
  /** \brief Save the vertex data to a binary archive */
  void save(graphlab::oarchive& arc) const {
    arc << nupdates << pvec << weight << bias;
  }
  /** \brief Load the vertex data from a binary archive */
  void load(graphlab::iarchive& arc) {
    arc >> nupdates >> pvec >> weight >> bias;
  }
}; // end of vertex data

/**
 * \brief The edge data stores the entry in the matrix.
 *
 * In addition the edge data svdppo stores the most recent error estimate.
 */
struct edge_data : public graphlab::IS_POD_TYPE {
  /**
   * \brief The type of data on the edge;
   *
   * \li *Train:* the observed value is correct and used in training
   * \li *Validate:* the observed value is correct but not used in training
   * \li *Predict:* The observed value is not correct and should not be
   *        used in training.
   */
  enum data_role_type { TRAIN, VALIDATE, PREDICT  };

  /** \brief the observed value for the edge */
  float obs;

  /** \brief The train/validation/test designation of the edge */
  data_role_type role;

  /** \brief basic initialization */
  edge_data(float obs = 0, data_role_type role = PREDICT) :
    obs(obs), role(role) { }

}; // end of edge data

/**
 * \brief The graph type is defined in terms of the vertex and edge
 * data.
 */
typedef graphlab::distributed_graph<vertex_data, edge_data> graph_type;

#include "implicit.hpp"

double extract_l2_error(const graph_type::edge_type & edge);

/**
 * \brief Given a vertex and an edge return the other vertex in the
 * edge.
 */
inline graph_type::vertex_type
get_other_vertex(graph_type::edge_type& edge,
    const graph_type::vertex_type& vertex) {
  return vertex.id() == edge.source().id()? edge.target() : edge.source();
}; // end of get_other_vertex

/**
 * \brief The gather type used to construct XtX and Xty needed for the BIASSGD
 * update
 *
 * To compute the ALS update we need to compute the sum of
 * \code
 *  sum: XtX = nbr.pvec.transpose() * nbr.pvec
 *  sum: Xy  = nbr.pvec * edge.obs
 * \endcode
 * For each of the neighbors of a vertex.
 *
 * To do this in the Gather-Apply-Scatter model the gather function
 * computes and returns a pair consisting of XtX and Xy which are then
 * added. The gather type represents that tuple and provides the
 * necessary gather_type::operator+= operation.
 *
 */
class gather_type {
  public:
    /**
     * \brief Stores the current sum of nbr.pvec.transpose() *
     * nbr.pvec
     */

    /**
     * \brief Stores the current sum of nbr.pvec * edge.obs
     */
    vec_type pvec;
    vec_type weight;
    double bias;

    /** \brief basic default constructor */
    gather_type() { }

    /**
     * \brief This constructor computes XtX and Xy and stores the result
     * in XtX and Xy
     */
    gather_type(const vec_type& X, const vec_type & _weight, double _bias) {
      pvec = X;
      bias = _bias;
      weight = _weight;
    } // end of constructor for gather type

    /** \brief Save the values to a binary archive */
    void save(graphlab::oarchive& arc) const { arc << pvec << bias << weight; }

    /** \brief Read the values from a binary archive */
    void load(graphlab::iarchive& arc) { arc >> pvec >> bias >> weight; }

    /**
     * \brief Computes XtX += other.XtX and Xy += other.Xy updating this
     * tuples value
     */
    gather_type& operator+=(const gather_type& other) {
      if (pvec.size() == 0){
        pvec = other.pvec;
        bias = other.bias;
        weight = other.weight;
        return *this;
      }
      else if (other.pvec.size() == 0)
        return *this;
      pvec += other.pvec;
      bias += other.bias;
      weight += other.weight;
      return *this;
    } // end of operator+=

}; // end of gather type

//typedef gather_type message_type;

enum{
  PHASE1 = 0, PHASE2 = 1
};

/**
 * BIASSGD vertex program type
 */
class svdpp_vertex_program :
  public graphlab::ivertex_program<graph_type, gather_type,
  gather_type> {
    public:
      /** The convergence tolerance */
      static double TOLERANCE;
      static double LAMBDA;
      static double GAMMA;
      static double MAXVAL;
      static double MINVAL;
      static double STEP_DEC;
      static bool debug;
      static size_t MAX_UPDATES;
      static double GLOBAL_MEAN;
      static size_t NUM_TRAINING_EDGES;
      static uint   USERS;

      gather_type pmsg;
      void save(graphlab::oarchive& arc) const {
        arc << pmsg;
      }
      /** \brief Load the vertex data from a binary archive */
      void load(graphlab::iarchive& arc) {
        arc >> pmsg;
      }

      /** The set of edges to gather along */
      edge_dir_type gather_edges(icontext_type& context,
          const vertex_type& vertex) const {
        return graphlab::ALL_EDGES;
      }; // end of gather_edges

      gather_type gather(icontext_type& context, const vertex_type& vertex,
          edge_type& edge) const {
        vec_type step = vec_type::Zero(vertex_data::NLATENT);
        double bias =0, other_bias = 0;
        vec_type delta, other_delta;

        //user node
        if (vertex.num_in_edges() == 0){
          vertex_type other_vertex(get_other_vertex(edge, vertex));
          vertex_type my_vertex(vertex);

          int phase = my_vertex.data().nupdates % 2;
          if (phase == PHASE1){
            //my_vertex.data().weight += movie.weight;
            context.signal(other_vertex, gather_type(vec_type::Zero(vertex_data::NLATENT), vec_type::Zero(vertex_data::NLATENT), 0));
            return gather_type(vec_type::Zero(vertex_data::NLATENT), other_vertex.data().weight, 0);
          }
          else if (phase == PHASE2){
            //vertex_data & my_data = my_vertex.data();
            double pred = svdpp_vertex_program::GLOBAL_MEAN +
              my_vertex.data().bias + other_vertex.data().bias + my_vertex.data().pvec.dot(other_vertex.data().pvec+other_vertex.data().weight);
            pred = std::min(pred, svdpp_vertex_program::MAXVAL);
            pred = std::max(pred, svdpp_vertex_program::MINVAL);
            const float err = edge.data().obs - pred;
            if (debug)
              std::cout<<"entering edge " << (int)edge.source().id() << ":" << (int)edge.target().id() << " err: " << err << " rmse: " << err*err <<std::endl;
            if (std::isnan(err))
              logstream(LOG_FATAL)<<"Got into numeric errors.. try to tune step size and regularization using command line flags" << std::endl;
            if (edge.data().role == edge_data::TRAIN){
              vec_type itmFctr = other_vertex.data().pvec;
              vec_type usrFctr = my_vertex.data().pvec;

              bias = usrBiasStep*(err - usrBiasReg*bias);
              other_bias = itmBiasStep*(err - itmBiasReg*other_bias);

              delta = usrFctrStep*(err*(itmFctr - usrFctrReg *usrFctr));
              other_delta = itmFctrStep*(err*(usrFctr+my_vertex.data().weight) - itmFctrReg*other_vertex.data().pvec);

              step = err*itmFctr;
              float usrNorm = double(1.0/sqrt(my_vertex.num_out_edges()));
              step *= itmFctr2Step*usrNorm;

              double mult = itmFctr2Step*itmFctr2Reg;
              step -= mult*other_vertex.data().weight;
              //A HACK: update memory cached values to reflect new vals
              /*my_vertex.data().bias += bias;
                other_vertex.data().bias += other_bias;
                my_vertex.data().pvec += delta;
                other_vertex.data().pvec += other_delta;*/

              if (debug)
                std::cout<<"new val:" << (int)edge.source().id() << ":" << (int)edge.target().id() << " U " << my_vertex.data().pvec.transpose() << " V " << other_vertex.data().pvec.transpose() << std::endl;

              if(other_vertex.data().nupdates < MAX_UPDATES)
                context.signal(other_vertex, gather_type(other_delta, step, other_bias));
            }
            return gather_type(delta, step, bias);

          } //end of PHASE2
        }
        return gather_type(delta, step, bias);
      }

      //typedef vec_type message_type;
      void init(icontext_type& context,
          const vertex_type& vertex,
          const message_type& msg) {

        int phase = vertex.data().nupdates % 2;
        //movie node receives updates here
        if (vertex.num_in_edges() > 0){
          if (phase == PHASE1){
            pmsg = msg;
          }
          else if (phase == PHASE2){
            pmsg = msg;
          }
        }
      }

      /** apply collects the sum of XtX and Xy */
      void apply(icontext_type& context, vertex_type& vertex,
          const gather_type& sum) {
        vertex_data& vdata = vertex.data();
        int phase = vdata.nupdates %2;

        if (phase == PHASE1){
          //user node receives the sum of movie weights
          if (vertex.num_out_edges() > 0){
            vertex.data().weight = sum.pvec;
            float usrNorm = double(1.0/sqrt(vertex.num_out_edges()));
            vertex.data().weight *= usrNorm;
          }
          //movie node doe nothing
          else {}
        }
        else if (phase == PHASE2){
          //user node update gradients and bias
          if (vertex.num_in_edges() == 0){
            vdata.pvec += sum.pvec;
            vdata.bias += sum.bias;
            //does not update weight here (since was done in phase1)
          }
          //movie node
          else {
            vdata.weight += pmsg.weight; //step
            vdata.pvec += pmsg.pvec;
            vdata.bias += pmsg.bias;
          }
        }
        ++vdata.nupdates;
      } // end of apply

      /** The edges to scatter along */
      edge_dir_type scatter_edges(icontext_type& context,
          const vertex_type& vertex) const {
        return graphlab::ALL_EDGES;
      }; // end of scatter edges

      /** Scatter reschedules neighbors */
      void scatter(icontext_type& context, const vertex_type& vertex,
          edge_type& edge) const {
        edge_data& edata = edge.data();
        if(edata.role == edge_data::TRAIN) {
          const vertex_type other_vertex = get_other_vertex(edge, vertex);
          // Reschedule neighbors ------------------------------------------------
          if(other_vertex.data().nupdates < MAX_UPDATES)
            context.signal(other_vertex, gather_type(vec_type::Zero(vertex_data::NLATENT),vec_type::Zero(vertex_data::NLATENT),0));
        }
      } // end of scatter function

      /**
       * \brief Signal all vertices on one side of the bipartite graph
       */
      static graphlab::empty signal_left(icontext_type& context,
          vertex_type& vertex) {
        if(vertex.num_out_edges() > 0) context.signal(vertex, gather_type(vec_type::Zero(vertex_data::NLATENT),vec_type::Zero(vertex_data::NLATENT),0));
        return graphlab::empty();
      } // end of signal_left

  }; // end of svdpp vertex program

struct error_aggregator : public graphlab::IS_POD_TYPE {
  typedef svdpp_vertex_program::icontext_type icontext_type;
  typedef graph_type::edge_type edge_type;
  double train_error, validation_error;
  size_t ntrain, nvalidation;
  error_aggregator() :
    train_error(0), validation_error(0), ntrain(0), nvalidation(0) { }
  error_aggregator& operator+=(const error_aggregator& other) {
    train_error += other.train_error;
    assert(!std::isnan(train_error));
    validation_error += other.validation_error;
    ntrain += other.ntrain;
    nvalidation += other.nvalidation;
    return *this;
  }
  static error_aggregator map(icontext_type& context, const graph_type::edge_type& edge) {
    error_aggregator agg;
    if (edge.data().role == edge_data::TRAIN){
      agg.train_error = extract_l2_error(edge); agg.ntrain = 1;
      assert(!std::isnan(agg.train_error));
    }
    else if (edge.data().role == edge_data::VALIDATE){
      agg.validation_error = extract_l2_error(edge); agg.nvalidation = 1;
    }
    return agg;
  }

  static void finalize(icontext_type& context, const error_aggregator& agg) {
    iter++;
    if (iter%2 == 0)
      return;
    ASSERT_GT(agg.ntrain, 0);
    const double train_error = std::sqrt(agg.train_error / agg.ntrain);
    assert(!std::isnan(train_error));
    context.cout() << std::setw(8) << context.elapsed_seconds() << std::setw(8) << train_error;
    if(agg.nvalidation > 0) {
      const double validation_error =
        std::sqrt(agg.validation_error / agg.nvalidation);
      context.cout() << std::setw(8) << validation_error;
    }
    context.cout() << std::endl;
    usrBiasStep *= svdpp_vertex_program::STEP_DEC;
    itmBiasStep *= svdpp_vertex_program::STEP_DEC;
    usrFctrStep  *= svdpp_vertex_program::STEP_DEC;
    itmFctrStep  *= svdpp_vertex_program::STEP_DEC;
    itmFctr2Step *= svdpp_vertex_program::STEP_DEC;

  }
}; // end of error aggregator

/**
 * \brief Given an edge compute the error associated with that edge
 */
double extract_l2_error(const graph_type::edge_type & edge) {
  double pred = svdpp_vertex_program::GLOBAL_MEAN +
    edge.source().data().bias +
    edge.target().data().bias +
    edge.source().data().pvec.dot(edge.target().data().pvec);
  pred = std::min(svdpp_vertex_program::MAXVAL, pred);
  pred = std::max(svdpp_vertex_program::MINVAL, pred);
  double rmse = (edge.data().obs - pred) * (edge.data().obs - pred);
  assert(rmse <= pow(svdpp_vertex_program::MAXVAL-svdpp_vertex_program::MINVAL,2));
  return rmse;
} // end of extract_l2_error

struct prediction_saver {
  typedef graph_type::vertex_type vertex_type;
  typedef graph_type::edge_type   edge_type;
  std::string save_vertex(const vertex_type& vertex) const {
    return ""; //nop
  }
  std::string save_edge(const edge_type& edge) const {
    if (edge.data().role != edge_data::PREDICT)
      return "";

    std::stringstream strm;
    double pred = svdpp_vertex_program::GLOBAL_MEAN +
      edge.target().data().bias + edge.source().data().bias + edge.source().data().pvec.dot(edge.target().data().pvec+edge.target().data().weight);
      pred = std::min(pred, svdpp_vertex_program::MAXVAL);
      pred = std::max(pred, svdpp_vertex_program::MINVAL);
    strm << edge.source().id() << '\t'
      << -edge.target().id()-SAFE_NEG_OFFSET << '\t'
      << pred << '\n';
    return strm.str();
  }
}; // end of prediction_saver

struct linear_model_saver_U {
  typedef graph_type::vertex_type vertex_type;
  typedef graph_type::edge_type   edge_type;
  /* save the linear model, using the format:
     nodeid) factor1 factor2 ... factorNLATENT \n
     */
  std::string save_vertex(const vertex_type& vertex) const {
    if (vertex.num_out_edges() > 0){
      std::string ret = boost::lexical_cast<std::string>(vertex.id()) + " ";
      for (uint i=0; i< vertex_data::NLATENT; i++)
        ret += boost::lexical_cast<std::string>(vertex.data().pvec[i]) + " ";
      ret += "\n";
      return ret;
    }
    else return "";
  }
  std::string save_edge(const edge_type& edge) const {
    return "";
  }
};

struct linear_model_saver_V {
  typedef graph_type::vertex_type vertex_type;
  typedef graph_type::edge_type   edge_type;
  /* save the linear model, using the format:
     nodeid) factor1 factor2 ... factorNLATENT \n
     */
  std::string save_vertex(const vertex_type& vertex) const {
    if (vertex.num_out_edges() == 0){
      std::string ret = boost::lexical_cast<std::string>(-vertex.id()-SAFE_NEG_OFFSET) + " ";
      for (uint i=0; i< vertex_data::NLATENT; i++)
        ret += boost::lexical_cast<std::string>(vertex.data().pvec[i]) + " ";
      ret += "\n";
      return ret;
    }
    else return "";
  }
  std::string save_edge(const edge_type& edge) const {
    return "";
  }
};

struct linear_model_saver_bias_U {
  typedef graph_type::vertex_type vertex_type;
  typedef graph_type::edge_type   edge_type;
  /* save the linear model, using the format:
     nodeid) factor1 factor2 ... factorNLATENT \n
     */
  std::string save_vertex(const vertex_type& vertex) const {
    if (vertex.num_out_edges() > 0){
      std::string ret = boost::lexical_cast<std::string>(vertex.id()) + " ";
      ret += boost::lexical_cast<std::string>(vertex.data().bias) + "\n";
      return ret;
    }
    else return "";
  }
  std::string save_edge(const edge_type& edge) const {
    return "";
  }
};
struct linear_model_saver_bias_V {
  typedef graph_type::vertex_type vertex_type;
  typedef graph_type::edge_type   edge_type;
  /* save the linear model, using the format:
     nodeid) factor1 factor2 ... factorNLATENT \n
     */
  std::string save_vertex(const vertex_type& vertex) const {
    if (vertex.num_out_edges() == 0){
      std::string ret = boost::lexical_cast<std::string>(-vertex.id()-SAFE_NEG_OFFSET) + " ";
      ret += boost::lexical_cast<std::string>(vertex.data().bias) + "\n";
      return ret;
    }
    else return "";
  }
  std::string save_edge(const edge_type& edge) const {
    return "";
  }
};

/**
 * \brief The graph loader function is a line parser used for
 * distributed graph construction.
 */
inline bool graph_loader(graph_type& graph,
    const std::string& filename,
    const std::string& line) {
  ASSERT_FALSE(line.empty());
  // Determine the role of the data
  edge_data::data_role_type role = edge_data::TRAIN;
  if(boost::ends_with(filename,".validate")) role = edge_data::VALIDATE;
  else if(boost::ends_with(filename, ".predict")) role = edge_data::PREDICT;
  // Parse the line
  std::stringstream strm(line);
  graph_type::vertex_id_type source_id(-1), target_id(-1);
  float obs(0);
  strm >> source_id >> target_id;

  if(role == edge_data::TRAIN || role == edge_data::VALIDATE){
    strm >> obs;
    if (obs < svdpp_vertex_program::MINVAL || obs > svdpp_vertex_program::MAXVAL)
      logstream(LOG_FATAL)<<"Rating values should be between " << svdpp_vertex_program::MINVAL << " and " << svdpp_vertex_program::MAXVAL << ". Got value: " << obs << " [ user: " << source_id << " to item: " <<target_id << " ] " << std::endl;
  }
  target_id = -(graphlab::vertex_id_type(target_id + SAFE_NEG_OFFSET));
  // Create an edge and add it to the graph
  graph.add_edge(source_id, target_id, edge_data(obs, role));
  return true; // successful load
} // end of graph_loader

size_t vertex_data::NLATENT = 20;
double svdpp_vertex_program::TOLERANCE = 1e-3;
double svdpp_vertex_program::LAMBDA = 0.001;
double svdpp_vertex_program::GAMMA = 0.001;
size_t svdpp_vertex_program::MAX_UPDATES = -1;
double svdpp_vertex_program::MAXVAL = 1e+100;
double svdpp_vertex_program::MINVAL = -1e+100;
double svdpp_vertex_program::STEP_DEC = 0.9;
bool svdpp_vertex_program::debug = false;
double svdpp_vertex_program::GLOBAL_MEAN = 0;
size_t svdpp_vertex_program::NUM_TRAINING_EDGES = 0;

/**
 * \brief The engine type used by the ALS matrix factorization
 * algorithm.
 *
 * The ALS matrix factorization algorithm currently uses the
 * synchronous engine.  However we plan to add support for alternative
 * engines in the future.
 */
typedef graphlab::omni_engine<svdpp_vertex_program> engine_type;

  double calc_global_mean(const graph_type::edge_type & edge){
    if (edge.data().role == edge_data::TRAIN)
      return edge.data().obs;
    else return 0;
  }

  size_t count_edges(const graph_type::edge_type & edge){
    if (edge.data().role == edge_data::TRAIN)
      return 1;
    else return 0;
  }

int main(int argc, char** argv) {
  global_logger().set_log_level(LOG_INFO);
  global_logger().set_log_to_console(true);

  // Parse command line options -----------------------------------------------
  const std::string description =
    "Compute the ALS factorization of a matrix.";
  graphlab::command_line_options clopts(description);
  std::string input_dir, output_dir;
  std::string predictions;
  size_t interval = 0;
  std::string exec_type = "synchronous";
  clopts.attach_option("matrix", input_dir,
      "The directory containing the matrix file");
  clopts.add_positional("matrix");
  clopts.attach_option("D", vertex_data::NLATENT,
      "Number of latent parameters to use.");
  clopts.attach_option("engine", exec_type,
      "The engine type synchronous or asynchronous");
  clopts.attach_option("max_iter", svdpp_vertex_program::MAX_UPDATES,
      "The maxumum number of udpates allowed for a vertex");
  clopts.attach_option("lambda", svdpp_vertex_program::LAMBDA,
      "SGD regularization weight");
  clopts.attach_option("gamma", svdpp_vertex_program::GAMMA,
      "SGD step size");
  clopts.attach_option("debug", svdpp_vertex_program::debug,
      "debug - additional verbose info");
  clopts.attach_option("tol", svdpp_vertex_program::TOLERANCE,
      "residual termination threshold");
  clopts.attach_option("maxval", svdpp_vertex_program::MAXVAL, "max allowed value");
  clopts.attach_option("minval", svdpp_vertex_program::MINVAL, "min allowed value");
  clopts.attach_option("step_dec", svdpp_vertex_program::STEP_DEC, "multiplicative step decrement");
  clopts.attach_option("user_bias_step", usrBiasStep, "user_bias_step");
  clopts.attach_option("user_bias_reg", usrBiasReg, "user_bias_reg");
  clopts.attach_option("item_bias_step",itmBiasStep, "item_bias_step");
  clopts.attach_option("item_bias_reg", itmBiasReg, "item_bias_reg");
  clopts.attach_option("user_factor_step", usrFctrStep, "user_factor_step");
  clopts.attach_option("user_factor_reg", usrFctrReg, "user_factor_reg");
  clopts.attach_option("item_factor_step", itmFctrStep, "item_factor_step");
  clopts.attach_option("item_factor_reg", itmFctrReg, "item_factor_reg");
  clopts.attach_option("item_factor2_step", itmFctr2Step, "item_factor2_step");
  clopts.attach_option("item_factor2_reg", itmFctr2Reg, "item_factor2_reg");
  clopts.attach_option("interval", interval, "The time in seconds between error reports");
  clopts.attach_option("predictions", predictions,
      "The prefix (folder and filename) to save predictions.");
  clopts.attach_option("output", output_dir, "Output results");

  parse_implicit_command_line(clopts);

  if(!clopts.parse(argc, argv) || input_dir == "") {
    std::cout << "Error in parsing command line arguments." << std::endl;
    clopts.print_description();
    return EXIT_FAILURE;
  }
  debug = svdpp_vertex_program::debug;
  //  omp_set_num_threads(clopts.get_ncpus());
  ///! Initialize control plain using mpi
  graphlab::mpi_tools::init(argc, argv);
  graphlab::distributed_control dc;

  dc.cout() << "Loading graph." << std::endl;
  graphlab::timer timer;
  graph_type graph(dc, clopts);
  graph.load(input_dir, graph_loader);
  dc.cout() << "Loading graph. Finished in "
    << timer.current_time() << std::endl;

  if (dc.procid() == 0)
    add_implicit_edges<edge_data>(implicitratingtype, graph, dc);

  dc.cout() << "Finalizing graph." << std::endl;
  timer.start();
  graph.finalize();
  dc.cout() << "Finalizing graph. Finished in "
    << timer.current_time() << std::endl;

  dc.cout()
    << "========== Graph statistics on proc " << dc.procid()
    << " ==============="
    << "\n Num vertices: " << graph.num_vertices()
    << "\n Num edges: " << graph.num_edges()
    << "\n Num replica: " << graph.num_replicas()
    << "\n Replica to vertex ratio: "
    << float(graph.num_replicas())/graph.num_vertices()
    << "\n --------------------------------------------"
    << "\n Num local own vertices: " << graph.num_local_own_vertices()
    << "\n Num local vertices: " << graph.num_local_vertices()
    << "\n Replica to own ratio: "
    << (float)graph.num_local_vertices()/graph.num_local_own_vertices()
    << "\n Num local edges: " << graph.num_local_edges()
    //<< "\n Begin edge id: " << graph.global_eid(0)
    << "\n Edge balance ratio: "
    << float(graph.num_local_edges())/graph.num_edges()
    << std::endl;

  dc.cout() << "Creating engine" << std::endl;
  engine_type engine(dc, graph, exec_type, clopts);

  // Add error reporting to the engine
  const bool success = engine.add_edge_aggregator<error_aggregator>
    ("error", error_aggregator::map, error_aggregator::finalize) &&
    engine.aggregate_periodic("error", interval);
  ASSERT_TRUE(success);

  svdpp_vertex_program::GLOBAL_MEAN = graph.map_reduce_edges<double>(calc_global_mean);
  svdpp_vertex_program::NUM_TRAINING_EDGES = graph.map_reduce_edges<size_t>(count_edges);
  svdpp_vertex_program::GLOBAL_MEAN /= svdpp_vertex_program::NUM_TRAINING_EDGES;
  dc.cout() << "Global mean is: " <<svdpp_vertex_program::GLOBAL_MEAN << std::endl;

  // Signal all vertices on the vertices on the left (libersgd)
  engine.map_reduce_vertices<graphlab::empty>(svdpp_vertex_program::signal_left);

  // Run the PageRank ---------------------------------------------------------
  dc.cout() << "Running Bias-SGD" << std::endl;
  dc.cout() << "(C) Code by Danny Bickson, CMU " << std::endl;
  dc.cout() << "Please send bug reports to danny.bickson@gmail.com" << std::endl;
  dc.cout() << "Time   Training    Validation" <<std::endl;
  dc.cout() << "       RMSE        RMSE " <<std::endl;
  timer.start();
  engine.start();

  const double runtime = timer.current_time();
  dc.cout() << "----------------------------------------------------------"
    << std::endl
    << "Final Runtime (seconds):   " << runtime
                                        << std::endl
                                        << "Updates executed: " << engine.num_updates() << std::endl
                                        << "Update Rate (updates/second): "
                                          << engine.num_updates() / runtime << std::endl;

  // Compute the final training error -----------------------------------------
  dc.cout() << "Final error: " << std::endl;
  engine.aggregate_now("error");

  // Make predictions ---------------------------------------------------------
  if(!predictions.empty()) {
    std::cout << "Saving predictions" << std::endl;
    const bool gzip_output = false;
    const bool save_vertices = false;
    const bool save_edges = true;
    const size_t threads_per_machine = 1;
    graph.save(predictions, prediction_saver(),
        gzip_output, save_vertices,
        save_edges, threads_per_machine);
    //save the linear model
    graph.save(predictions + ".U", linear_model_saver_U(),
        gzip_output, save_edges, save_vertices, threads_per_machine);
    graph.save(predictions + ".V", linear_model_saver_V(),
        gzip_output, save_edges, save_vertices, threads_per_machine);
    graph.save(predictions + ".bias.U", linear_model_saver_bias_U(),
        gzip_output, save_edges, save_vertices, threads_per_machine);
    graph.save(predictions + ".bias.V", linear_model_saver_bias_V(),
        gzip_output, save_edges, save_vertices, threads_per_machine);

  }

  graphlab::mpi_tools::finalize();
  return EXIT_SUCCESS;
} // end of main


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <limits>
#include <vector>
#include <algorithm>
#include <string>
#include <sstream>
#include <chrono>

#include <boost/filesystem.hpp>
#include <boost/circular_buffer.hpp>

#include <ScrimpSequVec.hpp>
#include <logging.hpp>
#include <papiwrapper.hpp>

using namespace matrix_profile;

static FactoryRegistration<ScrimpSequVec> s_sequRegistr("scrimp_sequ_vec");
static const int notification_interval_iter = 10000;

PerfCounters cumsum_perf("cumulative_sum");
PerfCounters eval_perf("vectorized evaluation");

tsa_dtype ScrimpSequVec::init_diagonal(const aligned_tsdtype_vec& ASigmaInv, aligned_tsdtype_vec& cumDotproduct, aligned_tsdtype_vec& profile, const int diag, const aligned_tsdtype_vec& A, aligned_int_vec& profileIndex, const int windowSize, const aligned_tsdtype_vec& AMeanScaledSigSqrM)
{
	tsa_dtype corr_score=0;
	tsa_dtype lastz=0;

	//evaluate the fist distance value in the current diagonal
	for (int k = 0; k < windowSize; k++)
	{
		cumDotproduct[k+diag]=lastz;
		lastz += A[k+diag]*A[k];
	}
	//j is the column index, i is the row index of the current distance value in the distance matrix

	return lastz;
}

void ScrimpSequVec::eval_diagonal(idx_dtype* const profileIndex, const tsa_dtype* const A, tsa_dtype& lastz, const idx_dtype windowSize, tsa_dtype* const cumDotproduct, tsa_dtype* const ASigmaInv, const tsa_dtype* const AMeanScaledSigSqrM, const idx_dtype diag, tsa_dtype* const profile, const idx_dtype profileLength)
{
	{
		ScopedPerfAccumulator monitor(cumsum_perf);
		tsa_dtype cumProd = lastz; //TODO: rename lastz. The idea of this line is toavoid unnecessary writes to the input reference....
		for (idx_dtype j=diag; j<profileLength; j++)
		{
			cumDotproduct[j+windowSize]=cumProd;
			cumProd += A[j+windowSize]*A[j+windowSize-diag];
		}
	}
	{
		ScopedPerfAccumulator monitor(eval_perf);
        #pragma omp simd aligned(profile, profileIndex, ASigmaInv, AMeanScaledSigSqrM, cumDotproduct : alignment)
		for (idx_dtype j=diag; j<profileLength; j++)
		{
			idx_dtype i=j-diag;
			tsa_dtype corrScore = ( (cumDotproduct[j+windowSize] - cumDotproduct [j]) * (ASigmaInv[j] * ASigmaInv[i]) - AMeanScaledSigSqrM[j] * AMeanScaledSigSqrM[i]) ;

			bool update_j = (corrScore > profile[j]);
			profile[j] = update_j?corrScore:profile[j];
			profileIndex [j] = update_j?i:profileIndex [j];

			bool update_i = (corrScore > profile[i]);
			profile[i] = update_i?corrScore:profile[i];
			profileIndex [i] = update_i?j:profileIndex[i];
		}
	}
}

void ScrimpSequVec::compute_matrix_profile(const Scrimppp_params& params)
{
	std::chrono::high_resolution_clock::time_point tstart, tend;
	std::chrono::duration<double> time_elapsed;
	aligned_tsdtype_vec A = fetch_time_series<aligned_tsdtype_vec::allocator_type>(params); //load the time series data
	aligned_tsdtype_vec AMeanScaledSqrtM(A.size());
	aligned_tsdtype_vec ASigmaInv(A.size());
	int windowSize = params.query_window_len;
	int exclusionZone = windowSize / 4;
	int timeSeriesLength = A.size();
	int ProfileLength = timeSeriesLength - windowSize + 1;
	//Initialize Matrix Profile and Matrix Profile Index
	aligned_tsdtype_vec profile(ProfileLength, 0.0);
	aligned_int_vec profileIndex(ProfileLength, 0);
	aligned_tsdtype_vec dotproduct(timeSeriesLength); // stores products between two Timeseries values with a distinct offset
	aligned_int_vec idx; // store indices of the diagonals, defining their evaluation order
	idx.reserve(ProfileLength-exclusionZone-1);

	//several monitors for performance measurement
	PerfCounters setup_perf("setup performance");
	PerfCounters init_diag_perf("diagonal initialization");
	//PerfCounters eval_diag_perf("diagonal evaluation");

	//validation of parameters
	if (timeSeriesLength < windowSize) {
		throw std::invalid_argument("ERROR: Time series is shorter than the window length, can not proceed");
	}

	EXEC_INFO( "Sequential SCRIMP matrix profile computation with profile length " << ProfileLength << " and window size " << windowSize);

	{
		ScopedPerfAccumulator monitor(setup_perf);
		//precompute the mean and standard deviations of the sliding windows along the time series
		precompute_window_statistics(windowSize, A, ProfileLength, AMeanScaledSqrtM, ASigmaInv);

		//start time measurment
		tstart = std::chrono::high_resolution_clock::now();

		/******************** SCRIMP ********************/
		//Random shuffle the computation order of the diagonals of the distance matrix
		for (int i = exclusionZone+1; i < ProfileLength; i++) {
			idx.push_back(i);
		}
		std::random_shuffle(idx.begin(), idx.end());
	}

	//iteratively evaluate the diagonals of the distance matrix
	for (int ri = 0; ri < idx.size(); ri++)
	    {
		//select a random diagonal
		int diag = idx[ri];
		tsa_dtype lastz=0; //the dot product of a subsequence

		// compute the first distance value in the diagonal (i.e. compute the distance between the first windows)
		{
			ScopedPerfAccumulator monitor(init_diag_perf);
			lastz = init_diagonal(ASigmaInv, dotproduct, profile, diag, A, profileIndex, windowSize, AMeanScaledSqrtM);
		}

		//evaluate the second to the last distance values along the diagonal in the matrix and update the matrix profile/matrix profile index.
		{
			//ScopedPerfAccumulator monitor(eval_diag_perf);
			// eval_diagonal(profileIndex, A, lastz, windowSize, dotproduct, ASigmaInv, AMeanScaledSqrtM, diag, profile);
			eval_diagonal(profileIndex.data(), A.data(), lastz, windowSize, dotproduct.data(), ASigmaInv.data(), AMeanScaledSqrtM.data(), diag, profile.data(), AMeanScaledSqrtM.size());
		}

		//Show time per 10000 iterations
		if ((ri+1) % notification_interval_iter == 0)
		{
			tend = std::chrono::high_resolution_clock::now();
			time_elapsed = tend - tstart;
			EXEC_INFO ( "finished " << ri+1 << " iterations after " << std::setprecision(std::numeric_limits<tsa_dtype>::digits10 + 2) << time_elapsed.count() << " seconds.");
		}
	}

	// apply a correction of the distance values, as we dropped a factor of 2 to avoid unnecessary computations
	tsa_dtype twice_m = 2.0*static_cast<tsa_dtype>(windowSize);
	for (auto iter=profile.begin(); iter<profile.end(); ++iter) {
		(*iter) = twice_m - 2.0 * (*iter);
	}

	// end timer
	// tend = time(0);
	tend = std::chrono::high_resolution_clock::now();
	time_elapsed = tend - tstart;

	PERF_LOG ( "total computation time: " << std::setprecision(std::numeric_limits<tsa_dtype>::digits10 + 2) << time_elapsed.count() << " seconds." );
	const double triang_len = ProfileLength-exclusionZone;
	PERF_LOG ( "throughput computations: " << triang_len * triang_len / time_elapsed.count() << " matrix entries/second");

	//store the result
	store_matrix_profile(profile, profileIndex, params);

	setup_perf.log_perf();
	init_diag_perf.log_perf();
	cumsum_perf.log_perf();
	eval_perf.log_perf();
	//eval_diag_perf.log_perf();
#ifdef PROFILING
	PERF_LOG ( "number of matrix profile updates: " << _profileUpdateCounter);
#endif

}


/*
 * dijkstra.cpp
 *
 * 	\brief     My dijkstra implementation for the fifth exercise
 *  \details   This class computes the longest shortest path for starting point 1.
 *  \author    Julia Baumbach
 *  \date      17.06.2017
 */

#include "dijkstra.h"
#include <climits>
#include <iostream>
#include <boost/heap/fibonacci_heap.hpp>
#include <exception>

using VisitedMap = std::vector<bool>;

/**
 * \struct compare_function
 * \brief defines how 2 dijkstra pairs should be compared at fibonacci-heap
 * \param edge1 first edge to compare
 * \param edge2 second edge to compare
 * \return true if the weight of edge2 is smaller than edge1's weight
 */
struct compare_function{
	bool operator()(const DijkstraPair edge1, const DijkstraPair edge2) const{
		return edge1.second > edge2.second;
	}
};

/**
 * \fn the constructor
 * \brief initialize a new dijkstra instance while sorting the given edges
 * \param weights Vector of the weights for the given graph
 * \param edges Vector of the edges for the given graph
 * \return the new dijkstra instance
 */
dijkstra::dijkstra(WeightMap weights, Edges edges, unsigned int numberOfVertices):
	numberOfVertices(numberOfVertices) {
	//sort the edges in a more efficient data structure
	sortedEdges.resize(numberOfVertices);
	for (unsigned int i = 0; i < edges.size(); i++){
		sortedEdges.at(edges.at(i).first).push_back(std::make_pair(edges.at(i).second, weights.at(i)));
		sortedEdges.at(edges.at(i).second).push_back(std::make_pair(edges.at(i).first, weights.at(i)));
	}
}

/**
 * \fn vector<int> dijkstra::computeShortestPath
 * \brief computes all shortest paths from given start vertex for the initialized dijkstra instance
 * \param unsigned int numberOfVertices Number of Vertices for the graph
 * \return vector of the weights for all shortest paths
 */
WeightMap dijkstra::computeShortestPath(int startNode){
	if(startNode > numberOfVertices){
		std::cerr << "Index of StartVertex must be less or equal to number of vertices" << std::endl;
		throw std::exception();
	}
	WeightMap weightsToVertices(numberOfVertices); //should be returned
	std::vector<int> predecessorMap(numberOfVertices);
	VisitedMap alreadyVisited(numberOfVertices);

	for (unsigned int i = 0; i < numberOfVertices; i++) {
		weightsToVertices[i] = INT_MAX;
		predecessorMap[i] = -1;
	}
	//Start in point startNode
	weightsToVertices[startNode] = 0;
	predecessorMap[startNode] = startNode;
	int currentVertex = startNode;
	int currentDist = 0;

	boost::heap::fibonacci_heap<DijkstraPair, boost::heap::compare<compare_function>> heap;
	//Put the start vertex in the verticesToVisit list
	heap.push(std::make_pair(startNode, 0));

	while(!heap.empty()){
		if (!alreadyVisited.at(heap.top().first)){
			currentVertex = heap.top().first;
			currentDist = heap.top().second;

			heap.pop();

			std::vector<DijkstraPair> currentEdges = sortedEdges.at(currentVertex);

			for (const DijkstraPair pair : currentEdges){
				//Find out the current neighbor vertex and the weight of the current edge
				int neighborVertex = pair.first;
				int currentWeight = pair.second;
				//Search for the right position in verticesToVisit-list. If Vertex is already visited, don't add this edge
				if (!alreadyVisited.at(neighborVertex)){
					heap.push(std::make_pair(neighborVertex, currentDist + currentWeight));
				}

				int currentDistance = currentDist + currentWeight;

				//Update the predecessor and weightsToVerices maps
				if (currentDistance < weightsToVertices[neighborVertex]){
					weightsToVertices[neighborVertex] = currentDistance;
					predecessorMap[neighborVertex] = currentVertex;
				}
			}
			//Current vertex is visited now
			alreadyVisited.at(currentVertex) = true;
		} else {
			heap.pop();
		}
	}
	return weightsToVertices;
}