#include "solvespace.h"

void System::WriteJacobian(int eqTag, int paramTag) {
    int a, i, j;

    j = 0;
    for(a = 0; a < param.n; a++) {
        Param *p = &(param.elem[a]);
        if(p->tag != paramTag) continue;
        mat.param[j] = p->h;
        j++;
    }
    mat.n = j;

    i = 0;
    for(a = 0; a < eq.n; a++) {
        Equation *e = &(eq.elem[a]);
        if(e->tag != eqTag) continue;

        mat.eq[i] = e->h;
        Expr *f = e->e->DeepCopyWithParamsAsPointers(&param, &(SS.param));
        f = f->FoldConstants();

        // Hash table (31 bits) to accelerate generation of zero partials.
        DWORD scoreboard = f->ParamsUsed();
        for(j = 0; j < mat.n; j++) {
            Expr *pd;
            if(scoreboard & (1 << (mat.param[j].v % 31))) { 
                pd = f->PartialWrt(mat.param[j]);
                pd = pd->FoldConstants();
            } else {
                pd = Expr::FromConstant(0);
            }
            mat.A.sym[i][j] = pd;
        }
        mat.B.sym[i] = f;
        i++;
    }
    mat.m = i;
}

void System::EvalJacobian(void) {
    int i, j;
    for(i = 0; i < mat.m; i++) {
        for(j = 0; j < mat.n; j++) {
            mat.A.num[i][j] = (mat.A.sym[i][j])->Eval();
        }
    }
}

bool System::IsDragged(hParam p) {
    if(SS.GW.pending.point.v) {
        // The pending point could be one in a group that has not yet
        // been processed, in which case the lookup will fail; but
        // that's not an error.
        Entity *pt = SS.entity.FindByIdNoOops(SS.GW.pending.point);
        if(pt) {
            switch(pt->type) {
                case Entity::POINT_N_TRANS:
                case Entity::POINT_IN_3D:
                    if(p.v == (pt->param[0]).v) return true;
                    if(p.v == (pt->param[1]).v) return true;
                    if(p.v == (pt->param[2]).v) return true;
                    break;

                case Entity::POINT_IN_2D:
                    if(p.v == (pt->param[0]).v) return true;
                    if(p.v == (pt->param[1]).v) return true;
                    break;
            }
        }
    }
    if(SS.GW.pending.circle.v) {
        Entity *circ = SS.entity.FindByIdNoOops(SS.GW.pending.circle);
        if(circ) {
            Entity *dist = SS.GetEntity(circ->distance);
            switch(dist->type) {
                case Entity::DISTANCE:
                    if(p.v == (dist->param[0].v)) return true;
                    break;
            }
        }
    }
    if(SS.GW.pending.normal.v) {
        Entity *norm = SS.entity.FindByIdNoOops(SS.GW.pending.normal);
        if(norm) {
            switch(norm->type) {
                case Entity::NORMAL_IN_3D:
                    if(p.v == (norm->param[0].v)) return true;
                    if(p.v == (norm->param[1].v)) return true;
                    if(p.v == (norm->param[2].v)) return true;
                    if(p.v == (norm->param[3].v)) return true;
                    break;
                // other types are locked, so not draggable
            }
        }
    }
    return false;
}

void System::SolveBySubstitution(void) {
    int i;
    for(i = 0; i < eq.n; i++) {
        Equation *teq = &(eq.elem[i]);
        Expr *tex = teq->e;

        if(tex->op    == Expr::MINUS &&
           tex->a->op == Expr::PARAM &&
           tex->b->op == Expr::PARAM)
        {
            hParam a = (tex->a)->x.parh;
            hParam b = (tex->b)->x.parh;
            if(!(param.FindByIdNoOops(a) && param.FindByIdNoOops(b))) {
                // Don't substitute unless they're both solver params;
                // otherwise it's an equation that can be solved immediately,
                // or an error to flag later.
                continue;
            }

            if(IsDragged(a)) {
                // A is being dragged, so A should stay, and B should go
                hParam t = a;
                a = b;
                b = t;
            }

            int j;
            for(j = 0; j < eq.n; j++) {
                Equation *req = &(eq.elem[j]);
                (req->e)->Substitute(a, b); // A becomes B, B unchanged
            }
            for(j = 0; j < param.n; j++) {
                Param *rp = &(param.elem[j]);
                if(rp->substd.v == a.v) {
                    rp->substd = b;
                }
            }
            Param *ptr = param.FindById(a);
            ptr->tag = VAR_SUBSTITUTED;
            ptr->substd = b;

            teq->tag = EQ_SUBSTITUTED;
        }
    }
}

static int BySensitivity(const void *va, const void *vb) {
    const int *a = (const int *)va, *b = (const int *)vb;

    hParam pa = SS.sys.mat.param[*a];
    hParam pb = SS.sys.mat.param[*b];
    
    if(System::IsDragged(pa) && !System::IsDragged(pb)) return  1;
    if(System::IsDragged(pb) && !System::IsDragged(pa)) return -1;

    double as = SS.sys.mat.sens[*a];
    double bs = SS.sys.mat.sens[*b];

    if(as < bs) return  1;
    if(as > bs) return -1;
    return 0;
}
void System::SortBySensitivity(void) {
    // For each unknown, sum up the sensitivities in that column of the
    // Jacobian
    int i, j;
    for(j = 0; j < mat.n; j++) {
        double s = 0;
        int i;
        for(i = 0; i < mat.m; i++) {
            s += fabs(mat.A.num[i][j]);
        }
        mat.sens[j] = s;
    }
    for(j = 0; j < mat.n; j++) {
        mat.permutation[j] = j;
    }

    qsort(mat.permutation, mat.n, sizeof(mat.permutation[0]), BySensitivity);

    int origPos[MAX_UNKNOWNS];
    int entryWithOrigPos[MAX_UNKNOWNS];
    for(j = 0; j < mat.n; j++) {
        origPos[j] = j;
        entryWithOrigPos[j] = j;
    }

#define SWAP(T, a, b) do { T temp = (a); (a) = (b); (b) = temp; } while(0)
    for(j = 0; j < mat.n; j++) {
        int dest = j; // we are writing to position j
        // And the source is whichever position ahead of us can be swapped
        // in to make the permutation vectors line up.
        int src = entryWithOrigPos[mat.permutation[j]];

        for(i = 0; i < mat.m; i++) {
            SWAP(double, mat.A.num[i][src], mat.A.num[i][dest]);
            SWAP(Expr *, mat.A.sym[i][src], mat.A.sym[i][dest]);
        }
        SWAP(hParam, mat.param[src], mat.param[dest]);

        SWAP(int, origPos[src], origPos[dest]);
        if(mat.permutation[dest] != origPos[dest]) oops();

        // Update the table; only necessary to do this for src, since dest
        // is already done.
        entryWithOrigPos[origPos[src]] = src;
    }
}

bool System::Tol(double v) {
    return (fabs(v) < 0.001);
}

void System::GaussJordan(void) {
    int i, j;

    for(j = 0; j < mat.n; j++) {
        mat.bound[j] = false;
    }

    // Now eliminate.
    i = 0;
    for(j = 0; j < mat.n; j++) {
        // First, seek a pivot in our column.
        int ip, imax;
        double max = 0;
        for(ip = i; ip < mat.m; ip++) {
            double v = fabs(mat.A.num[ip][j]);
            if(v > max) {
                imax = ip;
                max = v;
            }
        }
        if(!Tol(max)) {
            // There's a usable pivot in this column. Swap it in:
            int js;
            for(js = j; js < mat.n; js++) {
                double temp;
                temp = mat.A.num[imax][js];
                mat.A.num[imax][js] = mat.A.num[i][js];
                mat.A.num[i][js] = temp;
            }

            // Get a 1 as the leading entry in the row we're working on.
            double v = mat.A.num[i][j];
            for(js = 0; js < mat.n; js++) {
                mat.A.num[i][js] /= v;
            }

            // Eliminate this column from rows except this one.
            int is;
            for(is = 0; is < mat.m; is++) {
                if(is == i) continue;

                // We're trying to drive A[is][j] to zero. We know
                // that A[i][j] is 1, so we want to subtract off
                // A[is][j] times our present row.
                double v = mat.A.num[is][j];
                for(js = 0; js < mat.n; js++) {
                    mat.A.num[is][js] -= v*mat.A.num[i][js];
                }
                mat.A.num[is][j] = 0;
            }

            // And mark this as a bound variable.
            mat.bound[j] = true;

            // Move on to the next row, since we just used this one to
            // eliminate from column j.
            i++;
            if(i >= mat.m) break;
        }
    }
}

bool System::SolveLinearSystem(void) {
    if(mat.m != mat.n) oops();
    // Gaussian elimination, with partial pivoting. It's an error if the
    // matrix is singular, because that means two constraints are
    // equivalent.
    int i, j, ip, jp, imax;
    double max, temp;

    for(i = 0; i < mat.m; i++) {
        // We are trying eliminate the term in column i, for rows i+1 and
        // greater. First, find a pivot (between rows i and N-1).
        max = 0;
        for(ip = i; ip < mat.m; ip++) {
            if(fabs(mat.A.num[ip][i]) > max) {
                imax = ip;
                max = fabs(mat.A.num[ip][i]);
            }
        }
        if(fabs(max) < 1e-12) return false;

        // Swap row imax with row i
        for(jp = 0; jp < mat.n; jp++) {
            temp = mat.A.num[i][jp];
            mat.A.num[i][jp] = mat.A.num[imax][jp];
            mat.A.num[imax][jp] = temp;
        }
        temp = mat.B.num[i];
        mat.B.num[i] = mat.B.num[imax];
        mat.B.num[imax] = temp;

        // For rows i+1 and greater, eliminate the term in column i.
        for(ip = i+1; ip < mat.m; ip++) {
            temp = mat.A.num[ip][i]/mat.A.num[i][i];

            for(jp = 0; jp < mat.n; jp++) {
                mat.A.num[ip][jp] -= temp*(mat.A.num[i][jp]);
            }
            mat.B.num[ip] -= temp*mat.B.num[i];
        }
    }

    // We've put the matrix in upper triangular form, so at this point we
    // can solve by back-substitution.
    for(i = mat.m - 1; i >= 0; i--) {
        if(fabs(mat.A.num[i][i]) < 1e-10) return false;

        temp = mat.B.num[i];
        for(j = mat.n - 1; j > i; j--) {
            temp -= mat.X[j]*mat.A.num[i][j];
        }
        mat.X[i] = temp / mat.A.num[i][i];
    }

    return true;
}

bool System::NewtonSolve(int tag) {
    WriteJacobian(tag, tag);
    if(mat.m != mat.n) oops();

    int iter = 0;
    bool converged = false;
    int i;

    // Evaluate the functions at our operating point.
    for(i = 0; i < mat.m; i++) {
        mat.B.num[i] = (mat.B.sym[i])->Eval();
    }
    do {
        // And evaluate the Jacobian at our initial operating point.
        EvalJacobian();

        if(!SolveLinearSystem()) break;

        // Take the Newton step; 
        //      J(x_n) (x_{n+1} - x_n) = 0 - F(x_n)
        for(i = 0; i < mat.m; i++) {
            (param.FindById(mat.param[i]))->val -= mat.X[i];
        }

        // Re-evalute the functions, since the params have just changed.
        for(i = 0; i < mat.m; i++) {
            mat.B.num[i] = (mat.B.sym[i])->Eval();
        }
        // Check for convergence
        converged = true;
        for(i = 0; i < mat.m; i++) {
            if(!Tol(mat.B.num[i])) {
                converged = false;
                break;
            }
        }
    } while(iter++ < 50 && !converged);

    if(converged) { 
        return true;
    } else {
        dbp("no convergence");
        return false;
    }
}

bool System::Solve(void) {
    int i, j;
    
/*
    dbp("%d equations", eq.n);
    for(i = 0; i < eq.n; i++) {
        dbp("  %.3f = %s = 0", eq.elem[i].e->Eval(), eq.elem[i].e->Print());
    }
    dbp("%d parameters", param.n);
    for(i = 0; i < param.n; i++) {
        dbp("   param %08x at %.3f", param.elem[i].h.v, param.elem[i].val);
    } */

    param.ClearTags();
    eq.ClearTags();
    
    SolveBySubstitution();

    WriteJacobian(0, 0);

    EvalJacobian();

    SortBySensitivity();

/*
    for(i = 0; i < mat.m; i++) {
        dbp("function %d: %s", i, mat.B.sym[i]->Print());
    }
    dbp("m=%d", mat.m);
    for(i = 0; i < mat.m; i++) {
        for(j = 0; j < mat.n; j++) {
            dbp("A[%d][%d] = %.3f", i, j, mat.A.num[i][j]);
        }
    } */

    GaussJordan();

/*    dbp("bound states:");
    for(j = 0; j < mat.n; j++) {
        dbp("  param %08x: %d", mat.param[j], mat.bound[j]);
    } */

    // Fix any still-free variables wherever they are now.
    for(j = mat.n-1; j >= 0; --j) {
        if(mat.bound[j]) continue;
        Param *p = param.FindByIdNoOops(mat.param[j]);
        if(!p) {
            // This is parameter does not occur in this group, so it's
            // not available to assume.
            continue;
        }
        p->tag = VAR_ASSUMED;
    }
    
    bool ok = NewtonSolve(0);

    if(ok) {
        // System solved correctly, so write the new values back in to the
        // main parameter table.
        for(i = 0; i < param.n; i++) {
            Param *p = &(param.elem[i]);
            double val;
            if(p->tag == VAR_SUBSTITUTED) {
                val = param.FindById(p->substd)->val;
            } else {
                val = p->val;
            }

            Param *pp = SS.GetParam(p->h);
            pp->val = val;
            pp->known = true;
            // The main param table keeps track of what was assumed.
            pp->assumed = (p->tag == VAR_ASSUMED);
        }
    }

    return true;
}