#define UNKNOWN_PIN 0xFF
#define TX_PIN 1
#define RX_PIN 0

/*
 * getMode function has been taken from https://forum.arduino.cc/t/check-current-pinmode/232140
 */
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(char command,  uint8_t argument0, uint8_t argument1){
  switch (command) {
    case 'm': // set pin mode
      Serial.print(argument1);
      if (argument0 >= NUM_DIGITAL_PINS) Serial.print("e");
      else if ((char)argument1=='i') {
        pinMode(argument0, INPUT);
        Serial.print("s");
      } else if ((char)argument1=='o') {
        pinMode(argument0, OUTPUT);
        Serial.print("s");
      } else {
        Serial.print("e");
      }
      return;
    case 'w': // write digital pin
      if (argument0 >= NUM_DIGITAL_PINS) Serial.print("e");
      else if ((char)argument1 == 'h') {
        digitalWrite(argument0, HIGH);
        Serial.print("s");
      } else if ((char)argument1 == 'l') {
        digitalWrite(argument0, LOW);
        Serial.print("s");
      } else {
        Serial.print("e");
      }
      return;
    case 'p': // set a pwm pin
      if (digitalPinHasPWM(argument0)) {
        analogWrite(argument0, argument1);
        Serial.print("s");
      } else {
        Serial.print("e");
      }
      return;
    case 'h':
      Serial.print("\n\rDid you mean 'H'?\n\re");
      return;
    case 'x': // toggle pin mode
      if (toggleMode(argument0) == UNKNOWN_PIN) Serial.print("e");
      else Serial.print("s");
      return;
    case 'a': // read analog pin
      if (argument0>=NUM_ANALOG_INPUTS || analogInputToDigitalPin(argument0)<0){
        Serial.print("e");
        return;
      }
      Serial.print(analogRead(analogInputToDigitalPin(argument0)));
      Serial.print("s");
      return;
    case 'r': // read digital value
      if (argument0 >= NUM_DIGITAL_PINS) Serial.print("e");
      else {
        Serial.print(digitalRead(argument0));
        Serial.print("s");
      }
      return;
    case 't': // toggle a pin
      if (toggleState(argument0)==UNKNOWN_PIN) Serial.print("e");
      else Serial.print("s");
      return;
    case 'l':
      if (argument0=='h') {
        digitalWrite(LED_BUILTIN, HIGH);
        Serial.print("s");
      } else if (argument0=='l') {
        digitalWrite(LED_BUILTIN, LOW);
        Serial.print("s");
      } else if (argument0=='t') {
        toggleState(LED_BUILTIN);
        Serial.print("s");
      } else {
        Serial.print("e");
      }
      return;
    default:
      Serial.print("\n\rCommand not found.\n\r");
      Serial.print("e");
      return;
  }
}

uint8_t getArgument0(char command){
  switch (command) {
    case 'm':
      return waitForSerialInput(2).toInt();
    case 'x':
      return waitForSerialInput(2).toInt();
    case 'a':
      return waitForSerialInput(2).toInt();
    case 'r':
      return waitForSerialInput(2).toInt();
    case 'w':
      return waitForSerialInput(2).toInt();
    case 'p':
      return waitForSerialInput(2).toInt();
    case 't':
      return waitForSerialInput(2).toInt();
    case 'l':
      return waitForSerialInput(1).charAt(0);
    default:
      return 0;
  }
}

uint8_t getArgument1(char command){
  switch (command) {
    case 'm':
      return waitForSerialInput(1).charAt(0);
    case 'w':
      return waitForSerialInput(1).charAt(0);
    case 'p':
      return waitForSerialInput(3).toInt();
    default:
      return 0;
  }
}

void getAndRunCommand(){
  uint8_t command = Serial.read();
  uint16_t loopCounter;
  uint8_t argument0;
  uint8_t argument1;
  switch ((char)command) {
    case 'R': // run setup() again
      Serial.print((char)command);
      Serial.end();
      setup();
      return;
    case 'H': // print help information
      Serial.print((char)command);
      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"
      "  \\n\\r data separator\n\r"
      "  \\r\\n ready\n\r"
      "commands (input to serial line)\n\r"
      "  R pseudo-reset\n\r"
      "    run setup again\n\r"
      "  H show help\n\r"
      "  Q lock\n\r"
      "    lock the Arduino's state by running an infinite loop\n\r"
      "  L loop\n\r"
      "    usage: followed by a 5 digit uint16_t number,\n\r"
      "      then one of the lowercas commands with arguments\n\r"
      "    repeat the command given as an argument\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"
      "\n\rs"));
      return;
    case 'Q':
      Serial.print((char)command);
      Serial.print("s");
      while (true) {
      }
      return;
    case 'L':
      Serial.print((char)command);
      loopCounter = waitForSerialInput(5).toInt();
      command = waitForSerialInput(1).charAt(0);
      argument0 = getArgument0((char)command);
      argument1 = getArgument1((char)command);
      Serial.print((char)command);
      for (loopCounter; loopCounter > 0; loopCounter--){
        Serial.print("\n\r");
        runCommand((char)command, argument0, argument1);
      }
      return;
    default:
      Serial.print((char)command);
      argument0 = getArgument0((char)command);
      argument1 = getArgument1((char)command);
      runCommand((char)command, argument0, argument1);
      return;
  }
}

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

void setup(){
  Serial.begin(230400);
  // set all pins to input and low
  for (uint8_t i=0; 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) {
      getAndRunCommand();
      Serial.print("\r\n"); // send "ready" status information
    }
  }
}