// 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_sparton.ino
 * @brief Test sketch: Sparton AHRS exercise.
 */

#include <WII5Sh3dConsole.h>
#include <WII5.h>
#include <WII5Sparton.h>

void setup() {
  Serial.begin(115200);
  console.begin(WII5_BUFFER_CONSOLE_PRINT, WII5_BUFFER_CONSOLE_CMD);
  console.add(&Serial);
  console.printf(F("WII5: Test Sparton"));

  wii5Sparton.begin();
  wii5Sparton.setDebug(true);

  wii5Sparton.setRecords(5000);
  wii5Sparton.start();
}

void loop() {
  wii5Sparton.loop();
  console.loop();
  if (console.available()) {
    uint32_t val = console.getVal();
    console.printf(F("CONSOLE: Got command %c=%d \r\n"),
      console.getCommand(),
      val
    );
    switch(console.getCommand()) {
      // Toggle Sparton passthrough
      case 'p':
        console.log(LOG_DEBUG, F("SPARTON: 'p' passthrough toggle"));
        wii5Sparton.setPassthrough(!wii5Sparton.getPassthrough());
        break;

      case 's':
        if (val < 100) val = 100;
        console.log(LOG_DEBUG, F("SPARTON: 's' start with %lu records"), val);
        wii5Sparton.setRecords(val);
        wii5Sparton.start();
        break;

      case 'S':
        console.log(LOG_DEBUG, F("SPARTON: 'S' stop"));
        wii5Sparton.stop();
        break;

      default:
        break;
    };
    console.log(LOG_DEBUG, F("SPARTON: Status = %d"), wii5Sparton.getPassthrough());
    wii5Sparton.displayStatus();
    wii5Sparton.displayError();
  }
}