/*-------------------------------------------------------------------

 *一個非同步通訊案例， 
  *-----------------------------------------------------------------*/
#ifndef WIN32
#define PROGNAME "AsyncIP"

// Standard ANSI C headers needed for this program
#include <errno.h>
#include <fcntl.h>
#include <stdio.h>
 

#include <stdlib.h>
#include <string.h>
#include <sys/wait.h>
#include <termios.h>
#include <unistd.h>

#if defined(__QNXNTO__)
#       include <process.h>
#       include <sys/sched.h>
#       include <pthread.h>
#       include <devctl.h>
#       include <sys/dcmd_chr.h>
 

#elif defined(__linux__)
#       define _GNU_SOURCE   1
#       include <sched.h>
#   if defined(__redhawk__)
#       include <cpuset.h>
#       include <mpadvise.h>
#   endif
#endif

// Define RTLAB before including OpalPrint.h for messages to be sent
// to the OpalDisplay. Otherwise stdout will be used.
 

#define RTLAB
#include "OpalPrint.h"
#include "AsyncApi.h"
#include "AsyncIPUtils.h"

// This is just for initializing the shared memory access to communicate
// with the RT-LAB model. It's easier to remember the arguments like this
#define ASYNC_SHMEM_NAME argv[1]
#define ASYNC_SHMEM_SIZE atoi(argv[2])
#define PRINT_SHMEM_NAME argv[3]

// This defines the maximum number of signals (doubles) that can be sent
// or received by any individual Send or Recv block in the model. This
// only applies to the "model <-> asynchronous process" communication.
#define MAXSENDSIZE 64
#define MAXRECVSIZE 64

// Set the stack size of each thread.
#define STACKSIZE 4096

//通過 #pragma pack(1)和#pragma pack()對 來使用最小的記憶體儲存data_out和data_in結構體
#pragma pack(1) //變數以一個位元組對齊。變數緊縮
struct data_out//定義輸出資料結構。
{
  short  dev_id;             // (2 bytes) Sender device ID
  int    msg_id;             // (4 bytes) Message ID
  short  msg_len;            // (2 bytes) Message length (data only)
  double data[MAXSENDSIZE];  // Up to MAXSENDSIZE doubles (8 bytes each)
};
//
struct data_in  //定義輸入資料結構。
{
  short  dev_id;             // (2 bytes) Sender device ID
  int    msg_id;             // (4 bytes) Message ID
  short  msg_len;            // (2 bytes) Message length (data only)
  double data[MAXRECVSIZE];  // Up to MAXRECVSIZE doubles (8 bytes each)
};

#pragma pack()//返回標準的記憶體分配模式

volatile int thread_count = 0;

/************************************************************************/
int AssignProcToCpu0(void)
{
#if defined(__linux__)
#   if defined(__redhawk__)
    int             rc;
    pid_t           pid = getpid();
    cpuset_t        *pCpuset;

    pCpuset = cpuset_alloc();
    if (NULL == pCpuset)
    {
        fprintf(stderr, "Error allocating a cpuset\n");
        return(ENOMEM);
    }
    cpuset_init(pCpuset);
    cpuset_set_cpu(pCpuset, 0, 1);
    rc = mpadvise(MPA_PRC_SETBIAS, MPA_TID, pid, pCpuset);
    if (MPA_FAILURE == rc)
    {
        rc = errno;
        fprintf(stderr, "Error from mpadvise, %d %s, for pid %d\n", errno, strerror(errno), pid);
        cpuset_free(pCpuset);
        return(rc);
    }
    cpuset_free(pCpuset);
    return EOK;
#   else
    cpu_set_t bindSet;
    CPU_ZERO( &bindSet );
    CPU_SET( 0, &bindSet );
    /* changing process cpu affinity */
    if ( sched_setaffinity( 0, sizeof(cpu_set_t), &bindSet ) != 0 )
    {
        (int) fprintf(stderr, "Unable to bind the process to cpu 0. (sched_setaffinity errno %d)\n", errno );
        return EINVAL;
    }
    return EOK;
#   endif
#endif  // __linux__
}
/************************************************************************/

/*******    將資料傳送到IP埠  ********************************************/
void *SendToIPPort (void * arg)
{
  unsigned int    SendID = 1;
  int    i,n;
  int    nbSend = 0;
  int    ModelState;

  double mdldata[MAXSENDSIZE];
  int    mdldata_size;
  struct data_out comdata;
  int    comdata_size;
  int   count = 0;

  OpalPrint("%s: SendToIPPort thread started\n", PROGNAME);

  OpalGetNbAsyncSendIcon(&nbSend);  //獲得傳送控制器的編號

  //王加
  OpalPrint("output ndSend%d\n", nbSend);

  if(nbSend >= 1)//大於等於1 則有資料需要被髮送
    {
      do
    {
      // This call unblocks when the 'Data Ready' line of a send icon is asserted.
        //正常情況下，該if語句沒有作用。
      if((n = OpalWaitForAsyncSendRequest (&SendID)) != EOK)    //若n!=EOK ，則 沒有傳送請求。
        {                                                       //SendID中儲存 發起 傳送請求的 控制器編號
            //當沒有傳送請求時，
          ModelState = OpalGetAsyncModelState();    //讀取模型執行狀態
          if ((ModelState != STATE_RESET) && (ModelState != STATE_STOP))//如果模型不是重置或停止狀態
        {
          OpalSetAsyncSendIconError(n, SendID); //向 發起傳送請求的控制器 返回錯誤資訊：ENODEV    No send icon registered.
          OpalPrint("%s: OpalWaitForAsyncSendRequest(), errno %d\n", PROGNAME, n);
        }
          continue;
        }

      // 當沒有遇到錯誤時，
      OpalSetAsyncSendIconError(0, SendID);  // 向SendID 傳送錯誤資訊：0.即沒有錯誤

      OpalGetAsyncSendIconDataLength (&mdldata_size, SendID);// 獲得由Send ID傳送的資料的長度
      OpalPrint("output mdldata_size%d\n", mdldata_size);//在rtlab介面列印處資料長度。

      if (mdldata_size/sizeof(double) > MAXSENDSIZE)//如果資料長度超過了最大資料長度
        {
          OpalPrint("%s: Number of signals for SendID=%d exceeds allowed maximum (%d)\n", PROGNAME, SendID, MAXSENDSIZE);
          return NULL;
        }

      // 從模型的傳送模組編號為SendID的模組中讀取資料  （從共享記憶體中讀取資料）
      OpalGetAsyncSendIconData (mdldata, mdldata_size, SendID);
//----------------資料修改~-------------
// ****** 定義資料包的封裝格式 ******************************
// 根據需要，修改本部分內容。如資料型別。 

      comdata.dev_id  = SendID;  // Use the SendID as a device ID here
      comdata.msg_id++;          // The message ID is just incremented
      comdata.msg_len = mdldata_size;
//    comdata.msg_len = (mdldata_size/sizeof(double)) * sizeof(int); // 如果 comdata.data 是 "int" 型的  
// 在本例中，因為資料包中的資料是double格式的，因此不需要型別轉換

      for (i=0; i < (mdldata_size / sizeof(double)); i++)
        comdata.data[i] = mdldata[i];
//      comdata.data[i] = (int)mdldata[i]; // If comdata.data was an "int"

      comdata_size = 8 + comdata.msg_len;//若傳送資料的長度是變化的
//    comdata_size = sizeof(comdata);   // 對於固定長度的資料包
// **********************************************************************

      //  把資料傳送到 IP埠
      if (SendPacket((char*)&comdata, comdata_size) < 0)
        OpalSetAsyncSendIconError  (errno, SendID); //向 SendID埠 傳送 錯誤資訊：errno
      else
        OpalSetAsyncSendIconError  (0, SendID);

      //下一個函式可以將非同步過程變為同步過程。
      //為此，必須將model傳送模組中的“Sending Mode”修改為NEED_REPLY_BEFORE_NEXT_SEND或NEED_REPLY_NOW
      //這會迫使model停下來等待OpalAsyncSendRequestDone函式執行完畢
      OpalAsyncSendRequestDone (SendID);//對提起傳送請求模組的回覆

      // 進行下一個步驟之前 確認模型沒有停止執行
      ModelState = OpalGetAsyncModelState();
    } while ((ModelState != STATE_RESET) && (ModelState != STATE_STOP));
    //當模型停止或重置時，跳出while迴圈
      OpalPrint("%s: SendToIPPort: Finished\n", PROGNAME);//傳送終止
    }

  else //當沒有資料需要傳送時
    {
      OpalPrint("%s: SendToIPPort: No transimission block for this controller. Stopping thread.\n", PROGNAME);
      //輸出： 沒有傳送模組
    }

  thread_count--;
  return NULL;
}


/*****************  從IP埠接收資料  ***********************************/
void *RecvFromIPPort (void * arg)
{
  int    RecvID = 1;
  int    i, n1, n2, nt, n;
  int    nbRecv = 0;
  int    ModelState;

  double mdldata[MAXRECVSIZE];
  int    mdldata_size;
  struct data_in comdata;
  int    comdata_size;

  OpalPrint("%s: RecvFromIPPort thread started\n", PROGNAME);
  //開始接收資料
  OpalGetNbAsyncRecvIcon(&nbRecv); //解析 接收模組的編號

  if(nbRecv >= 1)   //如果存在模組需要接收資料
    {
      do
    {
      memset (&comdata, 0, sizeof(comdata));//對結構體清零

// ****** FORMAT TO SPECIFIC PROTOCOL HERE ******************************
//
// 當需要接收不止一個數據包時，修改此處程式碼。
      comdata_size = sizeof(comdata);
      n  = RecvPacket((char*)&comdata, comdata_size, 1.0);
      nt = n;

// 在本例中，接收到的資料長度是可變的。如果資料長度是固定的，可以刪去下面這條語句
      comdata_size = 8 + comdata.msg_len;//該語句是否有問題。應該改為下行程式碼？？？？？？？？
      //comdata_size = 8 + comdata_size;//2018.3.13
// **********************************************************************

      if (n < 1)//沒有接收到資料
        {
          ModelState = OpalGetAsyncModelState(); // 獲取模型狀態
          if ((ModelState != STATE_RESET) && (ModelState != STATE_STOP))
        {
          // n ==  0 意味著超時，所以繼續即可
          //if (n == 0)
          // OpalPrint("%s: Timeout while waiting for data\n", PROGNAME, errno);
          // n == -1 意味著有嚴重的錯誤。
          if (n == -1)
            OpalPrint("%s: Error %d while waiting for data\n", PROGNAME, errno);
          continue;
        }
          break;
        }
      else if (nt != comdata_size)//如果nt和資料長度不等
        {
          // Disable this print. It may happen in TCP/IP mode. The server needs to be modified to check packet size.
          // OpalPrint("%s: Received incoherent packet (size: %d, complete: %d)\n", PROGNAME, nt, comdata_size);
          continue;
        }

// ****** FORMAT TO SPECIFIC PROTOCOL HERE ******************************
// 根據需要，修改本部分內容。如資料型別。 

      RecvID = comdata.dev_id;                             // Use the deviceID as the RecvID
      OpalSetAsyncRecvIconStatus(comdata.msg_id, RecvID); // Set the Status to the message ID
      OpalSetAsyncRecvIconError(0, RecvID);              // Set the Error to 0

      // Get the number of signals to send back to the model
      OpalGetAsyncRecvIconDataLength (&mdldata_size, RecvID);//獲得 RecvID模組中定義的接收資料的長度
      //上句是什麼意思？？？？？？？？
      if (mdldata_size/sizeof(double) > MAXRECVSIZE)
        {
          OpalPrint("%s: Number of signals for RecvID=%d (%d) exceeds allowed maximum (%d)\n", PROGNAME, RecvID, mdldata_size/sizeof(double), MAXRECVSIZE);
          return NULL;
        }
      if (mdldata_size > comdata.msg_len)
//        if (mdldata_size/sizeof(double) > comdata.msg_len/sizeof(int)) // If comdata.data was an "int"
        {
          OpalPrint("%s: Number of signals for RecvID=%d (%d) exceeds what was received (%d)\n", PROGNAME, RecvID, mdldata_size/sizeof(double), comdata.msg_len/sizeof(double));
        }

      for (i=0; i < (mdldata_size / sizeof(double)); i++)
        mdldata[i] = (double)comdata.data[i]; // 
// **********************************************************************

      OpalSetAsyncRecvIconData   (mdldata, mdldata_size, RecvID);//設定接收模組要接收的資料

      // Before continuing, we make sure that the real-time model
      // has not been stopped. If it has, we quit.
      ModelState = OpalGetAsyncModelState();
    } while ((ModelState != STATE_RESET) && (ModelState != STATE_STOP));

      OpalPrint("%s: RecvFromIPPort: Finished\n", PROGNAME);
    }

  else
    {
      OpalPrint("%s: RecvFromIPPort: No reception block for this controller. Stopping thread.\n", PROGNAME);
    }

  thread_count--;
  return NULL;
}
/************************************************************************/

/************************************************************************/
int main (int argc, char *argv[]) 
{
  Opal_GenAsyncParam_Ctrl IconCtrlStruct;
  int                     err;
  pthread_t               tid_send,  tid_recv;
  pthread_attr_t          attr_send, attr_recv;

  // Check for the proper arguments to the program
  if (argc < 4)
    {
      printf("Invalid Arguments: 1-AsyncShmemName 2-AsyncShmemSize 3-PrintShmemName\n");
      exit(0);
    }

/*王新增*/ 
 for( int argNum = 0; argNum < argc; argNum++ )
  {
      printf("%d -> %s\n", argNum, argv[argNum]);
      OpalPrint("%d -> %s\n", argNum, argv[argNum]);
  }
 ///-------接下來一段不用改動

  // Enable the OpalPrint function. This prints to the OpalDisplay.
  if (OpalSystemCtrl_Register(PRINT_SHMEM_NAME) != EOK)
    {
      printf("%s: ERROR: OpalPrint() access not available\n", PROGNAME);
      exit(-1); 
    }

  // 開闢共享記憶體 -----------------------
  if((OpalOpenAsyncMem (ASYNC_SHMEM_SIZE, ASYNC_SHMEM_NAME)) != EOK)
    {
      OpalPrint("%s: ERROR: Model shared memory not available\n", PROGNAME);
      exit(-1);
    }

  // For Redhawk, Assign this process to CPU 0 in order to support partial XHP
  AssignProcToCpu0();  

  // Get IP Controler Parameters (ie: ip address, port number...) and
  // initialize the device on the QNX node.
  memset(&IconCtrlStruct, 0, sizeof(IconCtrlStruct));
  if((err = OpalGetAsyncCtrlParameters(&IconCtrlStruct, sizeof(IconCtrlStruct))) != EOK)
    {
      OpalPrint("%s: ERROR: Could not get controller parameters (%d).\n", PROGNAME, err);
      exit(-1);
    }
  if(InitSocket (IconCtrlStruct) != EOK)
    {
      OpalPrint("%s: ERROR: Initialization failed.\n", PROGNAME);
      exit(-1);
    }
  //以上程式碼不用改動
//---------------開始傳輸---------
  thread_count++;
  pthread_attr_init (&attr_send);
  //pthread_attr_setstacksize (&attr_send, STACKSIZE); // Has been known to crash the application
  if ((pthread_create (&tid_send, &attr_send, SendToIPPort, NULL)) == -1)
    {
      OpalPrint("%s: ERROR: Could not create thread (SendToIPPort), errno %d\n", PROGNAME, errno);
      thread_count--;
    }


  // Start reception thread -----------------------------------------
  thread_count++;
  pthread_attr_init (&attr_recv);
  //pthread_attr_setstacksize (&attr_recv, STACKSIZE); // Has been known to crash the application
  if ((pthread_create (&tid_recv, &attr_recv, RecvFromIPPort, NULL)) == -1)
    {
      OpalPrint("%s: ERROR: Could not create thread (RecvFromIPPort), errno %d\n", PROGNAME, errno);
      thread_count--;
    }

  // Wait for both threads to finish --------------------------------
  if ((err = pthread_join (tid_send, NULL)) != 0)
    { OpalPrint("%s: ERROR: pthread_join (SendToIPPort), errno %d\n", PROGNAME, err); }
  if ((err = pthread_join (tid_recv, NULL)) != 0)
    { OpalPrint("%s: ERROR: pthread_join (RecvFromIPPort), errno %d\n", PROGNAME, err); }

  // Close the ip port and shared memories ----------------------
  CloseSocket (IconCtrlStruct);
  OpalCloseAsyncMem (ASYNC_SHMEM_SIZE,ASYNC_SHMEM_NAME);
  OpalSystemCtrl_UnRegister(PRINT_SHMEM_NAME);

  return(0);
}
/************************************************************************/
#endif