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

void attachHeadServo() {
  // head servo and set its position to HeadVal
  
  //  servoHeadEn = true;
  servoHead.setPeriodHertz(Head_Freq);    // set servo PWM frequency
  servoHead.attach(HeadPin,500,2400);
  servoHead.writeMicroseconds(HeadVal);
  HeadTgt = HeadVal; // correct target positions
  HeadAtt = true;
  Serial.println("Head servo attached.");
}

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

void battRead() {
  // read the analogue voltage connected to the battery every 20ms
  BatVol = analogRead(BatPin);
  BatSum = BatSum + BatVol - BatAvg;
  BatAvg = BatSum/50;     // ADC is averaged over 50 readings to remove noise
  if ((BatAvg > BatWarn) && USB) {USB = false; Serial.println(F("Power resumed"));}
  if (USB) {return;}

  // detect critical battery condition and disable BalanceBot MK1
  if (BatAvg <= BatCritical){
    // critical battery voltage reached
    DispMode = -2;                    // display warning
    FastLED.clear(); FastLED.show();  // turn OFF LEDs
    VL53L1X_OFF();                    // turn OFF LTOF
    PWM_Lft = 0; PWM_Rht = 0; detachHeadServo();
    // and that's it
    while (true) {yield();}
  }
}

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

void decodeKey(int zkeyVal) {
  // decodes zkeyVal and excutes commands on '.'
  // every time we receive a character set response to 8ms later, just in case
  // there is another packet behind it coming from the controller
  Print40ms = -6;
  if (zkeyVal == 10) {return;}
  if (zkeyVal == 13) {return;}
  keyChar = char(zkeyVal);
  switch (keyChar) {
    case '.': doCmd(); return;                        // command terminator
    case '~': // connected to OLED Mirror app either via USB or WiFi
      if (!DispMon) {DispMon = true; Display_Mirrored();} // display mirrored message
      DispTx = 50; return;
    case '!': resetMotor(); return;
    case '-': cmdSgn = -1; return;    // numeric sign
    case '*': cmdMode = ' '; cmdType = ' '; cmdVal = 0; cmdSgn = 1; return; // ESC abort command
    case '0': extendCmdVal(0); return;
    case '1': extendCmdVal(1); return;
    case '2': extendCmdVal(2); return;
    case '3': extendCmdVal(3); return;
    case '4': extendCmdVal(4); return;
    case '5': extendCmdVal(5); return;
    case '6': extendCmdVal(6); return;
    case '7': extendCmdVal(7); return;
    case '8': extendCmdVal(8); return;
    case '9': extendCmdVal(9); return;
  }
  if (cmdMode == ' ') {
    // test for new Command Mode char?
    // only accept certain characters as valid commands, or ignore them
    // delay battery update by 10 seconds whilst receiving commands
    cmdMode = keyChar;
    switch (keyChar) {
      // check for valid mode and convert upper-case
      case 'g': cmdMode = 'G'; break;
      case 'j': cmdMode = 'J'; break;
      case 'l': cmdMode = 'L'; break;
      case 'p': cmdMode = 'P'; break;
      case 's': cmdMode = 'S'; break;
      case 'v': cmdMode = 'V'; break;
      case 'w': cmdMode = 'W'; break;
    } cmdType = ' '; cmdVal = 0;
  } else {
    // test for Command Type char?
    cmdType = keyChar;
    switch (keyChar) {
      // check for lower-case and convert to upper-case
      case 'a': cmdType = 'A'; break;
      case 'c': cmdType = 'C'; break;
      case 'd': cmdType = 'D'; break;
      case 'e': cmdType = 'E'; break;
      case 'h': cmdType = 'H'; break;
      case 'l': cmdType = 'L'; break;
      case 'o': cmdType = 'O'; break;
      case 'p': cmdType = 'P'; break;
      case 'r': cmdType = 'R'; break;
      case 's': cmdType = 'S'; break;
      case 't': cmdType = 'T'; break;
      case 'u': cmdType = 'U'; break;
      case 'v': cmdType = 'V'; break;
      case 'x': cmdType = 'X'; break;
      case 'y': cmdType = 'Y'; break;
    }
  }
}

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

void detachHeadServo() {
  // detach head servo
  // as the micro servo uses GPIO2, which is also connected to the buitlin LED, we
  // need to pull this pin LOW when not attached to the servo, by setting PWM to 0.
  if (!HeadAtt) {return;} // already detached
  
  servoHead.release();
  HeadAtt = false;
}

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

void doCmd() {
  // a '.' has been received so execute command if valid
  // some commands are duplicated here, so that the code works with several apps
  // supported commands are:
  //
  //    D0.   - disable PID d-gain
  //    D1.   - enable PID d-gain
  //    DD.   - decrease the d-gain by 0.1
  //    DU.   - increase the d-gain by 0.1
  //    DVnn. - set 'D' gain value to nn/10
  //    G.    - report Scope data for graphing app
  //    GE.   - end of Scope graphing mode
  //    GPnn. - nn points to data to be sent
  //    I0.   - disable PID i-gain
  //    I1.   - enable PID i-gain
  //    ID.   - decrease the i-gain by 0.1
  //    IU.   - increase the i-gain by 0.1
  //    IVnn. - set 'I' gain value to nn/100
  //    JXnn. - set global variable JX to nn, used for debug purposes
  //    JYnn. - set global variable JY to nn, used for debug purposes
  //    PD.   - decrease the P-gain by 1.0
  //    PU.   - increase the P-gain by 1.0
  //    PVnn. - set 'P' gain value to nn/10
  //    R.    - report over the serial link
  //    RPnn. - report nn data over the serial link
  //    SGnn. - set 'S' gain value to nn
  //    STn.   - set servo target to 'n'
  //    SVnn.  - set value of servoPnt device to nn
  //  .     - report the P,I,D gains and much more!
  
  bool zPLF = true;   // if true at the end a line feed will be printed
  bool zErr = false;  // if a command is not recognised it simply echoes it
  GphMode = 0;        // reset graphing mode flag
  switch (cmdMode) {
    case ' ':
      // recevied a '.' only so report PID gains
      // block this if in graphing mode as it could be data corruption
      if (!RP_ON) {PrintTx += "\n"; printGains(); zPLF = false;}
      break;
    case 'D':
      // PID controller D commands
      switch (cmdType) {
        case ' ': 
          if (cmdVal > 0) {PID_d_En = true;}
          else {PID_d_En = false;} break;
        case 'D': pid_d_gain -= 0.1;  break;
        case 'U': pid_d_gain += 0.1; break;
        case 'V': pid_d_gain = (float)cmdVal/10.0; break;
        default: zErr = true; break;
      } 
      PrintTx += "D" + String((int)(pid_d_gain * 10.0)); break;
    case 'G':
      // Scope graphing app report commands
      GphMode = 1;  // indicate that Monitor+ commands are in use
      switch (cmdType) {
        case ' ': 
          // report the same set of values twice as WiFi data can easily be
          // corrupted. Therefore two matching consequitives messages must be
          // received for the graphing function to work
          RP_ON = true; // set flag to indicate that we are reporting data
          RPms = millis() - RPt0; RPt0 = millis();
          RPP = 0; DebugReport(RPnn);
          RPP = 1; DebugReport(RPnn);
          zPLF = false; break;
          break;
        case 'E': // GE. exits reporting mode when dropping out of graphing mode
          // return to Display Monitor mode if it was previously active
          if (DispTx < 0) {DispMon = true; PrintTx = ""; DispTx = 50;}
          break;
        case 'P': // GPnn. sets the reporting mode and triggers title reporting
          if (DispMon) {DispMon = false; PrintTx = ""; DispTx = -1;}
          RPnn = cmdVal; RP_Title = true;
          PrintTx += "GP" + String(RPnn) + "\n";
          break;
      } break;
    case 'I':
      // PID controller I commands
      switch (cmdType) {
        case ' ': 
          if (cmdVal > 0) {PID_i_En = true;}
          else {PID_i_En = false;} break;
        case 'D': pid_i_gain -= 0.1;  break;
        case 'U': pid_i_gain += 0.1; break;
        case 'V': pid_i_gain = (float)cmdVal/100.0; break;
        default: zErr = true; break;
      }
      if (pid_i_gain < 0.01) {pid_i_mem = 0.0;}
      PrintTx +="I" + String((int)(pid_i_gain * 100.0)); break;
    case 'J':
      // commands used to set global variables in association with Joystick app
      switch (cmdType) {
        case 'X': // set global variable JX to cmdVal
          JX = cmdVal; //Serial.println("JX=" + String(JX));
          break;
        case 'Y': // set global variable JY to cmdVal
          JY = cmdVal; //Serial.println("JY=" + String(JY));
          break;
        default: zErr = true; break;
      } break;
    case 'P':
      // PID controller P commands
      switch (cmdType) {
        case 'D': pid_p_gain -= 1.0;  break;
        case 'U': pid_p_gain += 1.0; break;
        case 'V': pid_p_gain = (float)cmdVal/10.0; break;
        default: zErr = true; break;
      }
      PrintTx += "P" + String((int)(pid_p_gain * 10.0)); break;
    case 'R':
      // PID controller report commands
      switch (cmdType) {
        case ' ': 
          // report the same set of values twice as WiFi data can easily be
          // corrupted. Therefore two matching consequitives messages must be
          // received for the PID Controller graphing function to work
          RP_ON = true; // set flag to indicate that we are reporting data
          RPms = millis() - RPt0; RPt0 = millis();
          RPP = 0; DebugReport(RPnn);
          RPP = 1; DebugReport(RPnn);
          zPLF = false; break;
        case 'E': // RE. exits reporting mode when dropping out of graphing mode
          RP_ON = false;
          break;
        case 'P': // RPnn. sets the reporting mode and triggers title reporting
          RPnn = cmdVal; RP_Title = true;
          PrintTx += "RP" + String(RPnn) + "\n"; break;
        default: zErr = true; break;
      } break;
    case 'S':
      // Servo & Motor Steer commands & PIS setpoint jolt
      switch (cmdType) {
        case 'G':
          PWM_Start = (float)cmdVal;
          PrintTx += "SG"+String(cmdVal); break;
        case 'T':
          ServoPnt = cmdVal;
          PrintTx += "ST"+String(cmdVal);
          break;
        case 'V':
          if (ServoPnt == 0) {
            // this is the head servo
            cmdVal = constrain(cmdVal,500,2400); HeadVal = cmdVal;
            if (!HeadAtt) {attachHeadServo();}
            else {servoHead.writeMicroseconds(HeadVal); HeadTgt = HeadVal;}
          } else {
            // this is the left motor
            cmdVal = cmdVal * cmdSgn;
            cmdVal = constrain(cmdVal,-255,255);
            if (ServoPnt == 1) {PWM_Lft = cmdVal;}
            if (ServoPnt == 2) {PWM_Rht = cmdVal;}
          }
          PrintTx += "SV"+String(cmdVal); break;
        default: zErr = true; break;
      } break;
    default: zErr = true; break;
  }
  // echo unrecognised commands
  if (zErr) {PrintTx += String(cmdMode) + String(cmdType) + String(cmdVal);}
  // now reset the variables
  cmdMode = ' '; cmdType = ' '; cmdVal = 0; cmdSgn = 1;
  if (zPLF) {PrintTx += "\n";}
}

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

void FlushRxBuffer() {
  // Read and ignore all characters in the serial port buffer  
  while (Serial.available() > 0) {
      // read data to empty buffer
    keyVal = Serial.read();
  }
}

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

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$;
}

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

void GoSafe() {
  // called due to a user buttom press or when falling over  
  startEn = 0;                            // Set the start variable to 0
  PWM_Lft = 0; PWM_Rht = 0;               // remove motor PWM drive
  MotorDriveTask();                       // update motor drives
  if (RangeEn) {VL53L1X_OFF();}           // turn laser OFF if ON
  pid_output = 0;                         // Set the PID controller output to 0 so the motors stop moving
  pid_i_mem = 0;                          // Reset the I-controller memory
  self_balance_pid_setpoint = 0.0;        // Reset the self_balance_pid_setpoint variable
  if (safeMode > 0) {reset_SafeMode();}
  DispMode = 9; DispNext = 5;             // display 'SAFE'
  StatMode = 1;                           // display default status bar
  Set_LEDTask(0);                         // default LED state
  Exp = ExpSafe;                          // safe expression
  SlotEn = false; Reset_Slots();          // turn OFF slot counting
}

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

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
  //  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
    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) {
    Serial.println("Error initializing ESP-NOW");
    return;
  }
  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;
      break;
    default:
      // 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){ 
    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);
}

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

int Limit255(int zI) {
  // return an integer value between 0 and 255
  if (zI > 255) {zI = 255;}
  if (zI < 0) {zI = 0;}
  return zI;
}

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

int16_t map16(int16_t x,int16_t in_min,int16_t in_max,int16_t out_min,int16_t out_max) {
  // standard map function, but for int16_t variables
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

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

float mapFloat(float x,float in_min,float in_max,float out_min,float out_max) {
  // floating point version of the map() function
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

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

void MPU_6050_Initialise() {
  // set up gyro hardware
  // By default the MPU-6050 sleeps. So we have to wake it up.
  Wire.beginTransmission(MPU_address);  // Start communication with the address found during search.
  Wire.write(0x6B);                     // We want to write to the PWR_MGMT_1 register (6B hex)
  Wire.write(0x00);                     // Set the register bits as 00000000 to activate the gyro
  I2C_Err = Wire.endTransmission();     // End the transmission with the MPU.
  if (I2C_Err != 0) {return;}
  delayMicroseconds(I2Cdel);                // Allow MPU time to respond

  //Set the full scale of the gyro to +/- 250 degrees per second
  Wire.beginTransmission(MPU_address);  // Start communication with the address found during search.
  Wire.write(0x1B);                     // We want to write to the GYRO_CONFIG register (1B hex)
  Wire.write(0x00);                     // Set the register bits as 00000000 (250dps full scale)
  I2C_Err = Wire.endTransmission();     // End the transmission with the MPU.
  if (I2C_Err != 0) {return;}
  delayMicroseconds(I2Cdel);                // Allow MPU time to respond

  //Set the full scale of the accelerometer to +/- 4g.
  Wire.beginTransmission(MPU_address);  // Start communication with the address found during search.
  Wire.write(0x1C);                     // We want to write to the ACCEL_CONFIG register (1A hex)
  //  Wire.write(0x00);                 // Set the register bits as 00000000 (+/- 2g full scale range)
  Wire.write(0x08);                     // Set the register bits as 00001000 (+/- 4g full scale range)
  I2C_Err = Wire.endTransmission();     // End the transmission with the MPU.
  if (I2C_Err != 0) {return;}
  delayMicroseconds(I2Cdel);                // Allow MPU time to respond

  //Set some filtering to improve the raw data.
  Wire.beginTransmission(MPU_address);  // Start communication with the address found during search
  Wire.write(0x1A);                     // We want to write to the CONFIG register (1A hex)
  Wire.write(0x03);                     // Set the register bits as 00000011 (Set Digital Low Pass Filter to ~43Hz)
  I2C_Err = Wire.endTransmission();     // End the transmission with the MPU.
  if (I2C_Err != 0) {return;}
  delayMicroseconds(I2Cdel);                // Allow MPU time to respond
}

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

void PrintAccRaw() {
  // print out the raw accelerometer values
  Serial.println("X=" + String(AccRawX) + "\tY=" + String(AccRawY) + "\tZ=" + String(AccRawZ));
}

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

void PwmAttach(uint8_t zCh) {
  // attached the micros internal PWM counter to an output pin
  switch (zCh) {
    case 0: analogWrite(PinLftA, 0); break;
    case 1: analogWrite(PinLftB, 0); break;
    case 2: analogWrite(PinRhtA, 0); break;
    case 3: analogWrite(PinRhtB, 0); break;
  }
}

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

void PwmSetup(uint8_t zCh) {
  // setup the PWM counter to an output pin
  switch (zCh) {
    case 0: analogWriteFrequency(PinLftA, PwmFreq); analogWriteResolution(PinLftA, PwmBits); break;
    case 1: analogWriteFrequency(PinLftB, PwmFreq); analogWriteResolution(PinLftB, PwmBits); break;
    case 2: analogWriteFrequency(PinRhtA, PwmFreq); analogWriteResolution(PinRhtA, PwmBits); break;
    case 3: analogWriteFrequency(PinRhtB, PwmFreq); analogWriteResolution(PinRhtB, PwmBits); break;
  }
}

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

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 Read_Slots() {
  // called regularly during main loops of both cores
  // assynchronously tracks slot pulse triggers
  //  if (SlotRd) {return;} // function is being used by other Core
  SlotRd = true;
  
  // left sensor:
  if (digitalRead(INT_P34) != IntP34State) {
    IntMicros[0] = micros(); IntP34State = !IntP34State; IntP34Cnt+= DIR_Lft;
    IntPd = IntMicros[0] - IntLast[0];
    if (IntPd > 3300) {
      if (abs(DIR_LTnd) >= 2000) {
        // PWM movement threshold has been reached
        if (IntPeriods[0] == 0) {IntPeriods[0] = IntPd;}  // 1st period is not averaged
        else {IntPeriods[0] = (IntPeriods[0] + IntPd)/2;} // average adjacent readings
        // 1 rpm period == 60,000,000/40 microseconds for a 20 slot, 40 edge wheel == 15,000,000 microseconds/period
        // so to return rpm we divide 15,000,000 by the period
        // here we work in x10 rpm units, so we divide 1,500,000 by the period
        Int34RPM = 1500000/(int)IntPeriods[0];         // value is 100 times true rpm value, 1 == 0.01 rpm
        if (DIR_Lft < 0) {Int34RPM = -Int34RPM;}  // correct rpm direction
        IntP34Ch = true;  // flag change has occurred, wheel is moving
      } else {
        // insufficient trend movement so reset RPM
        IntPeriods[0] = 0; Int34RPM = 0;
      }
    }
    IntLast[0] = IntMicros[0];
  }
  
  // right sensor:
  if (digitalRead(INT_P35) != IntP35State) {
    IntMicros[1] = micros(); IntP35State = !IntP35State; IntP35Cnt+= DIR_Rht;
    IntPd = IntMicros[1] - IntLast[1];
    if (IntPd > 3300) {
      if (abs(DIR_RTnd) >= 2000) {
        // PWM movement threshold has been reached
        if (IntPeriods[1] == 0) {IntPeriods[1] = IntPd;}  // 1st period is not averaged
        else {IntPeriods[1] = (IntPeriods[1] + IntPd)/2;} // average adjacent readings
        // 1 rpm period == 60,000,000/40 microseconds for a 20 slot, 40 edge wheel == 15,000,000 microseconds/period
        // so to return rpm we divide 15,000,000 by the period
        // here we work in x10 rpm units, so we divide 1,500,000 by the period
        Int35RPM = 1500000/(int)IntPeriods[1];         // value is 100 times true rpm value 1 == 0.01 rpm
        if (DIR_Rht < 0) {Int35RPM = -Int35RPM;}  // correct rpm direction
        IntP35Ch = true;  // flag change has occurred, wheel is moving
      } else {
        // insufficient trend movement so reset RPM
        IntPeriods[1] = 0; Int35RPM = 0;
      }
    }
    IntLast[1] = IntMicros[1];
  }
  SlotRd = false; // release function blocking

}

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

void readSW0() {
  // read the left button switch and respond accordingly, pressed == LOW
  // a button press will automatical drop the current task
  // SW0 is read every 8ms (125Hz) to debounce
  sw0State = digitalRead(sw0Pin); // record button state
  if (sw0_Nop) {return;}          // block this function whilst == true
  
  if (sw0State == LOW) {
    //##############################################################################
    //
    // SW0 button is pressed down
    //
    //##############################################################################
    if (sw0Wup) {return;}
    
    if (sw0LastState == HIGH) {
      // SW0 has just been pressed down
      if (TEST) {
        Serial.println("SW0 Hi-LO");
        if (HeadTask == 0) {HeadTask = 3;} else {HeadTask = 1;}
      } else {
        SLEEP = false;  // wake up if sleeping
        if ((Exp == ExpLying) || (Exp == ExpReady)) {
          // user is selecting status display modes
          if (StatSet == 0) {StatSet = 1;}            // first time pressed (default was batteries)
          StatSet++; if (StatSet > 11) {StatSet = 1;} // change the status displayed
          StatDel = 0; StatMode = 80; StatCnt = 0;    // set falks and hold off battery status whilst pressing buttons
        }
        else if ((safeMode > 0) || (Exp == ExpReadyAnx)) {GoSafe();}   // return to safe condition
      }
      sw0DwnTime = 0; // restart button down time counter
      sw0Cnt++;       // count on the falling edge
      sw0Timer = 0;   // reset the timer
  //      if (WiFiEn) {SetMainMode(0); return;}
    } else {
      // whilst the button is down adjust the timer at 20ms steps
      // if Wii is not connected test for a long press to invoke demo mode
      sw0DwnTime++;   // track button pressed time
      StatCnt = 0;    // hold off battery status updates whilst switch is pressed
      if (sw0Timer > 30) {
        // button SW0 held down for > 0.5 second
  //        if (WiFiEn) {SetMainMode(0);}
  //        SetMainMode(99);  // switch to mode select
        // block further actions until released
        sw0Cnt = 0; sw0Timer = 0; sw0Wup = true;
      }
    }
  } else {
    //##############################################################################
    //
    // SW0 button is released
    //
    //##############################################################################
    if (sw0Wup) {
      // waited for button release
      sw0Wup = false; sw0Cnt = 0; sw0Timer = 0;
    } else {
      if (sw0LastState == LOW) {
        // SW0 has just been released
        if (TEST) {Serial.println("SW0 Lo-Hi");}
      }
      if (sw0Timer > 60) {
        // button released for 1 sec so assume valid button counts
      //    Serial.print(F("swCnt = ")); Serial.println(swCnt);
  //        if (TEST && (sw0Cnt > 0)) {
  //          // single button press during test mode
  //        }
        sw0Cnt = 0; sw0Timer = 0;
      }
      sw0DwnTime = 0;
    }
  }
  sw0LastState = sw0State; // record current state
  if (sw0Cnt > 0) {sw0Timer++;}
}

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

void readSW1() {
  // read the right button switch and respond accordingly, pressed == LOW
  // a button press will automatical drop the WiFi mode
  // SW1 is read every 8ms (125Hz) to debounce
  sw1State = digitalRead(sw1Pin); // record button state
  if (sw1_Nop) {return;}          // block this function whilst == true
  
  if (sw1State == LOW) {
    //##############################################################################
    //
    // SW1 button is pressed down
    //
    //##############################################################################
    if (sw1Wup) {return;}
    
    if (sw1LastState == HIGH) {
      // SW1 has just been pressed down
      if (TEST) {
        Serial.println("SW1 Hi-LO");
        if (TEST) {MotorSwp++;}
      } else {
        SLEEP = false;  // wake up if sleeping
        if ((Exp == ExpLying) || (Exp == ExpReady)) {
          // user is changing modes
          MainMode++; if (MainMode > 3) {MainMode = 0;}
          setMainMode(MainMode);
        }
        else if ((safeMode > 0) || (Exp == ExpReadyAnx)) {GoSafe();}   // return to safe condition
      }
      sw1DwnTime = 0; // restart button down time counter
      sw1Cnt++; // count on the falling edge
      sw1Timer = 0; // reset the timer
  //      if (WiFiEn) {sw1Cnt = 0; SetMainMode(0); return;}
    } else {
      sw1DwnTime++;   // track button pressed time
      StatCnt = 0;    // hold off battery status updates whilst switch is pressed
      if(sw1Timer >= 250) {
        // button held down for >= 2 second
        CalGyr = 1;
        sw1Cnt = 0; sw1Timer = 0;
      }
    }
  } else {
    //##############################################################################
    //
    // sw1State == HIGH as SW1 button is not pressed
    //
    //##############################################################################
    if (sw1Wup) {
      // release locked-out state
      sw1Wup = false;
      sw1Cnt = 0; sw1Timer = 0;
    } else {
      // normal SW1 release
      if (sw1LastState == LOW) {
        // SW1 has just been released
        if (TEST) {Serial.println("SW1 Lo-Hi");}
      }
        // not in TEST mode so set task based on count value
      if ((sw1Timer > 60) && (sw1Cnt > 0)) {
        // 1 sec timeout + count > 0
        // task select will depend on MainMode
        sw1Cnt = 0; sw1Timer = 0; // clear events
      }
    }
  }
  sw1LastState = sw1State; // record current state
  if (sw1Cnt > 0) {sw1Timer++;}
}

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

void resetMotor() {
  // centre head servo and stop wheel motors  
  HeadVal = 1500; // H/W default value
  if (!HeadAtt) {attachHeadServo();}
  else {servoHead.writeMicroseconds(HeadVal); HeadTgt = HeadVal;}

  PWM_Lft = 0; PWM_Rht = 0;
}

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

void reset_PID() {
  // called when dropping out of active mode  
  startEn = false;        // shut down motor drive
  PID_d_En = 1;           // PID d-gain enable 1=ON, 0-OFF
  PID_En_Cnt = 0;         // counter used to enter gain disable modes
  PID_i_En = 1;           // PID i-gain enable 1=ON, 0-OFF
  pid_i_mem = 0.0;        // Reset the I-controller memory
  pid_output = 0.0;       // Set the PID controller vector output to 0 so the motors stop moving
  pid_output_trend = 0;   // tracker used in self-balancing
  PID_Tune = 0;           // > 0 when in tuning mode
  self_balance_pid_setpoint = 0.0;  // Reset the self_balance_pid_setpoint variable
}

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

void Reset_Slots() {
  // called before using the slot sensors to count pulses
  // left sensor:
  IntP34State = digitalRead(INT_P34); IntP34Ch = false; IntPeriods[0] = 0;
  IntMicros[0] = micros(); IntP34Cnt = 0; IntLast40[0] = 0; Int34RPM = 0;
 // right sensor:
  IntP35State = digitalRead(INT_P35); IntP35Ch = false; IntPeriods[1] = 0;
  IntMicros[1] = micros(); IntP35Cnt = 0; IntLast40[1] = 0; Int35RPM = 0;
}

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

void setMainMode(int zMM) {
  // set the appropriate conditions for a given mode
  DispDel = 0;          // set display for immediate change
  DispNext = 5;         // set display mode default as gyro angle
  MainTask = 0;         // reset task pointer
  RangeRate = 64;       // max rate of inter-measurement Range change, default = 64
  reset_PID();          // reset PID variables
  VL53L1X_OFF();        // turn OFF LTOF
  WiiOveride = false;   // set == true to overide Wii demands

  switch(zMM) {
    case 0: // normal balancing mode
      DispMode = 11;
      break;
    case 1: // enable 'Pilot' mode
      RangeRate = 24;       // max rate of inter-measurement Range change, default = 64
      DispMode = 13;
      break;
    case 2: // enable 'Play' mode
      DispMode = 14;
      break;
    case 3: // enable 'Autonomous' mode
      DispMode = 15;
      break;
  }
  MainMode = zMM;
  //  Serial.flush(); Serial.print(F("MM = ")); Serial.println(MainMode);
}

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

void VL53L1X_OFF() {
  // switches OFF the VL53L1X laser ranging device
  pinMode(XSHUT_PIN,OUTPUT); digitalWrite(XSHUT_PIN,LOW);     // switch VL53L1X OFF
  Serial.println("VL53L1X OFF");
  RangeEn = false; VL53L1X_Task = 0;
}

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

void VL53L1X_ON() {
  // switches ON the VL53L1X laser ranging device
  // if it was already ON then switch it OFF then ON again
  if (RangeEn) {VL53L1X_OFF(); delay(1);}
  // synchLoopTimers(1);}
  
  pinMode(XSHUT_PIN,OUTPUT); digitalWrite(XSHUT_PIN,HIGH);  // enable VL53L1X sensor
  if (VL53L1X.init() == false) {
    Serial.println("VL53L1X sensor detected!"); I2C_LTOF = true;
    VL53L1X.setDistanceModeShort();         // Sets to 1.3M range
    VL53L1X.setTimingBudgetInMs(33);        // Set timing budget to 33 ms
    VL53L1X.setIntermeasurementPeriod(33);  // Set measurement period in ms
    VL53L1X.setROI(VL53L1X_ROIX,VL53L1X_ROIY,VL53L1X_OC); // Set SPAD ROI
    VL53L1X.startRanging();                 // Starts taking measurements
    Serial.println("VL53L1X ROI SPAD W= " + String(VL53L1X.getROIX()));
    Serial.println("VL53L1X ROI SPAD H= " + String(VL53L1X.getROIY()));
    RangeEn = true; VL53L1X_Task = 1;
    Range = LtofLimit; RangeLast = LtofLimit;
  } else { I2C_LTOF = false; Serial.println("VL53L1X sensor failed!"); RangeEn = false;}
}

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

void VL53L1X_Run() {
  // control the VL53L1X sensor using a series of tasks
  // this code gets timing information, for using the sensor more efficiently
  // once measurements start a skip timer is used to avoid unnecessary readings
  if (millis() < VL53L1X_Skip) {return;}  // allowing for reading delay 32ms
  
  switch(VL53L1X_Task) {
    // case 8 is placed at the front of the list for speed
    case 8: // wait for measurement to complete
      // this case is placed at the head of the list as it will be used most often
      // if data is not ready we keep coming to this task
      // record previous range just before taking a new measurement
      RangeLast = Range;
      if (VL53L1X.checkForDataReady()) {
  //        t1 = millis() - t1; // how long did the measurement take?
  //        Serial.println("VL53L1X.checkForDataReady() in " + String(t1) + "ms");
  //        t1 = millis();
        VL53L1X_Skip = millis() + VL53L1X_SkipDel; // set the next measurement delay
        VL53L1X_Status = VL53L1X.getRangeStatus();
  //        Serial.println(VL53L1X_Status);
        if (VL53L1X_Status == 0) {
          // no ranging errors, valid measurement
  //          t0 = micros();
          RangeRaw = VL53L1X.getDistance();
  //          t0 = micros() - t0;
  //          Serial.println("VL53L1X.getDistance() in " + String(t0) + "µs");
        } else {
          // if an error occurs we lok at the error
          //  1 - Sigma estimator, uncertain measurement, above the internal threshold
          //  2 - Signal value is below the internal defined threshold
          //  4 - Raised when phase is out of bounds
          //  5 - Raised in case of HW or VCSEL failure
          //  7 - Wrapped target, not matching phases
          //  8 - Internal algorithm underflow or overflow
          // 14 -  The reported range is invalid
          // we reject a range if the error exceeds 4
          if (VL53L1X_Status < 5) {
            RangeRaw = VL53L1X.getDistance();
          } else {RangeRaw = RangeMax;}
        }
        LTOFt1 = millis();
  //        Serial.println(RangeRaw);
  //        t0 = micros();
        VL53L1X.clearInterrupt(); // 444µs clears the interrupt and data ready flag
  //        t0 = micros() - t0;
  //        Serial.println("VL53L1X.clearInterrupt() in " + String(t0) + "µs");
  //        Serial.println("");
        // sensor reading RangeRaw is rate limited to remove transient noise
        // if change is less then Range is averaged over two measurements to reduce noise
        if (RangeRaw > RangeRawF) {
          if ((RangeRaw - RangeRawF) > RangeRate){RangeRawF+= RangeRate;} else {RangeRawF = (RangeRaw + RangeRawF)/2;}
        } else if (RangeRaw < RangeRawF) {
          if ((RangeRawF - RangeRaw) > RangeRate){RangeRawF-= RangeRate;} else {RangeRawF = (RangeRaw + RangeRawF)/2;}
        }
        Range = RangeRawF; RangeNew = true;
        // limit the range to avoid erroneous values
        if (Range >= RangeMax) {Range = RangeMax; RangeUL = true;} else {RangeUL = false;}
        if (Range <= RangeMin) {Range = RangeMin; RangeLL = true;} else {RangeLL = false;}
  //        Serial.println("Range = " + String(Range) + "mm");
  //        Serial.println(String(Range) + "," + String(servoVal[0]) + "," + String(SteerAng));
        // Store the range at the current angle 30 - 150 == 0 - 40 in RangeData[]
  //        RangeDataDP = (getPanAngle() - 30)/3;
  //        RangeData[RangeDataDP] = Range;
        // extrapolate missing values from previous angle
        if (RangeDataDP > RangeDataDPL+1) {
          // there is a gap in the range data, going up the array, so fill in missing data
          // the value used is the intermediate value
          int16_t zDiff2 = (RangeData[RangeDataDP] - RangeData[RangeDataDPL])/2;
          for (int zP = RangeDataDPL+1;zP < RangeDataDP;zP++) {
            RangeData[zP] = RangeData[RangeDataDPL] + zDiff2;
          }
        } else if (RangeDataDP < RangeDataDPL-1) {
          // there is a gap in the range data, going down the array, so fill in missing data
          // the value used is the intermediate value
          int16_t zDiff2 = (RangeData[RangeDataDPL] - RangeData[RangeDataDP])/2;
          for (int zP = RangeDataDP+1;zP < RangeDataDPL;zP++) {
            RangeData[zP] = RangeData[RangeDataDP] + zDiff2;
          }
        } RangeDataDPL = RangeDataDP; // remember the latest array angle
        // calculate the intermeasurement period
        LTOFperiod = LTOFt1 - LTOFt0; LTOFt0 = LTOFt1;
        // calculate the fps frequency
        LTOFfps = 1000/LTOFperiod;
        VL53L1X_Task = 8; // look for another measurement
        if (LTOFcalc) {VL53L1X_Task = 9;}
  //        else {Serial.println("0,330," + String(RangeRawF) + "," + String(Range));}
      } break;
    case 9: // speed calculation is enabled
      // due to noise on the sensor we cannot measure low speed values, and therefore
      // set s threshold of 10mm which is equivalent to 300mm/s minimum
      RangeDiff = abs(RangeRaw - RangeLast); // get the difference
      if (RangeDiff > 10) {
        // movement is > 10mm so calculate speed
        // we apply a peak detector, with 1 sec sustain window
        LTOFspd10 = (RangeDiff * 10)/LTOFperiod;
        if (LTOFspd10 > LTOFspd10L) {LTOFspd10L = LTOFspd10; LTOFspdCnt = 30;}
        else {LTOFspd10 = LTOFspd10L;}
      }
      if (LTOFspdCnt > 0) {
        LTOFspdCnt--;
        if (LTOFspdCnt < 1) {LTOFspd10 = 0; LTOFspd10L = 0;}
      }
  //      Serial.println("0,1000," + String(Range) + "," + String(LTOFspd10));
      VL53L1X_Task = 8; // look for another measurement
      break;

    case 1: // switch on sensor
      digitalWrite(XSHUT_PIN,HIGH);  // enable VL53L1X sensor
      VL53L1X_Timer = micros(); 
  //      Serial.println("VL53L1X XSHUT HIGH");
        VL53L1X_Task++; break;
    case 2: // check for boot, will time-out after 10ms if sensor does not respond
  //      t0 = micros();
      if (VL53L1X.checkBootState()) {
  //        t0 = micros() - t0;
        VL53L1X_Timer = micros() - VL53L1X_Timer;
  //        Serial.println("VL53L1X.checkBootState() in " + String(t0) + "µs");
        // 825µs needed to boot the device from XSHUT going high
  //        Serial.println("VL53L1X Booted! in " + String(VL53L1X_Timer) + "µs");
        VL53L1X_Task++;
      }
      else if (micros() >= (VL53L1X_Timer + 10000)) {VL53L1X_Task = 99;} // time-out
      break;
    case 3: // device booted, so initialise
  //      t0 = micros();
      if (VL53L1X.init() == false) {
  //        t0 = micros() - t0;
  //        Serial.println("VL53L1X.init() in " + String(t0) + "µs"); // 151ms
        VL53L1X_Task++;
      } else {VL53L1X_Task = 99;} // failed to initialise, so turn sensor OFF
      break;
    case 4: // set the distance mode
  //      t0 = micros();
      VL53L1X.setDistanceModeShort();         // Sets to 1.3M range
  //      t0 = micros() - t0;
  //      Serial.println("VL53L1X.setDistanceModeShort() in " + String(t0) + "µs");
      VL53L1X_Task++; break;
    case 5: // set the timing budget as 33ms
  //      t0 = micros();
      VL53L1X.setTimingBudgetInMs(33);        // Set timing budget to 33 ms
  //      t0 = micros() - t0;
  //      Serial.println("VL53L1X.setTimingBudgetInMs(33) in " + String(t0) + "µs");
      VL53L1X_Task++; break;
    case 6: // set the distance mode
  //      t0 = micros();
      VL53L1X.setIntermeasurementPeriod(33);  // Set measurement period in ms
  //      t0 = micros() - t0;
  //      Serial.println("VL53L1X.setIntermeasurementPeriod(33) in " + String(t0) + "µs");
      RangeEn = true; VL53L1X_Task++; break;
    case 7: // initiate ranging
  //      t0 = micros();
      VL53L1X.startRanging();                 // 446µs Starts taking measurements
  //      t0 = micros() - t0;
  //      Serial.println("VL53L1X.startRanging() in " + String(t0) + "µs");
  //      t1 = millis();
      VL53L1X_Skip = millis() + VL53L1X_SkipDel; // set the initial delay
      LTOFt0 = millis(); VL53L1X_Task++; break;  // task 8 is earlier in this code
      
    case 99: // a time-out has occured or we want to turn the sensor OFF
      digitalWrite(XSHUT_PIN,LOW);  // disable VL53L1X sensor
  //      Serial.println("VL53L1X XSHUT LOW");
      RangeEn = false; VL53L1X_Task = 0; break;
  }
}

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

void WaitSwUp() {
  // waits here until both switches are released
  while ((digitalRead(sw0Pin) == LOW) || (digitalRead(sw1Pin) == LOW)) {yield();}
}

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



  //###############################################################################
  //
  //  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; C_Dn = 8; Z_Dn = 8;
}

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

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
  Serial.println("WiFiDisconnected!");
}

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

void WiFiInit_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
  //  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
    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) {
    Serial.println("Error initializing ESP-NOW");
    return;
  }
  ESP_NOW_Init = true;
  
  // change the broadcast address pointer each time
  ESP_NOW_BA++; if (ESP_NOW_BA > 1) {ESP_NOW_BA = 0;}
  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;
  }

  // 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){ 
    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 WiFiReadRx() {
  // 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
  //  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') {WiiType = 'C'; Rx_Task = 1; break;} // Classic
      else if (Rx_Buff.ESPdata[Rx_Pnt] == 'P') {WiiType = 'P'; Rx_Task = 1; break;} // Classic Pro
      else if (Rx_Buff.ESPdata[Rx_Pnt] == 'N') {WiiType = 'N'; Rx_Task = 1; break;} // Nunchuk
      else if (Rx_Buff.ESPdata[Rx_Pnt] == '-') {WiiType = '-'; 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
        RxWiFi[0] = Rx_Buff.ESPdata[1];
        RxWiFi[1] = Rx_Buff.ESPdata[2];
        RxWiFi[2] = Rx_Buff.ESPdata[3];
        RxWiFi[3] = Rx_Buff.ESPdata[4];
        RxWiFi[4] = Rx_Buff.ESPdata[5];
        RxWiFi[5] = Rx_Buff.ESPdata[6];

        // if Nunchuk data strip off the JX,JY and CZ values
        RxRec = true; // indicate a valid frame
        if (WiiType == 'N') {           // Wii Nunchuk
          JoyX = RxWiFi[0];             // joystick X 0-255
          JoyY = RxWiFi[1];             // joystick Y 0-255
          CZ = RxWiFi[5] & 3;           // CZ buttons, C = 2/0, Z = 1/0, active low
        } else if (WiiType == 'C') {    // Wii Classic
          JoyX = WiiRightStickX() * 8;  // joystick X 0-255
          JoyY = WiiRightStickY() * 8;  // joystick Y 0-255
          CZ = ((RxWiFi[4] & 0b100000)>>5) + (RxWiFi[4] & 0b10); // read LT and RT buttons as C,Z
        } else if (WiiType == 'P') {    // Wii Classic Pro
          JoyX = WiiRightStickX() * 8;  // joystick X 0-255
          JoyY = WiiRightStickY() * 8;  // joystick Y 0-255
          CZ = (RxWiFi[4] & 0b10) + ((RxWiFi[5] & 0b100)>2); // read RT and ZR buttons as C,Z
        } else {
          // not Nunchuk so set defaults
          JoyX = 128; JoyY = 128; CZ = 3;
        }
      } else {
        WiFiRxErr++;
      }
      Rx_Task = 0;  // reset the task pointer to lok 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 WiFiRxGetChecksum(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 WiFiTryToConnect() {
  // try to connect with the Wii Transceiver by sending devices name
  // initialise ESP-NOW link first
  WiFiInit_ESP_NOW();

  Tx_Buff.ESPdata[0] = 'B';
  Tx_Buff.ESPdata[1] = 'a';
  Tx_Buff.ESPdata[2] = 'l';
  Tx_Buff.ESPdata[3] = 'B';
  Tx_Buff.ESPdata[4] = 'o';
  Tx_Buff.ESPdata[5] = 't';
  Tx_Buff.ESPdata[6] = ' ';
  Tx_Buff.ESPdata[7] = 'M';
  Tx_Buff.ESPdata[8] = 'k';
  Tx_Buff.ESPdata[9] = '1';
  WiFiTx_len = 10;
  //  t0 = micros();
  esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *) &Tx_Buff, WiFiTx_len);
  if (result == ESP_OK) {
  //    Serial.println("Sent with success");
  } else {
  //    Serial.println("Error sending the data");
  } 

  WiFiTryNum++; // count the total number of attempts
  //  Serial.println("WiFiTryToConnect...");
}

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

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];
  }
}

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

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

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

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

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