#include <SPI.h>
#include <SoftwareSerial.h>
#include <Adafruit_MCP2515.h>
#include <CANTS_CANTSProtocol.h>

#include "hardware/watchdog.h"

#include "Config.h"
#include "TMTC.h"

#define COLOR_RED (0xFF0000)
#define COLOR_GREEN (0x00FF00)
#define COLOR_BLUE (0x0000FF)

#define ERROR_CODE_RF69_INIT_FAIL (2)
#define ERROR_CODE_RF69_FREQ_FAIL (3)
#define ERROR_CODE_MCP25625_BEGIN_FAIL (4)
#define ERROR_CODE_CANTS_INIT_FAIL (5)

// The encryption key used for the RF communications
uint8_t RFEncryptionKey[] = {
  0x5A, 0x71, 0x15, 0xA7, 0x11, 0x5A, 0x71, 0x15,
  0xA7, 0x11, 0x5A, 0x71, 0x15, 0xA7, 0x11, 0x5A
};

uint8_t RFReceiveBuffer[CONFIG_RF_RECEIVE_BUFFER_SIZE];
uint8_t UARTReceiveBuffer[CONFIG_UART_RECEIVE_BUFFER_SIZE];

// Used for thread to blink NeoPixel to indicate RF activity
bool RFActivity = false;

// The controller object to interact with the MCP25625 CAN transceiver
Adafruit_MCP2515 MCPController(CONFIG_MCP25625_PIN_CHIP_SELECT, CONFIG_MCP25625_PIN_MOSI, CONFIG_MCP25625_PIN_MISO, CONFIG_MCP25625_PIN_CLOCK);

void HardFault(uint8_t ErrorCode)
{
  // Turn off LED
  NeoPixelHandle.setPixelColor(0, 0);
  NeoPixelHandle.show();

  delay(500);

  for (uint8_t BlinkCount = 0; BlinkCount < ErrorCode; ++BlinkCount)
  {
    NeoPixelHandle.setPixelColor(0, COLOR_RED);
    NeoPixelHandle.show();
    delay(200);
    NeoPixelHandle.setPixelColor(0, 0);
    NeoPixelHandle.show();
    delay(200);
  }

  delay(2000);
  watchdog_reboot(0, 0, 0);
}

// Transmit bytes over RF and block until they're sent (no acknowledgmenet)
void TransmitBytes(uint8_t* BytesToSend, uint8_t BufferSize)
{
  // The driver 'send' function disallows a buffer size of 0
  if (BufferSize == 0) return;

  // Indicate RF activity with the LED (refer to loop1())
  RFActivity = true;

  // Send the buffer of data
  RF69Driver.send(BytesToSend, BufferSize);

  // Block until all bytes have been sent
  RF69Driver.waitPacketSent();
}

void OnCANPacketReceived(int PacketSize)
{
  tsCANTS_FRAME_GENERIC tsReceivedFrame = { 0U };

  // The CAN packet ID
  long iPacketID = MCPController.packetId();

  /* Populate the fields within the CAN-TS frame */
  /* This extracts the CAN-TS fields out of the identifier and assigns them to the frame */
  /* CAN-TS identifier format:
   * [ 8 bits: To Address ][ 3 bits: Type ][ 8 bits: From Address ][ 10 bits: Command ]
   */
  tsReceivedFrame.tsIdentifiers.iToAddress = ((iPacketID & 0x1FE00000) >> 21);
  tsReceivedFrame.tsIdentifiers.iFrameType = ((iPacketID & 0x1C0000) >> 18);
  tsReceivedFrame.tsIdentifiers.iFromAddress = ((iPacketID & 0x3FC00) >> 10);
  tsReceivedFrame.tsIdentifiers.iCommand = ((iPacketID & 0x3FF) >> 0);

  tsReceivedFrame.iDataLength = PacketSize;

  /* Copy the data into the frame */
  for (tUINT32 iByteIndex = 0U; iByteIndex < PacketSize; ++iByteIndex)
  {
    tsReceivedFrame.acData[iByteIndex] = (tUINT8)(MCPController.read());
  }

  CANTS_HandleFrame(&tsReceivedFrame);
}

teFUNC_STATUS CallbackSendCANFrame(const tsCANTS_FRAME_GENERIC* tsFrame)
{
  // Contains the frame identifiers as an integer
  tUINT32 iIdentifiers = 0U;

  // Shift the identifiers in
  iIdentifiers |= ((tsFrame->tsIdentifiers.iToAddress) << 21);
  iIdentifiers |= ((tsFrame->tsIdentifiers.iFrameType) << 18);
  iIdentifiers |= ((tsFrame->tsIdentifiers.iFromAddress) << 10);
  iIdentifiers |= ((tsFrame->tsIdentifiers.iCommand) << 0);

  // Begin the extended CAN packet with the 29-bit identifiers
  MCPController.beginExtendedPacket(iIdentifiers);

  // Write the data bytes into the packet
  MCPController.write(tsFrame->acData, tsFrame->iDataLength);

  // Send the CAN frame
  MCPController.endPacket();

  return BT_SUCCESS;
}

void InitialiseCAN()
{
  // Start the MCP controller
  uint32_t MCPBeginStatus = MCPController.begin(CONFIG_CAN_BAUDRATE);

  // Ensure that the MCP25625 controller successfully started
  if (MCPBeginStatus != 1)
  {
    HardFault(ERROR_CODE_MCP25625_BEGIN_FAIL);
  }

  // Assign the interrupt callback for the MCP controller
  MCPController.onReceive(CONFIG_MCP25625_PIN_INTERRUPT, OnCANPacketReceived);

  /* Setup the CAN-TS node address and callbacks */
  tsCANTS_CONFIGURATION tsCANTSConfig = { 0 };
  tsCANTSConfig.iNodeAddress = CONFIG_CANTS_NODE_ADDRESS;
  tsCANTSConfig.funcSendFrame = CallbackSendCANFrame;

  tsCANTSConfig.funcCallbackOnTelemetryRequest = OnTelemetryRequest;
  tsCANTSConfig.funcCallbackOnTelecommandRequest = OnTelecommandRequest;

  teFUNC_STATUS CANTSInitialiseStatus = CANTS_Initialise(&tsCANTSConfig);

  if (CANTSInitialiseStatus != BT_SUCCESS)
  {
    HardFault(ERROR_CODE_CANTS_INIT_FAIL);
  }

  MCPController.filter(0x05, 0x7F8);
}

void setup()
{
  // Begin the UART serial output driver
  Serial1.begin(CONFIG_UART_BAUDRATE);
  Serial.begin(CONFIG_UART_BAUDRATE);

  // Initialise the NeoPixel LED
  NeoPixelHandle.begin();
  NeoPixelHandle.show();
  NeoPixelHandle.setBrightness(20);

  // Configure the RF69 Reset Pin as an GPIO output
  pinMode(CONFIG_RF69_RESET_PIN, OUTPUT);

  // Set the jumpers as input pins
  pinMode(CONFIG_JUMPER_REGION_PIN, INPUT);
  pinMode(CONFIG_JUMPER_ADDRESS_0_PIN, INPUT);
  pinMode(CONFIG_JUMPER_ADDRESS_1_PIN, INPUT);
  pinMode(CONFIG_JUMPER_ADDRESS_2_PIN, INPUT);

  // Read the jumper values
  uint8_t RegionSelection = digitalRead(CONFIG_JUMPER_REGION_PIN);
  uint8_t Address0 = digitalRead(CONFIG_JUMPER_ADDRESS_0_PIN);
  uint8_t Address1 = digitalRead(CONFIG_JUMPER_ADDRESS_1_PIN);
  uint8_t Address2 = digitalRead(CONFIG_JUMPER_ADDRESS_2_PIN);

  // Get the Comms address from the jumper values
  RFAddress |= (RegionSelection << 3);
  RFAddress |= (Address0 << 2);
  RFAddress |= (Address1 << 1);
  RFAddress |= (Address2 << 0);

  // Start the RF69 Reset Pin at low
  digitalWrite(CONFIG_RF69_RESET_PIN, LOW);

  // Perform a board reset for the RF69
  digitalWrite(CONFIG_RF69_RESET_PIN, HIGH);
  delay(10);
  digitalWrite(CONFIG_RF69_RESET_PIN, LOW);

  if (!RF69Driver.init())
  {
    HardFault(ERROR_CODE_RF69_INIT_FAIL);
  }

  // If low then use Eruope frequencies
  if (RegionSelection == LOW)
  {
    RFFrequency = EuropeFrequencies[RFAddress & 0x07];
  }
  // Else high uses US frequencies
  else
  {
    RFFrequency = USFrequencies[RFAddress & 0x07];

    // US frequencies not implemented
    HardFault(ERROR_CODE_RF69_FREQ_FAIL);
  }

  // Set the frequency being used
  if (!RF69Driver.setFrequency(RFFrequency))
  {
    HardFault(ERROR_CODE_RF69_FREQ_FAIL);
  }

  // Enable high power mode and set the dBm
  RF69Driver.setTxPower(RFTransmitPower, true);

  // Set the RF encryption key
  RF69Driver.setEncryptionKey(RFEncryptionKey);

  // Setup the MCP controller and CAN-TS object
  InitialiseCAN();

  // Default color to green
  NeoPixelHandle.setPixelColor(0, COLOR_GREEN);
  NeoPixelHandle.show();
}

void loop()
{
  // If there are RF bytes pending to be received
  if (RF69Driver.available() == true)
  {
    RFActivity = true;

    // Receive the pending RF buffer
    uint8_t RFBufferSize = CONFIG_RF_RECEIVE_BUFFER_SIZE;
    RF69Driver.recv(RFReceiveBuffer, &RFBufferSize);

    // Send the received RF bytes over the UART
    Serial1.write(RFReceiveBuffer, RFBufferSize);
  }

  // Get the number of bytes pending on UART
  size_t UARTBytesToRead = Serial1.available();

  // If there are UART bytes waiting to be reada
  if (UARTBytesToRead > 0)
  {
    // Set the number of bytes to read, not exceeding the buffer size
    // This is done so the Serial object doesn't timeout waiting for more bytes to be received
    if (UARTBytesToRead > CONFIG_UART_RECEIVE_BUFFER_SIZE)
    {
      UARTBytesToRead = CONFIG_UART_RECEIVE_BUFFER_SIZE;
    }

    // Receive the pending UART bytes
    uint8_t UARTBytesWritten = Serial1.readBytes(UARTReceiveBuffer, UARTBytesToRead);

    // Transmit the bytes over UART
    TransmitBytes(UARTReceiveBuffer, UARTBytesWritten);
  }

  // Invoke the telemetry and telecommand file loop
  TMTCLoop();
}

void loop1()
{
  if (RFActivity == true)
  {
    RFActivity = false;

    NeoPixelHandle.setPixelColor(0, COLOR_BLUE);
    NeoPixelHandle.show();
    delay(50);
    NeoPixelHandle.setPixelColor(0, COLOR_GREEN);
    NeoPixelHandle.show();
  }
}