// SPDX-License-Identifier: Apache-2.0
// Copyright (c) 2012-2024 Scott Penrose <scottp@dd.com.au> and WII5 Buoy contributors
//
// This file is part of WII5 Buoy firmware.
// See LICENSE for full terms.

/**
 * @file wii5_pins.ino
 * @brief Test sketch: multi-port serial passthrough for pin diagnostics.
 */

/*

WII5 Pins

*/
#include <elapsedMillis.h>
#include <WII5Sh3dConsole.h>
#include <Sh3dNodeUtil.h>

// TODO This needs serious normalisation for each serial port ! (A class perhaps)
// Large Line buffers for passthrough
#define BUFFER_SIZE 250
#define DEVICE_MAX 6
Stream *streams[DEVICE_MAX];
char bufferSerial[DEVICE_MAX][BUFFER_SIZE + 1];
uint8_t bufferPos[DEVICE_MAX];
uint32_t count = 0;
elapsedMillis passthroughWait = 0;
bool passthroughSerial[DEVICE_MAX];

void setup() {
  // Short delay for debug / errors

  Serial.begin(57600);
  console.begin();
  console.add(&Serial);

  sh3dNodeUtil.begin();

  // Start each port at the right speed
  Serial1.begin(9600);
  streams[1] = &Serial1;
  Serial2.begin(9600);
  streams[2] = &Serial2;
  Serial3.begin(9600);
  streams[3] = &Serial3;
  // Serial4.begin(9600);
  // streams[4] = &Serial4;
  // Serial5.begin(9600);
  // streams[5] = &Serial5;
}

void processSerial(uint8_t id) {
  if (!streams[id]) {
    return;
  }
  while (streams[id]->available()) {
    char c = streams[id]->read();

    // Ignore start/stop character of Wind Sonic
    if (c == 2)
      c = 'W';
    if (c == 3)
      c = '*';

    if ( (c != '\n') && (c != '\r') ) {
      if (bufferPos[id] < BUFFER_SIZE) {
        bufferSerial[id][bufferPos[id]] = c;
        bufferPos[id]++;
        bufferSerial[id][bufferPos[id]] = '\0';
      }
    }
    else {
      if (passthroughSerial[id]) {
        console.printf(F("S%d: (%lu) %s\r\n"), id, millis(), bufferSerial[id]);
      }
    }
  }
}

elapsedMillis wait = 0;
void loop() {
  // TODO Should not be hard coded 60 seconds
  /*
  if (passthroughWait > 60000) {
    // Skips 0
    for (uint8_t id = 1; id < DEVICE_MAX; id++) {
      passthroughSerial[id] = false;
    }
    console.log(LOG_INFO, F("Passthrough time expired"));
  }
  */

  console.loop();
  if (console.available()) {
    console.log(LOG_INFO, F("WII5 Console command received - %c=%d"),
      console.getCommand(),
      console.getVal()
    );

    switch(console.getCommand()) {
      case '1':
      case '2':
      case '3':
      case '4':
      case '5':
        // Toggle
        passthroughSerial[(uint8_t)console.getCommand() - '0']
          = !passthroughSerial[(uint8_t)console.getCommand() - '0'];
        // Reset timer - all off when hits time
        passthroughWait = 0;
        console.log(LOG_INFO,
          F("Passthrough enabled for $d for %d seconds"),
          (uint16_t)console.getCommand() - '0',
          60
        );
        break;

      case 'A':
        for (uint8_t id = 1; id < DEVICE_MAX; id++) {
          passthroughSerial[id] = true;
        }
        passthroughWait = 0;
        break;

      // Output
      case 'o':
        console.printf(F("OUTPU pin=%d\r\n"), int(console.getVal()));
        pinMode(int(console.getVal()), OUTPUT);
        break;

      // Output
      case 'i':
        console.printf(F("INPUT pin=%d\r\n"), int(console.getVal()));
        pinMode(int(console.getVal()), INPUT);
        break;

      // Low
      case 'x':
        console.printf(F("LOW pin=%d\r\n"), int(console.getVal()));
        digitalWrite(int(console.getVal()), LOW);
        console.printf(F("pin=%d\r\n"), int(digitalRead(console.getVal())));
        break;

      // HIGH
      case 'X':
        console.printf(F("HIGH pin=%d\r\n"), int(console.getVal()));
        digitalWrite(int(console.getVal()), HIGH);
        console.printf(F("pin=%d\r\n"), int(digitalRead(console.getVal())));
        break;

      // Low
      case 's':
        console.printf(F("pin=%d, state=%d\r\n"), int(console.getVal()), int(digitalRead(console.getVal())));
        break;

      case 'd':
        // dumpPins();
        break;

      case 'h':
      case 'H':
        console.log(LOG_INFO, F(" WII5: Pins"));
        console.log(LOG_INFO, F(" 'L' - Stop logging"));
        break;
    }
  }

  if (wait > 500) {
    console.printf(F("pin=%d, state=%d\r\n"), int(console.getVal()), int(digitalRead(console.getVal())));
    wait = 0;
  }

  // Process all incoming Serial
  for (uint8_t id = 1; id < DEVICE_MAX; id++) {
    processSerial(id);
  }
}