//*-----------------------------------------------------------------------*
//|                                                                       |
//|  Copyright (c) 2019 by Paul Scherrer Institute (http://www.psi.ch)    |
//|                                                                       |
//|              Author Thierry Zamofing (thierry.zamofing@psi.ch)        |
//*-----------------------------------------------------------------------*

// >>>>>>> https://www.ashwinnarayan.com/post/xenomai-realtime-programming-part-2/
// >>>>>>> https://xenomai.org/documentation/xenomai-2.4/html/api/group__task.html

// Usage -> read the usage function
//samples: root@:/opt/ppmac#
// /tmp/triggerSync 40 11           trigger all 40 ms, simulated start, use real frame triggers, verbose
// /tmp/triggerSync 40 14           trigger all 40 ms, simulated start and frame triggers, no sync, verbose
// /tmp/triggerSync 40 15           trigger all 40 ms, simulated start and frame triggers, with sync, verbose
// /tmp/triggerSync 40 7            trigger all 40 ms, simulated start and frame triggers, with sync, minimal verbose

//before start set: Gather.Enable=1
//to trigger a simulated start: Coord[1].Q[10]=1

//the simulated frame trigger comes at 40.2 ms period.

#include <stdio.h>
#include <gplib.h>
#include <math.h>
#include <signal.h>
#include <unistd.h>
#include <sys/mman.h>
#include <native/task.h>
#include <native/timer.h>


extern struct SHM *pshm;
static char mode=0;
static float mtPt2Pt=40.f; //motion point to point time

#define CLOCK_RES 1e-9 //Clock resolution is 1 ns by default

#define  SIMFLAG0 (pshm->Coord[1].Q[10])
#define  SIMFLAG1 (pshm->Coord[1].Q[11])
//#define  SIMFLAG0 (pshm->P[10])
//#define  SIMFLAG1 (pshm->P[11])
//Power PMAC Software Reference Manual.pdf page 760
#define  FLAG0 (gate3_1->Chan[0].Status&0x800)
#define  FLAG1 (gate3_1->Chan[1].Status&0x800)

void trigsync_func(void *arg)
{
  RT_TASK *curtask;
  RT_TASK_INFO curtaskinfo;
  int iret = 0;
  int loopPeriod=1e5;//Expressed in ticks


  curtask = rt_task_self();
  rt_task_inquire(curtask, &curtaskinfo);

  //Print the info
  printf("Starting task %s with period of %f ms ....\n", curtaskinfo.name,loopPeriod/1.E6);

  //Make the task periodic with a specified loop period
  rt_task_set_periodic(NULL, TM_NOW, loopPeriod);

  int i;
  RTIME rtStart,rtLast,rtCur,rtDiff; //rt* is real time of the rt-process
  unsigned int scStart,scLast,scCur,scDiff; //sc* are servo counts of the motion
  float srvPer;
  float mtAct,mtDes; //mt* motion time. Actual and desired. this is the accumulated time that controls motion speed

  unsigned int maxDiff;
  struct GateArray3* gate3_1=GetGate3MemPtr(1);

  pshm = GetSharedMemPtr();
  if(!pshm->Gather.Enable)
  {
    printf("ending trigsync_func because Gather.Enable==0\n");
    return;
  }
  pshm->Coord[1].Q[0]=-2;
  srvPer=pshm->ServoPeriod;

  rtStart=rt_timer_read();
  printf("Wait for 'arm' event...\n");
  if(mode&2)
  {
    while(!SIMFLAG0)
      rt_task_wait_period(NULL);
  }
  else
  {
    while(!FLAG0)
      rt_task_wait_period(NULL);
  }
  printf("Flag 0: %.5f ms\n", (rt_timer_read() - rtStart)/1E6);

  pshm->Gather.Enable=2; //start gathering data
  pshm->Coord[1].Q[0]=-1;
  if(mode&4)
  {
    while(!SIMFLAG1)
      rt_task_wait_period(NULL);
  }
  else
  {
    while(!FLAG1)
      rt_task_wait_period(NULL);
  }
//puts("waitFlag1");
//gate3_1->Chan[1].Status f057f77f
//gate3_1->Chan[1].Status f057ff7f
//printf("gate3_1->Chan[1].Status %x\n",gate3_1->Chan[1].Status);

  printf("Flag 1: %.5f ms\n", (rt_timer_read() - rtStart)/1E6);
  rtStart=rtLast=rt_timer_read();
  scStart=scLast=pshm->ServoCount;
  mtAct=0.f;
  pshm->Coord[1].Q[0]=0;
  pshm->Coord[1].DesTimeBase=srvPer;  //start motion at default speed
  if(mode&8)
    printf("Start: %d, rtDiff: %.3f ms scDiff %d mtAct %.3f mtDes %.3f srvPer %.6f\n", i, rtLast/1E6,scLast, mtAct,mtDes,srvPer);


  for(i=1;pshm->Gather.Enable;i++)
  {
    if(mode&4)
    {
      while(SIMFLAG1)
        rt_task_wait_period(NULL);

      while(!SIMFLAG1)
        rt_task_wait_period(NULL);
    }
    else
    {
      while(FLAG1)
        rt_task_wait_period(NULL);

      while(!FLAG1)
        rt_task_wait_period(NULL);
    }
    //FEL shot arrived
    rtCur = rt_timer_read();
    scCur=pshm->ServoCount;
    rtDiff=rtCur-rtLast;
    scDiff=scCur-scLast;

    mtAct+=scDiff*srvPer;
    mtDes=i*mtPt2Pt;
    srvPer=(mtPt2Pt+mtDes-mtAct)/scDiff;
    pshm->Coord[1].DesTimeBase=srvPer;
    pshm->Coord[1].Q[0]++;
    if(mode&8)
      printf("Trigger count: %d, rtDiff: %.3f ms scDiff %d mtAct %.3f mtDes %.3f srvPer %.6f\n", i, rtDiff/1E6,scDiff, mtAct,mtDes,srvPer);
    rtLast=rtCur;
    scLast=scCur;
  }
  printf("trigsync_func done\n");
}

void trigsim_func(void *arg)
{
  RT_TASK *curtask;
  RT_TASK_INFO curtaskinfo;
  int iret = 0;
  curtask = rt_task_self();
  rt_task_inquire(curtask, &curtaskinfo);
  //Print the info
  //printf("Starting task %s with period of %f ms ....\n", curtaskinfo.name,10*1E6/1E6);
  //Make the task periodic with a specified loop period
  //rt_task_set_periodic(NULL, TM_NOW, 10*1E6);
  printf("Starting task %s \n", curtaskinfo.name);

  int i,j=0;
  RTIME rtStart,rtSlice;
  //printf("sizeof RTTIME %d\n",sizeof(rtStart));
  pshm = GetSharedMemPtr();
  SIMFLAG0=0;
  rtStart=rt_timer_read();

  for(i=0,rtSlice=rtStart;;i++)
  {
    //rtSlice=rtStart+i*40*1E6; // a slice is 40 ms
    rtSlice=rtStart+i*40.2*1E6; // a slice is 40 ms
    //rtSlice+=(rand()%(int)(1*1E6)); //0.1ms jitter
    rt_task_sleep_until(rtSlice); //in ns
    if(mode&4)
    {
      SIMFLAG1=1;
      //while(rt_timer_read()-rtStart<1E6*40*i)
      //  rt_task_wait_period(NULL);
      rt_task_sleep_until(rtSlice+5*1E6); //in ns
      SIMFLAG1=0;
      if(mode&8)
        {putchar('.');fflush(stdout);}
    }
    //if(pshm->Coord[1].DesTimeBase==0)
    if(SIMFLAG0==1)
    {
      if(j)
      {
        SIMFLAG0=0;j=0;
      }
      else if(mode&8)
      {putchar('x');fflush(stdout);}
      j++;
    }
    if(!pshm->Gather.Enable)
    {
      if((mode&4) && j<10)
      {
        putchar('o');
        j++;
        continue;
      }
      break;
    }
    //if(pshm->Coord[1].Q[10]==2)
    //  break;
  }
  printf("trigsim_func done\n");
}




void trigsync_run()
{
  RT_TASK trigsync_task;
  RT_TASK trigsim_task;
  const char* strSync="trigsync_task";
  const char* strSim="trigsim_task";
  //mode:
  // bit0: sync mode
  // bit1: simulate mode

  //Lock the memory to avoid memory swapping for this program
  mlockall(MCL_CURRENT | MCL_FUTURE);
  printf("Starting rt task...\n");

  if(mode&1)
  {
    rt_task_create(&trigsync_task, strSync, 0, 50, T_JOINABLE); //Create the real time task
    rt_task_start(&trigsync_task, &trigsync_func, 0);           //Since task starts in suspended mode, start task
  }

  if(mode&2)
  {
    rt_task_create(&trigsim_task, strSim, 0, 50, T_JOINABLE);
    rt_task_start(&trigsim_task, &trigsim_func, 0);
  }
  printf("Wait end of rt_task\n");

  if(mode&1)
  {
    rt_task_join(&trigsync_task);
    printf("trigsync_task done\n");
  }

  if(mode&2)
  {
    rt_task_join(&trigsim_task);
    printf("trigsim_task done\n");
  }
  printf("rt_task ended\n");

}

int usage(const char* cmd)
{
  const char* s="\n\
mode:\n\
 bit0:1: sync mode\n\
 bit1:2: simulate start trigger\n\
 bit2:4: simulate frame trigger\n\
 bit3:8: verbose\n\
\n\
simulate start trigger:\n\
set pshm->Coord[1].Q[10]=1 to simulate a Jungfrau aquire start\n\
\n\
simulate frame trigger\n\
is output to pshm->Coord[1].Q[11]\n\
 1: synchronize real frame and start triggers\n\
 3: synchronize real frame and simulated start triggers\n\
 6: simulated frame and start triggers (no sync)\n\
 7: synchronize simulated frame and start triggers\n\
\n\
in simulate mode:\n\
set pshm->Coord[1].Q[10]=1 to simulate a Jungfrau aquire start\n\
set pshm->Coord[1].Q[10]=2 to stop simulate trigger generation\n\
Coord[1].Q[11] is the simulated frame trigger\n\
\n\
in synchronize mode\n\
Coord[1].Q[0]=-2 : trigsync_func start, Wait for 'arm' trigger\n\
Coord[1].Q[0]=-1 : got 'arm' trigger, wait frame trigger\n\
Coord[1].Q[0]= 0 : got frame trigger 0\n\
Coord[1].Q[0] is incremented at each trigger\n\
sync task ends when Gather.Enable==0\n\
";
  printf("usage:\n%s pt2ptTime mode\n",cmd);
  puts(s);
  return -1;
}

int main(int argc, char *argv[])
{
    int err;
    int initialized=0;
    int i;
    char *s;
    if(argc!=3)
      return usage(argv[0]);
    mtPt2Pt= strtof(argv[1], &s);
    if (argv[1]==s)
      return usage(argv[0]);
    mode= (int)strtol(argv[2], &s, 10);
    if (argv[2]==s)
      return usage(argv[0]);

    if ((err = InitLibrary()) != 0) {
        abort();
    }
    //initialized = 1;
    //pshm = GetSharedMemPtr();
    //
    //printf("P:%g\n",pshm->P[1011]);
    //printf("pshm->MaxRtPlc:%d\n",pshm->MaxRtPlc);
    //
    //for(i=0;i<8;i++) //MAX_MOTORS instead of 8
    //  printf("pshm->Motor[%d].Ctrl:%p\n",i,pshm->Motor[i].Ctrl);
    //
    //for(i=0;i<8;i++) //MAX_MOTORS instead of 8
    //  printf("pshm->UserAlgo.ServoCtrlAddr[%d]:%d\n",i,pshm->UserAlgo.ServoCtrlAddr[i]);
    //
    //for(i=0;i<MAX_PLC;i++) //MAX_MOTORS instead of 8
    //  printf("pshm->UserAlgo.BgCplcPID[%d]:%d\n",i,pshm->UserAlgo.BgCplcPID[i]);
    //
    //printf("pshm->UserAlgo.RtiCplcPID:%d\n",pshm->UserAlgo.RtiCplcPID);
    //printf("pshm->UserAlgo.RtiCplcMaxTime:%d\n",pshm->UserAlgo.RtiCplcMaxTime);
    //printf("pshm->UserAlgo.CaptCompFuncAddr:%p\n",pshm->UserAlgo.CaptCompFuncAddr);

    trigsync_run();

    CloseLibrary();
    return !err;

    if (initialized)
        CloseLibrary();

    return 0;

}