#include "rtklib.h"

#define LAMBDA_P0 0.80
#define MIN_SAT 6

#define LOOPMAX 10000 /* maximum count of search loop */
#define SGN(x) ((x) <= 0.0 ? -1.0 : 1.0)
#define ROUND(x) (floor((x) + 0.5))
#define SWAP(x, y)   \
    do               \
    {                \
        double tmp_; \
        tmp_ = x;    \
        x = y;       \
        y = tmp_;    \
    } while (0)

static int parsearch(rtk_t *rtk, int n, const double *zhat, const double *Qzhat, const double *Z,
                     const double *L, const double *D, int m, double *F, double *s);

/* LD factorization (Q=L'*diag(D)*L) -----------------------------------------*/
static int LD(int n, const double *Q, double *L, double *D)
{
    int i, j, k, info = 0;
    double a, *A = mat(n, n);

    memcpy(A, Q, sizeof(double) * n * n);
    for (i = n - 1; i >= 0; i--)
    {
        if ((D[i] = A[i + i * n]) <= 0.0)
        {
            info = -1;
            break;
        }
        a = sqrt(D[i]);
        for (j = 0; j <= i; j++)
            L[i + j * n] = A[i + j * n] / a;
        for (j = 0; j <= i - 1; j++)
            for (k = 0; k <= j; k++)
                A[j + k * n] -= L[i + k * n] * L[i + j * n];
        for (j = 0; j <= i; j++)
            L[i + j * n] /= L[i + i * n];
    }
    free(A);
    if (info)
        fprintf(stderr, "%s : LD factorization error\n", __FILE__);
    return info;
}
/* integer gauss transformation ----------------------------------------------*/
static void gauss(int n, double *L, double *Z, int i, int j)
{
    int k, mu;

    if ((mu = (int)ROUND(L[i + j * n])) != 0)
    {
        for (k = i; k < n; k++)
            L[k + n * j] -= (double)mu * L[k + i * n];
        for (k = 0; k < n; k++)
            Z[k + n * j] -= (double)mu * Z[k + i * n];
    }
}
/* permutations --------------------------------------------------------------*/
static void perm(int n, double *L, double *D, int j, double del, double *Z)
{
    int k;
    double eta, lam, a0, a1;

    eta = D[j] / del;
    lam = D[j + 1] * L[j + 1 + j * n] / del;
    D[j] = eta * D[j + 1];
    D[j + 1] = del;
    for (k = 0; k <= j - 1; k++)
    {
        a0 = L[j + k * n];
        a1 = L[j + 1 + k * n];
        L[j + k * n] = -L[j + 1 + j * n] * a0 + a1;
        L[j + 1 + k * n] = eta * a0 + lam * a1;
    }
    L[j + 1 + j * n] = lam;
    for (k = j + 2; k < n; k++)
        SWAP(L[k + j * n], L[k + (j + 1) * n]);
    for (k = 0; k < n; k++)
        SWAP(Z[k + j * n], Z[k + (j + 1) * n]);
}
/* lambda reduction (z=Z'*a, Qz=Z'*Q*Z=L'*diag(D)*L) (ref.[1]) ---------------*/
static void reduction(int n, double *L, double *D, double *Z)
{
    int i, j, k;
    double del;

    j = n - 2;
    k = n - 2;
    //这里的调序变换类似插入排序的思路？
    while (j >= 0)
    {
        //由于第k+1，k+2，...，n-2列都进行过降相关并且没有被上一次调序变换影响，
        //因此只需对第0,1，...，k-1，k列进行降相关
        if (j <= k)
            for (i = j + 1; i < n; i++)
                gauss(n, L, Z, i, j); //从最后一列开始，各列非对角线元素从上往下依次降相关
        del = D[j] + L[j + 1 + j * n] * L[j + 1 + j * n] * D[j + 1];
        if (del + 1E-6 < D[j + 1])
        {                             /* 检验条件，若不满足检验条件则开始进行调序变换compared considering numerical error */
            perm(n, L, D, j, del, Z); //调序变换
            k = j;
            j = n - 2; //完成调序变换后重新从最后一列开始进行降相关及排序，k记录最后一次进行过调序变换的列序号
        }
        else
            j--;
    }
}
/* modified lambda (mlambda) search (ref. [2]) -------------------------------
* args   : n      I  number of float parameters
*          m      I  number of fixed solution
           L,D    I  transformed covariance matrix
           zs     I  transformed double-diff phase biases
           zn     O  fixed solutions
           s      O  sum of residuals for fixed solutions                    */
static int search(int n, int m, const double *L, const double *D,
                  const double *zs, double *zn, double *s)
{
    int i, j, k, c, nn = 0, imax = 0;
    double newdist, maxdist = 1E99, y; // maxdist，当前超椭圆半径
    double *S = zeros(n, n), *dist = mat(n, 1), *zb = mat(n, 1), *z = mat(n, 1), *step = mat(n, 1);

    k = n - 1;
    dist[k] = 0.0;       // k表示当前层，从最后一层（n-1）开始计算
    zb[k] = zs[k];       //即zn
    z[k] = ROUND(zb[k]); //四舍五入取整；取整后的数与未取整的数作差；step记录z[k]是四舍还是五入
    y = zb[k] - z[k];
    step[k] = SGN(y); /* step towards closest integer */
    for (c = 0; c < LOOPMAX; c++)
    {
        newdist = dist[k] + y * y / D[k]; /* newdist=sum(((z(j)-zb(j))^2/d(j))) */
        if (newdist < maxdist)
        { //如果当前累积目标函数计算值小于当前超椭圆半径
            /* Case 1: move down 情况1：若还未计算至第一层，继续计算累积目标函数值 */
            if (k != 0)
            {
                dist[--k] = newdist; //记录下当前层的累积目标函数值，dist[k]表示了第k,k+1,...,n-1层的目标函数计算和
                for (i = 0; i <= k; i++)
                    S[k + i * n] = S[k + 1 + i * n] + (z[k + 1] - zb[k + 1]) * L[k + 1 + i * n];
                zb[k] = zs[k] + S[k + k * n]; //计算Zk，即第k个整数模糊度参数的备选组的中心
                z[k] = ROUND(zb[k]);          /* next valid integer 四舍五入取整；取整后的数与未取整的数作差；记录是四舍还是五入*/
                y = zb[k] - z[k];
                step[k] = SGN(y);
            }
            /* Case 2: store the found candidate and try next valid integer 情况2：若已经计算至第一层，意味着所有层的累积目标函数值计算完毕 */
            else
            {
                // nn为当前候选解数，m为我们需要的固定解数，这里为2，表示需要一个最优解及一个次优解
                // s记录候选解的目标函数值，imax记录之前候选解中的最大目标函数值的坐标
                if (nn < m)
                { //若候选解数还没满
                    /* store the first m initial points */
                    if (nn == 0 || newdist > s[imax])
                        imax = nn; //若当前解的目标函数值比之前最大的目标函数值都大，那么更新imax使s[imax]指向当前解中具有的最大目标函数值
                    for (i = 0; i < n; i++)
                        zn[i + nn * n] = z[i]; // zn存放所有候选解
                    s[nn++] = newdist;         // s记录当前目标函数值newdist，并加加当前候选解数nn
                }
                else
                { //若候选解数已满（即当前zn中已经存了2个候选解）
                    if (newdist < s[imax])
                    { //若 当前解的目标函数值 比 s中的最大目标函数值 小
                        for (i = 0; i < n; i++)
                            zn[i + imax * n] = z[i]; //用当前解替换zn中具有较大目标函数值的解
                        s[imax] = newdist;           //用当前解的目标函数值替换s中的最大目标函数值
                        for (i = imax = 0; i < m; i++)
                            if (s[imax] < s[i])
                                imax = i; //更新imax保证imax始终指向s中的最大目标函数值
                    }
                    maxdist = s[imax]; //用当前最大的目标函数值更新超椭圆半径
                }
                z[0] += step[0]; /* next valid integer在第一层，取下一个有效的整数模糊度参数进行计算（若zb为5.3，则z取值顺序为5,6,4,7，...） */
                y = zb[0] - z[0];
                step[0] = -step[0] - SGN(step[0]);
            }
        }
        /* Case 3: exit or move up情况3：如果当前累积目标函数计算值大于当前超椭圆半径 */
        else
        {
            if (k == n - 1)
                break; //如果当前层为第n-1层，意味着后续目标函数各项的计算都会超出超椭圆半径，因此终止搜索
            else
            {                    //若当前层不是第n-1层
                k++;             /* move up 退后一层，即从第k层退到第k+1层*/
                z[k] += step[k]; /* next valid integer 计算退后一层后，当前层的下一个有效备选解*/
                y = zb[k] - z[k];
                step[k] = -step[k] - SGN(step[k]);
            }
        }
    }
    // 对s中的目标函数值及zn中的候选解进行排序（以s中目标函数值为排序标准，进行升序排序）
    // RTKLIB中最终可以得到一个最优解一个次优解，存在zn中，两解对应的目标函数值，存在s中
    for (i = 0; i < m - 1; i++)
    { /* sort by s */
        for (j = i + 1; j < m; j++)
        {
            if (s[i] < s[j])
                continue;
            SWAP(s[i], s[j]);
            for (k = 0; k < n; k++)
                SWAP(zn[k + i * n], zn[k + j * n]);
        }
    }
    free(S);
    free(dist);
    free(zb);
    free(z);
    free(step);

    if (c >= LOOPMAX)
    {
        fprintf(stderr, "%s : search loop count overflow\n", __FILE__);
        return -2;
    }
    return 0;
}
static float my_normcdf(float a) // 获取正态分布CDF
{
    float h, l, r;
    const float SQRT_HALF_HI = 0x1.6a09e6p-01f; // 7.07106769e-1
    const float SQRT_HALF_LO = 0x1.9fcef4p-27f; // 1.21016175e-8
    /* clamp input as normcdf(x) is either 0 or 1 asymptotically */
    if (fabsf(a) > 14.171875f)
        a = (a < 0.0f) ? -14.171875f : 14.171875f;
    h = fmaf(-SQRT_HALF_HI, a, -SQRT_HALF_LO * a);
    l = fmaf(SQRT_HALF_LO, a, fmaf(SQRT_HALF_HI, a, h));
    r = erfcf(h);
    if (h > 0.0f)
        r = fmaf(2.0f * h * l, r, r);
    return 0.5f * r;
}

static void nmatgetkmat_RD(const double *nmat, int n1, int n2, int m1, int m2, double *mmat)
{
    // double *mmat;
    int i, j;
    // mmat = mat(m1, m2);
    //从大矩阵里取小矩阵
    for (i = 0; i < m1; i++)
        for (j = 0; j < m2; j++)
        {
            *(mmat + i * m2 + j) = *(nmat + (n1 - m1) * n2 + (n2 - m2) + i * n2 + j);
        }

    // return mmat;
}

static void nmatgetkmat_RU(const double *nmat, int n1, int n2, int m1, int m2, double *mmat)
{
    // double *mmat;
    int i, j;
    // mmat = mat(m1, m2);
    //从大矩阵里取小矩阵
    for (i = 0; i < m1; i++)
        for (j = 0; j < m2; j++)
        {
            *(mmat + i * m2 + j) = *(nmat + (n2 - m2) + i * n2 + j);
        }

    // return mmat;
}
/* parlambda ------------------------------
 * integer least-square estimation. reduction is performed by lambda (ref.[1]),
 * and search by mlambda (ref.[2]).
 * args   :
 *          rtk_t* rtk    I
 *          int    n      I  number of float parameters
 *          int    m      I  number of fixed solutions
 *          double *a     I  float parameters (n x 1) (double-diff phase biases)
 *          double *Q     I  covariance matrix of float parameters (n x n)
 *          double *F     O  fixed solutions (n x m)
 *          double *s     O  sum of squared residulas of fixed solutions (1 x m)
 * return : status (0:ok,other:error)
 * notes  : matrix stored by column-major order (fortran convension)
 *-----------------------------------------------------------------------------*/
extern int parlambda(rtk_t *rtk, int n, int m, const double *a, const double *Q, double *F,
                     double *s)
{
    int info;
    double *L, *D, *Z, *z, *E, *Qt, *Qzhat;
    if (n <= 0 || m <= 0)
        return -1;
    L = zeros(n, n);
    D = mat(n, 1);
    Z = eye(n);
    z = mat(n, 1);
    E = mat(n, m);

    /* LD (lower diaganol) factorization (Q=L'*diag(D)*L) */
    if (!(info = LD(n, Q, L, D)))
    {

        /* lambda reduction (z=Z'*a, Qz=Z'*Q*Z=L'*diag(D)*L) */
        reduction(n, L, D, Z);
        matmul("TN", n, 1, n, 1.0, Z, a, 0.0, z); /* z=Z'*a */

        /*par search*/
        // Qz = Z'*Q*Z
        Qt = mat(n, n);
        Qzhat = mat(n, n);
        matmul("TN", n, n, n, 1.0, Z, Q, 0.0, Qt);     // Qt = Z'*Q
        matmul("NN", n, n, n, 1.0, Qt, Z, 0.0, Qzhat); // Qz =QtZ
        if (!(info = parsearch(rtk, n, z, Qzhat, Z, L, D, 2, F, s)))
        {
            ;
        }
        free(Qt);
        free(Qzhat);
    }
    free(L);
    free(D);
    free(Z);
    free(z);
    free(E);
    return info;
}

/*parsearch
*by LongRui Peng&ZiWen Qu from ZJUT
args:
  rtk_t *rtk            I
  int n                 I     number of float parameter
  const double *zhat    I     decorrelated float ambiguities
  const double *Qzhat   I     variance-covariance matrix of decorrelated float ambiguities
  const double *Z       I     Z-matrix from decorrel
  L,D                   I     lower-triangular and diagonal matrix from LtDL-decomposition of Qzhat
  m                     I     Number of requested integer candidate vectors [DEFAULT=2]
  double *F             O     fixed solutions (n x m)
  double *s             O     sum of squared residulas of fixed solutions (1 x m)

*/
static int parsearch(rtk_t *rtk, int n, const double *zhat, const double *Qzhat, const double *Z,
                     const double *L, const double *D, int m, double *F, double *s)

{
    int info = 0, p, k, i, j;
    float Ps = 1; /* Ps ：cumulative success rate  P0:Minimum required sucess rate [DEFAULT=0.8] */

    double *zk_n, *Lk_n, *Dk_n, *z1_fix, *Q11, *Q12, *Qp, *z2_fix, *z_t1, *z_t2, *z_fix;
    for (k = n; k > 2; k--)
    {
        if (Ps > LAMBDA_P0)
        {
            Ps = Ps * (2 * my_normcdf((float)(1 / (2 * sqrt(20 * D[k - 1])))) - 1);
        }
        else
            break;
    }
    // Lk_n: new matrix formed by  [k:n,k:n]  of  L
    // Dk_n: new diag formed by  [k:n] row of  D
    // zk_n: new column formed by [k:n] element of  zhat
    // Lk_n: new matrix formed by  [k:n,k:n]  of  L
    // Dk_n: new diag formed by  [k:n] row of  D
    // zk_n: new column formed by [k:n] element of  zhat
    p = n - k + 1;

    while (p >= MIN_SAT)
    {
        Dk_n = mat(p, 1);
        zk_n = mat(p, 1);
        Lk_n = mat(p, p);
        z1_fix = mat(p, m);
        j = k - 1;
        for (i = 0; i < p; i++)
        {
            Dk_n[i] = D[j];
            zk_n[i] = zhat[j];
            j++;
        }
        nmatgetkmat_RD(L, n, n, p, p, Lk_n);

        if (!(info = search(p, m, Lk_n, Dk_n, zk_n, z1_fix, s))) /* returns 0 if no error */
        {
            if (s[0] <= 0.0 || s[1] / s[0] >= rtk->sol.thres) // Ratio test
            {
                Q12 = mat(k - 1, p);
                Q11 = mat(p, p);
                Qp = mat(k - 1, p);
                z2_fix = mat(k - 1, m);
                z_t1 = mat(p, m);
                z_t2 = mat(k - 1, m);
                z_fix = mat(n, m);
                // first k - 1 ambiguities are adjusted based on correlation with the fixed ambiguities
                nmatgetkmat_RD(Qzhat, n, n, p, p, Q11);
                nmatgetkmat_RU(Qzhat, n, n, k - 1, p, Q12);
                matinv(Q11, p);
                matmul("NN", k - 1, p, p, 1.0, Q12, Q11, 0.0, Qp); // Qp = Q12 / Q11;
                for (i = 0; i < 2 * p; i++)
                {
                    if (i < p)
                        z_t1[i] = zk_n[i] - z1_fix[i];
                    else
                        z_t1[i] = zk_n[i - p] - z1_fix[i];
                }

                matmul("NN", k - 1, m, p, 1.0, Qp, z_t1, 0.0, z_t2); // zt_2=Qp(zk_n - z1_fix)
                /*z2_fix = z2_float - Qp(zk_n - z1_fix);*/
                for (i = 0; i < 2 * k - 2; i++)
                {
                    if (i < k - 1)
                        z2_fix[i] = zhat[i] - z_t2[i];
                    else
                        z2_fix[i] = zhat[i - k + 1] - z_t2[i];
                }
                // for (i = 0; i < 2 * k - 2; i++)
                //{
                //     z2_fix[i] = ROUND(z2_fix[i]);
                // }
                /* z_fix = [z2_fix, z1_fix]; */
                for (i = 0; i < 2 * n; i++)
                {
                    if (i < k - 1)
                        z_fix[i] = z2_fix[i];
                    else if (i < n)
                        z_fix[i] = z1_fix[i - k + 1];
                    else if (i < k - 1 + n)
                        z_fix[i] = z2_fix[i - n + k - 1];
                    else
                        z_fix[i] = z1_fix[i - 2 * k + 2];
                }

                /* 将在新空间中固定的模糊度逆变换回双差模糊度空间中 */
                info = solve("T", Z, z_fix, n, m, F); /* F=Z'\E */
                free(Dk_n);
                free(zk_n);
                free(Lk_n);
                free(z1_fix);
                free(Q12);
                free(Q11);
                free(Qp);
                free(z2_fix);
                free(z_t1);
                free(z_t2);
                free(z_fix);
                break;
            }
            else
            {
                trace(3, "PAR ratio test failed ! ,remove the ambiguity with the largest variance \n");
                p--;
                k++;
                free(Dk_n);
                free(zk_n);
                free(Lk_n);
                free(z1_fix);
            }
        }
    }
    if (p < MIN_SAT) // set the minimum number of part ambiguities  6
    {
        trace(3, "PAR lack sat! Now:%d output float solution... \n", p);
        z_fix = mat(n, m);
        if (!(info = search(n, m, L, D, zhat, z_fix, s)))
        { /* returns 0 if no error */

            /* 将在新空间中固定的模糊度逆变换回双差模糊度空间中 */
            info = solve("T", Z, z_fix, n, m, F); /* F=Z'\E */
        }
        free(z_fix);
        info = 0;
    }

    return info;
}