
// ---------------------------------------------------------------------------------

void AddToMem() {
  // a comma has been received on the serial port, so load cmdVal into memory if
  // a memory load command has initiated this sequence of events
  // the memory must be filled from the bottom up
  if (MemRow < 0) {cmdVal = 0; return;}
  
  cmdVal *= cmdSgn; // correct cmdVal for -ve numbers
  if (MemPnt < MemWidth) {
    // only add values if the pointer is less than the memory row width, otherwise
    // the process wraps onto the next row
    moves[MemRow + MemPnt] = cmdVal;
    MemPnt++;
  }
  if (MemPnt == MemWidth) {
    // data has filled the row
    MemRow++; MemPnt = 0; // point at next row
    MemMax = max(MemMax,(MemRow / MemWidth) + 1); // record the highest filled as the max row number
  }
  // zero cmdVal for receipt of next value
  cmdMode = ' '; cmdType = ' '; cmdVal = 0; cmdSgn = 1;
}

// ----------------------------------------------------------------------

void attachServos(int zDel) {
  // create 4 servos and attach them to pins
  // delay used during power-up reset, otherwiswe set to 0
  servoEn = true; // flag servos are attached
  if (!ServoAttOnce) {
    servoInst[0].setPeriodHertz(PWM_Freq);    // standard 50 hz servo
    servoInst[0].attach(servoPins[0],500,2400);
  }
  servoInst[0].writeMicroseconds(servoVal[0] + servoOff0);
  delay(zDel);
  if (!ServoAttOnce) {
    servoInst[1].setPeriodHertz(PWM_Freq);    // standard 50 hz servo
    servoInst[1].attach(servoPins[1],500,2400);
  }
  servoInst[1].writeMicroseconds(servoVal[1]);
  delay(zDel);
  if (Calibrated) {setVertMinMax();} // ensure vertical channel is within limits
  if (!ServoAttOnce) {
    servoInst[2].setPeriodHertz(PWM_Freq);    // standard 50 hz servo
    servoInst[2].attach(servoPins[2]);
  }
  servoInst[2].writeMicroseconds(servoVal[2]);
  delay(zDel);
  if (!ServoAttOnce) {
    servoInst[3].setPeriodHertz(PWM_Freq);    // standard 50 hz servo
    servoInst[3].attach(servoPins[3]);
  }
  servoInst[3].writeMicroseconds(servoVal[3]);
  if (APP > 0) {PrintTx+= "PWR1\n";}  // signal servo enabled in App mode
  ServoAttOnce = true;    // set flag once servos have been attached once
}

// ----------------------------------------------------------------------

void Blink() {
  // called every 20ms, toggles the micros on-board LED every second
  if (BlinkCnt > 0) {BlinkCnt--;}
  else {
    BlinkCnt = 50;
    if (LEDOP == HIGH) {LEDOP = LOW;} else {LEDOP = HIGH;}
    digitalWrite(LEDPin,LEDOP);
  }
}

// ----------------------------------------------------------------------

void calcDiffs() {
  // calculate fixed differential values
  fwdArmFwdDiff = fwdArmMax - fwdArmVert;
  fwdArmBwdDiff = fwdArmVert - fwdArmMin;
  vertArmFwdMaxDiff = vertArmMaxB - vertArmMaxA;
  vertArmFwdMinDiff = vertArmMinB - vertArmMinA;
  vertArmBwdMaxDiff = vertArmMaxB - vertArmMaxB; // 0
  vertArmBwdMinDiff = vertArmMinC - vertArmMinB;
}

// ----------------------------------------------------------------------

void checkForCmd() {
  // checks to see if point is currently a command code, if so replace it
  if (moves[playPnt] > 5000) {
    // this is a command code
    grabTgts(); // load the current position
  }
}

// ----------------------------------------------------------------------

void clearMem(int zC) {
  // Clear the contents of the move memory moves[] before loading values
  // if zC > 0 then clear the whole of the memory contents
  int zMax = MemMax * MemWidth;
  int zReset0 = Reset0 - Home0;
  int zReset1 = Reset1 - Home1;
  int zReset2 = Reset2 - Home2;
  int zReset3 = Reset3 - Home3;
  if (zC > 0) zMax = moveArraySize;
  for (int zM = 0; zM < zMax; zM+= 4) {
    moves[zM]     = zReset0;
    moves[zM + 1] = zReset1;
    moves[zM + 2] = zReset2;
    moves[zM + 3] = zReset3;
  }

  // Clear flags ready for a new move
  MemRow = 0; MemMax = 0; Mem1st = 0;
  CmdFlag = 0; CmdForNum = 0; CmdForRow = 0;
  playCnt = 4;      // lines stored * 4
  playPauseDef = 0; // intermove pause
  playPnt = 0;      // play pointer
  forPnt = -1;      // forStack[] pointer, stack is empty
  gosubPnt = -1;    // gosubStack[] pointer, stack is empty
}

// ----------------------------------------------------------------------

void clearMoveAt(int zP) {
  // overwrites a moves[zP] position with zeros  
  moves[zP] = 0;
  moves[zP+1] = 0;
  moves[zP+2] = 0;
  moves[zP+3] = 0;
}

// ----------------------------------------------------------------------

void copyMovesUp(int zF,int zT) {
  // copies memory values from zF to zT, starting with the highest value
  int zN = zT - zF;       // the size of teh increase
  int zC = playCnt - zF;  // how many values to copy
  zF = playCnt - 1; // first value to be copied
  zT = zF + zN;     // first target value
  playCnt+= zN;     // increase the overall count
  for (zN = zC; zN > 0; zN--) {
    // ensure that we don't exceed mem depth
    if (zT < moveArraySize) {moves[zT] = moves[zF]; zT--; zF--;}
  }
}

// ---------------------------------------------------------------------------------

void decodeKey(int zkeyVal) {
  // reads a key from the keyboard annd reacts accordingly
  next40ms = millis() + 10;
  if (zkeyVal == 10) {return;}
  if (zkeyVal == 13) {return;}
  keyChar = char(zkeyVal); cmdRec = 1; // value received
    //  Serial.print("keyVal=");
    //  Serial.print(keyChar);
    //  Serial.print("   ASCII=");
    //  Serial.println(keyVal);
  int zNF = 0;
  switch (keyChar) {
    case '.': doCmd(); zNF = 1; break;
    case '+': cmdVal = -1; return;      // set flag for ML+. command
    case '-': cmdSgn = -1; zNF = 1; break;
    case ',': AddToMem(); return;       // assume cmdVal contains a memory value
    case '!': // Soft RESET
      Display_Reboot(); delay(1000);
      resetFunc();    //call reset
      break;
    // case '#': cmdMode = ' '; cmdType = ' '; cmdSgn = 1; cmdVal = 0; zNF = 1; break;
    case '~': // remote app special character received as a 'Ping'
      // this could be the diplay mirror app or a controller app, so we respond as if either
      if (PrintTgt == 1) {
        // for the display mirror app, which only works over WiFi
        if (!DispMon) {DispMon = true; Display_Mirrored();} // display mirrored message
        DispTx = 50;  // reset the DispMon timeout
      } else {
        // for controller apps, which only work over USB serial link, like 16-Ch controller
        Ping = 50;              // hold off Wi-Fi connecting whilst receiving App pings
        if (WiFiConnected) {
          WiFiDisconnect();
          ESP_NOW_BA = -1;
          WiFiTryOnce = true;
        }
        PrintTx+= "~\n";        // null tick received so respond to maintain link
        if (servoEn) {PWR_timeout = PWR_tPing;}
        REC = false; APP = 55;  // set APP time-out flag to 1 sec
      }
      return;
    case '*': cmdMode = ' '; cmdType = ' '; cmdVal = 0; cmdSgn = 1; return; // ESC abort command
    case '0': extendCmdVal(0); zNF = 1;break;
    case '1': extendCmdVal(1); zNF = 1;break;
    case '2': extendCmdVal(2); zNF = 1;break;
    case '3': extendCmdVal(3); zNF = 1;break;
    case '4': extendCmdVal(4); zNF = 1;break;
    case '5': extendCmdVal(5); zNF = 1;break;
    case '6': extendCmdVal(6); zNF = 1;break;
    case '7': extendCmdVal(7); zNF = 1;break;
    case '8': extendCmdVal(8); zNF = 1;break;
    case '9': extendCmdVal(9); zNF = 1;break;
  }
  if (zNF == 0) {
    if (cmdMode == ' ') {
      // test for new Command Mode char?
      switch (keyChar) {
        case 'c': cmdMode = 'C'; break;
        case 'C': cmdMode = 'C'; break;
        case 'e': cmdMode = 'E'; break;
        case 'E': cmdMode = 'E'; break;
        case 'h': cmdMode = 'H'; break;
        case 'H': cmdMode = 'H'; break;
        case 'k': cmdMode = 'K'; break;
        case 'K': cmdMode = 'K'; break;
        case 'm': cmdMode = 'M'; break;
        case 'M': cmdMode = 'M'; break;
        case 'r': cmdMode = 'R'; break;
        case 'R': cmdMode = 'R'; break;
        case 's': cmdMode = 'S'; break;
        case 'S': cmdMode = 'S'; break;
      } cmdType = ' '; cmdVal = 0;
    } else {
      // test for Command Type char?
      switch (keyChar) {
        case '<': cmdType = '<'; break;
        case '>': cmdType = '>'; break;
        case '^': cmdType = '^'; break;
        case 'a': cmdType = 'A'; break;
        case 'A': cmdType = 'A'; break;
        case 'b': cmdType = 'B'; break;
        case 'B': cmdType = 'B'; break;
        case 'c': cmdType = 'C'; break;
        case 'C': cmdType = 'C'; break;
        case 'd': cmdType = 'D'; break;
        case 'D': cmdType = 'D'; break;
        case 'e': cmdType = 'E'; break;
        case 'E': cmdType = 'E'; break;
        case 'f': cmdType = 'F'; break;
        case 'F': cmdType = 'F'; break;
        case 'g': cmdType = 'G'; break;
        case 'G': cmdType = 'G'; break;
        case 'l': cmdType = 'L'; break;
        case 'L': cmdType = 'L'; break;
        case 'm': cmdType = 'M'; break;
        case 'M': cmdType = 'M'; break;
        case 'n': cmdType = 'N'; break;
        case 'N': cmdType = 'N'; break;
        case 'o': cmdType = 'O'; break;
        case 'O': cmdType = 'O'; break;
        case 'p': cmdType = 'P'; break;
        case 'P': cmdType = 'P'; break;
        case 'r': cmdType = 'R'; break;
        case 'R': cmdType = 'R'; break;
        case 's': cmdType = 'S'; break;
        case 'S': cmdType = 'S'; break;
        case 't': cmdType = 'T'; break;
        case 'T': cmdType = 'T'; break;
        case 'u': cmdType = 'U'; break;
        case 'U': cmdType = 'U'; break;
        case 'v': cmdType = 'V'; break;
        case 'V': cmdType = 'V'; break;
        case 'w': cmdType = 'W'; break;
        case 'W': cmdType = 'W'; break;
        case 'x': cmdType = 'X'; break;
        case 'X': cmdType = 'X'; break;
        case 'z': cmdType = 'Z'; break;
        case 'Z': cmdType = 'Z'; break;
      }
    }
  }
}

// ----------------------------------------------------------------------

void decMoveOffset() {
  // called every cycle to reduce servo 0 offset to zero
  if (servoOffMax != 0) {
    // a thermal offset has been defined by the user
    if (servoOff0 != 0) {
      servoOffDec--;
      if (servoOffDec < 1) {
        servoOffDec = servoOffDecT;
        if (servoOffMax > 0) {servoOff0--;} // increment offset towards zero
        else {servoOff0++;} // increment offset towards zero
      //  Serial.print(F("Offset = ")); Serial.println(servoOff0);
      }
    }
  }
}

// ----------------------------------------------------------------------

void detachServos() {
  // detach all servos
  servoEn = false; // flag servos are detached
  servoInst[0].release();
  servoInst[1].release();
  servoInst[2].release();
  servoInst[3].release();
  if (APP > 0) {PrintTx+= "PWR0\n";}  // signal servo disabled in App mode
}

// ---------------------------------------------------------------------------------

void doCmd() {
  // a '.' has been received so execute command if valid
  // Commands:
  // !       - forces a 'soft' runPOST RESET
  // #       - <BACKSPACE> so reset receive buffer
  // Cn.     - 1 = Calibrated mode, 0 = uncalibrated mode
  // HM.     - move to HOME position
  // K<.     - move arm left [IR] Servo 0
  // K>.     - move arm right [IR] Servo 0
  // K^.     - move vertical arm down, head moves up, Servo 2
  // Kv.     - move vertical arm up, head moves down, Servo 2
  // KA.     - open jaws, Servo 3
  // KD.     - close jaws, Servo 3
  // KE.     - enable all servos PWM
  // KL.     - speed limit mode, clears speed increments
  // KO.     - disable all servos PWM
  // KS.     - move arm backwards, Servo 1
  // KW.     - move arm forwards, Servo 1
  // KZ.     - dither last value to see if central
  // MB.     - STOP the play engine
  // MC.     - clears memory contents
  // ME0.    - stop the move engine
  // ME1.    - restart the move engine
  // MFnn.   - first row to be played in a memory sequence, default = 0
  // MLnn.n,n,. - start a memory load sequence, values separate by a comma, terminated by a comma
  // MO.     - run the contents of the memory once only.
  // MP.     - toggle play/pause for loaded memeory
  // MR.     - run the contents of the memory continuously.
  // MS.     - move to START position
  // MTnn.   - goto predefined position nn
  // MXnn.   - run one row of the memory
  // RC.     - enter into Record mode
  // RLnn.   - load a value into moves[] and increment playCnt
  // RM.     - move to RESET rest position
  // RP0.    - report current servo values, S0 - S3 as CSV Sn=NNNN
  // RP1.    - report current servo values, S0 - S3 relative to the Home values
  // RP2.    - report current servo values as a moveLoadPosRFV() statement
  // RP3.    - report current servo values, S0 - S3 as SVn=NNNN LF
  // RP4.    - report servo lower and upper limits
  // RP5.    - report Home values
  // SAnn.   - set servo angle in degrees 0 - 180
  // SD.     - detach the active servo pin
  // SFnn.   - set the PWM frequency.
  // SLnn.   - set target servo lower limit in microseconds
  // SMnn.   - set servoMain angle in microseconds servoLL - servoUL
  // SVnn.   - set target servo angle
  // SSnn.   - set the target servo number 0 - 3, but don't feedback value
  // SUnn.   - set target servo upper limit in microseconds
  // SVnn.   - set target servo angle
  // SW.     - re-writes current servo values

  int zPrint = 1; // set flag for echo Rx response
  switch (cmdMode) {
    case ' ': break;
    case 'C':
      switch (cmdType) {
        case ' ':
          // set Calibrated flag and report associated limits
          // if Calibrated == false then max limits are reported
          if (cmdVal == 0) {Calibrated = false;}
          if (cmdVal == 1) {Calibrated = true;}
          setLimits();
          reportLimits();
          break;
      } break;
    case 'H':
      switch (cmdType) {
        case 'M':
          // move to HOME position
          if (servoEn) {
            moveGoHome();
          } break;
      } zPrint = 0; break;
    case 'K':
      zPrint = 0;
      if (servoEn) {
        switch (cmdType) {
          case '<': if (servoEn){keyBdTurnLeft();} break;
          case '>': if (servoEn){keyBdTurnRight();} break;
          case '^': if (servoEn){keyBdVertDwn();} break;
          case 'V': if (servoEn){keyBdVertUp();} break;
          case 'A': if (servoEn){keyBdJawOpen();} break;
          case 'D': if (servoEn){keyBdJawClose();} break;
          case 'L': incS0 = 1; incS1 = 1; incS2 = 1;  incS3 = 1; break;
          case 'S': if (servoEn){keyBdArmBck();} break;
          case 'W': if (servoEn){keyBdArmFwd();} break;
        }
      }
      switch (cmdType) {
        case 'E': servoEn = true; PWR_timeout = PWR_tMax; attachServos(100); movePnt = -1; zPrint = 1; break;
        case 'O': servoEn = false; PWR_timeout = 0; detachServos(); zPrint = 1; break;
        case 'Z': keyBdDither(); break;
      } break;
    case 'M':
      switch (cmdType) {
        case 'B':
          // STOP playing the memory contents and goto row 0
          PrintTx += "MB.\n";
          playStopApp(); break;
        case 'C': 
          // clears memory contents
          PrintTx += "MC.\n";
          clearMem(0); break;
        case 'E':
          switch (cmdVal) {
            case 0:
              // stop the move engine
              moveLast = movePnt; movePnt = -1;
              detachServos(); break;
            case 1:
              // restart the move engine
              movePnt = moveLast; moveRun = -1; attachServos(10);
              break;
          } break;
        case 'F':
          // change the start of a play sequence, default is 0
          Mem1st = cmdVal;
          PrintTx += "MF" + String(cmdVal) + ".\n"; break;
        case 'L':
          // start a memory load sequence, values separate by a comma, terminated by a period '.'
          if (cmdVal >= 0) {
            // initiate a memory load at a specific memory location
            MemRow = MemWidth * cmdVal;
            PrintTx += "ML" + String(cmdVal) + ".\n";
          } else {
            // initiate a memory load at the next row
            MemRow += MemWidth;
            PrintTx += "ML+.\n";
          } MemPnt = 0; break;
        case 'O':
          // run the contents of the memory once only
          PrintTx += "MO.\n";
          playOnce(); break; // run sequence from moves[] once
        case 'P':
          // run the contents of the memory once only
          PrintTx += "MP.\n";
          playPausePlay(); break; // sytart/pause sequence from moves[] once
        case 'R':
          // run the contents of the memory continuously
          PrintTx += "MR.\n";
          playContinuously(); break; // run sequence from moves[] continuously
        case 'S':
          // move to START position
          if (servoEn) {
            moveGoFloor();
          } break;
        case 'T':
          // load a predefined position and move to it
          if ((cmdVal >= 0) && (cmdVal < posMax)) {
            movePnt = 0; moveLoad2(cmdGoPos, cmdVal);
            moveLoad1(cmdEndOn); moveStart();
          } break;
        case 'X':
          // move to the memory points Mem[MemRow]
          PrintTx += "MX" + String(cmdVal) + ".\n";
          MemRow = cmdVal; playRow(); break;
      } zPrint = 0; break;
    case 'R':
      switch (cmdType) {
        case 'C':
          // enter Record mode, clearing all data and re-setting flags
          REC_ON(); playCnt = 0; break;
        case 'L':
          // store a servo value in moves[] and increment the counter
          // four consecutive values are expected to be recieved, each
          // with the RLnnn. format
          moves[playCnt] = cmdVal; playCnt++; break;
        case 'M':
          // move to RESET position
          if (servoEn) {
            moveGoRESET();
          } break;
        case 'P':
          if (movePnt >= 0) {moveRun++; moveRpt = 1;}
          else {
            if (cmdVal == 0) {reportValues();}
            if (cmdVal == 1) {reportOffsets();}
            if (cmdVal == 2) {reportMoveData();}
            if (cmdVal == 3) {reportEachValue();}
            if (cmdVal == 4) {reportLimits();}
            if (cmdVal == 5) {reportHomeValues();}
            if (cmdVal == 6) {reportRestValue();}
          } break;
      } zPrint = 0; break;
    case 'S':
      switch (cmdType) {
        case ' ': break;
        case 'A':
          Angle = cmdVal; servoMain.write(Angle);
          // report the angle in microseconds
          PrintTx+= "SM=" + String(servoMain.readMicroseconds()) + ".\n"; zPrint = 0;
          break;
        case 'D': servoPin = -1; servoMain.detach(); break;
        case 'F': PWM_Freq = cmdVal; break;
        case 'L':
          // set the lower limit of the target servo 0 - 3
          servoMin[servoNum] = cmdVal;
          if (servoVal[servoNum] < cmdVal) {
            // if servo is outside of this limit move it
            PWR_timeout = PWR_tMax; if (!servoEn) {attachServos(0); movePnt = -1;}
            servoVal[servoNum] = cmdVal;
            servoInst[servoNum].writeMicroseconds(servoVal[servoNum]);
            PrintTx+= "SV" + String(servoNum) + "=" + String(servoVal[servoNum]) + "\n"; zPrint = 0;
          } break;
        case 'M':
          Angle = cmdVal; servoMain.writeMicroseconds(Angle);
          // report the angle in degrees
          PrintTx+= "SA=" + String(servoMain.read()) + ".\n"; zPrint = 0;
          break;
        case 'S':
          // set the angle of the target servo 0 - 3
          // but don't report the angle to the serial port
          // ensure that it is within the current limits
          cmdVal = max(servoMin[servoNum], cmdVal);
          cmdVal = min(servoMax[servoNum], cmdVal);
          servoVal[servoNum] = cmdVal;
          PWR_timeout = PWR_tMax; if (!servoEn) {attachServos(0); movePnt = -1;}
          if (Calibrated && (servoNum == 1)) {
            // it is when servo[1] is moved we auto-adjust servo[2]
            setVertMinMax(); servoInst[2].writeMicroseconds(servo2Mod);
          } 
          if (servoNum == 0) {
            incOffset = true;  // turn demand so increment thermal offset counter
            servoInst[0].writeMicroseconds(servoVal[0] - servoOff0);
          } else {
            servoInst[servoNum].writeMicroseconds(servoVal[servoNum]);
          } break;
        case 'T':
          // set the target servo number 0 - 3
          servoNum = cmdVal; break;
        case 'U':
          // set the upper limit of the target servo 0 - 3
          servoMax[servoNum] = cmdVal;
          if (servoVal[servoNum] > cmdVal) {
            // if servo is outside of this limit move it
            PWR_timeout = PWR_tMax; if (!servoEn) {attachServos(0); movePnt = -1;}
            servoVal[servoNum] = cmdVal;
            servoInst[servoNum].writeMicroseconds(servoVal[servoNum]);
            PrintTx+= "SV" + String(servoNum) + "=" + String(servoVal[servoNum]) + "\n"; zPrint = 0;
          } break;
        case 'V':
          // set the angle of the target servo 0 - 3
          // ensure that it is within the current limits
          cmdVal = max(servoMin[servoNum], cmdVal);
          cmdVal = min(servoMax[servoNum], cmdVal);
          servoVal[servoNum] = cmdVal;
          PWR_timeout = PWR_tMax; if (!servoEn) {attachServos(0); movePnt = -1;}
          if (Calibrated && (servoNum == 1)) {
            // it is when servo[1] is moved we auto-adjust servo[2]
            setVertMinMax(); servoInst[2].writeMicroseconds(servo2Mod);
            PrintTx+= "SV2=" + String(servoVal[2]) + "\n";
          } 
          if (servoNum == 0) {
            incOffset = true;  // turn demand so increment thermal offset counter
            servoInst[0].writeMicroseconds(servoVal[0] - servoOff0);
          } else {
            servoInst[servoNum].writeMicroseconds(servoVal[servoNum]);
          }
          PrintTx+= "SV" + String(servoNum) + "=" + String(servoVal[servoNum]) + "\n"; zPrint = 0;
          break;
        case 'W':
          // re-writes current servo values by setting ValChg = true
          PrintTx+= "Servo values re-written\n";
          ValChg = true; break;
      } break;
  }
  if (zPrint > 0) {
    // is not a reporting function, echo the received command
    PrintTx+= String(cmdMode) + String(cmdType) + String(cmdVal) + ".\n";
  }
  // now reset the variables
  cmdMode = ' '; cmdType = ' '; cmdSgn = 1; cmdVal = 0;
}

// ---------------------------------------------------------------------------------

void emptySerial() {
  // empty the serial input buffer
  keyVal = 0;
  while (keyVal != -1) {
    keyVal = Serial.read();
  } cmdRec = 0; // clear the recevied 
}

// ---------------------------------------------------------------------------------

void extendCmdVal(int zVal) {
  // adds a new digit to the right-hand end of cmdVal
  cmdVal = (cmdVal * 10) + zVal;  
}

// --------------------------------------------------------------------------------

String getBAhex() {
  // Rerturns the current Broardcast Address MAC, ie: 50:02:91:68:F7:3F
  String zRet$ = ""; uint8_t zV;
  for (int zP = 0;zP < 6;zP++) {
    zV = broadcastAddress[zP]/16;
    if (zV < 10) {zRet$ += char(zV + 48);} else {zRet$ += char(zV + 55);}
    zV = broadcastAddress[zP] - (zV *16);
    if (zV < 10) {zRet$ += char(zV + 48);} else {zRet$ += char(zV + 55);}
    if (zP < 5) {zRet$ += ":";}
  }
  return zRet$;
}

// --------------------------------------------------------------------------------

String GetBIN(byte zVal) {
  // returns an 8 character binary representation of zVal
  String zBIN$ = "";
  // convert zVal into binary
  if (zVal & 0B10000000) {zBIN$ += "1";} else {zBIN$ += "0";}
  if (zVal & 0B01000000) {zBIN$ += "1";} else {zBIN$ += "0";}
  if (zVal & 0B00100000) {zBIN$ += "1";} else {zBIN$ += "0";}
  if (zVal & 0B00010000) {zBIN$ += "1";} else {zBIN$ += "0";}
  if (zVal & 0B00001000) {zBIN$ += "1";} else {zBIN$ += "0";}
  if (zVal & 0B00000100) {zBIN$ += "1";} else {zBIN$ += "0";}
  if (zVal & 0B00000010) {zBIN$ += "1";} else {zBIN$ += "0";}
  if (zVal & 0B00000001) {zBIN$ += "1";} else {zBIN$ += "0";}
  return zBIN$;
}

// --------------------------------------------------------------------------------

String getHmTime(unsigned long zMillis) {
  // returns a 'time' string in Hrs:Min base on zMillis
  String zTime$ = ""; zMillis = zMillis/60000;  // reduce zMillis to minutes
  long zU = zMillis/60; zMillis = zMillis%60;    // get hours and minutes
  zTime$ = String(zU) + ":";
  if (zMillis < 10) {zTime$ += "0";}
  zTime$ += String(zMillis);  // add minutes
  return zTime$;
}

// --------------------------------------------------------------------------------

String getHmsTime(unsigned long zMillis) {
  // returns a 'time' string in Hrs:Min:sec base on zMillis
  String zTime$ = "";
  long zU = zMillis/3600000; zMillis = zMillis%3600000;
  if (zU > 0) {zTime$ = String(zU) + ":";}
  zU = zMillis/60000; zMillis = zMillis%60000;
  if (zU < 10) {zTime$ += "0";}
  zTime$ += String(zU);
  zU = zMillis/1000;
  if (zU < 10) {zTime$ += ":0";} else {zTime$ += ":";}
  zTime$ += String(zU);
  return zTime$;
}

// -------------------------------------------------------------------------------

String getNumCSV(long zNum ) {
  // returns a string of zNum with commas ie. 1,000,000
  String zVal$;
  if (zNum > 999) {zVal$ = "," + getNumNNN(zNum % 1000); zNum /= 1000;}
  else {zVal$ = String(zNum); return zVal$;}
  if (zNum > 999) {zVal$ = "," + getNumNNN(zNum % 1000) + zVal$; zNum /= 1000;}
  else {zVal$ = String(zNum) + zVal$; return zVal$;}
  if (zNum > 999) {zVal$ = "," + getNumNNN(zNum % 1000) + zVal$; zNum /= 1000;}
  else {zVal$ = String(zNum) + zVal$; return zVal$;}
  if (zNum > 999) {zVal$ = "," + getNumNNN(zNum % 1000) + zVal$; zNum /= 1000;}
  else {zVal$ = String(zNum) + zVal$; return zVal$;}
  if (zNum > 999) {zVal$ = "," + getNumNNN(zNum % 1000) + zVal$; zNum /= 1000;}
  else {zVal$ = String(zNum) + zVal$; return zVal$;}
  if (zNum > 999) {zVal$ = "," + getNumNNN(zNum % 1000) + zVal$; zNum /= 1000;}
  else {zVal$ = String(zNum) + zVal$; return zVal$;}
  return zVal$;
}

// -------------------------------------------------------------------------------

String getNumNNN(long zNum) {
  // returns a 3 digit string for numbers 000 - 999
  String zVal$ = String(zNum);
  if (zNum < 10) zVal$ = "00" + zVal$;
  else if (zNum < 100) zVal$ = "0" + zVal$;
  return zVal$;
}

// ----------------------------------------------------------------------

void grabTgts() {
  // called to grab target values if in RECORD mode
  if (!REC) {return;} // abort

  // note playengine moves are relative to the HOME point
  moves[playPnt]     = servoTgt[0] - Home0;
  moves[playPnt + 1] = servoTgt[1] - Home1;
  moves[playPnt + 2] = servoTgt[2] - Home2;
  moves[playPnt + 3] = servoTgt[3] - Home3;
}

// ----------------------------------------------------------------------

void grabVals() {
  // called to grab current values if in RECORD mode
  if (!REC) {return;} // abort

  // note playengine moves are relative to the HOME point
  moves[playPnt]     = servoVal[0] - Home0;
  moves[playPnt + 1] = servoVal[1] - Home1;
  moves[playPnt + 2] = servoVal[2] - Home2;
  moves[playPnt + 3] = servoVal[3] - Home3;
}

// ----------------------------------------------------------------------

void incMoveOffset() {
  // called when servo[0] is being driven, to change an offset
  incOffset--; // reduce this flag so that auto-decrement can occur
  servoOffDec = servoOffDecT; // keep decrement counter at max
  if (servoOffMax != 0) {
    // a thermal offset has been defined by the user
    if (servoOff0 != servoOffMax) {
      servoOffInc++; 
      if (servoOffInc > servoOffIncT) {
        servoOffInc = 0;
        if (servoOffMax > 0) {servoOff0++;} // increment offset towards the maximum offset value
        else {servoOff0--;} // decrement offset towards the maximum -ve offset value
        // Serial.print(F("Offset = ")); Serial.println(servoOff0);
      }
    }
  }
}

// --------------------------------------------------------------------------------

void Init_ESP_NOW() {
  // called to initiate an ESEP-NOW link or connect to another trannsceiver
  // each time this function is called it will attempt to connect to a different
  // boradcast address, cycling through them, over and over
  if (!WiFiEn) {return;}  // WiFi was not enabled from RESET

  if (WiFiPrt) {Serial.println("Init_ESP_NOW()");}

  // if we have previously tried a connection then we need to de-initialise it
  if (ESP_NOW_Init) {
    // already attempted to initialise ESP-NOW so strip everything back
    if (WiFiPrt) {Serial.println("   Closing ESP-NOW");}
    esp_now_del_peer(broadcastAddress);
    // WiFi.disconnect();
  }
  
  // Init ESP-NOW and returns its status
  if (esp_now_init() != ESP_OK) {
    if (WiFiPrt) {Serial.println("   Error initializing ESP-NOW");}
    return;
  }
  ESP_NOW_Init = true;
  if (WiFiPrt) {Serial.println("   ESP_NOW_Init = true");}
  
  // change the broadcast address pointer each time
  ESP_NOW_BA++;
  // Serial.println("ESP_NOW_BA = " + String(ESP_NOW_BA));
  
  switch (ESP_NOW_BA) {
    case 0: // Wii Transciever unique MAC: 50:02:91:68:F7:3F
      broadcastAddress[0] = 0x50;
      broadcastAddress[1] = 0x02;
      broadcastAddress[2] = 0x91;
      broadcastAddress[3] = 0x68;
      broadcastAddress[4] = 0xF7;
      broadcastAddress[5] = 0x3F;
      break;
    case 1: // Wii Transciever unique MAC: 50:02:91:68:F0:D2
      broadcastAddress[0] = 0x50;
      broadcastAddress[1] = 0x02;
      broadcastAddress[2] = 0x91;
      broadcastAddress[3] = 0x68;
      broadcastAddress[4] = 0xF0;
      broadcastAddress[5] = 0xD2;
      break;
    case 2: // Wii D1 Mk1 Transciever unique MAC: 58:BF:25:DB:27:A2
      broadcastAddress[0] = 0x58;
      broadcastAddress[1] = 0xBF;
      broadcastAddress[2] = 0x25;
      broadcastAddress[3] = 0xDB;
      broadcastAddress[4] = 0x27;
      broadcastAddress[5] = 0xA2;
      break;
    case 3: // Wii ESP32-C3 Zero Transciever unique MAC: EC:DA:3B:BD:4B:AC
      broadcastAddress[0] = 0xEC;
      broadcastAddress[1] = 0xDA;
      broadcastAddress[2] = 0x3B;
      broadcastAddress[3] = 0xBD;
      broadcastAddress[4] = 0x4B;
      broadcastAddress[5] = 0xAC;

      // End of list reached, so extend pause time and reset cycle
      // Turn off serial port reporting after first pass, unless connection fails
      ESP_NOW_BA = -1;      // 1st case will be 0
      WiFiConCnt = 75;      // 3 sec delay between trying cycles
      WiFiTryOnce = false;  // don't report trying after one cycle
      break;
  }

  // Once ESPNow is successfully Init, we will register for Send CB to
  // get the status of Trasnmitted packet
  esp_now_register_send_cb(OnDataSent);

  // Register peer
  esp_now_peer_info_t peerInfo; //initialize and assign the peer information as a pointer to an addres
  memset(&peerInfo, 0, sizeof(peerInfo));
  memcpy(peerInfo.peer_addr, broadcastAddress, 6); //copy the value of  broadcastAddress with 6 bytes to peerInfo.peer_addr
  peerInfo.channel = 0;     //channel at which the esp talk. 0 means undefined and data will be sent on the current channel. 1-14 are valid channels which is the same with the local device 
  peerInfo.encrypt = false; //not encrypted

  //Add the device to the paired device list 
  if (esp_now_add_peer(&peerInfo) != ESP_OK){ 
    if (WiFiPrt) {Serial.println("    Failed to add peer");}
    return;
  }

  // Register for a callback function that will be called when data is received
  esp_now_register_recv_cb(OnDataRecv);
}

// ----------------------------------------------------------------------

void JoyArmBck(int JoyY) {
  // Wii joystick move arm backward received 
  PWR_timeout = 3000;             // preset timer for auto servo power removal after 30 sec
  checkForCmd();                  // if point is currently a command code then replace it
  moveInterval = moveDefInterval; // ensure default speed

  // shape Joystick demand profile
  if (rate == 0) {
    if (JoyY < 15) {JoyY = 1;} else {JoyY = map(JoyY,15,31,1,31);}
  }
  
  if (servoVal[1] > servoMin[1]) {
    if (!servoEn) {attachServos(0); movePnt = -1;}
    servoVal[1] = servoVal[1] - JoyY; 
    servoVal[1] = max(servoMin[1], servoVal[1]);
    servoInst[1].writeMicroseconds(servoVal[1]);
    if (Calibrated) {
      setVertMinMax(); servoInst[2].writeMicroseconds(servo2Mod);
      if (REC) {printServoVals();}
      else {PrintTx+= "SV2=" + String(servoVal[2]) + "\n";}
    } 
    servoInst[1].writeMicroseconds(servoVal[1]);
    if (REC) {moves[playPnt + 1] = servoVal[1] - Home1;}  // update moves memory
    else {PrintTx+= "SV1=" + String(servoVal[1]) + "\n";}
  }
}

// ----------------------------------------------------------------------

void JoyArmFwd(int JoyY) {
  // Wii joystick move arm forward received 
  PWR_timeout = 3000;             // preset timer for auto servo power removal after 30 sec
  checkForCmd();                  // if point is currently a command code then replace it
  moveInterval = moveDefInterval; // ensure default speed

  // shape Joystick demand profile
  if (rate == 0) {
    if (JoyY < 15) {JoyY = 1;} else {JoyY = map(JoyY,15,31,1,31);}
  }
  
  if (servoVal[1] < servoMax[1]) {
    if (!servoEn) {attachServos(0); movePnt = -1;}
    servoVal[1] = servoVal[1] + JoyY; 
    servoVal[1] = min(servoMax[1], servoVal[1]);
    servoInst[1].writeMicroseconds(servoVal[1]);
    if (Calibrated) {
      setVertMinMax(); servoInst[2].writeMicroseconds(servo2Mod);
      if (REC) {printServoVals();}
      else {PrintTx+= "SV2=" + String(servoVal[2]) + "\n";}
    } 
    servoInst[1].writeMicroseconds(servoVal[1]);
    if (REC) {moves[playPnt + 1] = servoVal[1] - Home1;}  // update moves memory
    else {PrintTx+= "SV1=" + String(servoVal[1]) + "\n";}
  }
  // Serial.println("JoyY:" + String(JoyY));
}

// ----------------------------------------------------------------------

void JoyJawClose(int JoyX) {
  // Wii joystick close jaws received 
  PWR_timeout = 3000;             // preset timer for auto servo power removal after 30 sec
  checkForCmd();                  // if point is currently a command code then replace it
  moveInterval = moveDefInterval; // ensure default speed

  // shape Joystick demand profile
  if (rate == 0) {
    if (JoyX < 15) {JoyX = 1;} else {JoyX = map(JoyX,15,31,1,31);}
  }
  
  if (servoVal[3] > servoMin[3]) {
    if (!servoEn) {attachServos(0); movePnt = -1;}
    servoVal[3] = servoVal[3] - JoyX; 
    servoVal[3] = max(servoMin[3], servoVal[3]);
    servoInst[3].writeMicroseconds(servoVal[3]);
    if (REC) {moves[playPnt + 3] = servoVal[3] - Home3;}  // update moves memory
    else {PrintTx+= "SV3=" + String(servoVal[3]) + "\n";}
  } keyPnt = 3;
}

// ----------------------------------------------------------------------

void JoyJawOpen(int JoyX) {
  // Wii joystick open jaws received 
  PWR_timeout = 3000;             // preset timer for auto servo power removal after 30 sec
  checkForCmd();                  // if point is currently a command code then replace it
  moveInterval = moveDefInterval; // ensure default speed

  // shape Joystick demand profile
  if (rate == 0) {
    if (JoyX < 15) {JoyX = 1;} else {JoyX = map(JoyX,15,31,1,31);}
  }
  
  if (servoVal[3] < servoMax[3]) {
    if (!servoEn) {attachServos(0); movePnt = -1;}
    servoVal[3] = servoVal[3] + JoyX; 
    servoVal[3] = min(servoMax[3], servoVal[3]);
    servoInst[3].writeMicroseconds(servoVal[3]);
    if (REC) {moves[playPnt + 3] = servoVal[3] - Home3;}  // update moves memory
    else {PrintTx+= "SV3=" + String(servoVal[3]) + "\n";}
  } keyPnt = 3;
}

// ----------------------------------------------------------------------

void JoyTurnLeft(int JoyX) {
  // Wii joystick left received 1 - 16
  PWR_timeout = 3000;             // preset timer for auto servo power removal after 30 sec
  checkForCmd();                  // if point is currently a command code then replace it
  moveInterval = moveDefInterval; // ensure default speed

  // shape Joystick demand profile
  if (rate == 0) {
    if (JoyX < 8) {JoyX = 1;} else {JoyX = map(JoyX,8,16,1,31);}
  }
  
  if (servoVal[0] < servoMax[0]) {
    if (!servoEn) {attachServos(0); movePnt = -1;}
    incOffset = 10; // turn demand so increment thermal offset counter
    servoVal[0] = servoVal[0] + JoyX; 
    servoVal[0] = min(servoMax[0], servoVal[0]);
    servoInst[0].writeMicroseconds(servoVal[0] + servoOff0);
    if (REC) {moves[playPnt] = servoVal[0] - Home0;}  // update moves memory
    else {PrintTx+= "SV0=" + String(servoVal[0]) + "\n";}
    // Serial.print(F("WiiRX=")); Serial.println(WiiRX);
  } keyPnt = 0;
}

// ----------------------------------------------------------------------

void JoyTurnRight(int JoyX) {
  // Wii joystick right received 1 - 16
  PWR_timeout = 3000;             // preset timer for auto servo power removal after 30 sec
  checkForCmd();                  // if point is currently a command code then replace it
  moveInterval = moveDefInterval; // ensure default speed

  // shape Joystick demand profile
  if (rate == 0) {
    if (JoyX < 8) {JoyX = 1;} else {JoyX = map(JoyX,8,16,1,31);}
  }
  
  if (servoVal[0] > servoMin[0]) {
    if (!servoEn) {attachServos(0); movePnt = -1;}
    incOffset = 10; // turn demand so increment thermal offset counter
    servoVal[0] = servoVal[0] - JoyX; 
    servoVal[0] = max(servoMin[0], servoVal[0]);
    servoInst[0].writeMicroseconds(servoVal[0] + servoOff0);
    if (REC) {moves[playPnt] = servoVal[0] - Home0;}  // update moves memory
    else {PrintTx+= "SV0=" + String(servoVal[0]) + "\n";}
  } keyPnt = 0;
}

// ----------------------------------------------------------------------

void JoyVertDwn(int JoyY) {
  // Wii joystick move head up 1 - 16
  if (Calibrated) {setVertMinMax();}// ensure limits are correct with respect to arm position
  PWR_timeout = 3000;               // preset timer for auto servo power removal after 30 sec
  checkForCmd();                    // if point is currently a command code then replace it
  moveInterval = moveDefInterval;   // ensure default speed

  // shape Joystick demand profile
  if (rate == 0) {
    if (JoyY < 8) {JoyY = 1;} else {JoyY = map(JoyY,8,16,1,31);}
  }
  
  if (servoVal[2] < servoMax[2]) {
    if (!servoEn) {attachServos(0); movePnt = -1;}
    servoVal[2] = servoVal[2] + JoyY;
    servoVal[2] = min(servoMax[2], servoVal[2]);
    servoInst[2].writeMicroseconds(servoVal[2]);
    if (REC) {moves[playPnt + 2] = servoVal[2] - Home2;}  // update moves memory
    else {PrintTx+= "SV2=" + String(servoVal[2]) + "\n";}
  } keyPnt = 2;
}

// ----------------------------------------------------------------------

void JoyVertUp(int JoyY) {
  // Wii joystick move head down 1 - 16
  if (Calibrated) {setVertMinMax();}  // ensure limits are correct with respect to arm position
  PWR_timeout = 3000;                 // preset timer for auto servo power removal after 30 sec
  checkForCmd();                      // if point is currently a command code then replace it
  moveInterval = moveDefInterval;     // ensure default speed

  // shape Joystick demand profile
  if (rate == 0) {
    if (JoyY < 8) {JoyY = 1;} else {JoyY = map(JoyY,8,16,1,31);}
  }
  
  if (servoVal[2] > servoMin[2]) {
    if (!servoEn) {attachServos(0); movePnt = -1;}
    servoVal[2] = servoVal[2] - JoyY; 
    servoVal[2] = max(servoMin[2], servoVal[2]);
    servoInst[2].writeMicroseconds(servoVal[2]);
    if (REC) {moves[playPnt + 2] = servoVal[2] - Home2;}  // update moves memory
    else {PrintTx+= "SV2=" + String(servoVal[2]) + "\n";}
  } keyPnt = 2;
}

// ----------------------------------------------------------------------

void keyBdArmBck() {
  // keyboard move arm backward received 
  PWR_timeout = 3000; // preset timer for auto servo power removal after 30 sec
  if (servoVal[1] > servoMin[1]) {
    if (!servoEn) {attachServos(0); movePnt = -1;}
    servoVal[1] = servoVal[1] - incS1; incS1++; incS1 = min(incS1, 24); 
    servoVal[1] = max(servoMin[1], servoVal[1]);
    servoInst[1].writeMicroseconds(servoVal[1]);
    if (Calibrated) {
      setVertMinMax(); servoInst[2].writeMicroseconds(servo2Mod);
      PrintTx+= "SV2=" + String(servoVal[2]) + "\n";
    } 
    servoInst[1].writeMicroseconds(servoVal[1]);
    PrintTx+= "SV1=" + String(servoVal[1]) + "\n";
  } keyPnt = 1;
}

// ----------------------------------------------------------------------

void keyBdArmFwd() {
  // keyboard move arm forward received 
  PWR_timeout = 3000; // preset timer for auto servo power removal after 30 sec
  if (servoVal[1] < servoMax[1]) {
    if (!servoEn) {attachServos(0); movePnt = -1;}
    servoVal[1] = servoVal[1] + incS1; incS1++; incS1 = min(incS1, 24); 
    servoVal[1] = min(servoMax[1], servoVal[1]);
    servoInst[1].writeMicroseconds(servoVal[1]);
    if (Calibrated) {
      setVertMinMax(); servoInst[2].writeMicroseconds(servo2Mod);
      PrintTx+= "SV2=" + String(servoVal[2]) + "\n";
    } 
    servoInst[1].writeMicroseconds(servoVal[1]);
    PrintTx+= "SV1=" + String(servoVal[1]) + "\n";
  } keyPnt = 1;
}

// ----------------------------------------------------------------------

void keyBdDither() {
  // dither the last value to see if it is central
  if (keyPnt >= 0) {
    // previous key demand has occured
    PrintTx+= "KZ = Dither\n";
    moveInc = 5;
    for (int zP = 0; zP < 20; zP++) {
      servoInst[keyPnt].writeMicroseconds(servoVal[keyPnt] + moveInc);
      PrintTx+= "KZ = " + String(servoVal[keyPnt] + moveInc) + "\n";
      delay(250);
      servoInst[keyPnt].writeMicroseconds(servoVal[keyPnt]);
      PrintTx+= "KZ = " + String(servoVal[keyPnt]) + "\n";
      delay(250);
      servoInst[keyPnt].writeMicroseconds(servoVal[keyPnt] - moveInc);
      PrintTx+= "KZ = " + String(servoVal[keyPnt] - moveInc) + "\n";
      delay(250);
      servoInst[keyPnt].writeMicroseconds(servoVal[keyPnt]);
      PrintTx+= "KZ = " + String(servoVal[keyPnt]) + "\n";
      delay(250); moveInc++;
    }
  }
}

// ----------------------------------------------------------------------

void keyBdJawClose() {
  // keyboard close jaws received 
  PWR_timeout = 3000; // preset timer for auto servo power removal after 30 sec
  if (servoVal[3] > servoMin[3]) {
    if (!servoEn) {attachServos(0); movePnt = -1;}
    servoVal[3] = servoVal[3] - incS3; incS3 = incS3 + 2; incS3 = min(incS3, 48); 
    servoVal[3] = max(servoMin[3], servoVal[3]);
    servoInst[3].writeMicroseconds(servoVal[3]);
    PrintTx+= "SV3=" + String(servoVal[3]) + "\n";
  } keyPnt = 3;
}

// ----------------------------------------------------------------------

void keyBdJawOpen() {
  // keyboard open jaws received 
  PWR_timeout = 3000; // preset timer for auto servo power removal after 30 sec
  if (servoVal[3] < servoMax[3]) {
    if (!servoEn) {attachServos(0); movePnt = -1;}
    servoVal[3] = servoVal[3] + incS3; incS3 = incS3 + 2; incS3 = min(incS3, 48); 
    servoVal[3] = min(servoMax[3], servoVal[3]);
    servoInst[3].writeMicroseconds(servoVal[3]);
    PrintTx+= "SV3=" + String(servoVal[3]) + "\n";
  } keyPnt = 3;
}

// ----------------------------------------------------------------------

void keyBdTurnLeft() {
  // keyboard left received
  PWR_timeout = 3000; // preset timer for auto servo power removal after 30 sec
  if (servoVal[0] < servoMax[0]) {
    if (!servoEn) {attachServos(0); movePnt = -1;}
    incOffset = 10; // turn demand so increment thermal offset counter
    servoVal[0] = servoVal[0] + incS0; incS0++; incS0 = min(incS0, 24); 
    servoVal[0] = min(servoMax[0], servoVal[0]);
    servoInst[0].writeMicroseconds(servoVal[0] + servoOff0);
    PrintTx+= "SV0=" + String(servoVal[0]) + "\n";
  } keyPnt = 0;
}

// ----------------------------------------------------------------------

void keyBdTurnRight() {
  // keyboard right received  
  PWR_timeout = 3000; // preset timer for auto servo power removal after 30 sec
  if (servoVal[0] > servoMin[0]) {
    if (!servoEn) {attachServos(0); movePnt = -1;}
    incOffset = 10; // turn demand so increment thermal offset counter
    servoVal[0] = servoVal[0] - incS0; incS0++; incS0 = min(incS0, 24); 
    servoVal[0] = max(servoMin[0], servoVal[0]);
    servoInst[0].writeMicroseconds(servoVal[0] + servoOff0);
    PrintTx+= "SV0=" + String(servoVal[0]) + "\n";
  } keyPnt = 0;
}

// ----------------------------------------------------------------------

void keyBdVertDwn() {
  // keyboard move head up
  if (Calibrated) {setVertMinMax();} // ensure limits are correct with respect to arm position
  else {servoMax[2] = servoMaxPWM;}  // not calibrated so use high limit
  PWR_timeout = 3000; // preset timer for auto servo power removal after 30 sec
  if (servoVal[2] < servoMax[2]) {
    if (!servoEn) {attachServos(0); movePnt = -1;}
    servoVal[2] = servoVal[2] + incS2; incS2++; incS2 = min(incS2, 24); 
    servoVal[2] = min(servoMax[2], servoVal[2]);
    servoInst[2].writeMicroseconds(servoVal[2]);
    PrintTx+= "SV2=" + String(servoVal[2]) + "\n";
  } keyPnt = 2;
}

// ----------------------------------------------------------------------

void keyBdVertUp() {
  // keyboard move head down
  if (Calibrated) {setVertMinMax();} // ensure limits are correct with respect to arm position
  else {servoMin[2] = servoMinPWM;}  // not calibrated so use low limit
  PWR_timeout = 3000; // preset timer for auto servo power removal after 30 sec
  if (servoVal[2] > servoMin[2]) {
    if (!servoEn) {attachServos(0); movePnt = -1;}
    servoVal[2] = servoVal[2] - incS2; incS2++; incS2 = min(incS2, 24); 
    servoVal[2] = max(servoMin[2], servoVal[2]);
    servoInst[2].writeMicroseconds(servoVal[2]);
    PrintTx+= "SV2=" + String(servoVal[2]) + "\n";
  } keyPnt = 2;
}

// --------------------------------------------------------------------------------

void LED_Tasks() {
  // called from the main loop() every 20ms (50Hz) to perform a range of LED tasks
  // LED_Task functions:
  //  -1  colour pulse, default
  if (LED_Nop) {return;}

  // check for button press?
  if ((sw0State == LOW) || (sw1State == LOW)) {
    if (LED_SwDwn) {return;}
    NeoPixel_Clear_Array();
    NeoPixel_SetLED(0, 15, 0, 0); NeoPixel_SetLED(1, 15, 0, 0);
    NeoPixel_ShowArray(false);
    LED_Task = -abs(LED_Task); LED_SwDwn = true; return;
  } else {LED_SwDwn = false;}
  
  // test for the delay skip counter, and exit if > 0
  if (LED_Del > 0) {LED_Del--; return;}
  
  // when changing task set the LED_Task value -ve to reset variables
  if (LED_Task < 0) {
    // change of LED task mode so clear variables
    LED_Task = -LED_Task; LED_SubTask = 0; LED_Cnt = 0; LED_Inc = 1;
    LED_Del = 0; LED_Pnt = -1; LED_Blink = true;
  }

  // test for movement, joystick demand and switch tasks accordingly
  if (MOVING || playRun || REC || WiiJoy || (APP > 0)) {
    // in either RECORD mode, APP mode or there is a Wii joystick demand
    if (LED_Task < 10) { 
       // switch to red cycle colour immediately
      LED_Task = 10;
    } LED_Timer = millis() + 3000; // set hold-on delay to 3 sec
  } else {
    // no Wii joystick demand at the moment
    if (LED_Task >= 10) {
      // switch to blue dimming colour after a timer delay
      if ((millis() >= LED_Timer) || LED_REC) {LED_Task = 1;}
    }
  }


  // perform specified tasks
  switch(LED_Task) {
    case 0: break;  // NOP
    case 1:
      // mood lighting
      if (WiiError == 0) {colR = 0; colG = 0; colB = 100;}  // set default blue colour
      else {colR = 0; colG = 100; colB = 0;}  // set green colour, no Wii
      LED_Task++; break;
    case 2:
      NeoPixel_Clear_Array();   // set all LEDs OFF
      LED_Del = 10;             // set normal speed timer for dimming process
      if (WiiError == 0) {
        colG = 0; if (colB > 0) {colB-= 4; }
        if (colB <= 0) {LED_Task++; LED_Del = 50;}  // set OFF delay
      } else {
        colB = 0; if (colG > 0) {colG-= 4; }
        if (colG <= 0) {LED_Task++; LED_Del = 50;}  // set OFF delay
      }
      NeoPixel_SetLED(0,colR,colG,colB); NeoPixel_SetLED(1,colR,colG,colB);
      NeoPixel_ShowArray(false); break;
    case 3:
      NeoPixel_Clear_Array();   // set all LEDs OFF
      if (WiiError == 0) {colB = 100; colG = 0;}  // colour is blue
      else {colB = 0; colG = 100;}                // colour is green
      LED_Del = 50;                     // set bright ON delay
      NeoPixel_SetLED(0,colR,colG,colB); NeoPixel_SetLED(1,colR,colG,colB);
      NeoPixel_ShowArray(false); LED_Task = 2; break;

    case 10:
      // flash colours left/right
        if (REC || (APP > 0)) {colG = 0; colB = 50; colR = 50;}  // purple
      else {colG = 0; colB = 0; colR = 100;}                      // red
      NeoPixel_Clear_Array();   // set all LEDs OFF
      if (!WiiJoy && !MOVING) {
        // no joystick demand, so alternate
        NeoPixel_SetLED(0,colR,colG,colB); NeoPixel_SetLED(1,0,colG,0);
        LED_Task++;
      } else {
        // joystick demand, so full ON
        NeoPixel_SetLED(0,colR,colG,colB); NeoPixel_SetLED(1,colR,colG,colB);
      } NeoPixel_ShowArray(false); LED_Del = 20; break;
    case 11:
      NeoPixel_Clear_Array();   // set all LEDs OFF
      NeoPixel_SetLED(0,0,colG,0); NeoPixel_SetLED(1,colR,colG,colB);
      NeoPixel_ShowArray(false);LED_Del = 20; LED_Task = 10; break;
  }
  LED_Blink = !LED_Blink; // toggle the 'blink' flag
  LED_REC = REC;          // store current REC satatus
}

// --------------------------------------------------------------------------------

void NeoPixel_Clear_Array() {
  // clears the contents of the colour array
  for (int zL = 0; zL < 2; zL++) {
    colBlu[zL] = 0; colGrn[zL] = 0; colRed[zL] = 0;
  }
}

// --------------------------------------------------------------------------------

void NeoPixel_SetLED(byte zPixNum, byte zR, byte zG, byte zB) {
  // set the colour of LED zPixNum.
  colBlu[zPixNum] = zB; colGrn[zPixNum] = zG; colRed[zPixNum] = zR;
  pixels.setPixelColor(zPixNum,pixels.Color(colRed[zPixNum],colGrn[zPixNum],colBlu[zPixNum]));
  LEDshow = true;
}

// --------------------------------------------------------------------------------

void NeoPixel_ShowArray(bool zShow) {
  // load all LED colours and either show them immediately or set a flag
  for (int zP = 0; zP < 2; zP++) {
    pixels.setPixelColor(zP,pixels.Color(colRed[zP],colGrn[zP],colBlu[zP]));
  }
  if (zShow) {pixels.show(); LEDshow = false;} else {LEDshow = true;}
}

// ----------------------------------------------------------------------

void printBinary(int zByte) {
  // prints eight 1's and 0's to represent zByte
  for (int zB = 7; zB >= 0; zB--) {
    byte zMask = (1 << (zB));
    if ((zByte & zMask) > 0) {
      PrintTx+= "1";
    } else {
      PrintTx+= "0";
    } PrintTx+= " ";
  }
}

// ----------------------------------------------------------------------

void printServoVals() {
  // print all four values for use with the serial plotter
  PrintTx+= String(servoVal[0] + servoOff0) + ",";
  PrintTx+= String(servoVal[1]) + ",";
  PrintTx+= String(servoVal[2]) + ",";
  PrintTx+= String(servoVal[3]) + "\n";
}

// ----------------------------------------------------------------------

void printValues() {
  // report values and home offsets when Wii Classic B-button is pressed
  if (button_BB) return;
  PrintTx+= "\n";
  reportValues();
  reportOffsets();
  reportMoveData();
  PrintTx+= "\n";
  button_BB = true;
}

// --------------------------------------------------------------------------------

void readBattery(int zM) {
  // called from the readSensors() function every 200ms
  // read the voltage on the analog input and average it over ten counts
  // zM = 0 - simple read + average
  // zM = 1 - calc float and percentage
  BatVol = analogRead(BatPin); BatTms = millis(); // take the reading
  BattNow = false;  // clear the battery read flag
  // Serial.println("BatVol = " + String(BatVol));
  BatSum = BatSum + BatVol - BatAvg;
  BatAvg = BatSum/20;     // ADC is averaged over 20 readings to remove noise
  // uncomment the following line if you want to see readings on serial monitor
  // Serial.print(F("BatAv = ")); Serial.print(BatAv); Serial.print(F("\t")); Serial.print((5.0 * BatAv)/608.9); Serial.println(F("v"));
  
  //#############################################################################
  //
  // Battery life prediction code + data logger
  //
  //#############################################################################
  // BatDel is set to 10 second interval to start with, to ignore initial readings
  if (!USBPwr) {
    if (BatDel > 0) {BatDel--;}
    else {
      BatDel = 5;  // use a 1 second timer for prediction updates & data logging
      if (BatAvg > BatAvgMax) {BatAvgMax = BatAvg;}
      BatData[BatDP] = (62L * (BatAvg - BatCritical))/(BatAvgMax - BatCritical);
      // Serial.println("BatData: " + String(BatData[BatDP]));
      if (BatDP < 59) {BatDP++;} else {shiftBatData();}
      
      if (BatT0 == 0) {
        // this is the first measurement after 10 second start delay
        // this values are the cornerstone of the prediction
        BatT0 = BatTms; BatV0 = BatAvg; BatV1 = BatAvg;
        // Serial.print("BatT0 "); Serial.println(BatT0);
      } else {
        if (BatAvg != BatV1) {
          // there has been a change in the battery voltage setting a new discharge point
          // and the current battery voltage is different, so avoid divide by zero error
          // recalculate battery life predicition base on new value and time
          // Serial.print("BatAvg "); Serial.println(BatAvg);
          BatV1 = BatAvg; // record the latest low point
          if  (BatV0 == BatV1) {BatV1--;} // avoid divide by zero error
          AnyLong = ((BatTms - BatT0)*(BatV1 - BatCritical))/(BatV0 - BatV1);
      //      Serial.print("AnyLong "); Serial.println(AnyLong);
          if (AnyLong > 0) {
            BatTime$ = getHmTime(AnyLong);
          }
        }
      }
    }  
  
  //##############################################################################
  //
  //  critical battery test
  //
  //##############################################################################
  // test for battery low condition, 683 == 6.6v
    if (BatAvg < BatCritical) {
      // battery has reached critical level so squat down and blink centre LED
      BatCritCnt++;
      if (BatCritCnt >= 100) {
        // low voltage sustained for 4 seconds or more, so we shut down
        // the delay ignores brown-outs caused by heavy servo movements
        pixels.fill(0,0,5); pixels.show();          // turn OFF all LEDs
        NeoPixel_SetLED(2, 1, 0, 0); pixels.show(); // centre LED on dim red
        Display_Text2x16("LOW","BATTERY");
        // MoveSTOP(0); ServoOFF(); IR_SensorsOFF();
        delay(3000);
        display.clear(); display.display();
        
        while (true) {
          // blink mouth centre LED red forever
          delay(20); BatCnt--;
          if (BatCnt < 1) {
            BatCnt = 100; NeoPixel_SetLED(2, 0, 0, 0); pixels.show();
          }
          if (BatCnt == 5) {NeoPixel_SetLED(2, 1, 0, 0); pixels.show();}
          // continue to take readings in case this is a deliberate power-down
          BatVol = analogRead(BatPin);
          BatSum = BatSum + BatVol - BatAvg; BatAvg = BatSum /20;
          // test for user button press
          if ((digitalRead(sw0Pin) == LOW) && (BatAvg > (BatCritical + 10))) {break;}
          yield();
        }
        // user has pressed button during power-down
        // battery looks ok so revive system
        setDefaults();
        SyncTimers();
      }
    } else {BatCritCnt = 0;}  // reset the 4 second timer
  }
  
  // if (Ping == 9) {Ping = 0; zM = 1;} // force a battery report every 10 pings
  if ((zM < 1) && (millis() < BatRep)) {return;} // exit if not a report measurement
  
  // send battery voltage and % to serial port
  BatRep = millis() + 10000; // update reading every 10 seconds
  
  AnyLong = (BatAvg - BatCritical)*100/(BatMax - BatCritical);
  if (AnyLong > 100) {AnyLong = 100;} // limit max to 100%
  if (AnyLong < 0) {AnyLong = 0;}     // limit min to 0%
  // Serial.print("Batt " + String(float(BatAvg)/BatCal) + "v " + String(AnyLong) + "%\n");
}

// --------------------------------------------------------------------------------

void readSerial() {
  // reads characters from the serial port and responds to commands
  // set PrintTgt to indicate source as serial port for responses
  keyVal = Serial.read();
  if (keyVal != -1) {SerialRx = true; PrintTgt = 0; decodeKey(keyVal);}
}

// ----------------------------------------------------------------------

void readSW0() {
  // read the left button switch and respond accordingly, pressed == LOW
  // a button press will automatical drop the current task
  sw0State = digitalRead(sw0Pin); // record button state
  if (SW0_Nop) {return;}          // block this function whilst == true
  
  bool sw0Clr = false;
  if (sw0State == LOW) {
    //##############################################################################
    //
    // SW0 button is pressed down
    //
    //##############################################################################
    if (sw0LastState == HIGH) {
      // SW0 has just been pressed down
  //    Serial.println("SW0 Hi-LO");
      if (playRun) {moveStopButton(); return;}  // button pressed during movement
      sw0DwnTime = 0; // restart button down time counter
      sw0Cnt++; // count on the falling edge
      sw0Timer = 0; // reset the timer
    } else {
      // whilst the button is down adjust the timer at 10ms steps
      sw0DwnTime++; // track button pressed time
      if (sw0DwnTime >= 200) {
        // long press in Wii disconnected mode to do a demo after resetting flags
        button_HME = false; // Wii ensure Home button is not pressed
        playXcuteDemo1();   // invoke but clear flag
        WaitWhilstDwn();    // ensure button is released before starting
        return;
      }
    }
  } else {
    //##############################################################################
    //
    // SW0 button is released
    //
    //##############################################################################
    if (sw0LastState == LOW) {
      // SW0 has just been released
  //    Serial.println("SW0 Lo-Hi");
      DispBack = false;
      if (DispNOP) {
        // we are changing the display menu mode
        DispNOP = false; DispClr = true; DispDisp = false; DispCnt = 0;
        DispPnt = 0; DispMode = 0;
        sw0Cnt = 0; sw0Timer = 0;
      }
    }
    if (sw0Timer > 0) {
      if (sw0DwnTime < 60) {
        // short press so check what mode the display is in?
        // if not in an action mode simply switch the display mode
        LED_Task = -1;    // set LED tasks to default unless changed here
        DispChng = true;  // flag to initialise certain functions
        if (REC) {
          REC_Disp++; if (REC_Disp > 1) {REC_Disp = 0;} // toggle RECORD screen
        } else if (APP > 0) {
          APP_Disp++; if (APP_Disp > 1) {APP_Disp = 0;} // toggle APP screen
        } else {
          // not in RECORD mode so normal display functions
          switch(DispMenu) {
            case 0: // in 'Sensors' display mode, so toggle between sensors
              switch(DispPnt) {
                case 0: // battery display
                  DispMode++; if (DispMode > 1) {DispMode = 0;}
                  break;
                case 1: // servo display
                  DispMode++; if (DispMode > 1) {DispMode = 0;}
                  break;
                case 2: // Wii display
                  DispMode++; if (DispMode > 0) {DispMode = 0;}
                  break;
              } break;
          }
        }
      } else {
        // button pressed longer so decrement screen displayed
        LED_Task = -1;    // set LED tasks to default unless changed here
        DispChng = true;  // flag to initialise certain functions
        if (DispMenu == 0) {
          // in sensor menu mode
          switch(DispPnt) {
            case 0: // battery display
              DispMode--; if (DispMode < 0) {DispMode = 1;}
              break;
          }
        }
      }
        // button released for 1 sec so assume valid button count
    //    Serial.print(F("swCnt = ")); Serial.println(swCnt);
        // SW0_Nop = true; flashLEDs(sw0Cnt); // indicate count by flashing
        // SW0_Nop = false;
      sw0Cnt = 0; sw0Timer = 0; sw0DwnTime = 0; DispCnt = 0;
    }
  }
  if (ESC0 && !sw0State) {return;}
  sw0LastState = sw0State; // record current state
  ESC0 = false;
  if (sw0Cnt > 0) {sw0Timer++;}
}

// ----------------------------------------------------------------------

void readSW1() {
  // read the right button switch and respond accordingly, pressed == LOW
  // a button press will automatical drop the current task
  sw1State = digitalRead(sw1Pin); // record button state
  if (SW1_Nop) {return;}          // block this function whilst == true
  
  if (sw1State == LOW) {
    //##############################################################################
    //
    // SW1 button is pressed down
    //
    //##############################################################################
    if (DispCnt < 10) {DispCnt = 10;} // don't allow display to change when button is pressed
    if (sw1LastState == HIGH) {
      // SW1 has just been pressed down
      // Serial.println("SW1 Hi-LO");
      if (playRun) {moveStopButton(); return;}  // button pressed during movement
      sw1DwnTime = 0; // restart button down time counter
      sw1Cnt++; // count on the falling edge
      sw1Timer = 0; // reset the timer
    } else {
      sw1DwnTime++; // track button pressed time
    }
    if (ESC1 && !sw1State) {return;}
    ESC1 = false;
    if ((sw1DwnTime >= 60) && (!DispNOP)) {
      // long press so present 'BACK' arrow
      if (!DispBack) {DispCnt = 0; DispBack = true; Display_40ms();}  // force display update with a back arrow
    }
  } else {
    //##############################################################################
    //
    // sw1State == HIGH as SW1 button is not pressed
    //
    //##############################################################################
    if (sw1LastState == LOW) {
      // SW1 has just been released
      // Serial.println("SW1 Lo-Hi");
      DispBack = false;
      if (DispNOP) {
        // we are changing the display menu mode
        DispNOP = false; DispClr = true; DispDisp = false; DispCnt = 0;
        DispPnt = 0; DispMode = 0;
        sw1Cnt = 0; sw1Timer = 0;
      }
    }
    if (sw1Timer > 0) {
        // PrintTx += "sw1Timer = " + String(sw1Timer) + "\n";
      if (sw1DwnTime < 60) {
        // short press so increment the display type pointer
        LED_Task = -1;    // set LED tasks to default unless changed here
        DispChng = true;  // flag to initialise certain functions
        switch (DispMenu) {
          case 0: // in 'Sensors' display mode, so toggle sensor sub-screens
            DispPnt++; if (DispPnt > 2) {DispPnt = 0;}
            DispMode = 0; // reset the display mode
            break;
        }
      } else {
        // extended press so decrement the display backwards
        LED_Task = -1;    // set LED tasks to default unless changed here
        DispChng = true;  // flag to initialise certain functions
        if (DispMenu == 0) {
          DispPnt--; if (DispPnt < 0) {DispPnt = 2;}
        }
      }
      sw1Cnt = 0; sw1Timer = 0; sw1DwnTime= 0; DispCnt = 0;
    }
  }
  sw1LastState = sw1State; // record current state
  if (sw1Cnt > 0) {sw1Timer++;}
}

// ---------------------------------------------------------------------------------

void readWiFiRx() {
  // called from the main loop every cycle to check for ESP-NOW data  
  // a state machine is used to separate the data blocks by header type
  // Note: Wii data is ignored due to direct connection
  // DispDel = 500;  // hold off display updates for 10 sec
  switch(Rx_Task) {
    case 0: // determine the type of data to handle
           if (Rx_Buff.ESPdata[Rx_Pnt] == 'B') {Rx_Task = 3; break;}
      else if (Rx_Buff.ESPdata[Rx_Pnt] == 'C') {Rx_Task = 1; break;}
      else if (Rx_Buff.ESPdata[Rx_Pnt] == 'N') {Rx_Task = 1; break;}
      else if (Rx_Buff.ESPdata[Rx_Pnt] == 'P') {Rx_Task = 1; break;}
      else if (Rx_Buff.ESPdata[Rx_Pnt] == '-') {Rx_Task = 1; break;}
      else if (Rx_Buff.ESPdata[Rx_Pnt] == '$') {Rx_Task = 2; break;}
      // oh dear! bloick header not recognised so discard the block and flag error
      else {WiFiRxErr++; WiFiRx = false;}
      break;
      
    case 1: // load Wii I2C data
      // after the header byte there are 6 data bytes + checksum
      // perform XOR on 6 bytes to check for errors
      if (Rx_Buff.ESPdata[7] == (Rx_Buff.ESPdata[1] ^ Rx_Buff.ESPdata[2] ^ Rx_Buff.ESPdata[3] ^ Rx_Buff.ESPdata[4] ^ Rx_Buff.ESPdata[5] ^ Rx_Buff.ESPdata[6])) {
        // data has passed the checksum test
        // but we do not use received data as Wii Classic is directly connected
        RxRec = true; // indicate a valid frame
      } else {
        WiFiRxErr++;
      }
      Rx_Task = 0;  // reset the task pointer to look at another block if needed
      Rx_Pnt += 8;  // increase the data pointer beyond this block
      if (Rx_Pnt >= Rx_len) {WiFiRx = false; Rx_len = 0;} // reached end of buffer
      break;

    case 2: // load text data as if from a serial port
      // read characters up to the end of the buffer
      Rx_Pnt++;
      if (Rx_Pnt >= Rx_len) {WiFiRx = false; Rx_len = 0; break;} // reached end of buffer
      keyVal = Rx_Buff.ESPdata[Rx_Pnt]; PrintTgt = 1; decodeKey(keyVal);
      // Serial.write(keyVal);
      break;

    case 3: // load binary data block
      // data length is specified in second byte
      Rx_Pnt++;
      // at the moment just jump over it, binary transfer is not needed
      Rx_Pnt += Rx_Buff.ESPdata[Rx_Pnt] + 1;
      if (Rx_Pnt >= Rx_len) {WiFiRx = false; break;} // reached end of buffer
      break;
  }
}

// ----------------------------------------------------------------------

void reportEachValue() {
  // called to report indiviual servo values over USB
  PrintTx+= "SV0=" + String(servoVal[0]) + "\n";
  PrintTx+= "SV1=" + String(servoVal[1]) + "\n";
  PrintTx+= "SV2=" + String(servoVal[2]) + "\n";
  PrintTx+= "SV3=" + String(servoVal[3]) + "\n";
}

// ----------------------------------------------------------------------

void reportHomeValues() {
  // called to report indiviual HomeN values over USB
  PrintTx+= "H0=" + String(Home0) + "\n";
  PrintTx+= "H1=" + String(Home1) + "\n";
  PrintTx+= "H2=" + String(Home2) + "\n";
  PrintTx+= "H3=" + String(Home3) + "\n";
}

// ----------------------------------------------------------------------

void reportLimits() {
  // called to report servo LL/UL limits
  // if Calibrated = false then report max limits to allow calibration
  if (Calibrated) {
    // report limits based on calibration process
    PrintTx+= "SN0LL=" + String(servoMin[0]) + "\n";
    PrintTx+= "SN0UL=" + String(servoMax[0]) + "\n";
    PrintTx+= "SN1LL=" + String(servoMin[1]) + "\n";
    PrintTx+= "SN1UL=" + String(servoMax[1]) + "\n";
    PrintTx+= "SN2LL=" + String(servoMin[2]) + "\n";
    PrintTx+= "SN2UL=" + String(servoMax[2]) + "\n";
    PrintTx+= "SN3LL=" + String(servoMin[3]) + "\n";
    PrintTx+= "SN3UL=" + String(servoMax[3]) + "\n";
  } else {
    // report max limits to enable calibration to succeed
    PrintTx+= "SN0LL=" + String(servoMinPWM) + "\n";
    PrintTx+= "SN0UL=" + String(servoMaxPWM) + "\n";
    PrintTx+= "SN1LL=" + String(servoMinPWM) + "\n";
    PrintTx+= "SN1UL=" + String(servoMaxPWM) + "\n";
    PrintTx+= "SN2LL=" + String(servoMinPWM) + "\n";
    PrintTx+= "SN2UL=" + String(servoMaxPWM) + "\n";
    PrintTx+= "SN3LL=" + String(servoMinPWM) + "\n";
    PrintTx+= "SN3UL=" + String(servoMaxPWM) + "\n";
  }
}

// ----------------------------------------------------------------------

void reportMoveData() {
  // called to report servo value offsets relative to Home over USB
  PrintTx+= "moveLoadPosRFV(0, Home0";
  anyVal =servoVal[0] - Home0;
  if (anyVal > 0) {PrintTx+= "+";}
  if (anyVal != 0) {PrintTx+= String(anyVal);}
  PrintTx+= ", Home1";
  anyVal =servoVal[1] - Home1;
  if (anyVal > 0) {PrintTx+= "+";}
  if (anyVal != 0) {PrintTx+= String(anyVal);}
  PrintTx+= ", Home2";
  anyVal =servoVal[2] - Home2;
  if (anyVal > 0) {PrintTx+= "+";}
  if (anyVal != 0) {PrintTx+= String(anyVal);}
  PrintTx+= "); // \n";
}

// ----------------------------------------------------------------------

void reportOffsets() {
  // called to report servo value offsets relative to Home over USB
  PrintTx+= "SH0= " + String(servoVal[0] - Home0) + ", ";
  PrintTx+= "SH1= " + String(servoVal[1] - Home1) + ", ";
  PrintTx+= "SH2= " + String(servoVal[2] - Home2) + ", ";
  PrintTx+= "SH3= " + String(servoVal[3] - Home3) + "\n";
}

// ----------------------------------------------------------------------

void reportPlayTgts() {
  // called to report the servo target values in CSV format
  // data is encapsulated in curly braces {SV0,SV1,SV2,SV3} with <RET>
  PrintTx+= "{" + String(servoTgt[0]) + ",";
  PrintTx+= String(servoTgt[1]) + ",";
  PrintTx+= String(servoTgt[2]) + ",";
  PrintTx+= String(servoTgt[3]) + "}\n";
}

// ----------------------------------------------------------------------

void reportPlayVals() {
  // dump the recorded values to the serial port
  PrintTx+= "\n  // Record memory count = " + String(playCnt) + "\n";
  PrintTx+= "  // Home0 = " + String(Home0) + "\n";
  PrintTx+= "  // Home1 = " + String(Home1) + "\n";
  PrintTx+= "  // Home2 = " + String(Home2) + "\n";
  PrintTx+= "  // Home3 = " + String(Home3) + "\n";
  int zE;
  for (int zP = 0; zP < playCnt; zP+=4) {
    zE = 0;
    PrintTx+= "  playMemLoad(" + String(moves[zP]) + ",";
    if (moves[zP] == (Reset0 - Home0)) {zE++;}
    PrintTx+= String(moves[zP+1]) + ",";
    if (moves[zP+1] == (Reset1 - Home1)) {zE++;}
    PrintTx+= String(moves[zP+2]) + ",";
    if (moves[zP+2] == (Reset2 - Home2)) {zE++;}
    PrintTx+= String(moves[zP+3]) + ");\t// ";
    if (moves[zP+3] == (Reset3 - Home3)) {zE++;}
    if (moves[zP] >= 5000) {
      // 1st servo value exceeds servo limit so must be a command
      if (moves[zP] == cmdPlayDelay) {PrintTx+= "Cmd delay " + String(moves[zP + 1]) + "0ms";}
      if (moves[zP] == cmdPlayClap) {PrintTx+= "Cmd clap " + String(moves[zP + 1]);}
    } 
    else if (zE == 4) {PrintTx+= "Reset";}
    PrintTx+= "\n";
  }
  PrintTx+= "\n";
  PrintTx+= "  // Recorded " + String(playCnt/4);
  PrintTx+= " element(s). " + String((playCnt * 100)/moveArraySize);
  PrintTx+= "% Max total = " + String(moveArraySize/4) + "\n";
  PrintTx+= "  // End\n";
  loopWiiWait(); // wait for both buttons to be released  
}

// ----------------------------------------------------------------------

void reportRestValue() {
  // called to report indiviual servo values over USB
  PrintTx+= "SR0=" + String(Reset0) + "\n";
  PrintTx+= "SR1=" + String(Reset1) + "\n";
  PrintTx+= "SR2=" + String(Reset2) + "\n";
  PrintTx+= "SR3=" + String(Reset3) + "\n";
}

// ----------------------------------------------------------------------

void reportServoTgts() {
  // called to report the servo target values in CSV format
  // data is encapsulated in square braces [SV0,SV1,SV2,SV3] with <RET>
  PrintTx+= "[" + String(servoTgt[0]) + ",";
  PrintTx+= String(servoTgt[1]) + ",";
  PrintTx+= String(servoTgt[2]) + ",";
  PrintTx+= String(servoTgt[3]) + "]\n";
}

// ----------------------------------------------------------------------

void reportValues() {
  // called to report servo values over USB as CSV line
  PrintTx+= "S0=" + String(servoVal[0]) + ", ";
  PrintTx+= "S1=" + String(servoVal[1]) + ", ";
  PrintTx+= "S2=" + String(servoVal[2]) + ", ";
  PrintTx+= "S3=" + String(servoVal[3]) + "\n";
}

// ----------------------------------------------------------------------

void resetServoLimits() {
  // reload the default servo min/max limits
  // note that movements can modify values for servo[2]
  servoMax[0] = turntableMax; servoMin[0] = turntableMin;
  servoMax[1] = fwdArmMax; servoMin[1] = fwdArmMin;
  servoMax[2] = vertArmMaxB; servoMin[2] = vertArmMinA;
  servoMax[3] = gripWide; servoMin[3] = gripClose;
}

// --------------------------------------------------------------------------------

void RxGetChecksum(int zlen) {
  // performs an XOR checksum calc on the Rx buffer of length zlen
  Rx_Chk = 0x55;  // define checksum seed as 01010101
  for (int zP = 0;zP < zlen;zP++) {
    Rx_Chk = Rx_Chk ^ Rx_Buff.ESPdata[zP];
  }
}

// ----------------------------------------------------------------------

void saveServoTgts() {
  // save the current servo target values
  LSV0 = servoTgt[0]; LSV1 = servoTgt[1]; LSV2 = servoTgt[2]; LSV3 = servoTgt[3];
}

// ----------------------------------------------------------------------

void saveServoVals() {
  // save the current servo values
  LSV0 = servoVal[0]; LSV1 = servoVal[1]; LSV2 = servoVal[2]; LSV3 = servoVal[3];
}

// ----------------------------------------------------------------------

void setLimits() {
  // called after a change in calibration flag
  if (Calibrated) {
    // use calibrated definitions to constrain the robot
    servoMax[0] = turntableMax;
    servoMax[1] = fwdArmMax;
    servoMax[2] = vertArmMaxB;
    servoMax[3] = gripWide;
    servoMin[0] = turntableMin;
    servoMin[1] = fwdArmMin;
    servoMin[2] = vertArmMinA;
    servoMin[3] = gripClose;
  } else {
    // use wide min/max limits to allow calibration to succeed
    servoMax[0] = servoMaxPWM;
    servoMax[1] = servoMaxPWM;
    servoMax[2] = servoMaxPWM;
    servoMax[3] = servoMaxPWM;
    servoMin[0] = servoMinPWM;
    servoMin[1] = servoMinPWM;
    servoMin[2] = servoMinPWM;
    servoMin[3] = servoMinPWM;
  }
}

// ----------------------------------------------------------------------

void setVertMinMax() {
  // sets the min/max limits for the vertical arm servo 2, based on the 
  // angular position of the forward arm servo 1
  long zVal;
  if (servoVal[1] >= fwdArmVert) {
    // determine max servo 2 limit in forward leaning posture
    zVal = servoVal[1] - fwdArmVert;
    zVal = zVal * vertArmFwdMaxDiff;
    zVal = zVal/fwdArmFwdDiff;
    servoMax[2] = vertArmMaxB - zVal;

    // determine min servo 2 limit in forward leaning posture
    zVal = servoVal[1] - fwdArmVert;
    zVal = zVal * vertArmFwdMinDiff;
    zVal = zVal/fwdArmFwdDiff;
    servoMin[2] = vertArmMinB - zVal;
  } else {
    // determine max servo 2 limit in backward leaning posture
    servoMax[2] = vertArmMaxB; // due to geometry this limit does not change
    // determine min servo 2 limit in backward leaning posture
    zVal = servoVal[1] - fwdArmMin;
    zVal = zVal * vertArmBwdMinDiff;
    zVal = zVal/fwdArmBwdDiff;
    servoMin[2] = vertArmMinC - zVal;
  }
  // force value to be within limits
  servo2Mod = servoVal[2];                // grab the current value
  servo2Mod = max(servo2Mod,servoMin[2]); // do not fall below the minimum
  servo2Mod = min(servo2Mod,servoMax[2]); // do not exceed the maximum
}

// ---------------------------------------------------------------------------------

void shiftBatData() {
  // shift battery data so that it is a rolling average
  for (int zB = 0; zB < 59; zB++) {BatData[zB] = BatData[zB + 1];}
}

// ----------------------------------------------------------------------

void testCtrlApp() {
  // listens to serial port for '~' command to set initial mode
  // test waits for 1 second
  for (int zL = 100; zL > 0; zL--) { 
    readSerial();
    delay(10);
  }
}

// ----------------------------------------------------------------------

void TestForApp() {
  // test the APP timer every 40ms to determine APP presence
  if (APP > 0) {
    // APP > 0, so a '~' ping has been received so time it out
    if (APPs == 0) {PrintTx+= "App connected\n";}
    APP--; if (APP < 1) {PrintTx+= "App disconnected\n";} 
  } APPs = APP;
}

// ----------------------------------------------------------------------

void TogIntAdj() {
  // toggle move loop auto rate adjustment
  XbutCnt++;
  if (XbutCnt > 100) {moveIntervalAdj = true;} // held button > 1 sec forces mode
  if (button_BX) return; // button still pressed so exit
  // toggle the adjustment flag
  if (moveIntervalAdj) {
    moveIntervalAdj = false;
  } else {
    moveIntervalAdj = true;
  }
  button_BX = true;
}

// ----------------------------------------------------------------------

void TogPrintMode() {
  // toggle sevo value print time mode
  YbutCnt++;
  if (YbutCnt > 100) {printTimeVals = true;} // held button > 1 sec forces mode
  if (button_BY) return; // button still pressed so exit
  // toggle the print time flag
  if (printTimeVals) {
    printTimeVals = false;
  } else {
    printTimeVals = true;
  }
  button_BY = true;
}

// ----------------------------------------------------------------------

void WaitWhilstDwn() {
  // wait for both SW0 and SW1 to be released, then clear flags and timers
  while (!digitalRead(sw0Pin) || !digitalRead(sw1Pin)) {
    // one or both SW0 & SW1 buttons are pressed
    yield();  
  }
  // clear button flags before exiting
  sw0Cnt = 0;             // button switch counter
  sw0DwnTime = 0;         // button pressed down time
  sw0LastState = HIGH;    // previous state of button switch, HIGH/LOW
  SW0_Nop = false;        // == true to block SW0 read tasks, default = false
  sw0State = HIGH;        // state of read button switch pin
  sw0Timer = 0;           // timer used to detemine button sequences
  sw1Cnt = 0;             // button switch counter
  sw1LastState = HIGH;    // previous state of button switch, HIGH/LOW
  SW1_Nop = false;        // == true to block SW1 read tasks, default = false
  sw1State = HIGH;        // state of read button switch pin
  sw1Timer = 0;           // timer used to detemine button sequences
  
  delay(20);              // avoid switch bounce
  SyncTimers();           // re-synch system timers
}

  //###############################################################################
  //
  //  WiFi Code
  //
  //###############################################################################

void WiFiClearRxBuff(int zNum) {
  // clears the contents of the receive buffer
  for (int zP = 0; zP < zNum;zP++) {Rx_Buff.ESPdata[zP] = 0;}
}

// --------------------------------------------------------------------------------

void WiFiClearTxBuff(int zNum) {
  // clears the contents of the transmit buffer
  for (int zP = 0; zP < zNum;zP++) {Tx_Buff.ESPdata[zP] = 0;}
}

// ---------------------------------------------------------------------------------

void WiFiDisable() {
  // when either a switch is pressed or WiFi disable is required
  WiFiEn = false; WiFiCntC = 0; WiFiRestCnt = 0;
}

// --------------------------------------------------------------------------------

void WiFiDisconnect() {
  // enter the WiFi disconnected state
  WiFiConnected = false;
  WiFiClearRxBuff(WiFiTx_len);
  WiFiClearTxBuff(WiFiTx_len);
  WiFiTryCnt = 0;     // reset the connection retry counter
  WiFiTx_CB = 1;      // set ready to send flag
  WiFiRxRec = false;  // clear the data received flag
  if (WiFiPrt) {Serial.println("WiFiDisconnected!");}
}

// --------------------------------------------------------------------------------

void WiFiTryToConnect() {
  // try to connect with the Wii Transceiver by sending devices name
  if (!WiFiEn) {return;}  // WiFi was not enabled from RESET

  // initialise ESP-NOW link first
  if (WiFiPrt) {Serial.println("\nWiFiTryToConnect()");}
  Init_ESP_NOW();
  
  Tx_Buff.ESPdata[0] = 'R';
  Tx_Buff.ESPdata[1] = 'e';
  Tx_Buff.ESPdata[2] = 'a';
  Tx_Buff.ESPdata[3] = 'c';
  Tx_Buff.ESPdata[4] = 'h';
  Tx_Buff.ESPdata[5] = 'M';
  Tx_Buff.ESPdata[6] = 'k';
  Tx_Buff.ESPdata[7] = '1';
  WiFiTx_len = 8;
  // t0 = micros();
  esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *) &Tx_Buff, WiFiTx_len);
  if (result == ESP_OK) {
    if (WiFiPrt) {Serial.println("Broadcast success");}
  } else {
    if (WiFiPrt) {Serial.println("Error sending broadcast");}
  } 

  WiFiTryNum++; // count the total number of attempts
}

// --------------------------------------------------------------------------------

void WiFiTxGetChecksum(int zlen) {
  // performs an XOR checksum calc on the Tx buffer of length zlen
  WiFiTx_Chk = 0x55;  // define checksum seed as 01010101
  for (int zP = 0;zP < zlen;zP++) {
    WiFiTx_Chk = WiFiTx_Chk ^ Tx_Buff.ESPdata[zP];
  }
}

// ----------------------------------------------------------------------

  //###############################################################################
  //
  //  Wii Code
  //
  //###############################################################################

void wiiClearData() {
  // sets all data bits to defaults for Wii Classic
  WiiData[0] = 160; RxWiFi[0] = WiiData[0];
  WiiData[1] =  32; RxWiFi[1] = WiiData[1];
  WiiData[2] =  16; RxWiFi[2] = WiiData[2];
  WiiData[3] =   0; RxWiFi[3] = WiiData[3];
  WiiData[4] = 255; RxWiFi[4] = WiiData[4];
  WiiData[5] = 255; RxWiFi[5] = WiiData[5];
}

// ----------------------------------------------------------------------

void wiiData() {
  // send Wii reported data to the serial port
  if (WiiError == 0) {
    PrintTx+= "Wii Data:\n"; // send title
    for(byte i = 0; i < 6; i++) {
      PrintTx+= "Byte[" + String(i) + "]\t  ";
      printBinary(WiiData[i]); PrintTx+= "\t" + String(WiiData[i]);  
      PrintTx+= "\n";
    } PrintTx+= "\n\n\n";
  }
}

// ----------------------------------------------------------------------

void wiiInitialise() {
  // called to check for Wii presence and initialise it if it is
  Wire.begin();
  WiiCnt = WiiError;
  Wire.beginTransmission ((byte)0x52);  // transmit to device (byte)0x52
  Wire.write ((byte)0xF0);              // writes memory address
  Wire.write ((byte)0x55);              // writes memory address
  Wire.write ((byte)0xFB);              // writes memory address
  Wire.write ((byte)0x00);              // writes memory address
  WiiError = Wire.endTransmission ();   // stop transmitting
  wiiClearData(); // clear Rx data bytes to default values
  if (WiiError == 0) {
    // Wii has responded
    WiiCnt = 0; wiiPhase = 1;
    WiiUpdate();    // read data from I2C
    wiiClearData(); // set data after 1st read to default values
    PrintTx+= "Wii Initialised\n";
  } else {
    // Wii has not responded so report error
    if (WiiCnt != WiiError) {PrintTx+= "Wii Not Detected On I2C Bus!\n";}
  }
}

// ----------------------------------------------------------------------

int WiiLeftStickX() {
  // returns an LX value 0/32/63
  return  ((WiiData[0] & (byte)0x3f));
}

// ----------------------------------------------------------------------

int WiiLeftStickY() {
  // returns an LY value 0/32/63
  return  ((WiiData[1] & (byte)0x3f));       
}

// ----------------------------------------------------------------------

boolean WiiPressedRowBit(byte row, byte bit) {
  /* Read a bit to test for button presses. Call directly using the following
    values  (row,bit):
    RT      (4,1)
    Start   (4,2)
    Home    (4,3)
    Select  (4,4)
    LT      (4,5)
    D-Down  (4,6)
    D-Right (4,7)
    D-Up    (5,0)
    D-Left  (5,1)
    RZ      (5,2)
    X       (5,3)
    A       (5,4)
    Y       (5,5)
    B       (5,6)
    LZ      (5,7)
  */
  byte mask = (1 << bit);
  return (!(WiiData[row] & mask ));
}

// ----------------------------------------------------------------------

int WiiRightStickX() {
  // returns a RX value 0/16/31
  return ((WiiData[0] & (byte)0xc0) >> 3) + ((WiiData[1] & (byte)0xc0) >> 5) +  ((WiiData[2] & (byte)0x80) >> 7);
}

// ----------------------------------------------------------------------

int WiiRightStickY() {
  // returns a RY value 0/16/31
  return WiiData[2] & (byte)0x1f;    
}

// ----------------------------------------------------------------------

void WiiUpdate() {
  // reads 6 bytes of data from the controller over I2C
  WiiCnt = 0;                   
  Wire.requestFrom (0x52, 6); // request data from Wii device
  while (Wire.available ()) {
    // read byte as an integer
    WiiData[WiiCnt] = Wire.read();    // read decrypted data
    RxWiFi[WiiCnt] = WiiData[WiiCnt]; // mirror data into WiFi mode
    WiiCnt++;
  }
  WiiWriteZero(); // write the request for next bytes
}

// ----------------------------------------------------------------------

void WiiWhilePressedXXX() {
  // loop hear until all buttons have been released
  while(true) {
    delay(20); // read Wii every 20ms
    WiiUpdate();
    if (WiiError != 0) {break;}
    if ((WiiData[4] == 255) && (WiiData[5] == 255)) {break;}
  }
  SyncTimers();
}

// ----------------------------------------------------------------------

void WiiWriteZero() {
  // writes one zero byte to the Wii to reset the data register pointer
  Wire.beginTransmission ((byte)0x52);  // transmit to device (byte)0x52
  Wire.write ((byte)0x00);              // writes one byte
  WiiError = Wire.endTransmission ();   // stop transmitting
}

// ----------------------------------------------------------------------
