/*
 * getMode function has been taken from https://forum.arduino.cc/t/check-current-pinmode/232140
 */
#define UNKNOWN_PIN 0xFF
#define TX_PIN 1
#define RX_PIN 0

uint8_t getMode(uint8_t pin){
  uint8_t bitmask = digitalPinToBitMask(pin);
  uint8_t port = digitalPinToPort(pin);

  // I don't see an option for mega to return this, but whatever...
  if (port == NOT_A_PIN) return UNKNOWN_PIN;

  // Is there a bit we can check?
  if (bitmask == 0) return UNKNOWN_PIN;

  // Is there only a single bit set?
  if (bitmask & bitmask - 1) return UNKNOWN_PIN;

  volatile uint8_t *reg, *out;
  reg = portModeRegister(port);
  out = portOutputRegister(port);

  if (*reg & bitmask)
    return OUTPUT;
  else if (*out & bitmask)
    return INPUT_PULLUP;
  else
    return INPUT;
}

uint8_t toggleState(uint8_t pin){
  uint8_t mode = getMode(pin);
  switch (mode) {
    case OUTPUT:
      if (digitalRead(pin) == HIGH) {
        digitalWrite(pin, LOW);
        return LOW;
      } else {
        digitalWrite(pin, HIGH);
        return HIGH;
      }
    case INPUT_PULLUP:
      pinMode(pin, INPUT);
      return LOW;
    case INPUT:
      pinMode(pin, INPUT_PULLUP);
      return HIGH;
    default:
      return UNKNOWN_PIN;
  } 
}

uint8_t toggleMode(uint8_t pin){
  uint8_t mode = getMode(pin);
  // declared here bc it won’t let me inside the switch/case statement
  uint8_t state;
  switch (mode) {
    case OUTPUT:
      state = digitalRead(pin);
      pinMode(pin, INPUT);
      digitalWrite(pin, state);
      return getMode(pin); // probably sacrificed performance for readability here
    case INPUT_PULLUP:
      pinMode(pin, OUTPUT);
      digitalWrite(pin, HIGH);
      return OUTPUT;
    case INPUT:
      pinMode(pin, OUTPUT);
      digitalWrite(pin, LOW);
      return OUTPUT;
    default:
      return UNKNOWN_PIN;
  }
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

String waitForSerialInput(uint8_t bytes){
  uint8_t inputLength = 0;
  String string = "";
  uint8_t waitingCycles = 0;
  while (inputLength < bytes) {
    if (Serial.available() > 0) {
      string = string + (char)Serial.read();
      inputLength = inputLength+1;
      waitingCycles = 0;
    } else {
      if (waitingCycles > 100) {
        Serial.print(".");
        delay(500);
        waitingCycles = 0;
      } else {
        waitingCycles = waitingCycles+1;
      }
    }
  }
  return string;
}


void runCommand(){
  uint8_t command = Serial.read();
  int analogPin;
  String mode;
  uint8_t pin;
  String state;
  uint8_t pwmState;
  switch ((char)command) {
    case 'R': // run setup() again
      Serial.print("R");
      Serial.end();
      setup();
      return;
    case 'h': // print help information
      Serial.print("h");
      Serial.print(F("\n\r"
      "status (output from serial line)\n\r"
      "  <letter of command> command acknowledgement\n\r"
      "  . internal operation or waiting for input\n\r"
      "  <data>\n\r"
      "  s success\n\r"
      "  e error\n\r"
      "  \\r\\n ready (this message uses \n\r to not show up as \"ready\" after the first line)\n\r"
      "commands (input to serial line)\n\r"
      "  R pseudo-reset\n\r"
      "    run setup again\n\r"
      "  h show help\n\r"
      "  m set mode\n\r"
      "    usage: followed by two digit pin number and o or i\n\r"
      "    set a pin to input or output mode\n\r"
      "    \"pinMode\"\n\r"
      "  x toggle mode\n\r"
      "    usage: followed by two digit pin number\n\r"
      "    toggle a pin's mode\n\r"
      "    enables pullup resistor for previously high output pins\n\r"
      "    sets output high for previous input pins with pullup enabled\n\r"
      "  a analog read\n\r"
      "    usage: followed by two digit analog pin number\n\r"
      "    returns state of an analog pin\n\r"
      "    pin number will be converted to digital pin automatically\n\r"
      "  r read\n\r"
      "    usage: followed by pin number\n\r"
      "    returns state of a pin\n\r"
      "    \"digitalRead\"\n\r"
      "  w write\n\r"
      "    usage: followed by two digit pin number and h or l\n\r"
      "    sets a pin to the desired state\n\r"
      "    sets pullup resistor for input pins\n\r"
      "    \"digitalWrite\"\n\r"
      "  p write pwm\n\r"
      "    usage: followed by two digit pin number and three digit value between 000 and 255\n\r"
      "    \"analogWrite\"\n\r"
      "  t toggle pin state\n\r"
      "    usage: followed by two digit pin number\n\r"
      "    toggles state of output pins\n\r"
      "    toggles pullup resistor for input pins\n\r"
      "  l change led state\n\r"
      "    usage: followed by h (high), l (low) or t (toggle)\n\r"
      "    sets or toggles the state of LED_BUILTIN\n\r"
      "  q lock\n\r"
      "    lock the Arduino's state by running an infinite loop\n\r"
      "\n\rs"));
      return;
    case 'm': // set pin mode
      Serial.print("m");
      pin = waitForSerialInput(2).toInt();
      mode = waitForSerialInput(1);
      if (pin >= NUM_DIGITAL_PINS) Serial.print("e");
      else if (mode=="i") {
        pinMode(pin, INPUT);
        Serial.print("s");
      } else if (mode=="o") {
        pinMode(pin, OUTPUT);
        Serial.print("s");
      } else {
        Serial.print("e");
      }
      return;
    case 'x': // toggle pin mode
      Serial.print("x");
      if (toggleMode((uint8_t)waitForSerialInput(2).toInt()) == UNKNOWN_PIN) Serial.print("e");
      else Serial.print("s");
      return;
    case 'a': // read analog pin
      Serial.print("a");
      analogPin = waitForSerialInput(2).toInt();
      if (analogPin>=NUM_ANALOG_INPUTS || analogInputToDigitalPin(analogPin)<0){
        Serial.print("e");
        return;
      }
      Serial.print(analogRead(analogInputToDigitalPin(analogPin)));
      Serial.print("s");
      return;
    case 'r': // read digital value
      /*bug:
       The following "r" (or any other text you may put there) will never get printed despite this code branch being executed. Don’t ask me why or how.
       Tested on Linux Mint 20.1, Arduino IDE version 2:1.0.5+dfsg2-4.1
       Tested with Arduino Uno and Nano 328P
      */
      Serial.print("r");
      pin = waitForSerialInput(2).toInt();
      if (pin >= NUM_DIGITAL_PINS) Serial.print("e");
      else {
        Serial.print(digitalRead(pin));
        Serial.print("s");
      }
      return;
    case 'w': // write digital pin
      Serial.print("w");
      pin = waitForSerialInput(2).toInt();
      state = waitForSerialInput(1);
      if (pin >= NUM_DIGITAL_PINS) Serial.print("e");
      else if (state == "h") {
        digitalWrite(pin, HIGH);
        Serial.print("s");
      } else if (state == "l") {
        digitalWrite(pin, LOW);
        Serial.print("s");
      } else {
        Serial.print("e");
      }
      return;
    case 'p': // set a pwm pin
      Serial.print("p");
      pin = waitForSerialInput(2).toInt();
      pwmState = waitForSerialInput(3).toInt();
      if (digitalPinHasPWM(pin)) {
        analogWrite(pin, pwmState);
        Serial.print("s");
      } else {
        Serial.print("e");
      }
      return;
    case 't': // toggle a pin
      Serial.print("t");
      if (toggleState(waitForSerialInput(2).toInt())==UNKNOWN_PIN) Serial.print("e");
      else Serial.print("s");
      return;
    case 'l':
      Serial.print("l");
      state = waitForSerialInput(1);
      if (state=="h") {
        digitalWrite(LED_BUILTIN, HIGH);
        Serial.print("s");
      } else if (state=="l") {
        digitalWrite(LED_BUILTIN, LOW);
        Serial.print("s");
      } else if (state=="t") {
        toggleState(LED_BUILTIN);
        Serial.print("s");
      } else {
        Serial.print("e");
      }
      return;
    case 'q':
      Serial.print("qs");
      while (true) {
      }
      return;
    default:
      Serial.print("e");
  }
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void setup(){
  Serial.begin(230400);
  // set all pins to input and low
  for (uint8_t i; i<=NUM_DIGITAL_PINS; i++) {
    Serial.print(".");
    if (i==TX_PIN || i==RX_PIN);
    else pinMode(i, INPUT);
  }
  Serial.print(".");
  pinMode(LED_BUILTIN, OUTPUT); // set LED to output bc having it as an input would make little sense
  while (Serial.available() > 0) {
    Serial.print(".");
    Serial.read();
  }
  Serial.print("s\r\n");
}

void loop(){
  while (true){
    if(Serial.available() > 0) {
      runCommand();
      Serial.print("\r\n"); // send "ready" status information
    }
  }
}