/*
 *  AX.25 protocol frame structure:
 *  -------------------------------
 *
 *  Flag Field: 8 bits (1 byte)
 *  Header Field: 24 bits (3 bytes)
 *      {
 *    Destination Address: 8 bits (1 bytes) *
 *    Source      Address: 8 bits (1 bytes) *
 *    Control Bits       : 8  bits (1 byte ) * Control Bits will be  (length of data-1) .
 *  }
 *  Data Field: 32-2048 bits (4-256 bytes)
 *  Frame Checksum Field: 16 bits (2 bytes)
 *  Flag Field: 8 bits (1 byte)
 *
 */

#include <Crc16.h>
Crc16 crc;

unsigned char AX25_Flag = 0x7E;    // flag = 0b01111110;
unsigned char AX25_Header[3]  = {0xAA,0xBB,0}; //0xAA is the GND Station Adress - 0xBB is the Sourse Address - the third byte will be data length-1
// Data is 7 bytes long, with 2 authentication bytes
unsigned char AX25_Data[9]   = {0};
unsigned char AX25_CheckSum[2] = {0};

// Authentication Key
unsigned char Auth_Key[2] = {0xAB, 0xCD};

// LUK Counter
unsigned char LUK = 0;

int inByte = 0;
int command= 0;
unsigned short value = 0;
int serav = 0;

void setup()
{
  // Serial1 : TX pin 18, RX pin 19
  // Serial2 : TX pin 16, RX pin 17
  // Serial1 Ground Station Interface
  // Serial2 On Board Computer interface
  // Serial3 Peripheral Interface

  Serial1.begin(9600);
  Serial2.begin(9600);
  Serial3.begin(9600);

}

void loop()
{

  // Wait until Command is received on Serial2 port
  while ( !(Serial1.available()) );

  // Read received data
  Serial1.readBytes(AX25_Flag,1);
  Serial1.readBytes(AX25_Header,3);
  Serial1.readBytes(AX25_Data,9);
  Serial1.readBytes(AX25_CheckSum,2);

  // CRC Check on the received command without the two authentication bytes
  crc.clearCrc();
  crc.updateCrc((int)(AX25_Data >> 16));
  value = crc.getCrc();
  if ( value != AX25_CheckSum )
  {
      // Send Ground station 'fail' flag
      AX25_Data = {'F','A','I','L',0,0,0,0,0}
      Serial1.write(AX25_Flag);
      Serial1.write(AX25_Header,3);
      Serial1.write(AX25_Data,9);
      Serial1.write(AX25_CheckSum,2);

      // Freeze afterward
      while(1);
  }

  // Authentication Check on the received data
  if ( (AX25_Data & 0xFFFF) ^ Auth_Key < LUK++ )
  {
      // Send Ground station 'fail' flag
      AX25_Data = {'F','A','I','L',0,0,0,0,0}
      Serial1.write(AX25_Flag);
      Serial1.write(AX25_Header,3);
      Serial1.write(AX25_Data,9);
      Serial1.write(AX25_CheckSum,2);

      // Freeze afterward
      while(1);
  }

  // Clear the two authentication bytes
  AX25_Data[8] = 0x00;
  AX25_Data[7] = 0x00;

  // Send data to OBC
  Serial1.write(AX25_Flag);
  Serial1.write(AX25_Header,3);
  Serial1.write(AX25_Data,9);
  Serial1.write(AX25_CheckSum,2);

  // Wait until either Serial2 or Serial3 is available

  while (1)
  {
      if ( Serial2.available() )
      {
          serav = 2;
          break;
      }
      if ( Serial3.available() )
      {
          serav = 3;
          break;
      }
  }

  // Receive data from the opened one
  if (serav == 2)
  {
      Serial2.readBytes(AX25_Flag,1);
      Serial2.readBytes(AX25_Header,3);
      Serial2.readBytes(AX25_Data,9);
      Serial2.readBytes(AX25_CheckSum,2);
  }
  else
  {
      Serial3.readBytes(AX25_Flag,1);
      Serial3.readBytes(AX25_Header,3);
      Serial3.readBytes(AX25_Data,9);
      Serial3.readBytes(AX25_CheckSum,2);
  }

  // CRC check on data
  crc.clearCrc();
  crc.updateCrc((int)(AX25_Data >> 16));
  value = crc.getCrc();
  if ( value != AX25_CheckSum )
  {
      // Send Ground station 'fail' flag
      AX25_Data = {'F','A','I','L',0,0,0,0,0}
      Serial1.write(AX25_Flag);
      Serial1.write(AX25_Header,3);
      Serial1.write(AX25_Data,9);
      Serial1.write(AX25_CheckSum,2);

      // Freeze afterward
      while(1);
  }

  // Send to Ground station
  Serial1.write(AX25_Flag);
  Serial1.write(AX25_Header,3);
  Serial1.write(AX25_Data,9);
  Serial1.write(AX25_CheckSum,2);

  // Wait until data is received on Serial2 port
  delay(2000);
}

//Check routine taken from
//http://web.mit.edu/6.115/www/miscfiles/amulet/amulet-help/xmodem.htm
int calcrc(char *ptr, int count)
{
    int  crc;
    char i;
    crc = 0;
    while (--count >= 0)
    {
        crc = crc ^ (int) *ptr++ << 8;
        i = 8;
        do
        {
            if (crc & 0x8000)
                crc = crc << 1 ^ 0x1021;
            else
                crc = crc << 1;
        } while(--i);
    }
    return (crc);
}