From 2fd8e7a6b2808a34b6dcefbff2ae0dcb9ef8d7e2 Mon Sep 17 00:00:00 2001
From: Jake Read <jake.read@cba.mit.edu>
Date: Mon, 18 May 2020 21:11:54 -0400
Subject: [PATCH] step generator success

---
 hunks/search/simplex.js                       |  10 +-
 hunks/statemachines/pendulum-ukx-is.js        | 450 ++++++++++++++++++
 .../{pendulum-ukx.js => pendulum-ukx-sg.js}   | 216 ++++++---
 3 files changed, 600 insertions(+), 76 deletions(-)
 create mode 100644 hunks/statemachines/pendulum-ukx-is.js
 rename hunks/statemachines/{pendulum-ukx.js => pendulum-ukx-sg.js} (55%)

diff --git a/hunks/search/simplex.js b/hunks/search/simplex.js
index 1817864..7de88ad 100644
--- a/hunks/search/simplex.js
+++ b/hunks/search/simplex.js
@@ -178,14 +178,14 @@ export default function Simplex() {
     }
     lastDelta = vdir(pts[pts.length - 1].params, centroid)
     console.log('started', pts)
-  }
+  } // end init
 
   let ce = 2 // coefficient for expansion
   let cc = 0.9 // coefficient for contraction
   let cs = 0.9 // coefficient for shrinking
   let cm = 0.1 // coefficient of momentum,
-  let cq = 0.1 // coefficient for expansion, when spread < ...
-  let minSpread = 0
+  let cq = 100 // coefficient for expansion, when spread < ...
+  let minSpread = 0.5
   let SIMPLEX_DEBUG = false
   let SIMPLEX_BEST_DEBUG = true
   let SIMPLEX_BEST_P_DEBUG = false
@@ -227,7 +227,11 @@ export default function Simplex() {
       for(let p of pts){
         let dir = vdir(p.params, centroid)
         p.params = vadd(p.params, vscalar(dir, cq))
+        p.eval = await queryPlant(p.params) // and reevaluate,
       }
+      pts.sort((a, b) => {
+        return a.eval - b.eval // and sort again,
+      })
       for (let i = 1; i < pts.length; i++) {
         spread += vdist(centroid, pts[i].params)
       }
diff --git a/hunks/statemachines/pendulum-ukx-is.js b/hunks/statemachines/pendulum-ukx-is.js
new file mode 100644
index 0000000..fdcb747
--- /dev/null
+++ b/hunks/statemachines/pendulum-ukx-is.js
@@ -0,0 +1,450 @@
+/*
+hunks/statemachines/pendulum
+
+purpose built statemachine for the https://gitlab.cba.mit.edu/jakeread/pendulum
+
+Jake Read at the Center for Bits and Atoms
+(c) Massachusetts Institute of Technology 2020
+
+This work may be reproduced, modified, distributed, performed, and
+displayed for any purpose, but must acknowledge the squidworks and cuttlefish projects.
+Copyright is retained and must be preserved. The work is provided as is;
+no warranty is provided, and users accept all liability.
+*/
+
+import {
+  Hunkify,
+  Input,
+  Output,
+  State
+} from '../hunks.js'
+
+let norm = (pt) => {
+  let len = Math.sqrt(pt[0] * pt[0] + pt[1] * pt[1])
+  return [pt[0] / len, pt[1] / len]
+}
+
+let vadd = (a, b) => {
+  let ret = []
+  for (let d in a) {
+    ret.push(a[d] + b[d])
+  }
+  return ret
+}
+
+let vsub = (a, b) => {
+  let ret = []
+  for (let d in a) {
+    ret.push(a[d] - b[d])
+  }
+  return ret
+}
+
+let vdir = vsub
+
+let vdist = (a, b) => {
+  let sum = 0
+  for (let d in a) {
+    sum += (b[d] - a[d]) * (b[d] - a[d])
+  }
+  return Math.sqrt(sum)
+}
+
+let vscalar = (a, s) => {
+  let ret = []
+  for (let d in a) {
+    ret.push(a[d] * s)
+  }
+  return ret
+}
+
+let vreverse = (dir) => {
+  for (let d in dir) {
+    dir[d] = -dir[d]
+  }
+}
+
+let vdot = (a, b) => {
+  let sum = 0
+  for (let d in a) {
+    sum += a[d] * b[d]
+  }
+  return sum
+}
+
+let randGaussian = () => {
+  let sum = 0
+  for (let i = 0; i < 6; i++) {
+    sum += Math.random()
+  }
+  return sum / 6
+}
+
+export default function Pendulum() {
+  Hunkify(this)
+
+  // reset / restart everything,
+  let stop = false
+  let resetState = this.state('boolean', 'reset', false)
+  resetState.onChange = (val) => {
+    stop = true // make sure old timers die
+    setTimeout(() => {
+      stop = false
+      startRender()
+      resetState.set(false)
+      if (!resetOut.io()) {
+        resetOut.put(true)
+      }
+    }, 50)
+  }
+  // set to 'default' known-decent k-vals
+  let resetK = this.state('boolean', 'defaults', false)
+  resetK.onChange = (val) => {
+    k0.set(1)
+    k1.set(2)
+    k2.set(-100)
+    k3.set(-100)
+    resetK.set(false)
+  }
+
+  let k0 = this.state('number', 'k0', 1)
+  let k1 = this.state('number', 'k1', 2)
+  let k2 = this.state('number', 'k2', -100)
+  let k3 = this.state('number', 'k3', -100)
+
+  // error, time from render
+  let rtOut = this.output('number', 'rt') // time output, for plotting
+  let rerrOut = this.output('number', 'rerr') // error output, for plotting
+  let ruOut = this.output('number', 'ru') // output (control result) for plotting
+  // on reset sim, to reset plots
+  let resetOut = this.output('boolean', 'rst')
+
+  // ---------------------------------------------------------------- DRAWING
+
+  // time / render update
+  let drawCount = 40 // ticks per canvas update,
+  let drawMs = 40 // ms per canvas update: if Count == Ms, update is in realtime,
+
+  // setup to draw,
+  let dom = this.document()
+  let width = 600
+  let height = 300
+
+  // draw
+  let drawScale = 50
+  let ballDiameter = 10
+
+  let svg = document.createElementNS('http://www.w3.org/2000/svg', 'svg')
+  svg.setAttribute('width', width)
+  svg.setAttribute('height', height)
+  let line = document.createElementNS('http://www.w3.org/2000/svg', 'line')
+  line.style.stroke = 'black'
+  line.style.strokeWidth = '2px'
+  let circle = document.createElementNS('http://www.w3.org/2000/svg', 'circle')
+  circle.style.fill = 'black'
+  circle.setAttribute('r', ballDiameter / 2)
+  let drawLine = (ex, oh) => {
+    oh += Math.PI
+    // we want canvas y + to be world y -
+    let x1 = 300 + ex * drawScale
+    if (x1 < 0) {
+      x1 = 600 - (-1 * x1) % 600
+    } else {
+      x1 = x1 % 600
+    }
+    let y1 = 150
+    let x2 = x1 + Math.sin(oh) * drawScale
+    let y2 = y1 + Math.cos(oh) * drawScale
+    line.setAttribute('x1', x1)
+    line.setAttribute('y1', y1)
+    line.setAttribute('x2', x2)
+    line.setAttribute('y2', y2)
+    circle.setAttribute('cx', x2)
+    circle.setAttribute('cy', y2)
+  }
+  svg.appendChild(line)
+  svg.appendChild(circle)
+  dom.appendChild(svg)
+
+  // ---------------------------------------------------------------- SIMULATION
+  let odamp = 0.1
+  let length = 1
+  let xddotLimit = 9.8 * 3
+
+  // start time, increment, count, init, k[4]
+  let runSimulation = (t, tstep, count, x, xdot, o, odot, k) => {
+    let err = 0 // cumulative
+    let xddot = 0
+    let oddot = 0
+    let oerr = 0
+    // calc 40 time steps, tstep is 1ms each
+    for (let i = 0; i < count; i++) {
+      // assume xddot is set / driving,
+      t += tstep
+      // use k-vals control law to generate new xddot
+      xddot = x * k[0] + xdot * k[1] + o * k[2] + odot * k[3]
+      if (xddot > xddotLimit) xddot = xddotLimit
+      if (xddot < -xddotLimit) xddot = -xddotLimit
+      // angular acceleration, given effect of gravity and cart accel
+      oddot = (xddot * Math.cos(o) + 9.8 * Math.sin(o)) / length
+      // integrations,
+      xdot += xddot * tstep
+      odot = odot + oddot * tstep - odot * odamp * tstep
+      x += xdot * tstep
+      o += odot * tstep
+      o = o % (Math.PI * 2)
+      // errors *after* this control input applied: more interested in theta error than x,
+      err += Math.abs(x) * 0.1 // + Math.abs(o) * 0.8
+      oerr = o % (Math.PI * 2)
+      if (oerr > Math.PI) {
+        oerr = -Math.PI * 2 + oerr
+      } else if (err < -Math.PI) {
+        oerr = Math.PI * 2 - oerr
+      }
+      err += Math.abs(oerr) * 0.9
+    }
+    // does JS copy new numbers into this object, or does it ptr to them?
+    // or does the copy-in happen at fn call, probably
+    return {
+      t: t,
+      x: x,
+      xdot: xdot,
+      xddot: xddot,
+      o: o,
+      odot: odot,
+      oddot: oddot,
+      err: err
+    }
+  }
+
+  let rt = 0
+  let rerr = 0
+  // start and run render w/
+  let startRender = () => {
+    // initial / stateful conditions
+    let t = 0
+    let x = Math.random() * 1 - 0.5 // [-10, 10]
+    let xdot = Math.random() * 2 - 1 // [-1, 1]
+    let o = Math.random() - 0.5 // [-0.5, 0.5]
+    let odot = Math.random() - 0.5 // [-0.5, 0.5]
+    // start new simplex state,
+    initSimplex(t, x, xdot, o, odot)
+    // so, to keep some local updates to render with ...
+    let iters = 0
+    let update = () => {
+      let kvals = runSimplex(t, x, xdot, o, odot)
+      //console.log('kv', kvals)
+      //let kvals = [k0.value, k1.value, k2.value, k3.value]
+      let result = runSimulation(t, 0.001, 1, x, xdot, o, odot, kvals)
+      // extract to render-global state,
+      t = result.t
+      x = result.x
+      xdot = result.xdot
+      o = result.o
+      odot = result.odot
+      // if on-draw-cycle, draw,
+      if(!(iters % 40)){
+        // render output vars
+        rt = t
+        rerr = result.err
+        drawLine(x, o)
+        // and watch k-vals,
+        k0.set(kvals[0])
+        k1.set(kvals[1])
+        k2.set(kvals[2])
+        k3.set(kvals[3])
+      }
+      if (!stop) setTimeout(update, 0)
+    }
+    update()
+  }
+
+  // ---------------------------------------------------------------- SIMPLEX
+
+  let dims = 4
+  let pts = []
+  let lastDelta = []
+  // dbf
+  let SIMPLEX_DEBUG = false
+  let SIMPLEX_BEST_DEBUG = true
+  let SIMPLEX_BEST_P_DEBUG = false
+  // const
+  let ce = 2
+  let cc = 0.9 // coefficient for contraction
+  let cs = 0.9 // coefficient for shrinking
+  let cm = 0.1 // coefficient for momentum
+  let cq = 1 // coefficient of 'spread'
+
+  // lookahead by...
+  let lookInterval = 0.0001
+  let lookCount = 10
+
+  let initSimplex = (t, x, xdot, o, odot) => {
+    pts.length = 0
+    // start pts,
+    for (let p = 0; p < dims + 1; p++) {
+      let pt = {}
+      let params = [
+        randGaussian() * 1,
+        randGaussian() * 1,
+        randGaussian() * -10,
+        randGaussian() * -10,
+      ]
+      pt.params = params
+      pt.eval = runSimulation(t, lookInterval, lookCount, x, xdot, o, odot, pt.params).err
+      pts.push(pt)
+    } // end for-pts,
+    // startup lastDelta is just [0]
+    lastDelta = [0, 0, 0, 0]
+    console.log('started', pts)
+    // SETUP COMPLETE
+  }
+
+  let momentum = (choice) => {
+    choice.params = vadd(choice.params, vscalar(lastDelta, cm)) // add momentum term to new choice
+    lastDelta = vdir(pts[pts.length - 1].params, choice.params) // retain this move's momentum for next
+  }
+
+
+  // tracks points, returns best-pt-this-turn
+  let runSimplex = (t, x, xdot, o, odot) => {
+    // ok, *this time* we need to re-evaluate every point at every step.
+    for(let pt of pts){
+      pt.eval = runSimulation(t, lookInterval, lookCount, x, xdot, o, odot, pt.params).err
+    }
+    // sort, best 1st
+    pts.sort((a, b) => {
+      return a.eval - b.eval
+    })
+
+    // now we run simplex
+    // ---------------------------------------------------- FIND CENTROID
+    // now we *either* do reflection / expansion / contraction
+    // *or* shrink, and then end the step.
+    // first, try all ref, exp, cont,
+    let centroid = []
+    for (let d = 0; d < pts[0].params.length; d++) {
+      let sum = 0
+      // collect all but worst,
+      for (let p = 0; p < pts.length - 1; p++) {
+        sum += pts[p].params[d]
+      }
+      centroid.push(sum / (pts.length - 1))
+    }
+    if (SIMPLEX_DEBUG) console.log('centroid', centroid)
+
+    // -------------------------------------------------------------- STOPPING CONDITION
+    let spread = 0
+    for (let i = 1; i < pts.length; i++) {
+      spread += vdist(centroid, pts[i].params)
+    }
+    if (spread < 10){
+      for(let p of pts){
+        let dir = vdir(p.params, centroid)
+        p.params = vadd(p.params, vscalar(dir, cq))
+        p.eval = runSimulation(t, lookInterval, lookCount, x, xdot, o, odot, p.params).err // and reevaluate,
+      }
+      pts.sort((a, b) => {
+        return a.eval - b.eval // and sort again,
+      })
+      for (let i = 1; i < pts.length; i++) {
+        spread += vdist(centroid, pts[i].params)
+      }
+    } //return pts[0].params // break run-loop : stopping condition
+
+    if (SIMPLEX_DEBUG || SIMPLEX_BEST_DEBUG) console.log(`eval: ${pts[0].eval.toFixed(3)}, spread: ${spread.toFixed(8)}`)
+    if (SIMPLEX_BEST_P_DEBUG) console.log('best at', pts[0].params)
+
+    // -------------------------------------------------------------- TRY REFLECT
+    let reflect = []
+    for (let d = 0; d < pts[0].params.length; d++) {
+      reflect.push(centroid[d] + (centroid[d] - pts[pts.length - 1].params[d]))
+    }
+    let candr = { params: reflect }
+    candr.eval = runSimulation(t, lookInterval, lookCount, x, xdot, o, odot, reflect).err
+    if (SIMPLEX_DEBUG) console.log('candr', candr)
+    if (pts[0].eval < candr.eval && candr.eval < pts[pts.length - 2].eval) {
+      // if the reflected element is better than the second worst,
+      // but not better than the best, continue
+      // replacing the worst, with this
+      if (SIMPLEX_DEBUG) console.log('fb < fc < fsw, continue')
+      momentum(candr)
+      pts[pts.length - 1] = candr // -------------------------------- USE REFLECT
+      return pts[0].params // last-best is still best,
+    } else if (candr.eval < pts[0].eval) { // ----------------------- TRY EXPAND
+      // reflected point is better than the best, greedy expand:
+      let expand = []
+      for (let d = 0; d < pts[0].params.length; d++) {
+        expand.push(centroid[d] + ce * (candr.params[d] - centroid[d]))
+      }
+      let cande = { params: expand }
+      cande.eval = runSimulation(t, lookInterval, lookCount, x, xdot, o, odot, expand).err
+      if (SIMPLEX_DEBUG) console.log('cande', cande)
+      if (cande.eval < candr.eval) {
+        // expansion is better still than reflection, continue
+        if (SIMPLEX_DEBUG) console.log('select expansion')
+        momentum(cande)
+        pts[pts.length - 1] = cande // ------------------------------ USE EXPAND
+        return pts[pts.length - 1].params // this is current best, use that in next sim step
+      } else {
+        // expansion is not better,
+        if (SIMPLEX_DEBUG) console.log('select reflection')
+        momentum(candr)
+        pts[pts.length - 1] = candr // ------------------------------ USE REFLECT
+        return pts[pts.length - 1].params // this is the current best,
+      }
+    } else { //if (candr.eval >= pts[pts.length - 2].eval) { // ---------- TRY SHRINK
+      // reflection is worse than the second worst.
+      // shrink on whichever side (reflected, or not) is better
+      if (candr.eval < pts[pts.length - 1].eval) {
+        // reflection is better,
+        let contractr = []
+        for (let d = 0; d < pts[0].params.length; d++) {
+          contractr.push(centroid[d] + cc * (candr.params[d] - centroid[d]))
+        }
+        let candcr = { params: contractr }
+        candcr.eval = runSimulation(t, lookInterval, lookCount, x, xdot, o, odot, contractr).err
+        if (candcr.eval < candr.eval) {
+          // take this shrink, it's better than the reflection
+          if (SIMPLEX_DEBUG) console.log('select contract-on-reflect')
+          momentum(candcr)
+          pts[pts.length - 1] = candcr // --------------------------- USE SHRINK
+          return pts[pts.length - 1].params // shrink was the best this turn, use that
+        }
+      } else { // --------------------------------------------------- TRY CONTRACT
+        // original worst is better than reflection,
+        let contracto = []
+        for (let d = 0; d < pts[0].params.length; d++) {
+          contracto.push(centroid[d] + cc * (pts[pts.length - 1].params[d] - centroid[d]))
+        }
+        let candco = { params: contracto }
+        candco.eval = runSimulation(t, lookInterval, lookCount, x, xdot, o, odot, contracto).err
+        if (candco.eval < pts[pts.length - 1].eval) {
+          // better than last time, OK
+          if (SIMPLEX_DEBUG) console.log('select contract-on-original')
+          momentum(candco)
+          pts[pts.length - 1] = candco // --------------------------- USE CONTRACT (around centroid)
+          return pts[0].params // hmmm ...
+        }
+      }
+      // ok, if we're here, we've tried contracting, neither were better,
+      // so we shrink: generate all new points, and eval each, except best
+      if (SIMPLEX_DEBUG) console.log('select shrink-on-centroid')
+      for (let p = 1; p < pts.length; p++) {
+        // ! doesn't update momentum ... ?
+        //lastDelta = vscalar(lastDelta, cs)
+        for (let d = 0; d < pts[0].params.length; d++) { // --------- USE SHRINK
+          pts[p].params[d] = pts[0].params[d] + cs * (pts[p].params[d] - pts[0].params[d])
+        }
+      }
+      return pts[0].params // hmmm
+    }
+  } // END RUN ----------------------------------------------------
+
+  // boot
+  startRender()
+
+  this.loop = () => {
+
+  }
+}
diff --git a/hunks/statemachines/pendulum-ukx.js b/hunks/statemachines/pendulum-ukx-sg.js
similarity index 55%
rename from hunks/statemachines/pendulum-ukx.js
rename to hunks/statemachines/pendulum-ukx-sg.js
index 5d57f78..d34a42f 100644
--- a/hunks/statemachines/pendulum-ukx.js
+++ b/hunks/statemachines/pendulum-ukx-sg.js
@@ -1,5 +1,7 @@
 /*
-hunks/statemachines/pendulum
+hunks/statemachines/pendulum-ukx-sg
+
+step-generator type
 
 purpose built statemachine for the https://gitlab.cba.mit.edu/jakeread/pendulum
 
@@ -19,6 +21,59 @@ import {
   State
 } from '../hunks.js'
 
+let norm = (pt) => {
+  let len = Math.sqrt(pt[0] * pt[0] + pt[1] * pt[1])
+  return [pt[0] / len, pt[1] / len]
+}
+
+let vadd = (a, b) => {
+  let ret = []
+  for (let d in a) {
+    ret.push(a[d] + b[d])
+  }
+  return ret
+}
+
+let vsub = (a, b) => {
+  let ret = []
+  for (let d in a) {
+    ret.push(a[d] - b[d])
+  }
+  return ret
+}
+
+let vdir = vsub
+
+let vdist = (a, b) => {
+  let sum = 0
+  for (let d in a) {
+    sum += (b[d] - a[d]) * (b[d] - a[d])
+  }
+  return Math.sqrt(sum)
+}
+
+let vscalar = (a, s) => {
+  let ret = []
+  for (let d in a) {
+    ret.push(a[d] * s)
+  }
+  return ret
+}
+
+let vreverse = (dir) => {
+  for (let d in dir) {
+    dir[d] = -dir[d]
+  }
+}
+
+let vdot = (a, b) => {
+  let sum = 0
+  for (let d in a) {
+    sum += a[d] * b[d]
+  }
+  return sum
+}
+
 let randGaussian = () => {
   let sum = 0
   for (let i = 0; i < 6; i++) {
@@ -30,25 +85,17 @@ let randGaussian = () => {
 export default function Pendulum() {
   Hunkify(this)
 
-  // constants,
-  let drawCount = 40 // ticks per canvas update,
-  let drawMs = 40 // ms per canvas update: if Count == Ms, update is in realtime,
-
-  // inputs
-  let gainsIn = this.input('array', 'gains')
-
-  // handles!
-  let stop = false
+  // reset / restart everything,
   let resetState = this.state('boolean', 'reset', false)
   resetState.onChange = (val) => {
-    stop = true
-    setTimeout(() => {
-      stop = false
-      startRender()
-      resetState.set(false)
-      if(!resetOut.io()){ resetOut.put(true) }
-    }, 50)
+    if (timer) clearTimeout(timer)
+    resetState.set(false)
+    startRender()
+    if (!resetOut.io()) {
+      resetOut.put(true)
+    }
   }
+  // set to 'default' known-decent k-vals
   let resetK = this.state('boolean', 'defaults', false)
   resetK.onChange = (val) => {
     k0.set(1)
@@ -63,15 +110,18 @@ export default function Pendulum() {
   let k2 = this.state('number', 'k2', -100)
   let k3 = this.state('number', 'k3', -100)
 
-  // error from last k-vals in,
-  let errOut = this.output('number', 'err')
   // error, time from render
-  let rtOut = this.output('number', 'rt')
-  let rerrOut = this.output('number', 'rerr')
+  let rtOut = this.output('number', 'rt') // time output, for plotting
+  let rerrOut = this.output('number', 'rerr') // error output, for plotting
+  let ruOut = this.output('number', 'ru') // output (control result) for plotting
   // on reset sim, to reset plots
   let resetOut = this.output('boolean', 'rst')
 
-  // ------------------------------------------------------ KEYDOWNS
+  // ---------------------------------------------------------------- DRAWING
+
+  // time / render update
+  let drawCount = 40 // ticks per canvas update,
+  let drawMs = 40 // ms per canvas update: if Count == Ms, update is in realtime,
 
   // setup to draw,
   let dom = this.document()
@@ -114,12 +164,13 @@ export default function Pendulum() {
   svg.appendChild(circle)
   dom.appendChild(svg)
 
+  // ---------------------------------------------------------------- SIMULATION
   let odamp = 0.1
   let length = 1
   let xddotLimit = 9.8 * 3
 
   // start time, increment, count, init, k[4]
-  let run = (t, tstep, count, x, xdot, o, odot, k) => {
+  let runSimulation = (t, tstep, count, x, xdot, o, odot, k) => {
     let err = 0 // cumulative
     let xddot = 0
     let oddot = 0
@@ -141,14 +192,14 @@ export default function Pendulum() {
       o += odot * tstep
       o = o % (Math.PI * 2)
       // errors *after* this control input applied: more interested in theta error than x,
-      err += Math.abs(x) * 0.1 // + Math.abs(o) * 0.8
+      err += Math.abs(x) * (1 - errpRotary) // + Math.abs(o) * 0.8
       oerr = o % (Math.PI * 2)
       if (oerr > Math.PI) {
         oerr = -Math.PI * 2 + oerr
       } else if (err < -Math.PI) {
         oerr = Math.PI * 2 - oerr
       }
-      err += Math.abs(oerr) * 0.9
+      err += Math.abs(oerr) * errpRotary
     }
     // does JS copy new numbers into this object, or does it ptr to them?
     // or does the copy-in happen at fn call, probably
@@ -164,74 +215,93 @@ export default function Pendulum() {
     }
   }
 
+  // search parameters
+  let mgRad = 1
+  let mgForecast = 1500
+  let errpRotary = 0.7
+
   let rt = 0
   let rerr = 0
+  let timer
   // start and run render w/
   let startRender = () => {
+    // initial / stateful conditions
     let t = 0
-    let x = Math.random() * 20 - 10 // [-10, 10]
+    let x = Math.random() * 1 - 0.5 // [-10, 10]
     let xdot = Math.random() * 2 - 1 // [-1, 1]
     let o = Math.random() - 0.5 // [-0.5, 0.5]
     let odot = Math.random() - 0.5 // [-0.5, 0.5]
+    // k!
+    let kvals = [
+      randGaussian() * 10 - 5,
+      randGaussian() * 10 - 5,
+      randGaussian() * 10 - 5,
+      randGaussian() * 10 - 5,
+    ]
+    // start new simplex state,
     // so, to keep some local updates to render with ...
+    let iters = 0
     let update = () => {
-      let result = run(t, 0.001, 40, x, xdot, o, odot, [k0.value, k1.value, k2.value, k3.value])
-      drawLine(result.x, result.o)
-      // extract to render-global state,
-      t = result.t
-      x = result.x
-      xdot = result.xdot
-      o = result.o
-      odot = result.odot
+      let result
+      for (let s = 0; s < 40; s++) {
+        kvals = moveGenerator(t, 0.001, mgForecast, x, xdot, o, odot, kvals)
+        //console.log('kv', kvals)
+        //let kvals = [k0.value, k1.value, k2.value, k3.value]
+        result = runSimulation(t, 0.001, 1, x, xdot, o, odot, kvals)
+        // extract to render-global state,
+        t = result.t
+        x = result.x
+        xdot = result.xdot
+        o = result.o
+        odot = result.odot
+      }
       // render output vars
       rt = t
       rerr = result.err
-      if (!stop) setTimeout(update, 40)
+      drawLine(x, o)
+      // and watch k-vals,
+      k0.set(parseFloat(kvals[0].toFixed(6)))
+      k1.set(parseFloat(kvals[1].toFixed(6)))
+      k2.set(parseFloat(kvals[2].toFixed(6)))
+      k3.set(parseFloat(kvals[3].toFixed(6)))
+      timer = setTimeout(update, 40)
     }
     update()
   }
 
-  startRender()
-
-  // OK!
+  // ---------------------------------------------------------------- SEARCH
 
-  let getNum = 0
-  this.loop = () => {
-    if (gainsIn.io() && !errOut.io()) {
-      // run,
-      let kvals = gainsIn.get()
-      getNum++ // for fun, set sim to kvals every 100 steps
-      if (getNum % 50 == 0) {
-        k0.set(kvals[0])
-        k1.set(kvals[1])
-        k2.set(kvals[2])
-        k3.set(kvals[3])
-      }
-      // run over four set startup conditions
-      /*
-      let r1 = run(0, 0.001, 4000, 4, 0, -0.25, -0.25, kvals)
-      let r2 = run(0, 0.001, 4000, 4, 0, 0.25, 0.25, kvals)
-      let r3 = run(0, 0.001, 4000, -4, 0, -0.25, -0.25, kvals)
-      let r4 = run(0, 0.001, 4000, -4, 0, 0.25, 0.25, kvals)
-      errOut.put(r1.err + r2.err + r3.err + r4.err)
-      */
-      // run over a random set of initial conditions,
-      let runCount = 500
-      let errSum = 0
-      for(let i = 0; i < runCount; i ++){
-        // same random startups as the simulation
-        let xs = randGaussian() * 1 - 0.5
-        let xdots = randGaussian() * 1 - 0.5
-        let os = randGaussian() * 2 - 1
-        let odots = randGaussian() * 1 - 0.5
-        errSum += run(0, 0.001, 5000, xs, xdots, os, odots, kvals).err
+  let moveGenerator = (t, step, count, x, xdot, o, odot, kvals) => {
+    // make grid of new pts around kvals,
+    let pvals = [kvals] // potential sets,
+    for (let i = 0; i < 4; i++) {
+      // make two new around this dim
+      let nval1 = []
+      let nval2 = []
+      for (let j = 0; j < 4; j++) {
+        nval1.push(kvals[j])
+        nval2.push(kvals[j])
       }
-      errOut.put(errSum / runCount)
+      nval1[i] = kvals[i] + mgRad
+      nval2[i] = kvals[i] - mgRad
+      pvals.push(nval1, nval2)
     }
-    // render vars,
-    if (!rtOut.io() && !rerrOut.io()) {
-      rtOut.put(rt)
-      rerrOut.put(rerr)
+    // this means we're stepping on a grid. manhattan steps
+    for (let p of pvals) {
+      p.vals = p
+      p.eval = runSimulation(t, step, count, x, xdot, o, odot, p.vals).err
     }
+    // sort
+    pvals.sort((a, b) => {
+      return a.eval - b.eval
+    })
+    return pvals[0]
+  }
+
+  // boot
+  startRender()
+
+  this.loop = () => {
+
   }
 }
-- 
GitLab