// 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 WII5Maths.cpp
 * @brief Maths CPU controller: power, hold, restart, and SD-card hand-off.
 */

/*

WII5Maths - Manage the power and serial to the Maths Compyter

TODO Seems Communications or Capture mode or something overrides hold - fix

Who is in charge ?
  * This applies to Pi vs AVR - they can both reset each other,
    who gets to say - nah you are being turned off, like it or
    not.
  * Also applies to within the Pi. Who decides to keep it turned
    on, and if it is turned on, who decides when to turn off
    and even more importantly - where is that triggered?
    since we don't rely on the daemon.
  * And - if no daemon - how do we receive commands

Scenario 1: Regular, Power on Pi to process SD Card
  * Power up, wii5auto runs as per normal
  * End of that wii5auto sends a "serial" command
    to tell the AVR that it will be shutting down soon.
  * End of that timeout, Pi shutsdown, one final serial
    before self shutdown/power off (time how long it takes
    before power is off)
  * Q:
    - How to run something, indipendent of the Daemon
      (another listener, or proxy through server)
      (complicated)

Scenario 2: Power Up boot without reason
      @WII5,maths,booted
      @WII5,maths!,task=mainreboot

Scenario 3: Power Up boot where Pi did the reboot/firmware etc
      Pi won't know it to send a booted command, hello etc still
      But AVR can send
        @WII5,maths,ask <- Ask what you want instead of telling
        @WII5,maths!,thanks <- lol
        Could just, on getting ask, decide to ignore (Beause in
        on mode for more minutes yet) or go straight to shutdown


COMMAND Utility:
  * Write a command utitily, for senging @ based commands
    to the wave buoy. In each case, the cli does not exit,
    until it gets a reply (or timeout)
  * Utility should do serial direct, but could potentially
    use the API to send and recevive commands, falling back
    to serial direct, when the daemon is non-responsive.
  * Boot scenraio:
    - Send a "why am I awake" - @WII5,maths,booted
      Reply with @WII5,maths!,task=on,min=15


Boot Sequence Issues:
  * AVR boot - even if WDT or on prupse at the end of sleep/Testing
    * Turns on the Pi sets it to ask and waits a long time.
    * LOT OF TIME and power.

setUntil - Not currently used, but allows a number to be set way off info
the future - not ideal though, because it could therefore be always on,
and dates change.

setHold - is simple and perfect reply to shut down.

*/

#include <Arduino.h>
#include <WII5Maths.h>
#include <WII5Sh3dConsole.h>
#include <TimeLib.h>

void WII5Maths::begin() {
  retry = 0;
  firstboot = false;
  holdFor = 0;
  mathsMode = WII5MATHSMODE_UNKNOWN;
  mathsUntil = 0;
  // NOTE: This is a two pin version - check it exists
  #ifdef POWER_MATHS_PIN_OFF
  powerSetPin(POWER_MATHS_PIN, POWER_MATHS_ON, POWER_MATHS_PIN_OFF);
  #else
  #ifdef POWER_MATHS_PIN
  powerSetPin(POWER_MATHS_PIN, POWER_MATHS_ON);
  #endif
  #endif

  // Make sure off at boot to allow capacitor time
  // #ifdef POWER_MATHS_PIN
  // WII5Power::powerOff(force);
  // #endif

  #ifdef MATHS_RETURNLINE
  // TODO 2024 - Using this to tell Maths to stay on !!!
  pinMode(MATHS_RETURNLINE, OUTPUT);
  digitalWrite(MATHS_RETURNLINE, LOW);
  #endif

  // NOTE: Do not set the off or state - wait until we work out what mode this is in
  //  e.g. Maths may be already on and require to stay on
  step = WII5MATHS_ASK;
  stepLast = WII5MATHS_UNKNOWN;
  stepWait = 0;
};

void WII5Maths::setReturnLine() {
  pinMode(MATHS_RETURNLINE, OUTPUT);
  digitalWrite(MATHS_RETURNLINE, HIGH);
  returnlineWait = 0;
}

void WII5Maths::cancelReturnLine() {
  pinMode(MATHS_RETURNLINE, OUTPUT);
  digitalWrite(MATHS_RETURNLINE, LOW);
}

void WII5Maths::gpioClick() {
  // elapsedGpio = 0;
}

// Already on - we don't want to boot up.... assume it is on. Lets assume ASK
void WII5Maths::woops_on() {
  // Over simplified - only when off. Race condition here from WARN to OFF, but safe
  switch(step) {
    case WII5MATHS_OFF:
      wii5Display.statusSend(F("Maths"), F("woopson"));
      console.log(LOG_DEBUG, F("Maths woops_on - got a hello or similar, switch to ready"));
      step = WII5MATHS_READY; stepWait = 0;
  }
}

bool WII5Maths::isMode(WII5_MATHSMODE mm) {
  return (mathsMode == mm);
}

void WII5Maths::start(WII5_MATHSMODE mm) {
  mathsMode = mm;
  console.log(LOG_DEBUG, F("Maths: start, step=%d off status=%d"), step, WII5MATHS_OFF);
  switch(step) {
    case WII5MATHS_OFF:
    case WII5MATHS_WARN:
    case WII5MATHS_ASK:
    case WII5MATHS_HOLD_ON:
      wii5Display.statusSend(F("Maths"), F("ModeSwitch=boot"));
      console.log(LOG_DEBUG, F("Maths: Changing to BOOT, step=%d off status=%d"), step, WII5MATHS_OFF);
      step = WII5MATHS_BOOT; stepWait = 0;
      retry = 0;
  }
}
void WII5Maths::stop(bool force) {
  if (step != WII5MATHS_OFF) {
    // Delays - request off, that sort of thing... ! YES
    console.log(LOG_INFO, F("Maths: stop called, hold was=%lu, now=0"), remainingHold());
    holdFor = 0;
    step = WII5MATHS_WARN; stepWait = 0;
  }
}

bool WII5Maths::isOff() {
  return (step == WII5MATHS_OFF);
}
bool WII5Maths::isRunning() {
  return (step != WII5MATHS_OFF);
}

void WII5Maths::setDebug(bool in) {
  debug = in;
}
bool WII5Maths::getDebug() {
  return debug;
}

// NOTE: Turn on/off shared - even when not necessary, when Maths is on
void WII5Maths::powerOn(bool force) {
  // Turn on shared here always, but at boot, turn on shared and delay for capacitor charge 
  wii5Controller.shared5On();
  WII5Power::powerOn(force);
}

// Block powerOff maybe
void WII5Maths::powerOff(bool force) {
  // Leaves it i the mode, e.g. OFF, but set powerOn
  if ((mathsUntil == 0) || (mathsUntil < now())) {
    wii5Display.statusSend(F("Maths"), F("power=off,DISABLED"));
    mathsUntil = 0;

    wii5Display.atMathsSend(F("shutdown"), F("actual,power off"));
    #ifdef POWER_MATHS_PIN
    WII5Power::powerOff(force);
    #endif
    wii5Controller.shared5Off();
  }
}

// Set hold time and switch to HOLD mode
//  - allowLower is false by default, so it won't change unless bigger
void WII5Maths::setHold(uint32_t in, bool allowLower) {
  // TODO - check Maths CPU is on. Reset hold count. Set how long for, and tell it to HOLD
  if ((in == 0) || allowLower || (remainingHold() < in)) {
    holdWait = 0;
    holdFor = in;  // Seconds, not milliseconds
  }

  // if state is maths off - fix it
  switch(step) {
    case WII5MATHS_OFF:
    case WII5MATHS_WARN:
    case WII5MATHS_ASK:
      step = WII5MATHS_HOLD_ON; stepWait = 0;
      break;
  }
  // Always make sure power is on.
  powerOn(true);
}

void WII5Maths::cancelHold() {
  if (holdFor > 0) {
    console.log(LOG_INFO, F("Maths: cancelHold called, hold was=%lu, now=0"), remainingHold());
    holdFor = 0;
    powerOff();
    step = WII5MATHS_OFF; stepWait = 0;
  }
}

// SUPER simple send Queue
void WII5Maths::sendQueue() {
  // TODO how to know that it has been done... lets just try three times.
  if (queueCount > 0) {
    console.log(LOG_FATAL, F("MathsQeuue: SENDING %s. Retry=%d"), queueMessage[queueCount - 1], queueRetry);
    console.printf(F("@Maths,%s\n"), queueMessage[queueCount - 1]);
    queueRetry++;

    if (queueRetry > 3) {
      queueDone();
    }
  }
}

// Mark first as done
void WII5Maths::queueDone() {
  queueRetry = 0;
  if (queueCount > 1) {
    // Copy the message 1 to 0
    strncpy(queueMessage[0], queueMessage[1], MATHS_QUEUE_BUFFER);
    queueCount = 1;
    console.log(LOG_FATAL, F("MathsQueue: Second queued messaged moved to first"));
  }
  else {
    queueCount = 0;
    console.log(LOG_FATAL, F("MathsQueue: All messages done"));
  }
}

bool WII5Maths::queueCommand(char *val, uint8_t valSize) {
  // Not accessed queue in over 15 hours - lets clear it
  if (queueLast > 54000000) {
    if (queueCount != 0) {
      console.log(LOG_INFO, F("NOTE: Maths queue was cleared - not used in 15 hours"));
      queueCount = 0;
    }
  }
  queueLast = 0;
  if (queueCount == 0)
    queueRetry = 0;
  if (queueCount >= MATHS_QUEUE_MAX) {
    console.log(LOG_INFO, F("Maximum queued messages still waiting"));
    return false;
  }

  strncpy(queueMessage[queueCount], val, valSize < MATHS_QUEUE_BUFFER ? valSize : MATHS_QUEUE_BUFFER);
  queueMessage[queueCount][MATHS_QUEUE_BUFFER - 1] = '\0';
  console.log(LOG_INFO, F("MathsQueue: Added new request = @Maths,%s"), queueMessage[queueCount]);
  queueCount++;

  // Turn on and hold to do the above
  setHold(900);

  return true;
}

// Manual Hold Processing
//  - Check if Maths is still on - if off, close off hold and exit
//  - Manually check if we are still within hold period.
//  - If inside - just wait longer
//  - If outside - shut down Maths
//    . We assume this is only called during non normal maths loop modes.
bool WII5Maths::processHold() {
  // First time booted, we ASK the maths cpu - this should not last more than a minute
  if (step == WII5MATHS_ASK) {
    return true;
  }

  // Not actually running
  if (holdFor == 0) {
    return false;
  }

  // TODO console check for Maths held longer, or power OFF
  if (holdWait > (1000 * holdFor)) {
    cancelHold();
    return false;
  }

  return true;
}

uint32_t WII5Maths::remainingHold() {
  if (holdFor > MATHS_MAXHOLD) holdFor = MATHS_MAXHOLD;
  // NOTE: Safety, if checking remaining then break out if error
  if (holdWait > (MATHS_MAXHOLD * 1000)) {
    console.log(LOG_INFO, F("Maths: error holdWait too long = %lu, holdFor=%lu"), holdWait, holdFor);
    holdFor = 0;
    holdWait = 0;
    return 0;
  }
  return holdFor > 0 ? (holdFor - (holdWait / 1000)) : 0;
}

uint32_t WII5Maths::remainingUntil() {
  return mathsUntil > now() ? (mathsUntil - now()) : 0;
}

void WII5Maths::loop() {
  // Every hour cancel return line
  if ((returnlineWait/1000) > MATHS_RETURNHOLD) {
    cancelReturnLine();
    returnlineWait = 0;
  }

  bool first = (stepLast != step);
  stepLast = step;

  switch (step) {
    // ASK: This mode will ask the Maths CPU what state to be in
    //  - Timeout/noanswer will move on to OFF
    //  exit = hello response or timeout
    case WII5MATHS_ASK:
      // Setup as if in HOLD mode, but DO NOT turn on the power (this allows reboots without Pi)
      // TODO Mayube we want it
      holdWait = 0;
      if (holdFor < 900)
        holdFor = 900;
      step = WII5MATHS_HOLD_ON; stepWait = 0;
      break;

    // WARN: - Show Maths that it is about to be shut down then wait...
    case WII5MATHS_WARN:
      if (first) {
        if (remainingHold() > 0) {
          step = WII5MATHS_HOLD_ON; stepWait = 0;
        }
        else {
          wii5Display.statusSend(F("Maths"), F("mode=warn,power off,10s"));
          wii5Display.atMathsSend(F("shutdown"), F("now,15 seconds"));
          stepWait = 0;
        }
      }
      // TODO - consider sending updates every 5 seconds
      else if (stepWait > 10000) {
        // Direct to OFF - beacuse this is WARN
        step = WII5MATHS_OFF; stepWait = 0;
      }
      break;

    // OFF: Sends a message going off but then powers off !
    //  exit = NA - Stays here forever, until outside source changes step
    case WII5MATHS_OFF:
      if (first) {
        wii5Display.atMathsSend(F("shutdown"), F("now,power off"));
        wii5Display.statusSend(F("Maths"), F("mode=off"));
        console.log(LOG_DEBUG, F("MathsLoop. step=OFF first=%d"), first);
        powerOff(true);
        mathsMode = WII5MATHSMODE_UNKNOWN;
      }

      // TODO Normally maths is turned on as needed, but here we can have
      // maybe a situation where it hasn't been on for too long? And consider it?

      break;

    // REBOOT: Powers off the device, waits 10 seconds then back to BOOT
    //  exit = 10 secoonds back to BOOT
    case WII5MATHS_REBOOT:
      if (first) {
        powerOff(true);
        wii5Display.atMathsSend(F("reboot"), F("now"));
        wii5Display.statusSend(F("Maths"), F("reboot"));
        console.log(LOG_DEBUG, F("MathsLoop. step=REBOOT first=%d"), first);
      }
      // Reboot - turn off for 10 seconds then back to BOOT
      if (stepWait > 10000) {
        console.log(LOG_DEBUG, F("MATHS: step reboot -> boot"));
        step = WII5MATHS_BOOT; stepWait = 0;
      }
      break;

    // BOOT: Turn the power back on, and jump to BOOT_WAIT
    case WII5MATHS_BOOT:
      if (first) {
        console.log(LOG_INFO, F("MathsLoop. step=BOOT first=%d"), first);
        wii5Display.statusSend(F("Maths"), F("boot"));
        wii5Controller.shared5On();
        // setStatus(WII5STATUS_WAITING);
      }
      // TODO 2024 - Look at if this is required or else immediate?
      // TODO 200ms for capacitor charge? TODO 10 seconds for now for testing
      // else if (stepWait > 10000) 
      else {
        #ifdef POWER_MATHS_PIN
        powerOn();
        #endif
        console.log(LOG_DEBUG, F("MATHS: step boot -> wait"));
        step = WII5MATHS_BOOT_WAIT; stepWait = 0;
      }
      break;

    // BOOT_WAIT - We twiddlge our thumbs, waiting for the CPu to respond to us
    //  exit - Hello or similar command from the Maths, or Timeout
    //    NOTE: On timeout, will probably try reboot, but this is
    //    where we may get into far more complex code checking for
    //    limited retries etc
    case WII5MATHS_BOOT_WAIT:
      if (first) {
        console.log(LOG_INFO, F("MathsLoop. step=BOOT_WAIT first=%d"), first);
        wii5Display.statusSend(F("Maths"), F("wait"));
        displayWait = 30000;
      }
      // Do we have a recent hello from the Maths
      else if (
        wii5Controller.checkLastHello(WII5DEVICE_MATHS, powerElapsed())
      ) {
        // SUCCESS - Got a hello from a Maths CPU since we have turned power pin on
        console.log(LOG_DEBUG, F("MATHS: step wait -> ready"));
        step = WII5MATHS_READY; stepWait = 0;
      }
      else if (displayWait > 30000) {
        // Send Mode
        if (mathsMode == WII5MATHSMODE_BUTTON) {
          wii5Display.atMathsSend(F("mode"), F("button,no hello"));
        }
        else if (mathsMode == WII5MATHSMODE_COMMS) {
          wii5Display.atMathsSend(F("mode"), F("comms,no hello"));
        }
        else if (remainingHold() > 0) {
          wii5Display.atMathsSend(F("mode"), F("hold,no hello"));
        }
        else if (mathsMode == WII5MATHSMODE_AUTO) {
          wii5Display.atMathsSend(F("mode"), F("auto,no hello"));
        }
        else {
          wii5Display.atMathsSend(F("mode"), F("unknown,no hello"));
        }

        sendTime();
        displayWait = 0;
      }
      else if (stepWait > 120000) {
        retry++;
        if (retry > 2) {
          console.log(LOG_ERROR, F("MATHS: Timeout waiting for boot, switching off (multiple retries)"));
          step = WII5MATHS_OFF; stepWait = 0;
        }
        else {
          console.log(LOG_ERROR, F("MATHS: Timeout retry=%d"), retry);
          step = WII5MATHS_REBOOT; stepWait = 0;
        }
      }
      break;

    // READY: and waiting patiently for you to tell it what to do.
    //  - Maths may shut itself down, or send hold commands
    case WII5MATHS_READY:
      if (first) {
        console.log(LOG_DEBUG, F("MathsLoop. step=READY first=%d"), first);
        wii5Display.statusSend(F("Maths"), F("ready"));
        displayWait = 15000;
      }
      else if (displayWait > 15000) {
        if (mathsMode == WII5MATHSMODE_BUTTON) {
          wii5Display.atMathsSend(F("mode"), F("button,waiting"));
        }
        else if (mathsMode == WII5MATHSMODE_COMMS) {
          wii5Display.atMathsSend(F("mode"), F("comms,waiting"));
        }
        else if (remainingHold() > 0) {
          wii5Display.atMathsSend(F("mode"), F("hold,waiting"));
        }
        else if (mathsMode == WII5MATHSMODE_AUTO) {
          wii5Display.atMathsSend(F("mode"), F("auto,waiting"));
        }
        else {
          wii5Display.atMathsSend(F("mode"), F("unknown,waiting"));
        }
        sendQueue();
        sendTime();
        displayWait = 0;
      }
      // Wait 15 minutes - this is our worst case waiting - something gone wrong but looks right
      else if (stepWait > (900000)) {
        console.log(LOG_DEBUG, F("MATHS: step ready -> shutdown_wait"));
        step = WII5MATHS_WARN; stepWait = 0;
      }
      break;

    // HOLD_ON: Keep Maths CPU ON - there needs to be a maximum timeout, and maybe switch to ASK
    //  e.g. wait 1 hour, then ASK then 1 hour?
    case WII5MATHS_HOLD_ON:
      if (first) {
        wii5Display.atMathsSend(F("mode"), F("hold,remaining=%lu,seconds"), remainingHold());
        console.log(LOG_DEBUG, F("MathsLoop. step=HOLD_ON first=%d, holding for %lu seconds"), first, holdFor);
        wii5Display.statusSend(F("maths"), F("holding,%lu,seconds"), remainingHold());
        displayWait = 0;
      }
      else if (holdWait > (1000 * holdFor)) {
        // Have we told them we are shutting down? Maybe that is in the OFF
        holdFor = 0;
        wii5Display.statusSend(F("maths"), F("holding,end"));
        wii5Display.atMathsSend(F("mode"), F("auto"));
        step = WII5MATHS_WARN; stepWait = 0;
      }
      else if (displayWait > 15000) {
        wii5Display.atMathsSend(F("mode"), F("hold,remaining=%lu"), remainingHold());
        wii5Display.statusSend(F("maths"), F("holding,%lu,seconds"), remainingHold());
        sendQueue();
        sendTime();
        displayWait = 0;
      }
      // if (stepWait > (60 * 60 * 1000)) {
      //   TODO Force OFF !!!
      // }
      break;

    case WII5MATHS_FINISH:
      if (first)
        console.log(LOG_INFO, F("MathsLoop. step=FINISH first=%d"), first);
      console.log(LOG_DEBUG, F("MATHS: step finish -> off"));
      step = WII5MATHS_WARN; stepWait = 0;
      break;

    default:
      console.log(LOG_FATAL, F("MathsLoop: Default step... step=%d"), step);
      step = WII5MATHS_WARN; stepWait = 0;
      break;
      // TODO Error?
  }
  return;
}

void WII5Maths::sendTime() {
  if (timeWait < 60000) {
    return;
  }
  timeWait = 0;
  if (wii5Gps.isTimeValid()) {
    console.log(LOG_DEBUG, F("Sending time - valid from GPS"));
    wii5Display.atMathsSend(F("time"), F("%04d,%02d,%02d,%02d,%02d,%02d"),
      int(year()), int(month()), int(day()),
      int(hour()), int(minute()), int(second())
    );
  }
  else {
    console.log(LOG_DEBUG, F("Not sending time - not valid from GPS"));
  }
}
// Set time to be held on, and force ON if not already
void WII5Maths::setMathsUntil(time_t t) {
  if (t > now()) {
    mathsUntil = t;
  }
  else {
    mathsUntil = 0;
  }
}
time_t WII5Maths::getMathsUntil() {
  return mathsUntil;
}

WII5Maths wii5Maths;