#include <signal.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <sys/types.h>
#include <signal.h>
#include <tools/get_input.h>
#include <altro/altro.h>
#include <tools/rcc_time_stamp.h>
#include "u2f/u2f.h"

#include "iou2f.h"

int timer_out;
void tmlnk (int tout)
{ 
}

void tmulk ()
{

}


static u_int trigtype =0; // 0-SWTRG,1 HWTRG 
static u_int memdata[20048] = {0};				    

int HWTRG_Enable( u_int bit){
/* Configure the Trigger Configuration Register 2 - Enable HW trigger*/
  u_int rcode = O_RDOMOD;
  u_int rdata = 0x2 & (bit<<1);
  u_int ret = U2F_Reg_Write(rcode, rdata);
  if (ret)
    rcc_error_print(stdout, ret);

}

int CSRStatus() { 
  char *regname[]={
    "ERRST","TRCFG1","TRCNT","LWADD","IRADD","IRDAT","EVWORD","ACTFEC","FMIREG","FMOREG","TRCFG2"
  };
  u_int ret, rcode = 0, rdata = 0;
  fprintf(stderr, "CSR Status:\n");
  fprintf(stderr, "----------------------------------------\n");
  for (rcode=0;rcode< 0xb;rcode++){
      rdata = 0x0;
      ret = U2F_Reg_Read(rcode, &rdata);
      if (ret)	rcc_error_print(stdout, ret);
      fprintf(stderr, "%s \t| 0x%x | 0x%x\n",regname[rcode],rcode+0x7800,rdata);
  }  
  fprintf(stderr, "-----------------------------------------\n");
  return 0;
}

void u2f_init (int neve, char *data, int *nbytes)
{
  static u_int ret, rcode = 0, rdata = 0, fdata[20048] = {0}, fsize = 3, ccode = 0, cdata = 0;
  static u_int offset = 0, rsize = 10;
  static char fname[200] = "/home/tpctest/altro/u2f20050915/altro_usb/init_files/ginit2";
  u_int *udata=(u_int *)data;



  u_int loop ;
  

#ifdef DEBUG
        fprintf (stderr, "Server: *U2F* init\n");
#endif 
	//---------------------------------------------------------------------
	/* Open U2F_LIB */
  ret = U2F_Open(HW, DEFAULT_NODE);
  if (ret)
    rcc_error_print(stdout, ret);

  /* Open the rcc_time_stamp library */
  ret = ts_open(1, TS_DUMMY);
  if (ret)
    rcc_error_print(stdout, ret);
//------------------------------------------------------------

  HWTRG_Enable(0);

//------------------------------------------------------------

  /* Switch on FEC with Address = 0  */
  rcode = O_ACTFEC;
  rdata = 0x1;
  ret = U2F_Reg_Write(rcode, rdata);
  if (ret)
    rcc_error_print(stdout, ret);
    
  /* delay 1000000 us */
  ts_delay(1000000);

  /* Reset FEC */
  ccode = C_FECRST;
  cdata = 0;
  ret = U2F_Exec_Command(ccode, cdata);
  ccode = C_FECRST;
  cdata = 0;
  ret = U2F_Exec_Command(ccode, cdata);
  if (ret)
    rcc_error_print(stdout, ret);

    
  /* delay 100 us */
  ts_delay(100);

  //---------------------------------------------------------------------
  /* Load the Instruction Memory data */
  printf("Reading instructions from file %s\n", fname);
  ret = U2F_File_Read(fname, fsize, fdata);
  if (ret)
    rcc_error_print(stdout, ret);
  //--------------------------------------------------------------------- 
  /* write into the Instruction Memory */
  rsize = fsize;
  offset = 0;
  printf("Initializing instruction memory\n");
  for(loop=0; loop<fsize; loop++)
  {
    memdata[loop] = fdata[loop];
    printf("Data word %d = 0x%08x\n", loop, memdata[loop]);
  }
  ret = U2F_IMEM_Write(rsize, offset, memdata);
  if (ret)
    rcc_error_print(stdout, ret);
  //---------------------------------------------------------------------
  /* initialize the Channel Readout List  */
  rsize = 8;
  offset = 0;  
  
  for (loop=0;loop<rsize;loop++)   {
    memdata[loop] = udata[loop];
    printf("MASK %d. 0x%0x\n",loop,udata[loop]);
  }
  
  ret = U2F_ACL_Write(rsize, offset, memdata);
  if (ret)
    rcc_error_print(stdout, ret);
  //---------------------------------------------------------------------
  /* Execute the Instruction Memory */
  ccode = C_EXEC;
  cdata = 0;
  ret = U2F_Exec_Command(ccode, cdata);
  if (ret)
    rcc_error_print(stdout, ret);
    
  /* delay 1000 us */
  ts_delay(1000);
//------------------------------------------------------------
  /* Configure the Trigger Configuration Register */
  rcode = O_TRCFG;
  rdata = 0x50800;
  ret = U2F_Reg_Write(rcode, rdata);
  if (ret)
    rcc_error_print(stdout, ret);
    


    printf("U2F H/W is now initialized\n");

  //-----------------------------------------------------------
    CSRStatus();
}



void u2f_close (int neve, char *data, int *nbytes)
{
  static u_int ret;
#ifdef DEBUG
        fprintf (stderr, "Server: *U2F* close\n");
#endif 
  ret = U2F_Close();
  ret = ts_close(TS_DUMMY);

}
void u2f_driver_stop (int blen, char *data, int *nbytes)
{
        system("/etc/init.d/altro stop");
	nbytes[0]=0;
#ifdef DEBUG
        fprintf (stderr, "Server: *U2F* driver stop\n");
#endif 
}


void u2f_driver_start (int blen, char *data, int *nbytes)
{
        system("/etc/init.d/altro start");
	nbytes[0]=0;
#ifdef DEBUG
        fprintf (stderr, "Server: *U2F* driver start\n");
#endif 
}

void u2f_swtrg (int neve, char *data, int *nbytes)
{
static u_int ret, ccode = 0, cdata = 0;


#ifdef DEBUG
        fprintf (stderr, "Server: u2f swtrg\n");
#endif 
	ccode = C_SWTRG;
	cdata = 0;
	ret = U2F_Exec_Command(ccode, cdata);
	if (ret)
	  rcc_error_print(stdout, ret);
#ifdef DEBUG
	printf("\n\nSOFTWARE TRIGGER ISSUED\n\n");
#endif 
	data[0]=1;
	nbytes[0]=1;
}


void u2f_hwtrg (int neve, char *data, int *nbytes)
{
 static u_int ret, rcode=0 , rdata=0;  
     u_int  point=0, tick=0;

    data[0]=0;
    nbytes[0]=0;
 

//------------------------------------------------------------
    HWTRG_Enable(1);
#ifdef DEBUG
   fprintf (stderr, "Server: *U2F* hwtrg \n");
#endif 
   /*** Waiting for an external Trigger ***/
  /***  Read EVWORD Register  ***/

    rcode = O_EVWORD;
    ret = U2F_Reg_Read(rcode, &rdata); 
    if (ret)
      rcc_error_print(stdout, ret);
 
#ifdef DEBUG
      printf("hwtrg EVWORD = 0x%0x\n", rdata);
#endif

    while(!(rdata & 0x00000010))
    {
      tick++;

      /***  Read EVWORD Register  ***/
      rcode = O_EVWORD;
      ret = U2F_Reg_Read(rcode, &rdata); 
      if (ret)
        rcc_error_print(stdout, ret);

#ifdef DEBUG
	if(tick == 5000)
	{
	  printf("Waiting for external trigger\n");
	  CSRStatus();
	  point++;
	  tick = 0;
	} 
	if(point == 10) break;
	
#endif	

    }

    if (rdata & 0x00000010 ) {
      data[0]=1;
      nbytes[0]=1;
#ifdef DEBUG
      printf("\n\nHARDWARE TRIGGER ISSUED\n\n");
#endif
    }  
    CSRStatus();
    HWTRG_Enable(0);

}



void u2f_run (int blen, char *data, int *nbytes)
{

static u_int ret, rcode = 0, rdata = 0, ccode = 0, cdata = 0;
  static u_int offset = 0, rsize = 10; 
  FILE *file=stderr;
  u_int chnum=0;
  u_int loop, osize;
  u_int *udata= (u_int *)data;
  unsigned short *sdata = (unsigned short *) data;
 int nb, evcnt , neve;
 int * idata=(int *) data;
 int timeout=0;
 nbytes[0]=0;
 if (blen > 0) neve= udata[0]; else return;
 trigtype=udata[1]; 
#ifdef DEBUG
        fprintf (stderr, "Server: *U2F* run: Neve=%d trigtype=%d %d\n", neve,trigtype, sizeof(u_int));

#endif 

  evcnt = 0;
  nb=0;
  timer_out = FALSE;
  tmlnk (100);


  while (evcnt < neve)
  {
    if (trigtype) u2f_hwtrg (0, data, &nb);
    else u2f_swtrg (0, data, &nb);
    if (!nb){
      fprintf(stderr,"Trigger for event no. %d has not arrived\n",evcnt);
      return;
    }

/****************************************************************/
    /************************* PERFORM THE READOUT ******************/
    /****************************************************************/
    
    chnum=0;
    //rsize = 66;
    rsize = 27;// nsamples/4 + 2
    offset = 0;
    rdata = 0;
/***  Read LWADD Register  ***/
    rcode =  O_LWADD;
    ret = U2F_Reg_Read(rcode, &rdata); 
    if (ret)
      rcc_error_print(stdout, ret);
#ifdef DEBUG
      printf("[%i] U2F_Reg_Read  LVADD= 0x%0x\n", evcnt,rdata);
#endif
    /***  Read EVWORD Register  ***/
    rcode = O_EVWORD;
    ret = U2F_Reg_Read(rcode, &rdata); 
    if (ret)
      rcc_error_print(stdout, ret);
#ifdef DEBUG
      printf("[%i] U2F_Reg_Read  EVWORD= 0x%0x\n", evcnt,rdata);
#endif
    if((rdata & 0x00000010) && (rdata & 0x00000008)) 
    {
#ifdef DEBUG

      //	printf("STARTING EVENT READOUT\n");  	
	if(rdata & 0x00000004)
  	  printf("EVENT READOUT WAS CONCLUDED WITHOUT DATA TRANSFER\n");
#endif

      while(!(rdata & 0x00000004))
      {
	/*******************************/
	/***  Read EVWORD Register  ***/
	/******************************/
	rcode = O_EVWORD;
	ret = U2F_Reg_Read(rcode, &rdata); 
        if (ret)
          rcc_error_print(stdout, ret);

	/*******************************/
	/***  Reading Data  	     ***/
	/*******************************/

	/*** Reading DMEM1  ***/

	if (rdata & 0x00000002)
	{

#ifdef DEBUG
	    printf("\n\nReading Channel #%d from DMEM1\n", chnum);
#endif

	  ret = U2F_DM1_Read(rsize, &osize, offset, memdata);
	  if (ret)
	    rcc_error_print(stdout, ret);

	  //	  file = fopen(fname2,"a");
	  for (loop = 0; loop < osize; loop++) 
	  {
            sdata[nb++]=memdata[loop * 4];
            sdata[nb++]=memdata[loop * 4+1];
            sdata[nb++]=memdata[loop * 4+2];
            sdata[nb++]=memdata[loop * 4+3];
	    /*    
	    fprintf(file, "%d. %d 0x%x\n", loop, memdata[loop * 4], memdata[loop * 4]);
	    fprintf(file, "%d. %d 0x%x\n", loop, memdata[loop * 4 +1], memdata[loop * 4+1]);
	    fprintf(file, "%d. %d 0x%x\n", loop, memdata[loop * 4 +2], memdata[loop * 4+2]);
	    fprintf(file, "%d. %d 0x%x\n", loop, memdata[loop * 4 +3], memdata[loop * 4+3]);	
	    */
	  }

	  /* Read EVWORD */ 
	  rcode = O_EVWORD;
	  ret = U2F_Reg_Read(rcode, &rdata); 
	  if (ret)
	    rcc_error_print(stdout, ret);
#ifdef DEBUG
	    printf("RUN %d EVWORD = 0x%0x %d\n", evcnt, rdata, chnum);
#endif
	  /***  Reset DMEM1 flag    ***/
	  ccode = C_RS_DMEM1;
	  cdata = 0;
	  ret = U2F_Exec_Command(ccode, cdata);
	  if (ret)
	    rcc_error_print(stdout, ret);

	  /* Read EVWORD */ 
	  rcode = O_EVWORD;
	  ret = U2F_Reg_Read(rcode, &rdata); 
	  if (ret)
	    rcc_error_print(stdout, ret);
#ifdef DEBUG
	    printf("EVWORD = 0x%0x\n", rdata);
#endif

	}

	/***  Reading DMEM2  ***/
	if (rdata & 0x00000001)
	{
	  chnum++;
#ifdef DEBUG
	    printf("\n\nReading Channel #%d from DMEM2\n", chnum);
#endif
	  ret = U2F_DM2_Read(rsize, &osize, offset, memdata);
	  if (ret)
	    rcc_error_print(stdout, ret);

	  //file = fopen(fname2,"a");
	  for (loop = 0; loop < osize; loop++) 
	  {
	    sdata[nb++]=memdata[loop * 4];
            sdata[nb++]=memdata[loop * 4+1];
            sdata[nb++]=memdata[loop * 4+2];
            sdata[nb++]=memdata[loop * 4+3];
	    /* 
	    fprintf(file, "%d. %d 0x%x\n", loop, memdata[loop * 4], memdata[loop * 4]);
	    fprintf(file, "%d. %d 0x%x\n", loop, memdata[loop * 4 +1], memdata[loop * 4+1]);
	    fprintf(file, "%d. %d 0x%x\n", loop, memdata[loop * 4 +2], memdata[loop * 4+2]);
	    fprintf(file, "%d. %d 0x%x\n", loop, memdata[loop * 4 +3], memdata[loop * 4+3]);	
	    */
	  }

	  /* Read EVWORD */ 
	  rcode = O_EVWORD;
	  ret = U2F_Reg_Read(rcode, &rdata); 
	  if (ret)
	    rcc_error_print(stdout, ret);
#ifdef DEBUG
	    printf("EVWORD = 0x%0x\n", rdata);
#endif
	  /***  Reset DMEM2 flag    ***/
	  ccode = C_RS_DMEM2;
	  cdata = 0;
	  ret = U2F_Exec_Command(ccode, cdata);
	  if (ret)
	    rcc_error_print(stdout, ret);

	  /* Read EVWORD */ 
	  rcode = O_EVWORD;
	  ret = U2F_Reg_Read(rcode, &rdata); 
	  if (ret)
	    rcc_error_print(stdout, ret);
#ifdef DEBUG
	    printf("*RUN %d EVWORD = 0x%0x ch=%d\n", evcnt, rdata, chnum);
#endif
	}
	chnum++;
      }

      /*** Reset EVWORD ***/
      ccode = C_TRG_CLR;
      cdata = 0;
      ret = U2F_Exec_Command(ccode, cdata);
      if (ret)
	rcc_error_print(stdout, ret);

      /*** Read EVWORD ***/ 
      rcode = O_EVWORD;
      ret = U2F_Reg_Read(rcode, &rdata); 
      if (ret)
	rcc_error_print(stdout, ret);
#ifdef DEBUG
	printf("End EVWORD = 0x%0x evnt=%d ch=%d\n", rdata, evcnt,chnum);
#endif
    }

      //     fprintf (stderr, "Server: *U2F* Event: %d\n", i);
	if (nb==MAXDATA) break;
  
/***  Read TRCNT Register  ***/
    rcode =  O_TRCNT;
    ret = U2F_Reg_Read(rcode, &rdata); 
    if (ret)
      rcc_error_print(stdout, ret);
#ifdef DEBUG
      printf("[%x] U2F_Reg_Read  TRCNT= 0x%0x\n", evcnt,rdata);
#endif

     sdata[nb++]=(rdata >> 16 ) & 0xFF;
     sdata[nb++]= rdata & 0xFF; //event counter
#ifdef DEBUG
      printf("[%d]event counter  = 0x%0x 0x%0x \n", evcnt,sdata[nb-2] , sdata[nb-1] );
#endif
     evcnt++;
    
  }
  nbytes[0]+=nb*2;

  tmulk();

#ifdef DEBUG
   fprintf (stderr, "Server: *U2F* Event Datasize: %d\n",nbytes[0] );
#endif 

}





void (*u2f[MAXU2FCALLS])(int , char *, int *) = { 
    /*  0  */  u2f_close, 
    /*  1  */  u2f_init, 
    /*  2  */  u2f_run, 
    /*  3  */  u2f_hwtrg, 
    /*  4  */  u2f_swtrg,  
    /*  5  */  u2f_driver_stop,  
    /*  6  */  u2f_driver_start 
};