// 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 WII5Controller.cpp
 * @brief Main controller: mode dispatch, watchdog, and the top-level loop.
 */

/*

Controller - Main controller, for loops and processing within WII5.

Basically the controller is in charge of what is called for setup and loop.

SPLIT:

  * Setup - helpers and stuff to setup code - doesn't not need to be here
  * Mode, setMode, calculateMode etc
  * Console and Settings

HOWTO USE "yield" Here

  NOT READY - Causes infinite loops

  * create a "yield" in the sketch and call
    * wii5Controller.yield()l
    * And any other important entries, e.g. Network
  * And in loop() call yield at the start of loop

TEMPLATE:


LOOPS:
  loop:
    - This is the actual loop and it just calls the others, depending on mode
    - Calls loopWDT
    - Based on mode, calls
      .
    - Call loopSafe - always?

  loopWDT:
    - Just update the WDT and spin on

  loopSafe:
    - All the commands that must be always run
    - Calls
      . sh3dNodeIO.loop();
      . wii5Commands.processButtons();
      . console.loop();
      . wii5Commands.processConsoleAT();
      . wii5Commands.processConsoleStandard();
      . wii5Iridium.loop();
      . wii5Communications.loop();

  loopDefault:
    - Old - everything, instead, this is now loopOther

  loopOther:
    - Everything else
    - Calls
      . wii5Sparton.loop();
      . wii5RadioLoRa.loop();
      . wii5Commands.processNetwork();
      . wii5Gps.loop();
      . wii5Battery.loop();
      . wii5Weather_18B20.loop();



  loopNotCapturing:

Other places loop* is called from
  safeDelay (not complete)

*/

#include <Arduino.h>
#include <TimeLib.h>
#include <WII5.h>
#include <WII5Controller.h>
#include <WII5Strings.h>
#include <MemoryFree.h>
#include <avr/wdt.h>

// Debugging etc - move to header file
#define DEBUG_SLEEP

void WII5Controller::begin() {
  uptime = 0;
  minuteWait = 0;
  wii5ModeSleep.begin();
  wii5ModeCapture.begin();
  wii5ModeLowBattery.begin();
  wii5ModeManualTest.begin();
  wii5ModePosition.begin();
  wii5ModeSelfTest.begin();

  mode = WII5MODE_BOOT; // Not using setMode here

  currentSD = 0;

  #ifdef WII5_DEBUG_SERIAL
  SerialDebug.print(F("CONTROL:BOOTUP:freememory="));
  SerialDebug.print(freeMemory());
  SerialDebug.println();
  #endif

  // SD Card pins - should use Power object
  pinMode(POWER_STORAGE_1_PIN, OUTPUT);
  pinMode(STORAGE_SD1_MATHS_PIN, OUTPUT);
  pinMode(STORAGE_SD2_MATHS_PIN, OUTPUT);

  #ifdef WII5_WDT_INTERNAL
  wdt_enable(WDTO_8S);
  #endif
}

// Delay with SafeLoop
void WII5Controller::safeDelay(uint32_t waits) {
  delayWait = 0;
  while (delayWait < waits) {
    loopWDT();
    loopSafe();
  }
}

void WII5Controller::setLastHello(WII5_PORTS p, WII5_DEVICES d) {
  lastHelloReceived = now();
  lastHelloElapsed = 0;
  lastHelloPort = p;
  lastHelloDevice = d;
}

bool WII5Controller::checkLastHello(WII5_DEVICES d, uint32_t msOld) {
  // Expected device, and time is newer than we needed - TRUE!
  if ((d == lastHelloDevice) && (lastHelloElapsed < msOld)) {
    return true;
  }
  return false;
}

// Main Loop - broken up for easy reading
void WII5Controller::loop() {
  loopWDT();
  loopSafe();

  // The *reasonably* accurate minute count
  if (minuteWait > 60000) {
    // TODO - use uptime += (minuteWait / 60000)
    //  minuteWait = (minuteWait % 60000);
    //  And log if it is over 5 minutes.
    uptime++;
    minuteWait = minuteWait - 60000;
  }

  // TODO timeout
  switch (mode) {
    case WII5MODE_BOOT:
      // Safety: Automatically moves on
      console.log(LOG_DEBUG, F("WII5: Boot up process complete"));
      // Pretty safe to run
      loopOther();
      statusDump();
      setMode(WII5MODE_START);
      break;
    case WII5MODE_START:
      // Safety: Automatically moves on
      loopOther();
      setMode(calculateCurrentMode());
      break;
    case WII5MODE_DEMO:
      // Safety: TODO This one never exits. Needs work.
      //    modeWait is screwed
      loopOther();
      if (modeWait > 60000) {
        console.log(LOG_DEBUG, F("Demonstration mode, try T; for test"));
        modeWait = 0;
      }
      if (modeWait > (WII5TIME_1DAY * 1000)) {
        // Stuck here... now what. Move to sleep?
        mode = WII5MODE_SLEEP; modeWait = 0;
      }
      break;
    case WII5MODE_SLEEP:
      // Safety: Sleep mode handles exit
      wii5ModeSleep.loop();
      // TODO Maybe?
      loopOther();
      break;
    case WII5MODE_LOWBATTERY:
      // Safety: Battery mode handles exit
      wii5ModeLowBattery.loop();
      loopOther();
      break;
    case WII5MODE_POSITION:
      // Safety: Either temporary timed mode, or changed by sattelite comms
      wii5ModePosition.loop();
      loopOther();
      break;
    case WII5MODE_CAPTURE:
      wii5ModeCapture.loop();
      loopOther();
      break;
    case WII5MODE_MANUALTEST:
      wii5ModeManualTest.loop();
      if (modeWait > (WII5TIME_1DAY * 1000)) {
        // Stuck here... now what. Move to sleep? Check saved mode is sleep or capture
        console.log(LOG_FATAL, F("FATAL * FATAL * FATAL * reset is being called from manual test loop"));
        console.flush();
        sh3dNodeUtil.reset();
      }
      loopOther();
      break;
    case WII5MODE_SELFTEST:
      wii5ModeSelfTest.loop();

      // loopSafe plus some
      loopOther();

      if (modeWait > (WII5TIME_1DAY * 1000)) {
        // Stuck here... now what. Move to sleep? Check saved mode is sleep or capture
        console.log(LOG_FATAL, F("FATAL * FATAL * FATAL * reset is being called from self test loop"));
        console.flush();
        sh3dNodeUtil.reset();
      }
      break;
    default:
      console.log(LOG_FATAL, F("Invalide Mode, old=%d, new=%d"), mode, WII5MODE_CAPTURE);
      mode = WII5MODE_CAPTURE;
  }
}

// loopSafe - Minimal safe loop, to allow for callback from Iridium etc
void WII5Controller::loopSafe() {
  // Buttons, LEDs etc
  sh3dNodeIO.loop();
  wii5Commands.processButtons();

  // Serial port
  console.loop();
  wii5Commands.processConsoleAT();
  wii5Commands.processConsoleStandard();

  // Undesirable: Do not use these in loopSafe
  //  - Gps - lots of data
  //  - Sparton - lots of data
  //  - Iridium - lots of data and potential cost
  //  - Battery - Changes mode on us

  wii5Iridium.loop();
  wii5Communications.loop();

  wii5Maths.loop();
}

void WII5Controller::loopWDT() {
  #ifdef WDT_RESET
  // TODO consider compile this out and have specific version
  // TODO or don't call loopWDT inside TestMode and have own version
  // TODO manual test mode timeout
  // TODO Test the buoy - has it been more than 3 days since we sent a message - maybe reboot?
  if (tempDisableWDT) return;
  if (wdtWait > 5100) {
    digitalWrite(WDT_RESET, LOW);
    wdtWait = 0;
  }
  // TODO setting pin all the time for 100 ms
  else if (wdtWait > 5000) {
    digitalWrite(WDT_RESET, HIGH);
  }
  #endif
  #ifdef WII5_WDT_INTERNAL
  wdt_reset();
  #endif
}

void WII5Controller::loopOther() {
  #ifdef WII5_IMU_SPARTON
  wii5Sparton.loop();
  #endif

  // LORA or Other Modems
  #ifdef WII5_RADIO_LORA
  wii5RadioLoRa.loop();
  wii5Commands.processNetwork();
  #endif

  #ifdef WII5_GPS
  wii5Gps.loop();
  #endif

  wii5Battery.loop();
  wii5Weather_18B20.loop();
}

WII5_MODES WII5Controller::getMode() {
  return mode;
}

bool WII5Controller::setMode(WII5_MODES newMode, bool nodelay) {
  // Ignore same mode change - really this is an error
  if (newMode == mode)
    return false;

  console.log(LOG_INFO, F("Controller: new mode set %d:%s"), newMode, wii5Strings.strMode(newMode));
  #ifdef WII5_DEBUG_SERIAL
  SerialDebug.print(F("CONTROL:MODE:mode="));
  SerialDebug.print(newMode);
  SerialDebug.print(F(":"));
  SerialDebug.print(wii5Strings.strMode(newMode));
  SerialDebug.println();
  #endif

  // TODO Delay ???
  //  - Put in a "when ready or timeout" to change mode

  // Set the mode
  mode = newMode;
  modeWait = 0;

  // Reset all STEPS to start
  wii5ModeCapture.reset();
  wii5ModeLowBattery.reset();
  wii5ModePosition.reset();
  wii5ModeManualTest.reset();
  wii5ModeSelfTest.reset();
  wii5ModeSleep.reset();

  // Update status to tell users of mode change
  statusDump();

  return true;
}

// TODO move to SPI
void WII5Controller::disableChipSelect() {
  #ifdef RADIO_CS
  digitalWrite(RADIO_CS, HIGH);
  #endif
  #ifdef STORAGE_CS
  digitalWrite(RADIO_CS, HIGH);
  #endif
  #ifdef SPIMEMORY_CS
  digitalWrite(SPIMEMORY_CS, HIGH);
  #endif
}


// TODO move to Status Dump
void WII5Controller::statusDump(bool human) {
  // Basic status - mode, time, free memory
  // TODO 2024 - Add run count - check this still works in node? (yes)
  wii5Display.atCommandSend(F("status"), F("record=%lu,uptime=%lu,freeMemory=%d,run=%lu"),
    wii5Config.getRecordCount(),
    uptime, freeMemory(),
    sh3dNodeConfig.getRunCount()
  );

  period = 0;
  next = 0;
  size = 0;

  // CURRENT mode if we have one.
  if (mode != wii5Config.getDefaultMode()) {
    next = 0;
    // Calculate next depending on noce
    if (getMode() == WII5MODE_CAPTURE) {
      period = wii5Config.getCapturePeriod();
      size = wii5BinData.getSize(wii5Config.getCaptureBinaryType());
    }
    else if (getMode() == WII5MODE_POSITION) {
      period = wii5Config.getPositionPeriod();
      size = wii5BinData.getSize(wii5Config.getPositionBinaryType());
    }
    else if (getMode() == WII5MODE_SLEEP) {
      period = wii5Config.getSleepPeriod();
      size = wii5BinData.getSize(wii5Config.getSleepBinaryType());
    }
    next = wii5Display.minutesUntilNext(period);

    // Only show current if not the same as default
    wii5Display.atCommandSend(F("status"), F("modeType=current,mode=%s,modeTime=%lu,modePeriod=%lu,modeNext=%lu,binSize=%d,sd=%d"),
      wii5Strings.strMode(mode),
      (uint32_t)(modeWait / 60000), // Convert to Minutes
      period, next, size, currentSD
    );
  }

  // DEFAULT Mode
  next = 0;
  if (wii5Config.getDefaultMode() == WII5MODE_CAPTURE) {
    period = wii5Config.getCapturePeriod();
    size = wii5BinData.getSize(wii5Config.getCaptureBinaryType());
  }
  else if (wii5Config.getDefaultMode() == WII5MODE_POSITION) {
    period = wii5Config.getPositionPeriod();
    size = wii5BinData.getSize(wii5Config.getPositionBinaryType());
  }
  else if (wii5Config.getDefaultMode() == WII5MODE_SLEEP) {
    period = wii5Config.getSleepPeriod();
    size = wii5BinData.getSize(wii5Config.getSleepBinaryType());
  }
  next = wii5Display.minutesUntilNext(period);
  wii5Display.atCommandSend(F("status"), F("modeType=default,mode=%s,modeTime=%lu,modePeriod=%lu,modeNext=%lu,binSize=%d,sd=%d"),
    wii5Strings.strMode(wii5Config.getDefaultMode()),
    (uint32_t)(modeWait / 60000), // Convert to Minutes
    period, next, size, currentSD
  );

  wii5Display.atCommandSend(F("status"), F("version=%S,imei=%s"), F(WII5_SOFTWARE_VERSION), wii5Iridium.imei);

  wii5Display.atCommandSend(F("status"), F("voltage=%ld,temperature=%ld,lat=%ld,lon=%ld,alt=%ld"),
    wii5Battery.value, wii5Weather_18B20.value,
    long(wii5Gps.gps->location.lat() * GPS_POS_MULT),
    long(wii5Gps.gps->location.lng() * GPS_POS_MULT),
    long(wii5Gps.gps->altitude.meters() * GPS_ALT_MULT)
  );

  // Calculate Maths mode.
  wii5Display.atCommandSend(F("status"), F("maths_on=%d,maths_held_remaining=%lu,maths_power_remaining=%lu"),
    wii5Maths.isRunning(), wii5Maths.remainingHold() / 60, wii5Maths.remainingUntil() / 60
  );

  // Tell the Maths CPU to advertise this info
  wii5Display.atCommandSend(F("status"), F("update"));

  if (human) {
    console.log(LOG_INFO, F("# WII5: Waves In Ice 5 - 2019. Board:%S Version:%S ID=%lu"),
      F(WII5_BOARD_NAME),
      F(WII5_SOFTWARE_VERSION),
      wii5Config.getDeviceId()
    );
  }
}

WII5_MODES WII5Controller::calculateDefaultMode() {
  return wii5Config.getDefaultMode();
}

// TODO move to Status Dump
WII5_MODES WII5Controller::calculateCurrentMode() {
  // TODO Urgent - temporary Modes
  //  TODO Urgent - find everywhere setDefaultMode is called, probably should be setCurrentMode
  // TODO check we have valid date/time
  // TODO check temporaryMode
  // TODO check defaultMode
  // return WII5MODE_CAPTURE;
  // return WII5MODE_DEMO;
  return wii5Config.getDefaultMode();
}

// What was the last default mode - for reboot and post sleep
// TODO - Proabably move to sh3dNodeConfig (EEPROM) data
void WII5Controller::setDefaultMode() {
  // Get default mode from EEPROM and disable the Temporary Mode there
  setMode(calculateDefaultMode());
}

void WII5Controller::setCurrentMode() {
  // Get temporary mode from EEPROM, check it is valid, else fall back to Default
  setMode(calculateCurrentMode());
}

void WII5Controller::shared5On() {
  // Turn it on - and feel free to
  digitalWrite(SHARED_5VOLT_PIN, HIGH);
}
void WII5Controller::shared5Off() {
  // Turn 5 Volts off - ONLY if safe to do so
  if (wii5Maths.isRunning() || wii5Iridium.isRunning() || wii5Sparton.isRunning() || (sh3dNodeIO.led2Get() != LED_OFF)) {
    // Keep it on !
    digitalWrite(SHARED_5VOLT_PIN, HIGH);
  }
  else {
    // Yep, power off time
    digitalWrite(SHARED_5VOLT_PIN, LOW);
  }
}

uint8_t WII5Controller::getSD() {
  return currentSD;
}

void WII5Controller::setSDOff() {
  sdBlock.cardForceClose();
  digitalWrite(POWER_STORAGE_1_PIN, LOW);
  digitalWrite(STORAGE_SD1_MATHS_PIN, HIGH);
  digitalWrite(STORAGE_SD2_MATHS_PIN, LOW);
  currentSD = 0;
}

// TODO 2024 - delay - really ! Probably ok for now - consider safeDelay
void WII5Controller::setSD1() {
  digitalWrite(POWER_STORAGE_1_PIN, LOW);
  delay(500);
  digitalWrite(STORAGE_SD1_MATHS_PIN, HIGH);
  digitalWrite(STORAGE_SD2_MATHS_PIN, LOW);
  digitalWrite(STORAGE_CS, HIGH);
  delay(500);
  digitalWrite(POWER_STORAGE_1_PIN, HIGH);
  delay(500);
  currentSD = 1;
}

// TODO 2024 - delay - really ! Probably ok for now - consider safeDelay
void WII5Controller::setSD2() {
  digitalWrite(POWER_STORAGE_1_PIN, LOW);
  delay(500);
  digitalWrite(STORAGE_SD1_MATHS_PIN, LOW);
  digitalWrite(STORAGE_SD2_MATHS_PIN, HIGH);
  digitalWrite(STORAGE_CS, HIGH);
  delay(500);
  digitalWrite(POWER_STORAGE_1_PIN, HIGH);
  delay(500);
  currentSD = 2;
}

WII5Controller wii5Controller;