#include "BT_BaseTypes.h"
#include "hardware/watchdog.h"

uint16_t CommsStatusBuffer[4] = { 0 };

teFUNC_STATUS OnTelemetryRequest(const tsCANTS_FRAME_TELEMETRY_REQUEST* TelemetryRequest)
{
  // Handle the response for the specific telemetry channel
  switch (TelemetryRequest->iChannel)
  {
    case 0: /* Local Comms Temperature */
      CANTS_SendTelemetryAcknowledgement(TelemetryRequest->iChannel, TelemetryRequest->iFromAddress, 0, 0, CANTS_POSITIVE_ACKNOWLEDGEMENT);
      break;

    case 1: /* Comms Status - Frequency, Received signal strength, Transmit power, Frame error */
      CANTS_SendTelemetryAcknowledgement(TelemetryRequest->iChannel, TelemetryRequest->iFromAddress, (uint8_t*)CommsStatusBuffer, 8, CANTS_POSITIVE_ACKNOWLEDGEMENT);

      break;

    default:
      // Send negative acknowledgement because the channel isn't supported
      CANTS_SendTelemetryAcknowledgement(TelemetryRequest->iChannel, TelemetryRequest->iFromAddress, 0, 0, CANTS_NEGATIVE_ACKNOWLEDGEMENT);
      break;
  }

  return BT_SUCCESS;
}

teFUNC_STATUS OnTelecommandRequest(const tsCANTS_FRAME_TELECOMMAND_REQUEST* TelecommandRequest)
{
  // Forward definitions due to label jumps
  uint8_t NewTXPower = CONFIG_RF69_DEFAULT_TRANSMIT_POWER_DBM;
  uint8_t ColorR, ColorG, ColorB;
  float NewFrequency;
  uint8_t FrequencyIndex;

  if(TelecommandRequest->iToAddress == CONFIG_CANTS_NODE_ADDRESS)
  {
    switch (TelecommandRequest->iChannel)
    {
      case 0: /* Set frequency */
        // Check the parameter is the correct length
        if (TelecommandRequest->iDataLength != 1)
        {
          Serial.println("Set Frequency TC contains invalid parameter length");
          goto LABEL_NEGATIVE_ACKNOWLEDGEMENT;
        }

        // Get the frequency index number
        FrequencyIndex = TelecommandRequest->acData[0];

        // Ensure the frequency is within bounds
        if (FrequencyIndex > 7)
        {
          Serial.println("Set Frequency TC contains invalid parameter length");
          goto LABEL_NEGATIVE_ACKNOWLEDGEMENT;
        }

        // Assign the new frequency value
        NewFrequency = EuropeFrequencies[FrequencyIndex];

        // Set the new frequency
        if (!RF69Driver.setFrequency(NewFrequency))
        {
          Serial.println("Failed to set new frequency");
          goto LABEL_NEGATIVE_ACKNOWLEDGEMENT;
        }

        // Update the global
        RFFrequency = NewFrequency;

        // Send positive acknowledgement
        CANTS_SendTelecommandAcknowledgement(TelecommandRequest->iChannel, TelecommandRequest->iFromAddress, CANTS_POSITIVE_ACKNOWLEDGEMENT);

        break;

      case 1:/* Set TX power */
        // Check the parameter is the correct length
        if (TelecommandRequest->iDataLength != 1)
        {
          Serial.println("Set TX Power TC contains invalid parameter length");
          goto LABEL_NEGATIVE_ACKNOWLEDGEMENT;
        }

        // Get the new TX power value
        NewTXPower = TelecommandRequest->acData[0];

        // Ensure it does not exceed the maximum power
        if (NewTXPower > CONFIG_RF69_MAXIMUM_TRANSMIT_POWER_DBM)
        {
          Serial.println("Set TX Power value too high");
          goto LABEL_NEGATIVE_ACKNOWLEDGEMENT;
        }

        // Keep the global up to date
        RFTransmitPower = NewTXPower;

        // Set the new transmit power
        RF69Driver.setTxPower(RFTransmitPower, true);

        // Send positive acknowledgement
        CANTS_SendTelecommandAcknowledgement(TelecommandRequest->iChannel, TelecommandRequest->iFromAddress, CANTS_POSITIVE_ACKNOWLEDGEMENT);

        break;

      case 2: /* NOT IN DB: Set LED color until overwritten */
        // Check the parameter is the correct length
        if (TelecommandRequest->iDataLength != 3)
        {
          Serial.println("Set LED Color TC contains incorrect data length");
          goto LABEL_NEGATIVE_ACKNOWLEDGEMENT;
        }

        ColorR = TelecommandRequest->acData[0];
        ColorG = TelecommandRequest->acData[1];
        ColorB = TelecommandRequest->acData[2];

        NeoPixelHandle.setPixelColor(0, ColorR, ColorG, ColorB);
        NeoPixelHandle.show();

        // Send positive acknowledgement
        CANTS_SendTelecommandAcknowledgement(TelecommandRequest->iChannel, TelecommandRequest->iFromAddress, CANTS_POSITIVE_ACKNOWLEDGEMENT);

        break;

      case 3: /* NOT IN DB: Perform RP2040 reboot */
        CANTS_SendTelecommandAcknowledgement(TelecommandRequest->iChannel, TelecommandRequest->iFromAddress, CANTS_POSITIVE_ACKNOWLEDGEMENT);

        delay(1000);

        watchdog_reboot(0, 0, 0);

        break;

      default:
  LABEL_NEGATIVE_ACKNOWLEDGEMENT:
        // Send negative acknowledgement for unknown telecommand
        CANTS_SendTelecommandAcknowledgement(TelecommandRequest->iChannel, TelecommandRequest->iFromAddress, CANTS_NEGATIVE_ACKNOWLEDGEMENT);
        break;
    }
  }

  return BT_SUCCESS;
}

void TMTCLoop()
{
  static unsigned long PreviousSendTimeMs = 0;
  unsigned long CurrentTimeMs = millis();

  if (CurrentTimeMs >= (PreviousSendTimeMs + CONFIG_COMMS_UTLM_PERIOD_MS))
  {
    CommsStatusBuffer[0] = (uint16_t)((RFFrequency - 800.0) * 100.0); /* Polynomial in the DB is [0, 0.01, 800] */
    CommsStatusBuffer[1] = (uint16_t)RF69Driver.lastRssi();
    CommsStatusBuffer[2] = (uint16_t)RFTransmitPower;
    CommsStatusBuffer[3] = (uint16_t)RF69Driver.rxBad();

    PreviousSendTimeMs = CurrentTimeMs;

    CANTS_SendUnsolicitedTelemetry(
      1, /* Comms status channel */
      1, /* Send to OBC*/
      (uint8_t*)CommsStatusBuffer, /* Telemetry to send */
      8 /* Data length */
    );
  }
}