#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>

#if defined (__VXWORKS__)
 #include "taskLib.h"
#endif
/* Common Sample Program include files */
#include "include/naiapp_boardaccess_menu.h"
#include "include/naiapp_boardaccess_query.h"
#include "include/naiapp_boardaccess_access.h"
#include "include/naiapp_boardaccess_display.h"
#include "include/naiapp_boardaccess_utils.h"
#include "nai_1553_utils.h"

/* Common 1553 Sample Program include files */
#include "nai_1553_utils.h"
#include "BC/nai_1553_bc_utils.h"

/* naibrd include files */
#include "nai.h"
#include "naibrd.h"
#include "functions/naibrd_1553.h"
#include "functions/naibrd_1553_assisted.h"

static const int8_t *CONFIG_FILE = (int8_t *)"default_1553BC_SendMessageSchedule_FIFO.txt";

/* Function prototypes */
static bool_t Run_M1553_BC_SendMessageSchedule_FIFO(int32_t cardIndex, int32_t module, uint32_t modid);
static bool_t RunSendMessageSchedule(int32_t cardIndex, int32_t module, int32_t channel, uint32_t modid);

/* define message constants */
#define MSG1         1
#define MSG2         2
#define MSG3         3

/* define opcodes */
#define OP1          1
#define OP2          2
#define OP3          3
#define OP4          4
#define OP5          5
#define OP6          6

/* define frame constants */
#define MNR1         1
#define MNR2         2
#define MNR3         3
#define MJR          4

/* define data block numbers */
#define DBLK1        1
#define DBLK2        2
#define DBLK3        3

#define DEF_M1553_CARD_INDEX                 0
#define DEF_M1553_MODULE                     1
#define DEF_M1553_CHANNEL                    1
#define DEF_M1553_DEVNUM                     1

#define RT_ADDRESS                           1

#define RT_SUBADDRESS1                       1  /* Subaddress for Message 1 */
#define RT_SUBADDRESS2                       2  /* Subaddress for Message 2 */
#define RT_SUBADDRESS3                       8  /* Subaddress for Message 3 */

#define WORDCOUNT1                           11 /* Data Word count for Message 1 */
#define WORDCOUNT2                           21 /* Data Word count for Message 2 */
#define WORDCOUNT3                           30 /* Data Word count for Message 3 */

/**************************************************************************************************************/
/**
<summary>
The purpose of the M1553_BC_SendMessageSchedule_FIFO is to illustrate the methods to call in the naibrd library to configure
the 1553 channel as a Bus Controller and to send various messages at different periodic rates. Specifically,
Message 1 is sent at a rate of 40 Hz (every 25 ms), Message 2 is sent at a rate of 20 Hz (every 50 ms) and
Message 3 is sent at a rate of 10 Hz (every 100 ms). Three 2.5 ms minor frames are used to manage the periodicity
of these messages. Minor Frame 1 contains Messages 1, 2 and 3. Minor Frame 2 contains Messages 1 and 2 only. Minor
Frame 3 only contains Message 1. By chaining these frames together in the following order:

Minor Frame 1 | Minor Frame 3 | Minor Frame 2 | Minor Frame 3

Message 1 is sent every 25 ms, Message 2 is sent every 50 ms and Message 3 is sent every 100 ms.

The message will be decoded using FIFO mode.
Note: FIFO mode is available for the FTA, FTB, FTC, FTD, and FTF only.

The following system configuration routines from the nai_sys_cfg.c file are called to assist with the configuration
setup for this program prior to calling the naibrd 1553 routines.
 - ConfigDevice
 - DisplayDeviceCfg
 - GetBoardSNModCfg
 - CheckModule
</summary>
*/
/**************************************************************************************************************/
#if defined (__VXWORKS__)
int32_t M1553_BC_SendMessageSchedule_FIFO(void)
#else
int32_t main(void)
#endif
{
   bool_t stop = FALSE;
   int32_t cardIndex;
   int32_t moduleCnt;
   int32_t module;
   uint32_t moduleID = 0;
   int8_t inputBuffer[80];
   int32_t inputResponseCnt;

   if (naiapp_RunBoardMenu(CONFIG_FILE) == TRUE)
   {
      while (stop != TRUE)
      {
         /* Query the user for the card index */
         stop = naiapp_query_CardIndex(naiapp_GetBoardCnt(), 0, &cardIndex);
         if (stop != TRUE)
         {
            check_status(naibrd_GetModuleCount(cardIndex, &moduleCnt));

            /* Query the user for the module number */
            stop = naiapp_query_ModuleNumber(moduleCnt, 1, &module);
            if (stop != TRUE)
            {
               moduleID = naibrd_GetModuleID(cardIndex, module);
               if ((moduleID != 0))
               {
                  Run_M1553_BC_SendMessageSchedule_FIFO(cardIndex, module, moduleID);
               }
            }
         }

         printf("\nType Q to quit or Enter key to restart application:\n");
         stop = naiapp_query_ForQuitResponse(sizeof(inputBuffer), NAI_QUIT_CHAR, inputBuffer, &inputResponseCnt);
      }
   }

   printf("\nType the Enter key to exit the program: ");
   naiapp_query_ForQuitResponse(sizeof(inputBuffer), NAI_QUIT_CHAR, inputBuffer, &inputResponseCnt);
   naiapp_access_CloseAllOpenCards();

   return 0;
}

/**************************************************************************************************************/
/**
<summary>
Run_M1553_BC_SendMessageSchedule_FIFO initializes the 1553 device as a bus controller (BC),
FIFO mode NAI_1553_MESSAGE_FIFO_MODE, and sends out a message.
This routine demonstrates the following API functions in the 1553 naibrd library:
naibrd_1553_Open
naibrd_1553_Initialize
naibrd_1553_BcDataBlockCreate
naibrd_1553_BcMessageCreateBcToRt
naibrd_1553_BcCommandCreate
naibrd_1553_BcFrameCreate
naibrd_1553_BcDataBlockWrite
naibrd_1553_ClearMsgFIFO
naibrd_1553_BcStart
naibrd_1553_BcStop
naibrd_1553_GetMsgFIFOCount
naibrd_1553_ReadMsgFIFO
naibrd_1553_DecodeFIFOMsg
naibrd_1553_GetMsgFIFOFullStatus
naibrd_1553_Free
</summary>
*/
/**************************************************************************************************************/
static bool_t Run_M1553_BC_SendMessageSchedule_FIFO(int32_t cardIndex, int32_t module, uint32_t modid)
{
   bool_t bQuit = FALSE;
   int32_t channel;
   int32_t MaxChannel = 4;

   if (IsFTx1553(modid))
   {
      MaxChannel = 4;
      bQuit = naiapp_query_ChannelNumber(MaxChannel, DEF_M1553_CHANNEL, &channel);
      if (!bQuit)
      {
         bQuit = RunSendMessageSchedule(cardIndex, module, channel, modid);
      }
   }
   else
      printf("\nThis module does not support 1553 functionality.\n");

   return bQuit;
}

static bool_t RunSendMessageSchedule(int32_t cardIndex, int32_t module, int32_t channel, uint32_t modid)
{
   int32_t i;
   bool_t bQuit = FALSE;
   uint32_t usBus;
   nai_1553_t status;
   uint16_t increment = 0;
   uint16_t aData[32] = {0};
   int16_t aOpCodes[20] = { 0 };
   bool_t bContinue = TRUE;
   int16_t devnum;
   naiDecodedMessageStructureFIFO msgStruct;
   bool_t bSoftware;
   uint32_t fifoCount = 0;
   uint32_t fifoData[1024];
   int32_t currBlockIndex = 0, nextBlockIndex = 0;
   bool_t FIFIOfullStatusBit;
   int8_t inputBuffer[80];
   int32_t inputResponseCnt;

   /* Get the Logical Device Number */
   bQuit = Get1553LogicalDevNum(DEF_M1553_DEVNUM, &devnum);
   if (!bQuit)
   {
      /* Which bus are we firing on? */
      bQuit = GetBus(&usBus);
      if (!bQuit)
      {
         bQuit = Get1553BCSoftwareOverride(TRUE, &bSoftware);
         if (!bQuit)
         {
            /* Open 1553 Device(s) */
            status = naibrd_1553_Open(cardIndex, module, channel, devnum);
            if(status != 0)
            {
               printf("Error: naibrd_1553_Open Ch %d, status = %d", channel, status);
               return TRUE;
            }

            if (bSoftware)
            {
               /* Override external BC_DISABLE and M1760 (In order to configure as BC, this needs to */
               /* be set if BC_DISABLE and M1760 pins are not driven high) */
               naibrd_1553_WriteAuxReg(devnum, 0x2, 0xA000);
            }
            else
            {
               /* Do not override external BC_DISABLE and M1760 Inputs */
               naibrd_1553_WriteAuxReg(devnum, 0x2, 0);
            }

            if (modid == NAI_MODULE_ID_FT8)
            {
               /* Simplex Enable (for NAI internal testing only, do not enable) */
               /*naibrd_1553_WriteAuxReg(devnum, 0x3, 0x4000);*/
               /*naibrd_1553_WriteAuxReg(devnum, 0xF, 0x0001);*/
            }

            /* Reset Device */
            naibrd_1553_WriteAuxReg(devnum, 0x1, 1);

            nai_msDelay(1);

            naibrd_1553_WriteAuxReg(devnum, 0x1, 0);

            /* Initialize 1553 Device(s) */
            status = naibrd_1553_Initialize(devnum, NAI_1553_ACCESS_CARD,NAI_1553_MODE_BC | NAI_1553_MESSAGE_FIFO_MODE,0,0,0);
            if(status != 0)
            {
               printf("Error: naibrd_1553_Initialize Ch %d, status = %d", channel, status);
               return TRUE;
            }

            /* Create BC Data Block 1 */
            status = naibrd_1553_BcDataBlockCreate(devnum, DBLK1, NAI_1553_BC_DATABLOCK_SINGLE, NULL, 0);
            if(status != 0)
            {
               printf("Error: naibrd_1553_BcDataBlockCreate Ch %d, status = %d", channel, status);
               return TRUE;
            }

            /* Create BC Data Block 2 */
            status = naibrd_1553_BcDataBlockCreate(devnum, DBLK2, NAI_1553_BC_DATABLOCK_SINGLE, NULL, 0);
            if(status != 0)
            {
               printf("Error: naibrd_1553_BcDataBlockCreate Ch %d, status = %d", channel, status);
               return TRUE;
            }

            /* Create BC Data Block 3 */
            status = naibrd_1553_BcDataBlockCreate(devnum, DBLK3, NAI_1553_BC_DATABLOCK_SINGLE, NULL, 0);
            if(status != 0)
            {
               printf("Error: naibrd_1553_BcDataBlockCreate Ch %d, status = %d", channel, status);
               return TRUE;
            }

            /* Create BC to RT Message 1 */
            status = naibrd_1553_BcMessageCreateBcToRt(devnum, MSG1, DBLK1, RT_ADDRESS, RT_SUBADDRESS1, WORDCOUNT1, 0, usBus);
            if (status != 0)
            {
               printf("Error: naibrd_1553_BcMessageCreateBcToRt status = %d", status);
               return TRUE;
            }

            /* Create BC to RT Message 2 */
            status = naibrd_1553_BcMessageCreateBcToRt(devnum, MSG2, DBLK2, RT_ADDRESS, RT_SUBADDRESS2, WORDCOUNT2, 0, usBus);
            if (status != 0)
            {
               printf("Error: naibrd_1553_BcMessageCreateBcToRt status = %d", status);
               return TRUE;
            }

            /* Create BC to RT Message 3 */
            status = naibrd_1553_BcMessageCreateBcToRt(devnum, MSG3, DBLK3, RT_ADDRESS, RT_SUBADDRESS3, WORDCOUNT3, 0, usBus);
            if (status != 0)
            {
               printf("Error: naibrd_1553_BcMessageCreateBcToRt status = %d", status);
               return TRUE;
            }

            /* Create Execute Message Command 1 */
            status = naibrd_1553_BcCommandCreate(devnum, OP1, NAI_1553_OPCODE_EXECUTE_MESSAGE, NAI_1553_OPCODE_COND_ALWAYS, MSG1, 0, 0);
            if (status != 0)
            {
               printf("Error: naibrd_1553_BcCommandCreate status = %d", status);
               return TRUE;
            }

            /* Create Execute Message Command 2 */
            status = naibrd_1553_BcCommandCreate(devnum, OP2, NAI_1553_OPCODE_EXECUTE_MESSAGE, NAI_1553_OPCODE_COND_ALWAYS, MSG2, 0, 0);
            if (status != 0)
            {
               printf("Error: naibrd_1553_BcCommandCreate status = %d", status);
               return TRUE;
            }

            /* Create Execute Message Command 3 */
            status = naibrd_1553_BcCommandCreate(devnum, OP3, NAI_1553_OPCODE_EXECUTE_MESSAGE, NAI_1553_OPCODE_COND_ALWAYS, MSG3, 0, 0);
            if (status != 0)
            {
               printf("Error: naibrd_1553_BcCommandCreate status = %d", status);
               return TRUE;
            }

            /* Create Minor Frame 1 */
            aOpCodes[0] = OP1;      /* Execute Message 1 Command */
            aOpCodes[1] = OP2;      /* Execute Message 2 Command */
            aOpCodes[2] = OP3;      /* Execute Message 3 Command */
            status = naibrd_1553_BcFrameCreate(devnum, MNR1, NAI_1553_BC_FRAME_MINOR, aOpCodes, 3, 250, 0);     /* 250 for 25 ms minor frame (1 unit is 0.1 ms) */
            if (status != 0)
            {
               printf("Error: naibrd_1553_BcFrameCreate status = %d", status);
               return TRUE;
            }

            /* Create Minor Frame 2 */
            aOpCodes[0] = OP1;      /* Execute Message 1 Command */
            aOpCodes[1] = OP2;      /* Execute Message 2 Command */
            status = naibrd_1553_BcFrameCreate(devnum, MNR2, NAI_1553_BC_FRAME_MINOR, aOpCodes, 2, 250, 0);     /* 250 for 25 ms frame */
            if (status != 0)
            {
               printf("Error: naibrd_1553_BcFrameCreate status = %d", status);
               return TRUE;
            }

            /* Create Minor Frame 3 */
            aOpCodes[0] = OP1;      /* Execute Message 1 Command */
            status = naibrd_1553_BcFrameCreate(devnum, MNR3, NAI_1553_BC_FRAME_MINOR, aOpCodes, 1, 250, 0);     /* 250 for 25 ms frame */
            if (status != 0)
            {
               printf("Error: naibrd_1553_BcFrameCreate status = %d", status);
               return TRUE;
            }

            /* Create Call Subroutine Command 1 */
            status = naibrd_1553_BcCommandCreate(devnum, OP4, NAI_1553_OPCODE_CALL_SUBROUTINE, NAI_1553_OPCODE_COND_ALWAYS, MNR1, 0, 0);
            if (status != 0)
            {
               printf("Error: naibrd_1553_BcCommandCreate status = %d", status);
               return TRUE;
            }

            /* Create Call Subroutine Command 2 */
            status = naibrd_1553_BcCommandCreate(devnum, OP5, NAI_1553_OPCODE_CALL_SUBROUTINE, NAI_1553_OPCODE_COND_ALWAYS, MNR2, 0, 0);
            if (status != 0)
            {
               printf("Error: naibrd_1553_BcCommandCreate status = %d", status);
               return TRUE;
            }

            /* Create Call Subroutine Command 3 */
            status = naibrd_1553_BcCommandCreate(devnum, OP6, NAI_1553_OPCODE_CALL_SUBROUTINE, NAI_1553_OPCODE_COND_ALWAYS, MNR3, 0, 0);
            if (status != 0)
            {
               printf("Error: naibrd_1553_BcCommandCreate status = %d", status);
               return TRUE;
            }

            /* Create Major Frame */
            aOpCodes[0] = OP4;   /* Minor Frame 1 */
            aOpCodes[1] = OP6;   /* Minor Frame 3 */
            aOpCodes[2] = OP5;   /* Minor Frame 2 */
            aOpCodes[3] = OP6;   /* Minor Frame 3 */
            status = naibrd_1553_BcFrameCreate(devnum,MJR,NAI_1553_BC_FRAME_MAJOR,aOpCodes,4,0,0); /* If frameTime is set here to a value */
                                                                                                   /* other than zero, this value will    */
                                                                                                   /* override the minor frame times that */
                                                                                                   /* were set using the _BcFrameCreate() */
                                                                                                   /* function and set all minor frame    */
                                                                                                   /* times to the same duration.         */
            if (status != 0)
            {
               printf("Error: naibrd_1553_BcFrameCreate status = %d", status);
               return TRUE;
            }

            while (bContinue)
            {
               /* Load BC data block 1 with incremental data */
               for (i = 0; i < WORDCOUNT1; i++)
               {
                  aData[i] = increment++;
               }
               status = naibrd_1553_BcDataBlockWrite(devnum, DBLK1, aData, WORDCOUNT1, 0);
               if (status != 0)
               {
                  printf("Error: naibrd_1553_BcDataBlockWrite status = %d", status);
                  return TRUE;
               }

               /* Load BC data block 2 with 0x5566 */
               for (i = 0; i < WORDCOUNT2; i++)
               {
                  aData[i] = 0x5566;
               }
               status = naibrd_1553_BcDataBlockWrite(devnum, DBLK2, aData, WORDCOUNT2, 0);
               if (status != 0)
               {
                  printf("Error: naibrd_1553_BcDataBlockWrite status = %d", status);
                  return TRUE;
               }

               /* Load BC data block 3 with 0xBBCC */
               for (i = 0; i < WORDCOUNT3; i++)
               {
                  aData[i] = 0xBBCC;
               }
               status = naibrd_1553_BcDataBlockWrite(devnum, DBLK3, aData, WORDCOUNT3, 0);
               if (status != 0)
               {
                  printf("Error: naibrd_1553_BcDataBlockWrite status = %d", status);
                  return TRUE;
               }

               status = naibrd_1553_ClearMsgFIFO(devnum);
               if (status != 0)
               {
                  printf("Error: naibrd_1553_ClearMsgFIFO status = %d", status);
                  return TRUE;
               }

               /* Start BC */
               status = naibrd_1553_BcStart(devnum,MJR,NAI_1553_BC_FRAME_RUN_FOREVER);
               if (status != 0)
               {
                  printf("Error: naibrd_1553_BcStart status = %d", status);
                  return TRUE;
               }

               printf("BC Running for approximately 10 seconds...\n");

               /* Run the schedule for 10 seconds */
               nai_msDelay(10000);

               /* Stop BC (This call is not necessary here since transmission of the major frame should have completed by now) */
               status = naibrd_1553_BcStop(devnum);
               if (status != 0)
               {
                  printf("Error: naibrd_1553_BcStop status = %d", status);
                  return TRUE;
               }

               printf("BC Halted.\n");

               status = naibrd_1553_GetMsgFIFOCount(devnum, &fifoCount);
               if (status != 0)
               {
                  printf("Error: naibrd_1553_GetMsgFIFOCount %d", status);
                  return TRUE;
               }
               else
               {
                  if (fifoCount > 0)
                  {
                     currBlockIndex = 0;

                     /* This is necessary because otherwise, fifoData array gets used over and over without getting zero'ed out. */
                     /* When the array gets decoded, old data may get decoded unintentionally */
                     memset(fifoData, 0, sizeof(fifoData));

                     /* Read Message FIFO */
                     status = naibrd_1553_ReadMsgFIFO(devnum, fifoCount, fifoData);
                     if (status != 0)
                     {
                        printf("Error: naibrd_1553_ReadMsgFIFO  %d", status);
                        return TRUE;
                     }
                     else
                     {
                        /* Read Messages */
                        do
                        {
                           status = naibrd_1553_DecodeFIFOMsg(fifoData, currBlockIndex, &nextBlockIndex, &msgStruct);
                           if (status == 0)
                           {
                              printf("Command Word: 0x%04X\n", msgStruct.wCommandWord1);

                              if (msgStruct.bIsCommandWord2Relevant)
                                 printf("Command Word 2: 0x%04X\n", msgStruct.wCommandWord2);

                              printf("Block Status: 0x%04X\n", msgStruct.wBlockStatus);
                              printf("Time Tag: 0x%04X\n", msgStruct.wTimeTag);
                              printf("Word Count: 0x%04X\n", msgStruct.wDataWordCount);
                              printf("RT Status Word: 0x%04X\n", msgStruct.wStatus1);

                              if (msgStruct.bIsStatus2Relevant)
                                 printf("RT Status Word 2: 0x%04X\n", msgStruct.wStatus2);

                              printf("\n\n");
                           }
                           else if (status != NAI_1553_CMD_NO_MSG_TO_DECODE)
                           {
                              printf("Error: naibrd_1553_DecodeFIFOMsg  %d", status);
                              return TRUE;
                           }

                           /*** IMPORTANT !!! ***/
                           currBlockIndex = nextBlockIndex;

                           nai_msDelay(1);

                        } while ((status == NAI_SUCCESS) && (bContinue));
                     }
                     printf("\n\n");

                     status = naibrd_1553_GetMsgFIFOFullStatus(devnum, NAI_1553_STATUS_REALTIME, &FIFIOfullStatusBit);
                     if (status == NAI_SUCCESS)
                     {
                        printf("FIFO status: %d", FIFIOfullStatusBit);
                        printf((FIFIOfullStatusBit == TRUE) ? " Full\r\n" : " Not full\r\n");
                     }
                     else
                     {
                        printf("Error: naibrd_1553_GetMsgFIFOFullStatus  %d", status);
                        return TRUE;
                     }

                     printf("\nPress any key to send another message or Q to quit.");
                     bQuit = naiapp_query_ForQuitResponse(sizeof(inputBuffer), NAI_QUIT_CHAR, inputBuffer, &inputResponseCnt);
                     if (bQuit)
                     {
                        bContinue = FALSE;
                     }
                  }
               }
            }
         }
      }
   }

   /* Free 1553 Device */
   status = naibrd_1553_Free(devnum);
   if (status != 0)
   {
      printf("Error: naibrd_1553_Free status = %d", status);
      return TRUE;
   }

   return bQuit;
}