<commit_before>var mathjs = require("mathjs");
var AStarGraph = require("./AStarGraph");
var PathNode = require("./PathNode");

(function(exports) { 
    
    class PathFactory extends AStarGraph {
        constructor(options={}) {
            super(options);
            this.dimensions = options.dimensions || 1;
            this.estimateCost = options.estimateCost || this.cost_tsvva;
            this.cost = options.cost || this.cost_constant(0.2);
            this.vMax = options.maxVelocity || Array(this.dimensions).fill(10);
            this.aMax = options.maxAcceleration || Array(this.dimensions).fill(2);
            this.jMin = options.minJerk || Array(this.dimensions).fill(0.1);
            if (options.constrain) {
                this.constrain = options.constrain;
                this.isConstrained = options.isConstrained || ((node) => true);
            } else {
                this.constrain = ((neighbor) => neighbor);
                this.isConstrained = options.isConstrained || ((node) => false);
            }
            this.onNoPath = options.onNoPath || ((start, goal) => {
                throw new Error("No path found from:"+JSON.stringify(start)+" to:"+JSON.stringify(goal));
            });
            this.jerkScale = 1; // fraction of path length
            this.jMax = this.aMax; // initial value
            this.round = 2; // force discrete graph space
            this.nodeMap = {};
            this.nodeId = 0;
            this.stats = {
                lookupTotal: 0,
                lookupHit: 0,
                tsva: 0,
                neighborsOf: 0,
            };
        }
        isCruiseNode(node) {
            return node.a.length === node.a.reduce((acc,a) => !a ? (1+acc) : acc, 0) && // not accel
                node.v.reduce((acc,v) => v ? (1+acc) : acc, 0) > 0; // moving
        }
        svaToNode(position, velocity, acceleration) {
            var node = new PathNode(position, velocity, acceleration);
            return this.lookupNode(node);
        }
        toJSON() {
            var obj = super.toJSON();
            console.log("ha");
            obj.f && (obj.f = ((obj.f*100+0.5)|0)/100);
            return obj;
        }
        iAxisAccelerations(node, i, dsgoal) {
            var pf = this;
            var cullOvershoot = !node.c;
            function * iAxisAccelerations(node, i, dsgoal) {
                var ai = node.a[i];
                var aMax = pf.aMax[i];
                var aMin = -aMax;
                var jMax = pf.jMax[i];
                var aplus = ai + jMax;
                var aminus = ai - jMax;
                if (node.c) { // client must constrain neighbors
                    yield(ai); // maintain acceleration
                    aplus = aMax < aplus ? aMax : aplus;
                    aminus = aminus < aMin ? aMin : aminus;
                    if (dsgoal >= 0) { // [a, aplus, aminus]
                        ai < aplus && (yield aplus); // faster
                        aminus < ai && (yield aminus); // slower
                    } else { // [a, aminus, aplus]
                        aminus < ai && (yield aminus); // slower
                        ai < aplus && (yield aplus); // faster
                    }
                    return;
                }

                var vi = node.v[i];
                var vplus = vi + aplus;
                var vminus = vi + aminus;
                var vzero = vi + ai;
                var vMax = pf.vMax[i];
                var vMin = -vMax;
                if (dsgoal >= 0) {
                    var ds = dsgoal;
                    vMax = ds < vMax ? ds : vMax; // prevent overshoot

                    (vzero <= 0 || vzero <= ds) &&
                    (vMin <= vzero && vMin <= vzero && vzero <= vMax) &&
                    (yield ai); // maintain acceleration

                    (vplus <= 0 || vplus <= ds) &&
                    (ai < aMax && vMin <= vplus && vplus <= vMax) &&
                    (yield (aMax < aplus ? aMax : aplus));
                    
                    (vminus <= 0 || vminus <= ds) &&
                    (aMin < ai && vMin <= vminus && vminus <= vMax) &&
                    (yield (aMin > aminus ? aMin : aminus));
                } else {
                    vMin = vMin < dsgoal ? dsgoal : vMin; // prevent overshoot

                    (0 <= vzero || dsgoal <= vzero) &&
                    (vMin <= vzero && vMin <= vzero && vzero <= vMax) &&
                    (yield ai); // maintain acceleration

                    (0 <= vminus || dsgoal <= vminus) &&
                    (aMin < ai && vMin <= vminus && vminus <= vMax) &&
                    (yield (aMin > aminus ? aMin : aminus));

                    (0<vplus || dsgoal <= vplus) &&
                    (ai < aMax && vMin <= vplus && vplus <= vMax) &&
                    (yield (aMax < aplus ? aMax : aplus));
                }
            }
            return iAxisAccelerations(node, i, dsgoal);
        }
        lookupNode(node) {
            var key = node.key; 
            var result = this.nodeMap[key];
            this.stats.lookupTotal++;
            if (result) {
                this.stats.lookupHit++;
            } else {
                result = this.nodeMap[key] = node;
                node.c = this.isConstrained(node) ? 1 : 0;
                node.id = ++this.nodeId;
            }
            return result;
        }
        axisAccelerations(node, i, dsgoal) {
            return Array.from(this.iAxisAccelerations(node, i, dsgoal));
        }
        static validateNode(node) {
            if (!(node instanceof PathNode)) {
                throw new Error("Expected neigbhorsOf(?PathNode?,...)");
            }
            return node;
        }
        findPath(start, goal, options) { 
            start = this.svaToNode(
                mathjs.round(start.s, this.round),
                mathjs.round(start.v, this.round),
                mathjs.round(start.a, this.round)
            );
            goal = this.svaToNode(
                mathjs.round(goal.s, this.round),
                mathjs.round(goal.v, this.round),
                mathjs.round(goal.a, this.round)
            );
            var ds = mathjs.abs(mathjs.subtract(goal.s, start.s));
            var jerk = mathjs.round(mathjs.divide(ds, this.jerkScale), this.round);
            this.jMax = jerk.map((j,i) => mathjs.min(this.aMax[i], mathjs.max(j, this.jMin[i])));
            var result = super.findPath(start, goal, options);
            result.stats.start = start.s;
            result.stats.goal = goal.s;
            result.path.length || this.onNoPath(start, goal);

            return result;
        }
        isGoalNeighbor(node, goal) { // a goal is zero-velocity
            var verbose = false;

            for (var i = 0; i < this.dimensions; i++) {
                var jMax = this.jMax[i];
                var ds = goal.s[i] - node.s[i];

                if (ds < -jMax || jMax < ds) {
                    verbose && console.log("too far", ds,  JSON.stringify(node));
                    return false; // too far
                }

                var vi = node.v[i];
                if (vi < -jMax || jMax < vi) {
                    verbose && console.log("too fast", JSON.stringify(node));
                    return false; // too fast
                }
                if (vi < 0 && ds > 0 || vi > 0 && ds < 0) {
                    verbose && console.log("wrong way velocity", JSON.stringify(node));
                    return false; // wrong way velocity
                }

                var ai = node.a[i];
                if (ai < -jMax || jMax < ai) {
                    verbose && console.log("too jerky", JSON.stringify(node));
                    return false; // too jerky
                }

                var dv = 0 - vi;
                if (dv > 0 && ai < -jMax || dv < 0 && ai > jMax) { // allow 2 * jerk acceleration when stopping
                    verbose && console.log("wrong way acceleration", JSON.stringify(node));
                    return false; // wrong way acceleration
                }
            }
            return true;
        }
        neighborsOf(node, goal) {
            this.stats.neighborsOf++;
            if (goal && this.isGoalNeighbor(node, goal)) {
                return [goal];
            }
            var createNeighbor = (avariation) => {
                var vvariation = mathjs.add(node.v,avariation); // instantaneous acceleration
                var svariation = mathjs.add(node.s,vvariation); // instantaneous acceleration
                var neighbor = this.svaToNode(svariation, vvariation, avariation);
                var n = this.constrain(neighbor);
                if (n) {
                    if (this.estimateCost(neighbor, goal) < Number.MAX_SAFE_INTEGER) {
                        return n;
                    }
                } else {
                    this.onNeighbor && this.onNeighbor(neighbor,"-cn");
                }
                return null;
            }
            function* iNeighbors(pf) {
                // generate accelerations a [...[a,aup,adown]...]
                var anewbasis = node.a.map((a,i) => pf.axisAccelerations(node, i, goal.s[i]-node.s[i]));
                var neighbor;
                if (!node.c && pf.isCruiseNode(node)) { // generate restricted set of neighbors (8 for 3D)
                    var asteady = [];
                    for (var i = 0; i < anewbasis.length; i++) { 
                        var basis = anewbasis[i];
                        var a0 = basis[0];
                        if (a0 == null) {
                            return; // no values
                        }
                        asteady.push(a0); // asteady is no change in acceleration neighbor

                        // The adown neighbors decreases acceleration for all but one of the axes
                        var adown = [];
                        if (anewbasis.length === 1) {
                            adown.push(basis[basis.length-1]);
                        } else {
                            for (var j = 0; j < anewbasis.length; j++) {
                                adown.push(i===j ? basis[0] : basis[basis.length-1]);
                            }
                        }
                        (neighbor = createNeighbor(adown)) && (yield neighbor);
                    }
                    (neighbor = createNeighbor(asteady)) && (yield neighbor); // da==0
                    for (var i = 0; i < anewbasis.length; i++) { // da>0
                        var basis = anewbasis[i];
                        if (basis.length < 3) { // already generated
                            continue;
                        }
                        var aup = [];
                        for (var j = 0; j < anewbasis.length; j++) {
                            if (i==j) {
                                aup && aup.push(basis[1]);
                            } else {
                                aup && aup.push(basis[0]);
                            }
                        }
                        aup && (neighbor = createNeighbor(aup)) && (yield neighbor);
                    }
                } else { // generate all possible neighbors (27 for 3D)
                    var permutations = PathFactory.permutations(anewbasis);
                    for (var anew of permutations) { 
                        (neighbor = createNeighbor(anew)) && (yield neighbor);
                    }
                }
            }
            return iNeighbors(this);
        }
        tsvva(s, v1, v2, amax) {
            this.stats.tsva++;
            if (s < 0) {
                return this.tsvva(-s, -v1, -v2, amax);
            }
            var d = amax * s + 0.5*(v1*v1 + v2*v2);
            var pm = 2*mathjs.sqrt(d);
            var n1 = pm - v1 - v2;
            //var n2 = -pm - v1 - v2; // never positive
            //return (n2 < 0 ? n1 : n2)/amax;
            return n1/amax;
        }
        cost_constant(cost) {
            return (n1,n2) => n1 == n2 ? 0 : cost;
        }
        cost_tsvva(n1, goal) {
            if (n1.h) {
                return n1.h; // cached estimate
            }
            var ds = mathjs.subtract(goal.s, n1.s);
            var t = ds.map((s,i) => this.tsvva(s, n1.v[i], goal.v[i], this.aMax[i]));
            return mathjs.sum(t);
        }
        static get PathNode() {
            return PathNode;
        }
        static permutations(vv) {
            var vvelts = vv.reduce((acc, e) => e == null || !e.length ? 0:(acc+1), 0);
            if (vv.length !== vvelts) {
                return [];
            }
            function * variations() {
                var vi = Array(vv.length).fill(0);
                for(;;) {
                    yield vv.map((v,i) => v[vi[i]]);
                    for (var i = 0; i < vv.length; i++) {
                        if (++vi[i] < vv[i].length) {
                            break;
                        }
                        vi[i] = 0;
                        if (i+1 >= vv.length) {
                            return; 
                        }
                    }
                } 
            }
            return Array.from(variations());
        }
    }

    module.exports = exports.PathFactory = PathFactory;
})(typeof exports === "object" ? exports : (exports = {}));

(typeof describe === 'function') && describe("PathFactory", function() {
    var should = require("should");
    var PathFactory = exports.PathFactory;

    function testFindPath(pf, start, goal, verbose) {
        var result = pf.findPath(start, goal, {
            onCurrent: (current) => {
                if (verbose>2) {
                    var path = pf.pathTo(current);
                    console.log("path: "+path.map((node) => node.id));
                    console.log("  current:",
                        JSON.stringify(current),
                        "h:"+mathjs.round(pf.estimateCost(current, goal), pf.round),
                        pf.isCruiseNode(current) ? "cruise" : "accel",
                    "");
                }
                return true;
            },
            onNeighbor: (neighbor,outcome) => {
                if (verbose>3) {
                    var path = pf.pathTo(neighbor);
                    (verbose>4 || outcome !== "-cn") && console.log(" ",
                        outcome,
                        "nbr:",
                        JSON.stringify(neighbor),
                        "h:"+mathjs.round(pf.estimateCost(neighbor, goal), pf.round),
                        pf.isCruiseNode(neighbor) ? "cruise" : "accel",
                        "");
                }
            },
            onCull: (node, gscore_new) => {
                if (verbose>3) {
                    console.log("culling", JSON.stringify(node), gscore_new);
                }
                return null;
            },
        });
        var path = result.path;
        path.reduce((acc,n) => { // velocity should not switch directions
            acc < 0 && n.v[0].should.not.above(0);
            acc > 0 && n.v[0].should.not.below(0);
            acc = n.v[0];
        }, 0);
        verbose>1 && path.forEach((n,i) => console.log("path["+i+"]", JSON.stringify(n), pf.isCruiseNode(n) ? "cruise" : "accel"));
        verbose>0 && console.log("findPath", JSON.stringify(result.stats));
        if (path.length === 0) {
            console.log("start", JSON.stringify(start));
            console.log("goal", JSON.stringify(goal));
        }
        path.length.should.above(0);
        return result;
    }

    it("PathNode(name, pos, velocity) is the constructor", function() {
        var n1 = new PathNode([1,1,1]);
        should.deepEqual(n1.v, [0,0,0]); // default velocity is zero
        var n2 = new PathNode([1,1,1], [1,2,3]);
        should.deepEqual(n2.v, [1,2,3]);
    });
    it("estimateCost(n1,n2) estimates the cost to move between the given nodes", function() {
        var vmax = 10;
        var jmax = 5;
        var pf = new PathFactory({
            dimensions: 2,
            maxVelocity: [vmax,vmax],
            maxAcceleration: [jmax,jmax],
        });
        pf.estimateCost.should.equal(pf.cost_tsvva); // default cost estimator

        var goal1 = pf.svaToNode([1,1]);
        var node = pf.svaToNode([0,0]);
        pf.isGoalNeighbor(node,goal1).should.equal(true);
        pf.estimateCost(node,goal1).should.not.equal(1); 

        var cost = pf.tsvva(100,0,0,jmax);
        pf.estimateCost(node,pf.svaToNode([100,100])).should.equal(2*cost); // sum of tsvva costs
        pf.estimateCost(node,pf.svaToNode([-100,-100])).should.equal(2*cost); // sum of tsvva costs
    })
    it("tsvva(v1, v2, amax) return minimum time for velocity change over distance", function() {
        var pf = new PathFactory();
        var sqrt2 = mathjs.sqrt(2);
        pf.tsvva(0,0,0,1).should.approximately(0,0.001); // start at rest, end at rest
        pf.tsvva(4,0,0,1).should.approximately(4,0.001); // start at rest, end at rest
        pf.tsvva(2,0,2,1).should.approximately(2,0.001); // constant acceleration from rest
        pf.tsvva(2,2,0,1).should.approximately(2,0.001); // constant acceleration to rest
        pf.tsvva(6,2,4,1).should.approximately(2,0.001); // constant acceleration with initial velocity
        pf.tsvva(0,2,-2,1).should.approximately(4,0.001); // velocity change over zero distance
        pf.tsvva(0,-2,2,1).should.approximately(4,0.001); // velocity change over zero distance
        pf.tsvva(0,3,3,1).should.approximately(0,0.001); // no velocity change over zero distance
        pf.tsvva(-4,0,0,1).should.approximately(4,0.001); // negative distance
        pf.tsvva(-2,0,-2,1).should.approximately(2,0.001); // negative distance
        pf.tsvva(-2,-2,0,1).should.approximately(2,0.001); // negative distance
        for (var i=0; i<10000; i++) {
            var v = mathjs.random(0.0001,100);
            var s = mathjs.random(0.0001,100);<commit_msg>estimateFree<commit_after>var mathjs = require("mathjs");
var AStarGraph = require("./AStarGraph");
var PathNode = require("./PathNode");

(function(exports) { 
    
    class PathFactory extends AStarGraph {
        constructor(options={}) {
            super(options);
            this.dimensions = options.dimensions || 1;
            this.estimateFree = options.estimateFree || this.cost_tsvva;
            this.estimateConstrained = options.estimateConstrained || this.cost_constant(1);
            this.cost = options.cost || this.cost_constant(0.2);
            this.vMax = options.maxVelocity || Array(this.dimensions).fill(10);
            this.aMax = options.maxAcceleration || Array(this.dimensions).fill(2);
            this.jMin = options.minJerk || Array(this.dimensions).fill(0.1);
            if (options.constrain) {
                this.constrain = options.constrain;
                this.isConstrained = options.isConstrained || ((node) => true);
            } else {
                this.constrain = ((neighbor) => neighbor);
                this.isConstrained = options.isConstrained || ((node) => false);
            }
            this.onNoPath = options.onNoPath || ((start, goal) => {
                throw new Error("No path found from:"+JSON.stringify(start)+" to:"+JSON.stringify(goal));
            });
            this.jerkScale = 1; // fraction of path length
            this.jMax = this.aMax; // initial value
            this.round = 2; // force discrete graph space
            this.nodeMap = {};
            this.nodeId = 0;
            this.stats = {
                lookupTotal: 0,
                lookupHit: 0,
                tsva: 0,
                neighborsOf: 0,
            };
        }
        isCruiseNode(node) {
            return node.a.length === node.a.reduce((acc,a) => !a ? (1+acc) : acc, 0) && // not accel
                node.v.reduce((acc,v) => v ? (1+acc) : acc, 0) > 0; // moving
        }
        svaToNode(position, velocity, acceleration) {
            var node = new PathNode(position, velocity, acceleration);
            return this.lookupNode(node);
        }
        toJSON() {
            var obj = super.toJSON();
            console.log("ha");
            obj.f && (obj.f = ((obj.f*100+0.5)|0)/100);
            return obj;
        }
        iAxisAccelerations(node, i, dsgoal) {
            var pf = this;
            var cullOvershoot = !node.c;
            function * iAxisAccelerations(node, i, dsgoal) {
                var ai = node.a[i];
                var aMax = pf.aMax[i];
                var aMin = -aMax;
                var jMax = pf.jMax[i];
                var aplus = ai + jMax;
                var aminus = ai - jMax;
                if (node.c) { // client must constrain neighbors
                    yield(ai); // maintain acceleration
                    aplus = aMax < aplus ? aMax : aplus;
                    aminus = aminus < aMin ? aMin : aminus;
                    if (dsgoal >= 0) { // [a, aplus, aminus]
                        ai < aplus && (yield aplus); // faster
                        aminus < ai && (yield aminus); // slower
                    } else { // [a, aminus, aplus]
                        aminus < ai && (yield aminus); // slower
                        ai < aplus && (yield aplus); // faster
                    }
                    return;
                }

                var vi = node.v[i];
                var vplus = vi + aplus;
                var vminus = vi + aminus;
                var vzero = vi + ai;
                var vMax = pf.vMax[i];
                var vMin = -vMax;
                if (dsgoal >= 0) {
                    var ds = dsgoal;
                    vMax = ds < vMax ? ds : vMax; // prevent overshoot

                    (vzero <= 0 || vzero <= ds) &&
                    (vMin <= vzero && vMin <= vzero && vzero <= vMax) &&
                    (yield ai); // maintain acceleration

                    (vplus <= 0 || vplus <= ds) &&
                    (ai < aMax && vMin <= vplus && vplus <= vMax) &&
                    (yield (aMax < aplus ? aMax : aplus));
                    
                    (vminus <= 0 || vminus <= ds) &&
                    (aMin < ai && vMin <= vminus && vminus <= vMax) &&
                    (yield (aMin > aminus ? aMin : aminus));
                } else {
                    vMin = vMin < dsgoal ? dsgoal : vMin; // prevent overshoot

                    (0 <= vzero || dsgoal <= vzero) &&
                    (vMin <= vzero && vMin <= vzero && vzero <= vMax) &&
                    (yield ai); // maintain acceleration

                    (0 <= vminus || dsgoal <= vminus) &&
                    (aMin < ai && vMin <= vminus && vminus <= vMax) &&
                    (yield (aMin > aminus ? aMin : aminus));

                    (0<vplus || dsgoal <= vplus) &&
                    (ai < aMax && vMin <= vplus && vplus <= vMax) &&
                    (yield (aMax < aplus ? aMax : aplus));
                }
            }
            return iAxisAccelerations(node, i, dsgoal);
        }
        lookupNode(node) {
            var key = node.key; 
            var result = this.nodeMap[key];
            this.stats.lookupTotal++;
            if (result) {
                this.stats.lookupHit++;
            } else {
                result = this.nodeMap[key] = node;
                node.c = this.isConstrained(node) ? 1 : 0;
                node.id = ++this.nodeId;
            }
            return result;
        }
        axisAccelerations(node, i, dsgoal) {
            return Array.from(this.iAxisAccelerations(node, i, dsgoal));
        }
        static validateNode(node) {
            if (!(node instanceof PathNode)) {
                throw new Error("Expected neigbhorsOf(?PathNode?,...)");
            }
            return node;
        }
        findPath(start, goal, options) { 
            start = this.svaToNode(
                mathjs.round(start.s, this.round),
                mathjs.round(start.v, this.round),
                mathjs.round(start.a, this.round)
            );
            goal = this.svaToNode(
                mathjs.round(goal.s, this.round),
                mathjs.round(goal.v, this.round),
                mathjs.round(goal.a, this.round)
            );
            var ds = mathjs.abs(mathjs.subtract(goal.s, start.s));
            var jerk = mathjs.round(mathjs.divide(ds, this.jerkScale), this.round);
            this.jMax = jerk.map((j,i) => mathjs.min(this.aMax[i], mathjs.max(j, this.jMin[i])));
            var result = super.findPath(start, goal, options);
            result.stats.start = start.s;
            result.stats.goal = goal.s;
            result.path.length || this.onNoPath(start, goal);

            return result;
        }
        isGoalNeighbor(node, goal) { // a goal is zero-velocity
            var verbose = false;

            for (var i = 0; i < this.dimensions; i++) {
                var jMax = this.jMax[i];
                var ds = goal.s[i] - node.s[i];

                if (ds < -jMax || jMax < ds) {
                    verbose && console.log("too far", ds,  JSON.stringify(node));
                    return false; // too far
                }

                var vi = node.v[i];
                if (vi < -jMax || jMax < vi) {
                    verbose && console.log("too fast", JSON.stringify(node));
                    return false; // too fast
                }
                if (vi < 0 && ds > 0 || vi > 0 && ds < 0) {
                    verbose && console.log("wrong way velocity", JSON.stringify(node));
                    return false; // wrong way velocity
                }

                var ai = node.a[i];
                if (ai < -jMax || jMax < ai) {
                    verbose && console.log("too jerky", JSON.stringify(node));
                    return false; // too jerky
                }

                var dv = 0 - vi;
                if (dv > 0 && ai < -jMax || dv < 0 && ai > jMax) { // allow 2 * jerk acceleration when stopping
                    verbose && console.log("wrong way acceleration", JSON.stringify(node));
                    return false; // wrong way acceleration
                }
            }
            return true;
        }
        neighborsOf(node, goal) {
            this.stats.neighborsOf++;
            if (goal && this.isGoalNeighbor(node, goal)) {
                return [goal];
            }
            var createNeighbor = (avariation) => {
                var vvariation = mathjs.add(node.v,avariation); // instantaneous acceleration
                var svariation = mathjs.add(node.s,vvariation); // instantaneous acceleration
                var neighbor = this.svaToNode(svariation, vvariation, avariation);
                var n = this.constrain(neighbor);
                if (n) {
                    if (this.estimateCost(neighbor, goal) < Number.MAX_SAFE_INTEGER) {
                        return n;
                    }
                } else {
                    this.onNeighbor && this.onNeighbor(neighbor,"-cn");
                }
                return null;
            }
            function* iNeighbors(pf) {
                // generate accelerations a [...[a,aup,adown]...]
                var anewbasis = node.a.map((a,i) => pf.axisAccelerations(node, i, goal.s[i]-node.s[i]));
                var neighbor;
                if (!node.c && pf.isCruiseNode(node)) { // generate restricted set of neighbors (8 for 3D)
                    var asteady = [];
                    for (var i = 0; i < anewbasis.length; i++) { 
                        var basis = anewbasis[i];
                        var a0 = basis[0];
                        if (a0 == null) {
                            return; // no values
                        }
                        asteady.push(a0); // asteady is no change in acceleration neighbor

                        // The adown neighbors decreases acceleration for all but one of the axes
                        var adown = [];
                        if (anewbasis.length === 1) {
                            adown.push(basis[basis.length-1]);
                        } else {
                            for (var j = 0; j < anewbasis.length; j++) {
                                adown.push(i===j ? basis[0] : basis[basis.length-1]);
                            }
                        }
                        (neighbor = createNeighbor(adown)) && (yield neighbor);
                    }
                    (neighbor = createNeighbor(asteady)) && (yield neighbor); // da==0
                    for (var i = 0; i < anewbasis.length; i++) { // da>0
                        var basis = anewbasis[i];
                        if (basis.length < 3) { // already generated
                            continue;
                        }
                        var aup = [];
                        for (var j = 0; j < anewbasis.length; j++) {
                            if (i==j) {
                                aup && aup.push(basis[1]);
                            } else {
                                aup && aup.push(basis[0]);
                            }
                        }
                        aup && (neighbor = createNeighbor(aup)) && (yield neighbor);
                    }
                } else { // generate all possible neighbors (27 for 3D)
                    var permutations = PathFactory.permutations(anewbasis);
                    for (var anew of permutations) { 
                        (neighbor = createNeighbor(anew)) && (yield neighbor);
                    }
                }
            }
            return iNeighbors(this);
        }
        tsvva(s, v1, v2, amax) {
            this.stats.tsva++;
            if (s < 0) {
                return this.tsvva(-s, -v1, -v2, amax);
            }
            var d = amax * s + 0.5*(v1*v1 + v2*v2);
            var pm = 2*mathjs.sqrt(d);
            var n1 = pm - v1 - v2;
            //var n2 = -pm - v1 - v2; // never positive
            //return (n2 < 0 ? n1 : n2)/amax;
            return n1/amax;
        }
        cost_constant(cost) {
            return (n1,n2) => n1 == n2 ? 0 : cost;
        }
        cost_tsvva(n1, goal) {
            if (n1.h) {
                return n1.h; // cached estimate
            }
            var ds = mathjs.subtract(goal.s, n1.s);
            var t = ds.map((s,i) => this.tsvva(s, n1.v[i], goal.v[i], this.aMax[i]));
            return mathjs.sum(t);
        }
        estimateCost(n1, goal) {
            if (n1.h) {
                return n1.h; // cached estimate
            }
            if (n1.c) { // constrained node
                return this.estimateConstrained(n1, goal);
            }
            return this.estimateFree(n1, goal);
        }
        static get PathNode() {
            return PathNode;
        }
        static permutations(vv) {
            var vvelts = vv.reduce((acc, e) => e == null || !e.length ? 0:(acc+1), 0);
            if (vv.length !== vvelts) {
                return [];
            }
            function * variations() {
                var vi = Array(vv.length).fill(0);
                for(;;) {
                    yield vv.map((v,i) => v[vi[i]]);
                    for (var i = 0; i < vv.length; i++) {
                        if (++vi[i] < vv[i].length) {
                            break;
                        }
                        vi[i] = 0;
                        if (i+1 >= vv.length) {
                            return; 
                        }
                    }
                } 
            }
            return Array.from(variations());
        }
    }

    module.exports = exports.PathFactory = PathFactory;
})(typeof exports === "object" ? exports : (exports = {}));

(typeof describe === 'function') && describe("PathFactory", function() {
    var should = require("should");
    var PathFactory = exports.PathFactory;

    function testFindPath(pf, start, goal, verbose) {
        var result = pf.findPath(start, goal, {
            onCurrent: (current) => {
                if (verbose>2) {
                    var path = pf.pathTo(current);
                    console.log("path: "+path.map((node) => node.id));
                    console.log("  current:",
                        JSON.stringify(current),
                        "h:"+mathjs.round(pf.estimateCost(current, goal), pf.round),
                        pf.isCruiseNode(current) ? "cruise" : "accel",
                    "");
                }
                return true;
            },
            onNeighbor: (neighbor,outcome) => {
                if (verbose>3) {
                    var path = pf.pathTo(neighbor);
                    (verbose>4 || outcome !== "-cn") && console.log(" ",
                        outcome,
                        "nbr:",
                        JSON.stringify(neighbor),
                        "h:"+mathjs.round(pf.estimateCost(neighbor, goal), pf.round),
                        pf.isCruiseNode(neighbor) ? "cruise" : "accel",
                        "");
                }
            },
            onCull: (node, gscore_new) => {
                if (verbose>3) {
                    console.log("culling", JSON.stringify(node), gscore_new);
                }
                return null;
            },
        });
        var path = result.path;
        path.reduce((acc,n) => { // velocity should not switch directions
            acc < 0 && n.v[0].should.not.above(0);
            acc > 0 && n.v[0].should.not.below(0);
            acc = n.v[0];
        }, 0);
        verbose>1 && path.forEach((n,i) => console.log("path["+i+"]", JSON.stringify(n), pf.isCruiseNode(n) ? "cruise" : "accel"));
        verbose>0 && console.log("findPath", JSON.stringify(result.stats));
        if (path.length === 0) {
            console.log("start", JSON.stringify(start));
            console.log("goal", JSON.stringify(goal));
        }
        path.length.should.above(0);
        return result;
    }

    it("PathNode(name, pos, velocity) is the constructor", function() {
        var n1 = new PathNode([1,1,1]);
        should.deepEqual(n1.v, [0,0,0]); // default velocity is zero
        var n2 = new PathNode([1,1,1], [1,2,3]);
        should.deepEqual(n2.v, [1,2,3]);
    });
    it("estimateCost(n1,n2) estimates the cost to move between the given nodes", function() {
        var vmax = 10;
        var jmax = 5;
        var pf = new PathFactory({
            dimensions: 2,
            maxVelocity: [vmax,vmax],
            maxAcceleration: [jmax,jmax],
        });
        pf.estimateFree.should.equal(pf.cost_tsvva); // default cost estimator
        pf.estimateConstrained.toString().should.equal(pf.cost_constant(1).toString()); // default cost estimator

        var goal1 = pf.svaToNode([1,1]);
        var node = pf.svaToNode([0,0]);
        pf.isGoalNeighbor(node,goal1).should.equal(true);
        pf.estimateCost(node,goal1).should.not.equal(1); 

        var cost = pf.tsvva(100,0,0,jmax);
        pf.estimateCost(node,pf.svaToNode([100,100])).should.equal(2*cost); // sum of tsvva costs
        pf.estimateCost(node,pf.svaToNode([-100,-100])).should.equal(2*cost); // sum of tsvva costs
    })
    it("tsvva(v1, v2, amax) return minimum time for velocity change over distance", function() {
        var pf = new PathFactory();
        var sqrt2 = mathjs.sqrt(2);
        pf.tsvva(0,0,0,1).should.approximately(0,0.001); // start at rest, end at rest
        pf.tsvva(4,0,0,1).should.approximately(4,0.001); // start at rest, end at rest
        pf.tsvva(2,0,2,1).should.approximately(2,0.001); // constant acceleration from rest
        pf.tsvva(2,2,0,1).should.approximately(2,0.001); // constant acceleration to rest
        pf.tsvva(6,2,4,1).should.approximately(2,0.001); // constant acceleration with initial velocity
        pf.tsvva(0,2,-2,1).should.approximately(4,0.001); // velocity change over zero distance
        pf.tsvva(0,-2,2,1).should.approximately(4,0.001); // velocity change over zero distance
        pf.tsvva(0,3,3,1).should.approximately(0,0.001); // no velocity change over zero distance
        pf.tsvva(-4,0,0,1).should.approximately(4,0.001); // negative distance
        pf.tsvva(-2,0,-2,1).should.approximately(2,0.001); // negative distance
        pf.tsvva(-2,-2,0,1).should.approximately(2,0.001); // negative distance
        for (var i=0; i<10000; i++) {
            var v = mathjs.random(0.0001,100);
            var s = mathjs.random(0.0001,100);
{'fraction_non_alphanumeric': 0.13556492226844755, 'fraction_numerical': 0.0168053426757171, 'mean_word_length': 1.2791466533591167, 'pattern_counts': {'":': 0, '<': 129, '<?xml version=': 0, '>': 77, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 4, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '736690', 'n_tokens_mistral': 11250, 'n_tokens_neox': 10317, 'n_words': 2312}