#include "matlab_func.h"
#include "genenalfunc.h"
#include "math.h"

using namespace arma;

namespace Matlab {

mat polyval(mat p, mat x, mat S, mat mu)
{
    //p.print("p = ");
    mat y;

    uword nc = p.size(); // nc = length(p);
    // if isscalar(x) && (nargin < 3) && nc>0 && isfinite(x)
    if(x.size()==1 && nc>0 && x.is_finite() && S.is_empty() && mu.is_empty())
    {
        // Make it scream for scalar x.  Polynomial evaluation can be
        // implemented as a recursive digital filter.
        rowvec tempY = zeros<rowvec>(nc);
        rowvec B, A;
        A << 1 << -x(0);
        B << 1;
        filter(tempY, B, A, p, nc-1);   // y = filter(1,[1 -x],p);
        y << tempY(nc-1);               // y = y(nc);
        return y;
    }

    if(!mu.is_empty())      // if nargin == 4
    {
        x = (x - mu(0))/mu(1); // x = (x - mu(1))/mu(2);
    }

    // Use Horner's method for general case where X is an array.
    // siz_x = size(x);
    y.zeros(size(x)); // y = zeros(siz_x, superiorfloat(x,p));
    if (nc > 0)         // if nc>0
    {
        y.fill(p(0));   // y(:) = p(1);
    }
    for(uword i=1; i<nc; ++i) // for i=2:nc
    {
        y = x % y + p(i); // y = x .* y + p(i);
    }

    return y;

    /*if nargout > 1
        if nargin < 3 || isempty(S)
            error('MATLAB:polyval:RequiresS',...
                    'S is required to compute error estimates.');
        end

        // Extract parameters from S
        if isstruct(S),  % Use output structure from polyfit.
          R = S.R;
          df = S.df;
          normr = S.normr;
        else             % Use output matrix from previous versions of polyfit.
          [ms,ns] = size(S);
          if (ms ~= ns+2) || (nc ~= ns)
              error('MATLAB:polyval:SizeS',...
                    'S matrix must be n+2-by-n where n = length(p).');
          end
          R = S(1:nc,1:nc);
          df = S(nc+1,1);
          normr = S(nc+2,1);
        end

        // Construct Vandermonde matrix for the new X.
        x = x(:);
        V(:,nc) = ones(length(x),1,class(x));
        for j = nc-1:-1:1
            V(:,j) = x.*V(:,j+1);
        end

        // S is a structure containing three elements: the triangular factor of
        // the Vandermonde matrix for the original X, the degrees of freedom,
        // and the norm of the residuals.
        E = V/R;
        e = sqrt(1+sum(E.*E,2));
        if df == 0
            warning('MATLAB:polyval:ZeroDOF',['Zero degrees of freedom implies ' ...
                    'infinite error bounds.']);
            delta = repmat(Inf,size(e));
        else
            delta = normr/sqrt(df)*e;
        end
        delta = reshape(delta,siz_x);
    end */
}

void filter(mat& y, mat B, mat A, mat X, uword n)
{
    if(n == 0)
    {
        y(0) = B(0) * X(0) / A(0);
    }
    else
    {
        filter(y, B, A, X, n-1);
        double temp = 0;
        for(uword i=0; i<=n && i<B.n_elem; i++)
        {
            temp += B(i) * X(n-i);
        }
        for(uword i=1; i<=n && i<A.n_elem; i++)
        {
            temp -= A(i) * y(n-i);
        }
        y(n) = temp / A(0);
    }
}

void polyder(mat& a, mat& b, int nargout, mat u, mat v)
{
    if (v.is_empty())
    {
        v << 1;
    }

    u = vectorise(u).t();
    v = vectorise(v).t();

    uword nu = u.size();
    uword nv = v.size();

    rowvec up;
    rowvec vp;
    if ( nu < 2 ) up << 0;
    else
    {
        up = u.cols(0, nu-2) % RangeVec(nu-1, 1, -1);
    }

    if ( nv < 2 ) vp << 0;
    else
    {
        vp = v.cols(0, nv-2) % RangeVec(nv-1, 1, -1);
    }

    rowvec a1 = conv(up, v);
    rowvec a2 = conv(u, vp);

    int i = a1.size();
    int j = a2.size();

    int ij;
	if (i - j > 0)
		ij = i - j;
	else
		ij = j - i;

    rowvec z = zeros<rowvec>( ij );
    if ( i > j )
    {
        a2 = join_horiz(z, a2);
    }
    else if ( i < j )
    {
        a1 = join_horiz(z, a1);
    }

    rowvec temp_a;
    if ( nargout < 2 )
    {
        temp_a = a1 + a2;
    }
    else
    {
        temp_a = a1 - a2;
    }

    uvec f = find( temp_a!=0 );
    if ( !f.is_empty() )
    {
        temp_a = temp_a.cols(f.at(0), temp_a.size()-1 );
    }
    else temp_a << 0;

    rowvec temp_b = conv(v, v);

    f = find( temp_b!=0 );
    if ( !f.is_empty() )
    {
        temp_b = temp_b.cols(f.at(0), temp_b.size()-1 );
    }
    else temp_b << 0;

    /*if ( temp_a.size() > max(nu+nv-2, 1) )
    {
        temp_a = temp_a.cols(1, temp_a.size()-1 );
    }*/

    a = temp_a;
    b = temp_b;
}

colvec polyfit(colvec x, colvec y, int n)
{
    if ( x.size() != y.size() )
    {
        // **************** printf("MATLAB:polyfit:PolyNotUnique");
    }

    uword x_size = x.size();
    mat V = zeros(x_size, n+1);
    V.col(n) = ones(x_size);

    /* 运行出错
     * rowvec j = RangeVec2(n, 1, -1);
     * for (rowvec::iterator it = j.begin(); it!=j.end(); ++it)
    {
        V.col(*it) = x % V.col((*it)+1);
    }*/

    for(int j=n-1; j>=0; --j)
    {
        V.col(j) = x % V.col(j+1);
    }

//    mat Q;
//    mat R;
//    qr_econ(Q, R, V); // [Q,R] = qr(V,0);

    //p = R\(Q'*y);    % Same as p = V\y;
    //vec p = solve(R, trans(Q) * y);
    vec p = zeros(V.n_cols);
    if(!V.has_inf() && !y.has_inf())
    {
        p = solve(V, y);
    }
/*    try {
        p = solve(V, y);
    }
    catch(std::runtime_error &e)
    {
        qDebug() << e.what();
        p = zeros<vec>(V.n_cols);
    }*/

    //p = fliplr(p);
    return p;
}

double realmin(QString type)
{
    double xmin;
    if(type == "float")
    {
        int minexp = -126;
        xmin = pow(2,minexp); // xmin = single(pow2(1,minexp));
    }
    else {
        int minexp = -1022;
        xmin = pow(2,minexp); // xmin = pow2(1,minexp);
    }
    return xmin;
}

int fix(double x)
{
    if(x >= 0) return floor(x);
    else return ceil(x);
}

mat fix(mat dataMat)
{
    mat retMat(size(dataMat));
    for(uword i=0; i<dataMat.size(); i++)
    {
        if(dataMat(i) >= 0) retMat(i) = floor(dataMat(i));
        else retMat(i) = ceil(dataMat(i));
    }
    return retMat;
}

mat m_erf(mat x)
{
    for(uword i=0; i<x.size(); i++)
    {
        x(i) = erf(x(i));
    }
    return x;
}

PiecePoly mkpp(rowvec breaks, mat coefs, rowvec d)
{
    PiecePoly pp;
    if(d.is_empty()) d << 1;

    int dlk = numel(coefs);
    int l = breaks.size()-1;
    int dl = prod(d)*l;
    int k = fix(dlk/dl+100*eps(1.0));
    if(k<=0 || dl*k != dlk)
    {
       qDebug() << "The requested number of polynomial pieces is incompatible with the proposed size"
                   "of a coefficient and the number of scalar coefficients provided.";
    }

    pp.form = "pp";
    pp.b = reshape(breaks,1,l+1);
    pp.c = reshape(coefs,dl,k);
    pp.l = l;
    pp.k = k;
    pp.d = d;
    return pp;
}

mat ppval(mat xx, PiecePoly pp)
{
    /*if isstruct(xx) % we assume that ppval(xx,pp) was used
       temp = xx; xx = pp; pp = temp;
    end*/

    //  obtain the row vector xs equivalent to XX
    rowvec sizexx;
    sizexx << xx.n_rows << xx.n_cols;
    int lx = numel(xx);
    rowvec xs = reshape(xx,1,lx);
    //  if XX is row vector, suppress its first dimension
    //if length(sizexx)==2&&sizexx(1)==1, sizexx(1) = []; end
    if(sizexx.size() == 2 && sizexx(0) == 1)
    {
        sizexx.shed_col(0);
    }

    // if necessary, sort xs
    uvec ix;
    bool ixexist = false;
    if( any( diff(xs) < 0 ) )
    {
        ix = sort_index(xs, "ascend");
        xs = sort(xs, "ascend", 1);
        ixexist = true;
    }

    // take apart PP
    //[b,c,l,k,dd]=unmkpp(pp);

    // for each data point, compute its breakpoint interval
    vec index = UMatToMat(sort_index( join_horiz(pp.b.cols(0,pp.l-1), xs) ) );//[ignored,index] = sort([b(1:l) xs]);

    //index = find(index>l)-(1:lx);
    vec temp_idx = RangeVec2(0, lx-1).t();
    index = UMatToMat( find(index>pp.l-1) ) - temp_idx;

    //index(index<1) = 1;
    for(int i=0; i<index.size(); i++)
    {
        if(index(i) < 1) index(i) = 1;
    }

    // now go to local coordinates ...
    temp_idx = index - 1;
    xs = xs - IdxMat(pp.b, temp_idx);
    rowvec dd = pp.d;
    int d = prod(dd); // d = prod(dd);
    if(d>1) // ... replicate xs and index in case PP is vector-valued ...
    {
        xs = reshape(repmat(xs, d, 1), 1, d*lx); // xs = reshape(xs(ones(d,1),:), 1, d*lx);
        index = d*index;
        vec temp = RangeVec2(-d, -1).t();
        // index = reshape(1+index(ones(d,1),:)+temp(:,ones(1,lx)), d*lx, 1 );
        index = reshape(1+repmat(index, d, 1)+repmat(temp, 1, lx), d*lx, 1);
    } else {
        if(sizexx.size() > 1)
        {
            dd.clear();
        }
        else { dd << 1; }
    }

    // ... and apply nested multiplication:
    temp_idx = index - 1;
    mat v = MatRows(pp.c, temp_idx).col(0); //v = c(index,1);

    for(int i=1; i<pp.k; i++) {//for i=2:k
        v = vectorise(xs)%v + MatRows(pp.c, temp_idx).col(i);
    }

    v = reshape(v,d,lx);
    if(ixexist)
    {
        //v(:,ix) = v;
    }
    if(dd.is_empty())
    {
        v = reshape(v, xx.n_rows, xx.n_cols);
    }
    else {
        v = reshape(v, dd(0), xx.n_elem);
    }

    return v;
}

PiecePoly spline(mat x, mat y)
{
    // Check that data are acceptable and, if not, try to adjust them appropriately
    //[x,y,sizey,endslopes] = chckxy(x,y);
    rowvec sizey;
    mat endslopes;

    // 调用 chckxy 后 x 变为行向量
    chckxy(x, y, sizey, endslopes, 4);
    rowvec t_x = x;

    int n = t_x.size(), yd = prod(sizey);

    // Generate the cubic spline interpolant in ppform
    uvec dd = zeros<uvec>(yd); // dd = ones(yd,1);
    rowvec dx = diff(t_x);

    //divdif = diff(y,[],2) / dx(dd,:);
    mat t1 = diff(y, 1, 1);
    mat t2 = MatRows(dx, dd);
    mat divdif;
    if(size(t1) == size(t2))
    {
        divdif = t1 / t2;
    }
    else if(t2.size() == 1)
    {
        divdif = t1 / as_scalar(t2);
    }
    else if(t1.size() == 1)
    {
        divdif = as_scalar(t1) / t2;
    }
    else {
        qDebug() << "size(t1) must be same as size(t2)";
    }

    PiecePoly pp;
    if(n==2)
    {
        if(endslopes.is_empty()) // the interpolant is a straight line
        {
            pp = mkpp(t_x, join_horiz(divdif, y.col(0)), sizey); //pp = mkpp(x,[divdif y(:,1)],sizey);
        }
        else // the interpolant is the cubic Hermite polynomial
        {
            pp = pwch(t_x, y, endslopes, dx, divdif);
            pp.d = sizey;
        }
    }
    else if(n==3 && endslopes.is_empty()) // the interpolant is a parabola
    {
        y.cols(1,2) = divdif;                           //y(:,2:3)=divdif;
        y.col(2) = diff(divdif.t()).t() / (t_x(2)-t_x(0));  //y(:,3)=diff(divdif')'/(x(3)-x(1));
        y.col(1) = y.col(1) - y.col(2) * dx(0);         //y(:,2)=y(:,2)-y(:,3)*dx(1);
        pp = mkpp(t_x(uvec("0,2")), MatCols(y,uvec("2 1 0")), sizey); //pp = mkpp(x([1,3]),y(:,[3 2 1]),sizey);
    }
    else // set up the sparse, tridiagonal, linear system b = ?*c for the slopes
    {
        mat b = zeros(yd, n);

        // b(:,2:n-1)=3*(dx(dd,2:n-1).*divdif(:,1:n-2)+dx(dd,1:n-2).*divdif(:,2:n-1));
        mat dx_dd = MatRows(dx, dd);
        b.cols(1, n-2) = 3 * ( dx_dd.cols(1, n-2) % divdif.cols(0, n-3) + dx_dd.cols(0, n-3) % divdif.cols(1, n-2) );

        double x31, xn;
        if(endslopes.is_empty())
        {
            x31 = t_x(2) - t_x(0);      //x31=x(3)-x(1);
            xn = t_x(n-1) - t_x(n-3);   //xn=x(n)-x(n-2);
            // b(:,1) = ((dx(1)+2*x31)*dx(2)*divdif(:,1)+dx(1)^2*divdif(:,2))/x31;
            b.col(0) = ( (dx(0)+2*x31) * dx(1) * divdif.col(0) + pow(dx(0),2) * divdif.col(1) ) / x31;
            //b(:,n) = (dx(n-1)^2*divdif(:,n-2)+(2*xn+dx(n-1))*dx(n-2)*divdif(:,n-1))/xn;
            b.col(n-1) = ( pow(dx(n-2),2) * divdif.col(n-3) + (2*xn+dx(n-2)) * dx(n-3) * divdif.col(n-2) ) / xn;
        }
        else {
            x31 = 0;
            xn = 0;
            // b(:,[1 n]) = dx(dd,[2 n-2]).*endslopes;
            mat tmp = dx_dd.col(1);
            tmp.insert_cols(1, dx_dd.col(n-3));
            tmp = tmp % endslopes;
            b.col(0) = tmp.col(0);
            b.col(n-1) = tmp.col(1);
        }
        vec dxt = vectorise(dx);

        //c = spdiags([ [x31;dxt(1:n-2);0] [dxt(2);2*[dxt(2:n-1)+dxt(1:n-2)];dxt(n-2)] [0;dxt(2:n-1);xn] ],[-1 0 1],n,n);
        mat t_mat(n, 3);
        t_mat(0,0) = x31;   t_mat(0,1) = dxt(1);      t_mat(0,2) = 0;
        t_mat(n-1,0) = 0;   t_mat(n-1,1) = dxt(n-3);  t_mat(n-1,2) = xn;
        t_mat.col(0).rows(1,n-2) = dxt.rows(0,n-3);
        t_mat.col(1).rows(1,n-2) = 2*(dxt.rows(1,n-2)+dxt.rows(0,n-3));
        t_mat.col(2).rows(1,n-2) = dxt.rows(1,n-2);
        mat c, res2;
        spdiags(c, res2, t_mat, vec("-1 0 1"), n, n);

        // sparse linear equation solution for the slopes
        //mmdflag = spparms('autommd');
        //spparms('autommd',0);
        // s=b/c; 注:b/c = (c'\b')', c'\b' = solve(c', b')
        mat s = trans(solve(trans(c), trans(b)));

        //spparms('autommd',mmdflag);

        // construct piecewise cubic Hermite interpolant
        // to values and computed slopes
        pp = pwch(t_x,y,s,dx,divdif);
        pp.d = sizey;
    }
    return pp;
}

mat spline(mat x, mat y, mat xx)
{
    mat output;
    PiecePoly pp = spline(x, y);

    if(xx.is_empty())
    {
        //output = pp;
        output.set_size(1, 5+pp.l+pp.d(0)*pp.l*pp.k);
        output(0) = 10;
        output(1) = pp.d(0);
        output(2) = pp.l;
        MatAssign(output, 3, 3+pp.l, pp.b);
        output(4+pp.l) = pp.k;
        MatAssign(output, 5+pp.l, 4+pp.l+pp.d(0)*pp.l*pp.k, pp.c);
    }
    else output = ppval(xx, pp);

    return output;
}

void chckxy(mat &x, mat &y, rowvec &sizey, mat &endslopes, int nargout)
{
    // make sure X is a vector:
    if(x.n_rows > 1 && x.n_cols > 1) //if length(find(size(x)>1))>1
    {
        qDebug() << "MATLAB:chckxy:XNotVector,X must be a vector.";
    }
    rowvec t_x = vectorise(x).t();

    // deal with NaN's among the sites:
    uvec nanx = find_nonfinite(t_x);
    if(!nanx.is_empty())
    {
        for(int i=nanx.size()-1; i>=0; --i)
        {
            t_x.shed_col(i);
        }
        qDebug() << "MATLAB:chckxy:nan,All data points with NaN as their site will be ignored.";
    }

    int n = t_x.size();
    if(n < 2)
    {
        qDebug() << "MATLAB:chckxy:NotEnoughPts,There should be at least two data points.'";
    }

    // re-sort, if needed, to ensure strictly increasing site sequence:
    rowvec dx = diff(t_x);
    uvec ind;
    if( any(dx<0) )
    {
        //[x,ind] = sort(x);
        ind = sort_index(t_x, "ascend");
        t_x = sort(t_x,"ascend", 1);
        dx = diff(t_x);
    }
    else {
        ind = RangeVec(0, n-1).t(); // ind=1:n;
    }

    if(!all(dx))
    {
        qDebug() << "MATLAB:chckxy:RepeatedSites,The data sites should be distinct.'";
    }

    // if Y is ND, reshape it to a matrix by combining all dimensions but the last:
    sizey << y.n_rows << y.n_cols; // sizey = size(y);

    //while length(sizey)>2&&sizey(end)==1, sizey(end) = []; end

    int yn = sizey(sizey.size()-1); //yn = sizey(end);
    sizey.shed_col(sizey.size()-1); //sizey(end)=[];
    int yd = prod(sizey); //yd = prod(sizey);

    if(sizey.size()>1) {
        y = reshape(y, yd, yn);
    } else {
        // if Y happens to be a column matrix, change it to the expected row matrix.
        if(yn==1) {
            yn = yd;
            y = reshape(y,1,yn);
            yd = 1;
            sizey << yd;
        }
    }

    // determine whether not-a-knot or clamped end conditions are to be used:
    int nstart = n + nanx.size();
    if(yn == nstart){
        //endslopes = [];
    }
    else if(nargout == 4 && yn == nstart+2)
    {
        // endslopes = y(:,[1 n+2]);
        endslopes.set_size(y.n_rows, 2);
        endslopes.col(0) = y.col(0);
        endslopes.col(1) = y.col(n+1);
        //y(:,[1 n+2])=[];
        y.shed_col(n+1);
        y.shed_col(0);

        uvec tmp = find_nonfinite(endslopes);
        if(!tmp.is_empty()) {
            qDebug() << "MATLAB:chckxy:EndslopeNaN or EndslopeInf, The endslopes cannot be NaN or Inf.";
        }
    } else {
        qDebug() << "MATLAB:chckxy:NumSitesMismatchValues, The number of sites, int2str(nstart) "
            "is incompatible with the number of values, int2str(yn).";
    }

    // deal with NaN's among the values:
    if(!nanx.is_empty()) {
        //y(:,nanx) = [];
        for(int i=0; i<nanx.size(); i++)
        {
            y.shed_col( nanx(i) );
        }
    }

    //y = y(:,ind);
    y = MatCols(y, ind);
    //nany = find(sum(isnan(y),1));
    uvec nany = find( sum(find_nonfinite(y), 1) );
    if(!nany.is_empty())
    {
        //y(:,nany) = []; x(nany) = [];
        for(int i=nany.size()-1; i>=0; --i)
        {
            y.shed_col(nany(i));
            t_x.shed_col(nany(i));
        }
        qDebug() << "MATLAB:chckxy:IgnoreNaN,All data points with NaN in their value will be ignored.";
        n = t_x.size();
        if(n<2) {
            qDebug() << "MATLAB:chckxy:NotEnoughPts, There should be at least two data points.";
        }
    }

    x = t_x;
}

PiecePoly pwch(mat x, mat y, mat s, mat dx, mat divdif)
{
    if(dx.is_empty())
        dx = diff( vectorise(x).t() );

    int d = y.n_rows;
    mat dxd = repmat(dx, d, 1);
    if(divdif.is_empty())
        divdif = diff(y, 1, 1) / dxd; //divdif = diff(y,1,2) / dxd;

    int n = numel(x);
    mat dzzdx = (divdif-s.cols(0, n-2))/dxd; // dzzdx = (divdif-s(:,1:n-1))/dxd;
    mat dzdxdx = (s.cols(1, n-1)-divdif)/dxd; // dzdxdx = (s(:,2:n)-divdif)/dxd;
    int dnm1 = d*(n-1);

    mat coefs = join_horiz( join_horiz( reshape((dzdxdx-dzzdx)/dxd, dnm1, 1), reshape(2*dzzdx-dzdxdx, dnm1, 1) ),
                            join_horiz( reshape(s.cols(0, n-2), dnm1, 1), reshape(y.cols(0, n-2), dnm1, 1) ) );
    rowvec t_d; t_d << d;
    PiecePoly pc = mkpp(vectorise(x).t(), coefs, t_d);

    return pc;
}

void spdiags(mat& res1, mat& res2, mat arg1, mat arg2, int arg3, int arg4)
{
    if( arg3 == 0 ) //if nargin <= 2
    {
        if(arg1.n_rows != arg1.n_cols && min(arg1.n_rows, arg1.n_cols)==1)
        {
            qDebug() << "MATLAB:spdiags:VectorInput, A cannot be a vector.";
        }
        // Extract diagonals
        mat A = arg1;
        vec d;
        uvec i, j;
        if(arg2.is_empty()) //if nargin == 1
        {
            // Find all nonzero diagonals
            vec t_A;
            find(i, j, t_A, A); //[i,j] = find(A);
            d = UMatToMat(sort(j-i)); //d = sort(j-i);
            t_A << -qInf();
            t_A.insert_rows(1, d);
            d = d( find( diff( t_A ) ) ); // d = d(find(diff([-inf; d])));
        }
        else // Diagonals are specified
        {
            d = vectorise(arg2);
        }
        int m = A.n_rows, n = A.n_cols; //[m,n] = size(A);
        int p = d.size();
        mat B = zeros( min(m,n), p );
        for(int k=0; k<p; k++) //for k = 1:p
        {
            if(m >= n) {
                //i = max(1,1+d(k)):min(n,m+d(k));
                i = vectorise( RangeVec( max(1,1+d(k))-1, min(n,m+d(k))-1 ) );
            } else {
                //i = max(1,1-d(k)):min(m,n-d(k));
                i = vectorise( RangeVec( max(1,1-d(k))-1, min(m,n-d(k))-1 ) );
            }
            if(!i.is_empty())
            {
                // B(i,k) = diag(A,d(k));
                MatRows(B.col(k), i) = diagmat(A, d(k));
            }
        }
        res1 = B;
        res2 = d;
    }
    else //if nargin >= 3
    {
        // Create a sparse matrix from its diagonals
        mat A, B = arg1;
        vec d = vectorise(arg2);
        int p = d.size();
        bool moda = (arg4 == 0); //moda = (nargin == 3);
        if(moda) // Modify a given matrix
        {
            A << arg3;
        }
        else // Start from scratch
        {
            A = sp_mat(arg3, arg4); //A = sparse(arg3,arg4);
        }

        // Process A in compact form
        uvec i, j;
        vec r;
        find(i, j, r, A); //[i,j,a] = find(A);
        mat a = join_horiz( UMatToMat(join_horiz(i, j)), r); //a = [i j a];

        int m = A.n_rows, n = A.n_cols; //[m,n] = size(A);
        if(moda)
        {
            int t_mn = (m >= n);
            for(int k=0; k<p; k++) //for k = 1:p
            {
                // Delete current d(k)-th diagonal
                //i = find(a(:,2)-a(:,1) == d(k));
                i = find( a.col(1)-a.col(0) == d(k) );
                //a(i,:) = [];
                for(int ii=i.size()-1; ii>=0; --ii) a.shed_row( i(ii) );
                // Append new d(k)-th diagonal to compact form
                //i = (max(1,1-d(k)):min(m,n-d(k)))';
                i = RangeVec( max(1,1-d(k))-1, min(m,n-d(k))-1 ).t();
                //a = [a; i i+d(k) B(i+(m>=n)*d(k),k)];
                vec t_i = UMatToMat(i);
                vec t_ii = t_i+t_mn*d(k);
                mat temp = t_i;
                temp.insert_cols(1, t_i+d(k));
                temp.insert_cols(2, MatRows(B.col(k), t_ii));
                a = join_vert(a, temp);
            }
        } else
        {
            vec len = zeros(p+1,1); //len=zeros(p+1,1);
            for(int k=0; k<p; k++)//for k = 1:p
            {
                //len(k+1) = len(k)+length(max(1,1-d(k)):min(m,n-d(k)));
                len(k+1) = len(k) + min(m,n-d(k)) - max(1,1-d(k)) + 1;
            }
            a = zeros(len(p),3); //a = zeros(len(p+1),3);
            int t_mn = (m >= n);
            for(int k=0; k<p; k++)//for k = 1:p
            {
                // Append new d(k)-th diagonal to compact form
                //i = (max(1,1-d(k)):min(m,n-d(k)))';
                i = RangeVec( max(1,1-d(k))-1, min(m,n-d(k))-1 ).t();
                //a((len(k)+1):len(k+1),:) = [i i+d(k) B(i+(m>=n)*d(k),k)];
                vec t_i = UMatToMat(i);
                vec t_ii = t_i+t_mn*d(k);
                mat temp = t_i;
                temp.insert_cols(1, t_i+d(k));
                temp.insert_cols(2, MatRows(B.col(k), t_ii));
                a.rows( len(k), len(k+1)-1 ) = temp;
            }
        }
        //res1 = sparse(a(:,1),a(:,2),full(a(:,3)),m,n);
        umat t_rc(2, a.n_rows);
        for(int i=0; i<a.n_rows; ++i)
        {
            t_rc(0, i) = a(i, 0);
            t_rc(1, i) = a(i, 1);
        }
        res1 = sp_mat(t_rc, a.col(2), m, n);
    }
}

}