/*
 * ESP32 DOIT Dev Kit v1 EZ-B Firmware for Synthiam (c)2021
 * 
 * https://synthiam.com/Support/Hardware/DOIT-Esp32-DevKit-v1
 * 
 */

  // Uncomment the next line to use pin monitor
//#include <gpio_viewer.h> // Must me the first include in your project
#include <WiFi.h>
#include "analogWrite.h"
#include <WiFiAP.h>
#include "ESP32Servo.h"
#include "RoboClaw.h"
#include <stdlib.h>

// The WiFi mode for the ESP32 module
// Uncomment: Access Point Mode (configure AP mode settings below)
// Commented: Client Mode (configure client credentials below)
//#define AP_MODE

// AP Mode Settings
// This requires #define AP_MODE to be uncommented
// AP mode means you can connect to the ESP32 as if it's a router or server
// *note: For an open system with no password, leave the password field empty
#define WIFI_AP_SSID "GetOffMyLawn"
#define WIFI_AP_PWD  ""

// Client Mode Credentials
// This requires #define AP_MODE to be commented
// Client mode means the ESP32 will connect to your router and you will have to know its IP Address
// Or, you can check the USB serial debug output
#define WIFI_CLIENT_SSID "JsaleLinsys"
#define WIFI_CLIENT_PWD  "24B2i9LZQkP8Uhj"

// The first digital port that is usable on this controller
#define _PortStart 0

// The last digital port on this controller that is usable
#define _PortCnt 23

// The number of servos that can be active at one time
// This means the only first 16 ports can have a servo connected
#define _ServoCnt 16

// The number of analog ports
#define _AnalogPorts 6

// The firmware version that is reported to ARC to notify of capabilities
#define _FIRMWARE_ID 0x0000000F

// The primary communication interface between ARC and this controller
// This assists the WiFi TCP Server with a circular buffer of incoming data so the
// wifi driver doesn't have to hold the back data, and so the TCP stack doesn't have
// to throttle packets from the client
#define COMMUNICATION_PORT _CLIENT

// The amount of RX buffer on the communication interface for ARC
#define _BUFFER_SIZE 16000

// The amount of RX buffer on the Serial 1 (ttl) port
// This is because the default input buffer on the Serial2 is only
// 60 bytes or something... so this buffer will store RX data until
// it's queried by ARC. This emulates a DMA because arduino
// doesn't have one.
#define _BUFFER_SIZE1 16000

// Pin mode for the servos so we know if a servo has been assigned to a pin
#define SERVO 99

// Pin for homing switch
#define HOMESW     5

// length of parmeter string in commands that use string parms
#define MAXPARMLEN  50+1

// encoder count positions
#define MAX_ENC_CT    2250
#define FACE_FORWARD  1125
#define FACE_RIGHT    1687 
#define FACE_BACKWARD 0       
#define FACE_LEFT     562      

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

WiFiServer   _SERVER(23);
WiFiClient   _CLIENT;
Servo        _Servos[_ServoCnt];
int          _PIN_MODES[_PortCnt];

// EZB COMM BUFFER
byte         _INPUT_BUFFER[_BUFFER_SIZE];
unsigned int _WRITE_POSITION = 0;
unsigned int _READ_POSITION = 0;

// Serial2 TTL BUFFER
byte         _INPUT_BUFFER1[_BUFFER_SIZE1];
unsigned int _WRITE_POSITION1 = 0;
unsigned int _READ_POSITION1 = 0;
bool         _SERIAL_1_INITIALIZED = false;

// GPIO Viewer object
// Uncomment the next line to use pin monitor
//GPIOViewer gpio_viewer;

// RoboClaw interface object
RoboClaw roboclaw = RoboClaw(&Serial2,10000);


int _BAUD_RATES [] = {
  4800,
  9600,
  19200,
  38400,
  57600,
  115200,
  115200
};

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

#define CmdUnknown            0
#define CmdReleaseAllServos   1
#define CmdGetUniqueID        2
#define CmdEZBv3              3
#define CmdEZBv4              4
#define CmdSoundBeep          5
#define CmdEZServo            6
#define CmdI2CWrite           10
#define CmdI2CRead            11
#define CmdBootLoader         14
#define CmdSetPWMSpeed        15
#define CmdSetServoSpeed      39
#define CmdPing               0x55
#define CmdSetDigitalPortOn   100
#define CmdSetDigitalPortOff  124
#define CmdGetDigitalPort     148
#define CmdSetServoPosition   172
#define CmdGetADCValue        196
#define CmdSendSerial         204
#define CmdHC_SR04            228
#define CmdGetFirwareID       253
#define CmdSoundStreamCmd     254

// CmdEZBv4 Commands
// ----------------------------------------------------------------------------------
#define CmdV4SetLipoBatteryProtectionState 0
#define CmdV4SetBatteryMonitorVoltage      1
#define CmdV4GetBatteryVoltage             2
#define CmdV4GetCPUTemp                    3

#define CmdV4UARTExpansion0Init            5
#define CmdV4UARTExpansion0Write           6
#define CmdV4UARTExpansion0AvailableBytes  7
#define CmdV4UARTExpansion0Read            8

#define CmdV4UARTExpansion1Init            9
#define CmdV4UARTExpansion1Write           10
#define CmdV4UARTExpansion1AvailableBytes  11
#define CmdV4UARTExpansion1Read            12

#define CmdV4UARTExpansion2Init            13
#define CmdV4UARTExpansion2Write           14
#define CmdV4UARTExpansion2AvailableBytes  15
#define CmdV4UARTExpansion2Read            16

#define CmdV4I2CClockSpeed                 17
#define CmdV4UARTClockSpeed                18
#define CmdV4ResetToDefaults               19

// RoboClaw Commands
// ----------------------------------------------------------------------------------
#define CW                                 0  // clockwise
#define CCW                                1  // counter clockwise
#define RoboclawBegin                      50 // calls roboclaw.begin
#define RotationStop                       51 // calls roboclaw.ForwardM2 with zero speed
#define SpeedAccel                         52 // calls roboclaw.SpeedAccelM2
#define RotateCounterClockwise             53 // calls roboclaw.BackwardM2
#define SpeedAccelDeccelPosition           54 // calls roboclaw.SpeedAccelDeccelPositionM2
#define SignedSpeedAccelDistance          55
#define FaceBackward                       56
#define FaceLeft                           57
#define HeadShake                          58
#define GoHome                             59
#define ReadEncoder                        61
#define ResetEncoder                       62
#define AdjEncoder                         63
#define RoboTest                           70

// CmdSoundStreamCmd Commands
// ----------------------------------------------------------------------------------
#define CmdSoundInitStop 0
#define CmdSoundLoad     1
#define CmdSoundPlay     2

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

void setup() {

  Serial.begin(115200);

  // Uncomment the next line to use pin monitor
  //gpio_viewer.connectToWifi("JsaleLinsys", "24B2i9LZQkP8Uhj");
  // gpio_viewer.setPort(5555);                 // You can set the http port, if not set default port is 8080

  WiFi.onEvent(WiFiEvent);

#ifdef AP_MODE
  Serial.println("Configuring access point WiFi mode...");

  Serial.print("SSID: ");
  Serial.println(WIFI_AP_SSID);
  Serial.print("PWD: ");
  Serial.println(WIFI_AP_PWD);

  WiFi.softAP(WIFI_AP_SSID, WIFI_AP_PWD);

  // Wait for WiFi to start
  delay(500);

  IPAddress Ip(192, 168, 40, 1);
  IPAddress NMask(255, 255, 255, 0);
  WiFi.softAPConfig(Ip, Ip, NMask);

  IPAddress myIP = WiFi.softAPIP();
  Serial.print("AP IP address: ");
  Serial.println(myIP);
#else
  Serial.println("Configuring client WiFi mode...");

  WiFi.begin(WIFI_CLIENT_SSID, WIFI_CLIENT_PWD);

  while (WiFi.status() != WL_CONNECTED) {

    delay(1000);

    Serial.print("Connecting to WiFi network ");
    Serial.print(WIFI_CLIENT_SSID);
    Serial.println("...");
  }

  Serial.println("Connected to the WiFi network!");
  Serial.print("My IP address: ");
  Serial.println(WiFi.localIP());
#endif

  _SERVER.begin();
  _SERVER.setNoDelay(true);

  Serial.println("Server started... Waiting for client");

  // The communication status LED
  pinMode(2, OUTPUT);

  // initial the magnetic switch port
  Serial.print("Initializing HOMESW pin:");
  Serial.println(translateaDigitalPort(HOMESW));
  pinMode(translateaDigitalPort(HOMESW), INPUT_PULLDOWN);
  digitalRead(translateaDigitalPort(HOMESW));   // initializes port

  // Must be at the end of your setup
  // You can set the sampling interval in ms, if not set default is 100ms
  // gpio_viewer.setSamplingInterval(25); 
  // Uncomment the next line to use pin monitor
  //gpio_viewer.begin();

}

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

// Reset function at address 0
void(* resetFunc) (void) = 0;

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

// void calibrateEnc() - adjusts encorder to be within min-max rage
void calibrateEnc (void)
{
  int32_t encoder   = 0;

  delay(100);
  encoder = roboclaw.ReadEncM2(0x80);
  Serial.print("calibrate encoder reading:");
  Serial.println(encoder);
  if (encoder > MAX_ENC_CT)
  {
    while (encoder > MAX_ENC_CT)
    {
      encoder = encoder - MAX_ENC_CT;
    }
    roboclaw.SetEncM2(0x80,encoder);
  }
  else if (encoder < 0)
  {
    while (encoder < 0)
    {
      encoder = encoder + MAX_ENC_CT;
    }
    roboclaw.SetEncM2(0x80,encoder);
  }
  delay(100);
  Serial.print("Final encoder reading:");
  Serial.println(encoder);
  roboclaw.SpeedAccelDeccelPositionM2(0x80, 0, 0, 0, encoder, 0);
}

// void readRoboParms()
//    Reads a string parameters from EZB RoboClaw command
//    and returns the string
void readRoboParm  (char* parms)
{
  int x = 0;

  while (IsAvail() && x < MAXPARMLEN)
  {
    *parms = ReadByte();
    parms++;
  }
  *parms = '\0';
}

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

// void moveToHome() - moves waist motor until home switch is detected
void moveToHome ( void )
{
  int32_t position = 0;
  int16_t speed    = 20;
  Serial.println("moving HOME at speed");

  if ( digitalRead(translateaDigitalPort(HOMESW)) == LOW)   // if sensor reads LOW it has not detected stop 
  {
    position = roboclaw.ReadEncM2(0x80);
    Serial.print("Currently NOT HOME, current position is  ");
    Serial.print(position);

    Serial.print(", rotating CW at speed ");
    Serial.println(speed);
    roboclaw.ForwardM2(0x80, speed);      
    do {  
      // keep moving until stop is detected or limit reached
      position = roboclaw.ReadEncM2(0x80);
    } while ( digitalRead(translateaDigitalPort(HOMESW)) == LOW );

    roboclaw.ForwardM2(0x80, 0);  // stop moving
    Serial.print("stopped at: ");
    position = roboclaw.ReadEncM2(0x80);
    Serial.println(position);

    delay(250);
    Serial.println("adjusting position in CW direction");
    position = position + 55;
    roboclaw.SpeedAccelDeccelPositionM2(0x80, 0, 200, 0, position, 0);

    Serial.println("Now we are HOME - Hello there!");

  }
  else
  {
    Serial.println("Currently at *** HOME ***");
  }

  delay(1000);
  roboclaw.SetEncM2(0x80, FACE_FORWARD);
}

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

void WiFiEvent(WiFiEvent_t event) {

  switch (event) {
    case SYSTEM_EVENT_WIFI_READY:
      Serial.println("WiFi interface ready");
      break;
    case SYSTEM_EVENT_SCAN_DONE:
      Serial.println("Completed scan for access points");
      break;
    case SYSTEM_EVENT_STA_START:
      Serial.println("WiFi client started");
      break;
    case SYSTEM_EVENT_STA_STOP:
      Serial.println("WiFi clients stopped");
      break;
    case SYSTEM_EVENT_STA_CONNECTED:
      Serial.println("Connected to access point");
      break;
    case SYSTEM_EVENT_STA_DISCONNECTED:
      Serial.println("Disconnected from WiFi access point");
      break;
    case SYSTEM_EVENT_STA_AUTHMODE_CHANGE:
      Serial.println("Authentication mode of access point has changed");
      break;
    case SYSTEM_EVENT_STA_GOT_IP:
      Serial.print("Obtained IP address: ");
      Serial.println(WiFi.localIP());
      break;
    case SYSTEM_EVENT_STA_LOST_IP:
      Serial.println("Lost IP address and IP address is reset to 0");
      break;
    case SYSTEM_EVENT_STA_WPS_ER_SUCCESS:
      Serial.println("WiFi Protected Setup (WPS): succeeded in enrollee mode");
      break;
    case SYSTEM_EVENT_STA_WPS_ER_FAILED:
      Serial.println("WiFi Protected Setup (WPS): failed in enrollee mode");
      break;
    case SYSTEM_EVENT_STA_WPS_ER_TIMEOUT:
      Serial.println("WiFi Protected Setup (WPS): timeout in enrollee mode");
      break;
    case SYSTEM_EVENT_STA_WPS_ER_PIN:
      Serial.println("WiFi Protected Setup (WPS): pin code in enrollee mode");
      break;
    case SYSTEM_EVENT_AP_START:
      Serial.println("WiFi access point started");
      break;
    case SYSTEM_EVENT_AP_STOP:
      Serial.println("WiFi access point  stopped");
      break;
    case SYSTEM_EVENT_AP_STACONNECTED:
      Serial.println("Client connected");
      break;
    case SYSTEM_EVENT_AP_STADISCONNECTED:
      Serial.println("Client disconnected");
      break;
    case SYSTEM_EVENT_AP_STAIPASSIGNED:
      Serial.println("Assigned IP address to client");
      break;
    case SYSTEM_EVENT_AP_PROBEREQRECVED:
      Serial.println("Received probe request");
      break;
    case SYSTEM_EVENT_GOT_IP6:
      Serial.println("IPv6 is preferred");
      break;
    case SYSTEM_EVENT_ETH_START:
      Serial.println("Ethernet started");
      break;
    case SYSTEM_EVENT_ETH_STOP:
      Serial.println("Ethernet stopped");
      break;
    case SYSTEM_EVENT_ETH_CONNECTED:
      Serial.println("Ethernet connected");
      break;
    case SYSTEM_EVENT_ETH_DISCONNECTED:
      Serial.println("Ethernet disconnected");
      break;
    case SYSTEM_EVENT_ETH_GOT_IP:
      Serial.println("Obtained IP address");
      break;
  }
}

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

void loop() {

#ifndef AP_MODE
  if (WiFi.status() != WL_CONNECTED)
    resetFunc();
#endif

  if (_SERVER.hasClient()) {

    // reset the input buffer
    _WRITE_POSITION = 0;
    _READ_POSITION = 0;

    _CLIENT = _SERVER.available();

    Serial.print("Client connected from ");
    Serial.println(_CLIENT.remoteIP());


    Serial.print(__FILE__ );
    Serial.print(" ");
    Serial.println(__DATE__);

    while (_CLIENT.connected())
      doEZProtocol();

    Serial.println("Client disconnected");

    _CLIENT.stop();

    Serial.println("Waiting for client...");
  }
}

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

// For EZB Comm
bool IsAvail() {

  return _WRITE_POSITION != _READ_POSITION || COMMUNICATION_PORT.available();
}

// For Serial2
bool IsAvail1() {

  return _WRITE_POSITION1 != _READ_POSITION1 || Serial2.available();
}

// For EZB Com
int Avail() {

  if (_READ_POSITION > _WRITE_POSITION)
    return (_BUFFER_SIZE - _READ_POSITION) + _WRITE_POSITION + COMMUNICATION_PORT.available();

  return (_WRITE_POSITION - _READ_POSITION) + COMMUNICATION_PORT.available();
}

// For Serial2
int Avail1() {

  if (_READ_POSITION1 > _WRITE_POSITION1)
    return (_BUFFER_SIZE1 - _READ_POSITION1) + _WRITE_POSITION1 + Serial2.available();

  return (_WRITE_POSITION1 - _READ_POSITION1) + Serial2.available();
}

// For EZB Com
long Read32() {

  return (long)ReadByte() | ((long)ReadByte() << 8) | ((long)ReadByte() << 16) | ((long)ReadByte() << 24);
}

// For Serial2
long Read321() {

  return ReadByte1() | ((long)ReadByte1() << 8) | ((long)ReadByte1() << 16) | ((long)ReadByte1() << 24);
}

// For EZB Com
uint16_t Read16() {

  return (long)ReadByte() | ((long)ReadByte() << 8);
}

// For Serial2
uint16_t Read161() {

  return (long)ReadByte1() | ((long)ReadByte1() << 8);
}

// For EZB Com
byte ReadByte() {

  while (_WRITE_POSITION == _READ_POSITION && COMMUNICATION_PORT.available() == 0)
    delay(1);

  while (COMMUNICATION_PORT.available()) {

    _WRITE_POSITION++;

    _INPUT_BUFFER[_WRITE_POSITION % _BUFFER_SIZE] = COMMUNICATION_PORT.read();
  }

  _READ_POSITION++;

  return _INPUT_BUFFER[_READ_POSITION % _BUFFER_SIZE];
}

// For Serial2
byte ReadByte1() {

  while (_WRITE_POSITION1 == _READ_POSITION1 && Serial2.available() == 0)
    delay(1);

  while (Serial2.available()) {

    _WRITE_POSITION1++;

    _INPUT_BUFFER1[_WRITE_POSITION1 % _BUFFER_SIZE1] = Serial2.read();
  }

  _READ_POSITION1++;

  return _INPUT_BUFFER1[_READ_POSITION1 % _BUFFER_SIZE1];
}

// For Serial2
void UpdateBuffer1() {

  if (!_SERIAL_1_INITIALIZED)
    return;

  while (Serial2.available()) {

    _WRITE_POSITION1++;

    _INPUT_BUFFER1[_WRITE_POSITION1 % _BUFFER_SIZE1] = Serial2.read();
  }
}

void Write32(long val) {

  COMMUNICATION_PORT.write((byte)(val & 0xff));
  COMMUNICATION_PORT.write((byte)((val >> 8) & 0xff));
  COMMUNICATION_PORT.write((byte)((val >> 16) & 0xff));
  COMMUNICATION_PORT.write((byte)((val >> 24) & 0xff));
}

void Write16(int val) {

  COMMUNICATION_PORT.write((byte)(val & 0xff));
  COMMUNICATION_PORT.write((byte)((val >> 8) & 0xff));
}

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

// Get the pinmode of the specified ARC pin index(i.e. 0-23) from the array
int getPinMode(int port) {

  return _PIN_MODES[port];
}

// Set the pinmode of the specified ARC pin index (i.e. 0-23) and store the setting in array
// This lets us determine the pin mode so we don't have to continue setting it
void setPinMode(int port, int mode) {

  _PIN_MODES[port] = mode;

  if (mode != SERVO)
    pinMode(translateaDigitalPort(port), mode);
}

// Translate the EZ-Buider port to the gpio of the esp32 since it doesn't use friendly names
byte translateaADCPort(byte port) {

  switch (port) {
    case 0:
      return 36;
    case 1:
      return 39;
    case 2:
      return 34;
    case 3:
      return 35;
    case 4:
      return 32;
    case 5:
      return 33;
  }

  return 36;
}

// Translate the EZ-Buider port to the gpio of the esp32 since it doesn't use friendly names
byte translateaDigitalPort(byte port) {

  switch (port) {
    case 0:
      return 25;
    case 1:
      return 26;
    case 2:
      return 27;
    case 3:
      return 14;
    case 4:
      return 12;
    case 5:
      return 13;
    case 6:
      return 23;
    case 7:
      return 22;
    case 8:
      return 21;
    case 9:
      return 19;
    case 10:
      return 18;
    case 11:
      return 5;
    case 12:   
        // TX on ESP32    
//      return 17 or 1 or 35;  
      return 1;
    case 13:
        // RX on ESP32    
//      return 16 or 3 or 34;
      return 3;
    case 14:
      return 4;
    case 15:
      return 15;
    case 16:
      return 0;
  }

  return 25;
}
// -----------------------------------------------------------------------------------------------------------------------------------------------------------

void doEZProtocol() {

  UpdateBuffer1();

  digitalWrite(2, LOW);

  if (!IsAvail())
    return;

  byte cmd = ReadByte();

  digitalWrite(2, HIGH);

  Serial.print("EZB command type:" );
  Serial.println(cmd);

  if (cmd == CmdPing) {

    // return as a "Capability Controller"
    COMMUNICATION_PORT.write(222);
  } else if (cmd == CmdGetFirwareID) {

    Write32(_FIRMWARE_ID);
  } else if (cmd == CmdReleaseAllServos) {

    for (int port = _PortStart; port < _ServoCnt; port++)
      if (_Servos[port].attached())
        _Servos[port].detach();
  } else if (cmd >= CmdSetServoPosition && cmd <= CmdSetServoPosition + 23) {

    byte port = cmd - CmdSetServoPosition;
    byte pos = ReadByte();

    if (port >= _PortStart && port < _ServoCnt) {

      if (getPinMode(port) != SERVO)
        setPinMode(port, SERVO);

      if (pos == 0 && _Servos[port].attached()) {

        _Servos[port].detach();
      } else {

        if (!_Servos[port].attached())
          _Servos[port].attach(translateaDigitalPort(port));

        _Servos[port].write(pos);
      }
    }
  } else if (cmd >= CmdSetPWMSpeed && cmd <= CmdSetPWMSpeed + 23) {

    byte port = cmd - CmdSetPWMSpeed;
    byte pos = ReadByte();

    if (port >= _PortStart && port < _PortCnt) {

      if (port < _ServoCnt && _Servos[port].attached())
        _Servos[port].detach();

      if (getPinMode(port) != OUTPUT)
        setPinMode(port, OUTPUT);

      analogWrite((uint8_t) translateaDigitalPort(port), (uint16_t) map(pos, 0, 100, 0, 255));
    }
  } else if (cmd >= CmdSetDigitalPortOn && cmd <= CmdSetDigitalPortOn + 23) {

    byte port = cmd - CmdSetDigitalPortOn;

    if (port >= _PortStart && port < _PortCnt) {

      if (port < _ServoCnt && _Servos[port].attached())
        _Servos[port].detach();

      if (getPinMode(port) != OUTPUT)
        setPinMode(port, OUTPUT);

      digitalWrite(translateaDigitalPort(port), HIGH);
    }
  } else if (cmd >= CmdSetDigitalPortOff && cmd <= CmdSetDigitalPortOff + 23) {

    byte port = cmd - CmdSetDigitalPortOff;

    if (port >= _PortStart && port < _PortCnt) {

      if (port < _ServoCnt && _Servos[port].attached())
        _Servos[port].detach();

      if (getPinMode(port) != OUTPUT)
        setPinMode(port, OUTPUT);

      digitalWrite(translateaDigitalPort(port), LOW);
    }
  } else if (cmd >= CmdGetDigitalPort && cmd <= CmdGetDigitalPort + 23) {

    byte port = cmd - CmdGetDigitalPort;
    Serial.print("Setting pin ");
    Serial.print(port);
    Serial.println(" to INPUT and doing a digital read.");

    if (port >= _PortStart && port < _PortCnt) {

      if (port < _ServoCnt && _Servos[port].attached())
        _Servos[port].detach();

      if (getPinMode(port) != INPUT)
        setPinMode(port, INPUT);

      COMMUNICATION_PORT.write(digitalRead(translateaDigitalPort(port)));
    } else {

      COMMUNICATION_PORT.write((byte)0);
    }

    COMMUNICATION_PORT.flush();
  } else if (cmd >= CmdGetADCValue && cmd <= CmdGetADCValue + 7) {

    byte port = cmd - CmdGetADCValue;

    port = translateaADCPort(port);

    Write16(analogRead(port));

    COMMUNICATION_PORT.flush();
  } else if (cmd == CmdEZBv4) {

    byte cmd2 = ReadByte();
    Serial.print("EZBv4 command:" );
    Serial.println(cmd2);

    // ------------ UART #0 (i.e. Serial2)
    if (cmd2 == CmdV4UARTExpansion0Init) {

      // reset the input buffer
      _WRITE_POSITION1 = 0;
      _READ_POSITION1 = 0;

      if (_Servos[12].attached())
        _Servos[12].detach();

      if (_Servos[13].attached())
        _Servos[13].detach();

      Serial2.begin(Read32());

      _SERIAL_1_INITIALIZED = true;
      Serial.println("Serial (USB->ESP32 to Roboclaw) initialized");
    } else if (cmd2 == CmdV4UARTExpansion0Write) {

      char roboCmd       = ' ';
      uint32_t accel     = 0;
      uint32_t deccel    = 0;
      uint32_t speed     = 0;
      int32_t unsigedspeed = 0;
      uint32_t position  = 0;
      uint32_t distance  = 0;
      uint8_t flag       = 0;
      int32_t encoder    = 0;
      uint8_t status     = 0;
      bool valid         = 0;
      uint16_t length    = Read16();
      char buf[50]       = " ";
      char encoderVal[10] = " ";
      char parms[MAXPARMLEN+1] = "";

      roboCmd = ReadByte();

      switch (roboCmd)
      {
        case RoboclawBegin:
          Serial.println("Initializing RoboClaw");
          roboclaw.begin(115200);
          delay(10);
          moveToHome();
          COMMUNICATION_PORT.flush();
          break;

        case RotationStop:
          Serial.println("RoboClaw Stop");
//          roboclaw.ForwardM2(0x80, 0); 
          speed = roboclaw.ReadSpeedM2(0x80, &status, &valid);
          if (speed > 2000)
          {
            roboclaw.SpeedAccelM2(0x80, 5100, 0);
          }
          else if(speed < 1500)
          {
            roboclaw.SpeedAccelM2(0x80, 3000, 0);
          }
          else
          {
            roboclaw.SpeedAccelM2(0x80, 0, 0);
          }
          calibrateEnc();
          break;

        case SpeedAccel:
          readRoboParm(parms);
          Serial.print("parms=<"); 
          Serial.printf(parms); 
          Serial.println(">");
          sscanf(parms, "%ld%ld", &accel, &unsigedspeed);
          Serial.print("RoboClaw SpeedAccel<");
          Serial.print(unsigedspeed);
          Serial.print(">  accel<");
          Serial.print(accel);
          Serial.println(">");
          roboclaw.SpeedAccelM2(0x80, accel, unsigedspeed);
          break;

        case RotateCounterClockwise:
          speed = ReadByte();
          accel = ReadByte();
          accel = accel * 10;
          Serial.print("RoboClaw cmd Counter Clockwise at Speed:");
          Serial.println(speed);
//          roboclaw.BackwardM2(0x80, speed);
          roboclaw.SpeedAccelM2(0x80, accel, speed*-35);
          break;

        case SpeedAccelDeccelPosition:
          readRoboParm(parms);
          Serial.print("parms=<"); 
          Serial.printf(parms); 
          Serial.println(">");
          sscanf(parms, "%ld%ld%ld%ld%ld", &accel, &speed, &deccel, &position, &flag);
          Serial.print("accel = <");
          Serial.print(accel);
          Serial.print(">  speed = <");
          Serial.print(speed);
          Serial.print(">  deccel = <");
          Serial.print(deccel);
          Serial.print(">  position = <");
          Serial.print(position);
          Serial.print(">  flag = <");
          Serial.print(flag);
          Serial.println(">");

          encoder = roboclaw.ReadEncM2(0x80);
          Serial.print("encoder = <");
          Serial.print(encoder);
          Serial.println(">");
          if (encoder == position)
          {
            Serial.println("Already there - not moving.");
          }
          else //if (encoder < position)
          {
            Serial.println("Moving.");
            roboclaw.SpeedAccelDeccelPositionM2(0x80, accel, speed, deccel, position, flag);
            while (roboclaw.ReadEncM2(0x80) != position)
              delay(500);
            roboclaw.ForwardM2(0x80, 0); 
            Serial.println("Stopped.");
          }
          break;

        case SignedSpeedAccelDistance:
          readRoboParm(parms);
          Serial.print("parms=<"); 
          Serial.printf(parms); 
          Serial.println(">");
          sscanf(parms, "%ld%ld%ld%ld", &accel, &unsigedspeed, &distance, &flag);
          Serial.print("RoboClaw cmd Signed Speed Accel Dist: ");
          Serial.print("Speed<");
          Serial.print(unsigedspeed);
          Serial.print("> Accel<");
          Serial.print(accel);
          Serial.print("> Dist<");
          Serial.print(distance);
          Serial.println(">");
          roboclaw.SpeedAccelDistanceM2(0x80, accel, unsigedspeed, distance, flag);
          break;

        case FaceBackward:
          speed = ReadByte();
          accel = ReadByte();
          accel = accel * 10;
          Serial.print("RoboClaw cmd Face Backward at Speed:");
          Serial.println(speed);
          encoder = roboclaw.ReadEncM2(0x80);
          roboclaw.SpeedAccelDeccelPositionM2(0x80, accel, speed*35, accel, FACE_BACKWARD, 1);
          break;

        case FaceLeft:
          speed = ReadByte();
          accel = ReadByte();
          accel = accel * 10;
          Serial.print("RoboClaw cmd Face Left at Speed:");
          Serial.println(speed);
          encoder = roboclaw.ReadEncM2(0x80);
          if ( encoder == FACE_FORWARD )
          {
            roboclaw.SetEncM2(0x80, MAX_ENC_CT-1);
            roboclaw.SpeedAccelDeccelPositionM2(0x80, accel, speed*35, accel, FACE_LEFT+1, 0);
            while (roboclaw.ReadEncM2(0x80) != FACE_LEFT+1)
              delay(200);
            roboclaw.SetEncM2(0x80, FACE_LEFT);
          }
          else
          {
            roboclaw.SpeedAccelDeccelPositionM2(0x80, accel, speed*35, accel, FACE_LEFT, 1);
          }
          break;

        case HeadShake:
          break;    

        case GoHome:
          Serial.println("Executing GoHome.");
          moveToHome();
          break;    
          
        case ReadEncoder:
          position = roboclaw.ReadEncM2(0x80);
          sprintf(buf, "%ld", position);
          Serial.print("Reading encoder value:");
          Serial.print(position);
          Serial.print("  buf:");
          Serial.println(buf);
//          COMMUNICATION_PORT.println(' ' );  // not sure why a pre-write is needed
//          COMMUNICATION_PORT.write(buf[0]);
//          COMMUNICATION_PORT.write(buf[1]);
//          COMMUNICATION_PORT.write(buf[2]);
//          COMMUNICATION_PORT.write(buf[3]);
//          COMMUNICATION_PORT.flush();
          COMMUNICATION_PORT.println("Hello");
//          _SERVER.println("Hello");

          break;    

        case ResetEncoder:
          Serial.println("resetting encoder");
          roboclaw.SetEncM2(0x80, FACE_FORWARD);
          break;    

        case AdjEncoder:
          calibrateEnc();
          break;    

        case RoboTest:
          readRoboParm(parms);
          sscanf(parms, "%ld%ld", &speed, &accel);
          Serial.print("speed = <");
          Serial.print(speed);
          Serial.print(">  accel = <");
          Serial.print(accel);
          Serial.println(">");
          break;    

        default:
          // dump rest of unknown buffer
          Serial.println("Did not receive known RoboClaw command");
          for (uint16_t x = 1; x < length; x++)
            roboCmd = ReadByte();
          break;
      }

      //for (uint16_t x = 0; x < length; x++)
      //  Serial2.write(ReadByte());

      // Flush the output send buffer
      Serial2.flush();
    } else if (cmd2 == CmdV4UARTExpansion0AvailableBytes) {

      Write16(Avail1());
    } else if (cmd2 == CmdV4UARTExpansion0Read) {

      uint16_t length = Read16();

      for (uint16_t x = 0; x < length; x++)
        COMMUNICATION_PORT.write(ReadByte1());
    }

    COMMUNICATION_PORT.flush();
    // ------------ UART #1 (i.e. dummy)
    if (cmd2 == CmdV4UARTExpansion1Init) {

      Read32();
    } else if (cmd2 == CmdV4UARTExpansion1Write) {

      uint16_t length = Read16();

      for (uint16_t x = 0; x < length; x++)
        ReadByte();
    } else if (cmd2 == CmdV4UARTExpansion1AvailableBytes) {

      Write16(0);
    } else if (cmd2 == CmdV4UARTExpansion1Read) {

      uint16_t length = Read16();

      for (uint16_t x = 0; x < length; x++)
        COMMUNICATION_PORT.write((byte)0);
    }

    // ------------ UART #2 (i.e. dummy)
    if (cmd2 == CmdV4UARTExpansion2Init) {

      Read32();
    } else if (cmd2 == CmdV4UARTExpansion2Write) {

      uint16_t length = Read16();

      for (uint16_t x = 0; x < length; x++)
        ReadByte();
    } else if (cmd2 == CmdV4UARTExpansion2AvailableBytes) {

      Write16(0);
    } else if (cmd2 == CmdV4UARTExpansion2Read) {

      uint16_t length = Read16();

      for (uint16_t x = 0; x < length; x++)
        COMMUNICATION_PORT.write((byte)0);
    }
  }
}
