text: import { solveHexapodParams } from "./ik/hexapodSolver.js"
  
  /* * *
  
  Return format:
    poseSequence = {
      leftMiddle: {
          alpha: [],
          beta: [],
          gamma: [],
      }
      ....
    }
  
    gaitType = ["ripple", "tripod"]
    walkMode = ["rotating", "walking"]
    * * */
  
  const getWalkSequence = (
      dimensions,
      params = {
          tx: 0,
          tz: 0,
          rx: 0,
          ry: 0,
          legStance: 0,
          hipStance: 25,
          stepCount: 5,
          hipSwing: 25,
          liftSwing: 40,
      },
      gaitType = "tripod",
      walkMode = "walking"
  ) => {
      const { hipStance, rx, ry, tx, tz, legStance } = params
      const rawIKparams = {
          tx,
          ty: 0,
          tz,
          legStance,
          hipStance,
          rx,
          ry,
          rz: 0,
      }
  
      const [ikSolver] = solveHexapodParams(dimensions, rawIKparams, true)
  
      if (!ikSolver.foundSolution || ikSolver.hasLegsOffGround) {
          return null
      }
      //console.log("---------ikSolver.pose:")
      //console.log(ikSolver.pose)
  
  
      const { hipSwing, liftSwing, stepCount } = params
      const [aHipSwing, aLiftSwing] = [Math.abs(hipSwing), Math.abs(liftSwing)]
  
      //console.log("---------aHipSwing:")
      //console.log(aHipSwing)
      
      const hipSwings =
          walkMode === "rotating"
              ? getHipSwingRotate(aHipSwing)
              : getHipSwingForward(aHipSwing)
  
      return gaitType === "ripple"
          ? rippleSequence(ikSolver.pose, aLiftSwing, hipSwings, stepCount)
          : tripodSequence(ikSolver.pose, aLiftSwing, hipSwings, stepCount)
  }
  
  /* *
  
  powerStroke aka stancePhase
  returnStroke aka swingPhase
  
  1. > startPowerStroke / endReturnStroke < 6.
  
         \
          \
           *--*--*
          /   |   \   /
         * -- * -- * /
        \ \   |   /
         \ *--*--*
  
  2. > middlePowerStroke / middleReturnStroke < 5.
  
       --- *--*--*
          /   |   \
         * -- * -- * ---
          \   |   /
      ---- *--*--*
  
  3. > endPowerStroke / startReturnStroke < 4.
  
         / *--*--*
        / /   |   \
         * -- * -- *
          \   |   / \
           *--*--*   \
         /
        /
  
   TRIPOD1 - leftFront, rightMiddle, leftBack
  
   |----- powerStroke ----------|------- returnStroke -------|
                                |-- liftUp --|-- shoveDown --|
  
   TRIPOD2 rightFront, leftMiddle, rightBack
  
   |------- returnStroke -------|----- powerStroke ----------|
   |-- liftUp --|-- shoveDown --|
  
   * */
  const tripodSequence = (pose, aLiftSwing, hipSwings, stepCount, walkMode) => {
      const { forwardAlphaSeqs, liftBetaSeqs, liftGammaSeqs } = buildTripodSequences(
          pose,
          aLiftSwing,
          hipSwings,
          stepCount,
          walkMode
      )
      console.log("----------forwardAlphaSeqs: ")
      console.log(forwardAlphaSeqs)
      console.log("----------liftBetaSeqs: ")
      console.log(liftBetaSeqs)
      console.log("----------liftGammaSeqs: ")
      console.log(liftGammaSeqs)
  
  
      
      const doubleStepCount = 2 * stepCount
  
      
      const tripodA = tripodASequence(
          forwardAlphaSeqs,
          liftGammaSeqs,
          liftBetaSeqs,
          doubleStepCount
      )
      //console.log("tripodA sequence : ")
      //console.log(tripodA)
    
      const tripodB = tripodBSequence(
          forwardAlphaSeqs,
          liftGammaSeqs,
          liftBetaSeqs,
          doubleStepCount
      )
      
      console.log("---------------4")
  
      //console.log("|||tripodA: ")
      //console.log(tripodA)
      console.log("|||tripodB: ")
      console.log(tripodB)
     
      return { ...tripodA, ...tripodB }
  }
  
  const tripodASequence = (
      forwardAlphaSeqs,
      liftGammaSeqs,
      liftBetaSeqs,
      doubleStepCount
  ) =>
      ["leftFront", "rightMiddle", "leftBack"].reduce((sequences, legPosition) => {
          const forward = forwardAlphaSeqs[legPosition]
          const gammaLiftUp = liftGammaSeqs[legPosition]
          const betaLiftUp = liftBetaSeqs[legPosition]
  
          const gammaSeq = [
              ...gammaLiftUp,
              ...gammaLiftUp.slice().reverse(),
              ...fillArray(gammaLiftUp[0], doubleStepCount),
          ]
  
          const betaSeq = [
              ...betaLiftUp,
              ...betaLiftUp.slice().reverse(),
              ...fillArray(betaLiftUp[0], doubleStepCount),
          ]
  
          sequences[legPosition] = {
              alpha: [...forward, ...forward.slice().reverse()],
              gamma: gammaSeq,
              beta: betaSeq,
          }
  
          console.log("----tripodASequence")
          console.log(sequences)
  
  
  
          return sequences
      }, {})
  
  const tripodBSequence = (
      forwardAlphaSeqs,
      liftGammaSeqs,
      liftBetaSeqs,
      doubleStepCount
  ) =>
      ["rightFront", "leftMiddle", "rightBack"].reduce((sequences, legPosition) => {
          const forward = forwardAlphaSeqs[legPosition]
          const gammaLiftUp = liftGammaSeqs[legPosition]
          const betaLiftUp = liftBetaSeqs[legPosition]
  
          const gammaSeq = [
              ...fillArray(gammaLiftUp[0], doubleStepCount),
              ...gammaLiftUp,
              ...gammaLiftUp.slice().reverse(),
          ]
  
          const betaSeq = [
              ...fillArray(betaLiftUp[0], doubleStepCount),
              ...betaLiftUp,
              ...betaLiftUp.slice().reverse(),
          ]
  
          sequences[legPosition] = {
              alpha: [...forward.slice().reverse(), ...forward],
              gamma: gammaSeq,
              beta: betaSeq,
          }
  
          return sequences
      }, {})
  
  /* * *
  
  RIPPLE SEQUENCE
  a - lift-up
  b - shove-down
  [1, 2, 3, 4] - retract / power stroke sequence
  
  left-back     |-- a --|-- b --|   1   |   2   |   3   |   4   |
  left-middle   |   3   |   4   |-- a --|-- b --|   1   |   2   |
  left-front    |   1   |   2   |   3   |   4   |-- a --|-- b --|
  right-front   |   4   |-- a --|-- b --|   1   |   2   |   3   |
  right-back    |   1   |   2   |   3   |-- a --|-- b --|   4   |
  right-middle  |-- b --|   1   |   2   |   3   |   4   |-- a --|
  
   * * */
  
  const rippleSequence = (startPose, aLiftSwing, hipSwings, stepCount) => {
      
      console.log("in ripple sequence")
      	
      const legPositions = Object.keys(startPose)
  
      console.log("startPose")
      console.log(startPose)
      console.log("legPositions:")
      console.log(legPositions)
      let sequences = {}
      legPositions.forEach(position => {
          //console.log("---position") 
          //console.log(position) 
  
  	const { alpha, beta, gamma } = startPose[position]
          const betaLift = buildSequence(beta, aLiftSwing, stepCount)
          const gammaLift = buildSequence(gamma, -aLiftSwing / 2, stepCount)
  
          const delta = hipSwings[position]
          const fw1 = buildSequence(alpha - delta, delta, stepCount)
          const fw2 = buildSequence(alpha, delta, stepCount)
  
          const halfDelta = delta / 2
          const bk1 = buildSequence(alpha + delta, -halfDelta, stepCount)
          const bk2 = buildSequence(alpha + halfDelta, -halfDelta, stepCount)
          const bk3 = buildSequence(alpha, -halfDelta, stepCount)
          const bk4 = buildSequence(alpha - halfDelta, -halfDelta, stepCount)
          
          // prettier-ignore
          //	console.log("--- buildRippleLegSequence")
          sequences[position] = buildRippleLegSequence(
              position, betaLift, gammaLift, fw1, fw2, bk1, bk2, bk3, bk4
          )
  
  	//console.log("buildRippleLegSequence---")
      })
  
      return sequences
  }
  
  const buildRippleLegSequence = (position, bLift, gLift, fw1, fw2, bk1, bk2, bk3, bk4) => {
     
      //	console.log("1---")
      const stepCount = fw1.length
      const revGLift = gLift.slice().reverse()
      const revBLift = bLift.slice().reverse()
      const b0 = bLift[0]
      const g0 = gLift[0]
      // n stands for neutral
  	
      //	console.log("3---")
      const bN = fillArray(b0, stepCount)
      const gN = fillArray(g0, stepCount)
  
      //	console.log("---4")
      const alphaSeq = [fw1, fw2, bk1, bk2, bk3, bk4]
      const betaSeq = [bLift, revBLift, bN, bN, bN, bN]
      const gammaSeq = [gLift, revGLift, gN, gN, gN, gN]
  
      const moduloMap = {
          leftBack: 0,
          rightFront: 1,
          leftMiddle: 2,
          rightBack: 3,
          leftFront: 4,
          rightMiddle: 5,
      }
  
      //	console.log("2---")
      //console.log("---position")
      //console.log(position)
  
      //console.log("---moduloMap[position]")
      //console.log(moduloMap[position])
  
      //console.log("---moduloMap[position]")
      //console.log(moduloMap[position])
  
  
      return {
          alpha: modSequence(moduloMap[position], alphaSeq),
          beta: modSequence(moduloMap[position], betaSeq),
          gamma: modSequence(moduloMap[position], gammaSeq),
      }
  }
  
  const modSequence = (mod, seq) => {
      const sequence = [...seq, ...seq]
      //console.log("---sequence: ")
      //console.log(sequence)
  
      return sequence.slice(mod, mod + 6).flat()
  
  
  }
  
  const buildTripodSequences = (startPose, aLiftSwing, hipSwings, stepCount, walkMode) => {
      const doubleStepCount = 2 * stepCount
      const legPositions = Object.keys(startPose)
  
      let forwardAlphaSeqs = {}
      let liftBetaSeqs = {}
      let liftGammaSeqs = {}
  
      legPositions.forEach(legPosition => {
          const { alpha, beta, gamma } = startPose[legPosition]
          const deltaAlpha = hipSwings[legPosition]
          forwardAlphaSeqs[legPosition] = buildSequence(
              alpha - deltaAlpha,
              2 * deltaAlpha,
              doubleStepCount
          )
          liftBetaSeqs[legPosition] = buildSequence(beta, aLiftSwing, stepCount)
          liftGammaSeqs[legPosition] = buildSequence(gamma, -aLiftSwing / 2, stepCount)
      })
  
      return {
          forwardAlphaSeqs,
          liftBetaSeqs,
          liftGammaSeqs,
      }
  }
  
  const buildSequence = (startVal, delta, stepCount) => {
      const step = delta / stepCount
  
      let currentItem = startVal
      let array = []
      for (let i = 0; i < stepCount; i++) {
          currentItem += step
          array.push(currentItem)
      }
  
      return array
  }
  
  const getHipSwingForward = aHipSwing => {
      return {
          leftFront: -aHipSwing,
          rightMiddle: aHipSwing,
          leftBack: -aHipSwing,
          rightFront: aHipSwing,
          leftMiddle: -aHipSwing,
          rightBack: aHipSwing,
      }
  }
  
  const getHipSwingRotate = aHipSwing => {
      return {
          leftFront: aHipSwing,
          rightMiddle: aHipSwing,
          leftBack: aHipSwing,
          rightFront: aHipSwing,
          leftMiddle: aHipSwing,
          rightBack: aHipSwing,
      }
  }
  
  const fillArray = (value, len) => {
      if (len === 0) {
          return []
      }
      let a = [value]
      //console.log("||||||||||||||a:")
      //console.log(a)
      while (a.length * 2 <= len) {
          a = a.concat(a)
      }
  
      if (a.length < len) {
          a = a.concat(a.slice(0, len - a.length))
      }
      //console.log(a)
  
      return a
  }
  
  export default getWalkSequence
  