// https://wokwi.com/projects/411112944848391169
//
// 
//
// Added dependency for https://github.com/drf5n/Wokwi-Chip-TB6612FNG
// to elimitate tabs from 
// https://wokwi.com/projects/410323062531374081
// built from https://wokwi.com/projects/410302932215778305
// See https://github.com/drf5n/Wokwi-Chip-TB6612FNG
// and github dependency version 
// https://wokwi.com/projects/410798470781038593
// A combination of a Wokwi Chip-Scope and a simulated TB6612FNG motor
// driver, attached to scopes to see inputs and outputs.
//
// Also a modificaton of:
// https://wokwi.com/projects/409963275966630913
// to use a post-fix/Reverse Polish Notation protocol
// Code from https://www.gammon.com.au/forum/?id=11425&reply=1#reply1
// Other simulations https://forum.arduino.cc/t/wokwi-simulations-for-arduino-built-in-examples/1304754

// See also Nick Gammon's Serial examples
//   Serial Example 1: https://wokwi.com/projects/409963075177910273
//   Serial Example 2: https://wokwi.com/projects/409963275966630913

/*
  Non-Blocking Serial Parsing, RPN-style with switch-case

  Another way of processing incoming data without blocking, is to
  use single letter commands in a Post-fix/Reverse Polish Notation
  form.

  Effectively this means looking at each byte in the
  input stream, and handling it with the current state.

  As an example, say you had this coming into the serial port:

        4500R 80S 3G

  Where R is RPM, S is speed, and G is the gear setting, using
  the current accumulated number.

  The code below  acts on the bytes as they come in.  When it gets
  a letter "R", "S" or "G" it copies the number ot the register and
  acts. Otherwise it processes incoming digits by multiplying the
  previous result by 10, and adding in the new one.  Whitespace
  is skipped.

  This has the advantage of handling long messages without even
  needing any buffer, thus saving RAM. You can also process message
  as soon as the state changes, rather than waiting for end-of-line.

*/



void usage() {
  Serial.print(
    F(
      "TB6612FNG driver demo -- pots and serial commands\n"
      "Z test case: Forward(d) config using pot settings\n"
      "nnnnnn enter digits into main register\n"
      "R, S or G, save and process commands\n"
      "nnnT override pots\n"
      "t  cancel pot override\n"
      "asdf for direction\n"
      "nnn P -- set Pin register\n"
      "nC -- configure pin with pinMode(pinregister,n)\n"
      "WrwA  digital write, digital read, analog write/read\n"
      " eg: 9P200w  -- analogWrite(9,200)\n"
      "p print the current value\n"
      "H Help\n"
    ));
}

// LedMotor declarations
const byte EnableLeft = 11;
const byte EnableRight = 3;
const byte RightRed = 6;
const byte RightBlack = 5;
const byte LeftRed = 9;
const byte LeftBlack = 10;
const byte ThrottlePin = A1;
const byte TrimPin = A0;
uint32_t currentMillis;
bool trackingPots = true;
int verbose = 0;

// Example state machine reading serial input
// Author: DaveX
// Date: 2024-19-24
// Modified from https://www.gammon.com.au/forum/?id=11425&reply=1#reply1

// current partial number
unsigned int currentValue, lastValue, registerP;
bool inNumber = false;

unsigned int Rvalue, Svalue, Gvalue;
const uint8_t outPins[] = {EnableLeft, EnableRight, RightRed, RightBlack, LeftRed, LeftBlack};

void setupOutputPins() {
  if (verbose)Serial.print("\nOUTPUT: ");
  for (auto &pin : outPins) {
    pinMode(pin, OUTPUT);
    if (verbose) {
      Serial.print(pin);
      Serial.print(" ");
    }
  };
  if (verbose) Serial.println();
}

void processRPM (const unsigned int value)
{
  // do something with RPM
  Serial.print ("RPM = ");
  Serial.println (value);
  Rvalue = value;
  trackingPots = false; // ignore pots
  analogWrite(EnableLeft, constrain(value, 0, 255));
  analogWrite(EnableRight, constrain(value, 0, 255));
} // end of processRPM

void processSpeed (const unsigned int value)
{
  // do something with speed
  Serial.print ("Speed = ");
  Serial.println (value);
  Svalue = value;
  trackingPots = false; // ignore pots
  analogWrite(EnableLeft, constrain(value, 0, 255));
  analogWrite(EnableRight, constrain(value, 0, 255));
} // end of processSpeed

void processGear (const unsigned int value)
{
  // do something with gear
  Serial.print ("Gear = ");
  Serial.println (value);
  Gvalue = value;
  trackingPots = false; // ignore pots
  analogWrite(EnableLeft, constrain(value, 0, 255));
  analogWrite(EnableRight, constrain(value, 0, 255));
} // end of processGear

void processLine ()
{
  // do something with line
  char buff[50];
  snprintf(buff, 50, "Settings are %uRPM %uSpeed %uGear ", Rvalue, Svalue, Gvalue);
  Serial.println(buff);
} // end of processGear

void motorPins(int lr, int lb, int rr, int rb) {
  digitalWrite(LeftRed, lr);
  digitalWrite(LeftBlack, lb);
  digitalWrite(RightRed, rr);
  digitalWrite(RightBlack, rb);
}

void trackPots() {
  const uint32_t interval = 100;
  static uint32_t lastTrack = -interval ;
  static int lastTrim = -1,  lastThrottle, lastLeft, lastRight;

  if (trackingPots && currentMillis - lastTrack >= interval) {
    lastTrack = currentMillis;
    int trim = (analogRead(TrimPin) - 512) / 25; //+/-20
    int throttle = analogRead(ThrottlePin) / 4;
    int left = constrain(throttle + trim, 0, 255);
    int right = constrain(throttle - trim, 0, 255);
    //    analogWrite(EnableLeft, constrain(throttle * (100L + trim) / 100, 0, 255));
    //    analogWrite(EnableRight, constrain(throttle * (100L - trim) / 100, 0, 255));
    analogWrite(EnableLeft, left);
    analogWrite(EnableRight, right);
    if (verbose && trim != lastTrim) {
      Serial.print(trim); Serial.print(' ');
      Serial.print(left); Serial.print(':');
      Serial.print(right); Serial.print(' ');
      lastTrim = trim;
    }
    if (verbose && throttle != lastThrottle) {
      Serial.print(throttle); Serial.print(' ');
      lastThrottle = throttle;
    }
  }
}

void processIncomingByte (const byte c)
{
  switch (c)
  {
    case '0' ... '9':
      if (inNumber == false) {
        currentValue = 0;
        inNumber = true;
      }
      currentValue *= 10;
      currentValue += c - '0';
      break;
    case 'R': // Set RPM
      processRPM (currentValue);
      break;
    case 'S':
      processSpeed (currentValue);
      break;
    case 'G':
      processGear (currentValue);
      break;
    case 'Z': // test case enabled
      digitalWrite(EnableLeft, HIGH);
      digitalWrite(EnableRight, HIGH);
      digitalWrite(LeftRed, HIGH);
      digitalWrite(LeftBlack, LOW);
      digitalWrite(RightRed, HIGH);
      digitalWrite(RightBlack, LOW);
      break;
    // asdf keys:
    case 'a': // left
      motorPins(0, 1, 1, 0);
      break;
    case 's': // back
      motorPins(0, 1, 0, 1);
      break;
    case 'd': // forward
      motorPins(1, 0, 1, 0);
      break;
    case 'f': // right
      motorPins(1, 0, 0, 1);
      break;
    case 'T': // override pots
      analogWrite(EnableLeft, constrain(currentValue, 0, 255));
      analogWrite(EnableRight, constrain(currentValue, 0, 255));
      trackingPots = false;
      break;
    case 't': { // switch to track with variable resispots
        trackingPots = true;
      }
      break;
    // Txtzyme (https://github.com/WardCunningham/Txtzyme)-like commands
    //
    //   11P127w -- analogWrite(11,255);
    //   0PAp
    //
    //
    //  Pin = value
    //  configure pinMode(Pin,value)
    //  analogWrite(Pin,value)
    //  digitalWrite(Pin,value)
    //
    //  Get value=digitalRead(Pin)
    //  get value=analogRead(Pin)
    //  Show: print(value)
    //

    case 'P': // set the P-pin register
      registerP = currentValue;
      break;
    case 'C': // configure
      // uno: 0  = INPUT=0, OUTPUT=1, INPUT_PULLUP=2
      pinMode(registerP, currentValue);
      break;
    case 'w':
      analogWrite(registerP, currentValue);
      break;
    case 'W': // Digital write
      digitalWrite(registerP, currentValue);
      break;
    case 'r':
      currentValue = digitalRead(registerP);
      break;
    case 'A':
      currentValue = analogRead(registerP);
      break;
    case 'p':
      Serial.println(currentValue);
      break;
    case 'H':
      usage();
      break;
    case ' ':
    case '\t':
      break;
    case '\n':
      processLine();
      break;
    default:
      Serial.print("Character '");
      Serial.print(char(c));
      Serial.println("' is unknown.");
      break;
  }  // end of switch on incoming byte
  if (isalpha(c)) {
    inNumber = false;
    //Serial.print(currentValue);
    //Serial.println(char(c));
  }
} // end of processIncomingByte


void setup ()
{
  Serial.begin (115200);
  Serial.print("Serial RPN parser");
  usage();
  Serial.println("Try Testcase Z and twiddle the pots. asdf for direction");
  setupOutputPins();
  processIncomingByte('Z'); // testcae
}  // end of setup


void loop ()
{
  currentMillis = millis();
  while (Serial.available ())
    processIncomingByte (Serial.read ());

  trackPots();

  // do other stuff in loop as required

}  // end of loop