/*
  This file is part of CanFestival, a library implementing CanOpen Stack. 

  Copyright (C): Jaroslav Fojtik
*/

#if defined(WIN32) && !defined(__CYGWIN__)
#define usleep(micro) Sleep(micro%1000 ? (micro/1000) + 1 : (micro/1000))
#else
#include <stdio.h>
#include <string.h>
#include <errno.h>
#include <fcntl.h>
#endif


#include "cancfg.h"
#include "can_driver.h"
#include "def.h"

UNS8 LIBAPI canSend_driver(CAN_HANDLE fd0, Message const *m);

#define Data data  /* temporary fix */


#define VERSION_2

/* dummy implementation for older version. */
#ifndef VERSION_2
void CAN_SetRcvEvent(HANDLE hEventx)
{
  SetEvent(hEventx);
}
#endif


#define SLAVE_COUNT 10
#define QueueSize 100


#ifndef extra_PCAN_init_params
    #define extra_PCAN_init_params  /**/
#else
    long int print_getenv(const char* pcanparam)
    {
        char* param=NULL;
        long int res=0;

        param = getenv(pcanparam);
        if(param != NULL){
            res = strtol(param,NULL,0);
        }
        else
            printf("Environment variable %s not defined !\n", pcanparam);
        printf("Found environment variable %s : %ld\n", pcanparam ,res);
        return res;
    }
    #define extra_PCAN_init_params\
        ,print_getenv("PCANHwType")\
        ,print_getenv("PCANIO_Port")\
        ,print_getenv("PCANInterupt")
#endif



typedef struct 
{
  s_BOARD *board;
  Message MQueue[QueueSize];
  unsigned QStart, QEnd;
  HANDLE hEventx;
} QueueRecord;

int initialisedQ = 0;
QueueRecord Q_DATA[10];


/** Store message into a queue. */
static void PushMsgToQueue(QueueRecord *QR, Message *m)
{
  if(QR==NULL || m==NULL) return;
  if(QR->board==NULL) return;							// No Board assigned yet
  memcpy(&QR->MQueue[QR->QStart], m, sizeof(Message));
  QR->QStart = (QR->QStart + 1) % QueueSize;
  if(QR->QEnd==QR->QStart) QR->QEnd = (QR->QEnd+1) % QueueSize;
  if(QR->hEventx) SetEvent(QR->hEventx);					// Signalise internal flag
}


/** Get message from a queue. */
static int PopMsgFromQueue(QueueRecord *QR, Message *m)
{
  if(QR==NULL || m==NULL) return 0;
  if(QR->QEnd == QR->QStart) return 0;

  memcpy(m, &QR->MQueue[QR->QEnd], sizeof(Message));
  QR->QEnd = (QR->QEnd+1) % QueueSize;
  return 1;
}


/** Create the Event for the first board */
HANDLE hEvent1 = NULL;
CRITICAL_SECTION InitLock;


// Define for rtr CAN message
#define CAN_INIT_TYPE_ST_RTR MSGTYPE_STANDARD | MSGTYPE_RTR


/***************************************************************************/
static int TranslateBaudeRate(char* optarg)
{
  if(!strcmp( optarg, "1M")) return CAN_BAUD_1M;
  if(!strcmp( optarg, "500K")) return CAN_BAUD_500K;
  if(!strcmp( optarg, "250K")) return CAN_BAUD_250K;
  if(!strcmp( optarg, "125K")) return CAN_BAUD_125K;
  if(!strcmp( optarg, "100K")) return CAN_BAUD_100K;
  if(!strcmp( optarg, "50K")) return CAN_BAUD_50K;
  if(!strcmp( optarg, "20K")) return CAN_BAUD_20K;
  if(!strcmp( optarg, "10K")) return CAN_BAUD_10K;
  if(!strcmp( optarg, "5K")) return CAN_BAUD_5K;
  if(!strcmp( optarg, "none")) return 0;
  return 0x0000;
}


static UNS8 canInit(s_BOARD *board)
{
int baudrate;
int ret = 0;

  if(hEvent1==NULL)
  {	//Create the Event for the first board
    hEvent1 = CreateEvent(NULL, // lpEventAttributes
                        FALSE,  // bManualReset
                        FALSE,  // bInitialState
                        "");    // lpName
    InitializeCriticalSection(&InitLock);
  }

  EnterCriticalSection(&InitLock);  

  if(baudrate = TranslateBaudeRate(board->baudrate))
  {
    ret = CAN_Init(baudrate, CAN_INIT_TYPE_ST extra_PCAN_init_params);
    if(ret != CAN_ERR_OK) 
    {
      LeaveCriticalSection(&InitLock);      
      return 0;
    }
  }
        
  CAN_SetRcvEvent(hEvent1);  //Set Event Handler for CANReadExt
  LeaveCriticalSection(&InitLock);    
  return 1;
}


/********* functions which permit to communicate with the board ****************/
UNS8 LIBAPI canReceive_driver(CAN_HANDLE fd0, Message *m)
{
static int HeavyCounter = 0;
int ret=0;
UNS8 data;
TPCANMsg peakMsg;
DWORD Res;
DWORD result;
HANDLE hh[2];
int i;

#ifdef CAN_READ_EX
    TPCANTimestamp peakRcvTime;
#endif      

  i = strtol(((s_BOARD *)fd0)->busname,NULL,0);
  if(i>=SLAVE_COUNT || i<0) return 1;           // error
  if(Q_DATA[i].board!=(s_BOARD *)fd0) return 1;      // error
  
  hh[0]=hEvent1; hh[1]=Q_DATA[i].hEventx;
  
	// loop until valid message or fatal error
  do
  {
CONTINUE:
     if(PopMsgFromQueue(&Q_DATA[i],m)) return 0;	//message is waiting in the internal queue

        // We read the queue looking for messages.
#ifdef VERSION_2
     result = WaitForMultipleObjects(2,hh,FALSE,15);  
     if(Q_DATA[i].board==NULL) return 1;		//exit hook, exit immediatelly when device is closed

     if(result == WAIT_OBJECT_0+1)
	 goto CONTINUE;	//look to a PopMsgFromQueue() (continue will check while(), goto skips it)

     if(result==WAIT_OBJECT_0 || result==WAIT_TIMEOUT)
     {
#endif     
#ifdef CAN_READ_EX
       Res = CAN_ReadEx(&peakMsg, &peakRcvTime);
#else
       Res = CAN_Read(&peakMsg);
#endif
                // Exit receive thread when handle is no more valid
#ifdef CAN_ERRMASK_ILLHANDLE
       if(Res & CAN_ERRMASK_ILLHANDLE) return 1;
#else
       if(Res & CAN_ERR_ILLHANDLE) return 1;
#endif

#ifndef VERSION_2
       if(Res != CAN_ERR_OK) 
             result = WaitForSingleObject(hEvent1, 1);   //pooling for pcan release<2
#endif
	if(Res==CAN_ERR_QRCVEMPTY) goto CONTINUE;
#ifdef VERSION_2       
     }
     else
     {
       //if(result==WAIT_TIMEOUT || result==(WAIT_OBJECT_0+1)) 
       //     Res = CAN_ERR_BUSLIGHT;
       //else 
              Res = CAN_ERR_UNKNOWN;
     }
#endif

    if(Res==CAN_ERR_BUSHEAVY)
    {
      if(HeavyCounter++>10) 
      {
	HeavyCounter = 0;
	Res=CAN_ERR_BUSOFF;
      }
    }

    if(Res & CAN_ERR_BUSOFF)
    {      
      peakMsg.MSGTYPE = MSGTYPE_STATUS;
      peakMsg.DATA[3] = CAN_ERR_BUSOFF;
      Res = CAN_ERR_OK;
    }   

        // A message was received : we process the message(s)
    if(Res == CAN_ERR_OK)
    {
            // if something different that 11bit or rtr... problem      
	switch(peakMsg.MSGTYPE)
	{
	  case MSGTYPE_STATUS:
		  switch(peakMsg.DATA[3])
	          {
		    case CAN_ERR_BUSHEAVY: 
				      break;
	            case CAN_ERR_BUSOFF: 
			              printf ("Peak board read BUSOFF: re-init!!!\n");
				      canInit((s_BOARD*)fd0);
				      usleep(2000);
				      break;
		  }
		  return peakMsg.DATA[3];	/* if something different that 11bit or rtr... problem */

	  case MSGTYPE_STANDARD:		/* bits of MSGTYPE_ */
	  case MSGTYPE_EXTENDED:
			  m->rtr = 0;
			  break;

	  case MSGTYPE_RTR:			/* bits of MSGTYPE_ */
			  m->rtr = 1;
			  break;

	  default: return CAN_ERR_OVERRUN;	/* If status, return status if 29bit, return overrun. */
		    
	}
   
      m->cob_id = peakMsg.ID;

      if (peakMsg.MSGTYPE == CAN_INIT_TYPE_ST)  /* bits of MSGTYPE_ */
                m->rtr = 0;
      else
                m->rtr = 1;
      m->len = peakMsg.LEN; /* count of data bytes (0..8) */
      for(data=0; data<peakMsg.LEN; data++)
                m->Data[data] = peakMsg.DATA[data]; /* data bytes, up to 8 */
#if defined DEBUG_MSG_CONSOLE_ON
      MSG("in : ");
      print_message(m);
#endif
    }
    else
    {       // not benign error => fatal error
      if(!(Res & CAN_ERR_QRCVEMPTY
                    || Res & CAN_ERR_BUSLIGHT
                    || Res & CAN_ERR_BUSHEAVY))
      {
        printf ("canReceive returned error (%d)\n", Res);
        return 1;
      }
    }
  } while(Res != CAN_ERR_OK);


     // populate message received to other drivers
  for(i=0; i<SLAVE_COUNT; i++)
  {
    if(Q_DATA[i].board != (s_BOARD *)fd0)	// do not populate to own queue
    {
      PushMsgToQueue(&Q_DATA[i],m);
    }   
  }

  return 0;
}


/***************************************************************************/

/* return 0 - OK; 1 - failure */
UNS8 LIBAPI canSend_driver(CAN_HANDLE fd0, Message const *m)
{
UNS8 data;
TPCANMsg peakMsg;
int i, j;
int loc_errno;
int MaxLoops = 100;

  i = -1;
  for(j=0; j<SLAVE_COUNT; j++)
  {
    if(Q_DATA[j].board != (s_BOARD *)fd0)   // store this message forr all other drivers
    {
      PushMsgToQueue(&Q_DATA[j],m);
      i = j;
    }
  }

  if(i<0) return 1;                         // no board found

  peakMsg.ID = m->cob_id;                   /* 11/29 bit code */
  if(m->rtr == 0)
  {
    if(peakMsg.ID > 0x7FF)
	peakMsg.MSGTYPE = MSGTYPE_EXTENDED; /* bits of MSGTYPE_ */
    else
	peakMsg.MSGTYPE = MSGTYPE_STANDARD; /* bits of MSGTYPE_ */
  }
  else	
    peakMsg.MSGTYPE = MSGTYPE_RTR;	    /* bits of MSGTYPE_ */
  
  peakMsg.LEN = m->len;
    /* count of data bytes (0..8) */
  for(data = 0; data < m->len; data++)
        peakMsg.DATA[data] = m->Data[data]; /* data bytes, up to 8 */

  do 
  {
    errno = loc_errno = CAN_Write(&peakMsg);

    if(loc_errno)
    {
      if(loc_errno==CAN_ERR_BUSOFF && (MaxLoops%20)==1)
      {
#if defined DEBUG_MSG_CONSOLE_ON
        printf ("Peak board write: re-init!!!\n");
#endif
        canInit((s_BOARD*)fd0);
        usleep(1000);
      }
      usleep(80);
    }
    if(MaxLoops-- == 0) break;			// limit max looping
  } while(loc_errno != CAN_ERR_OK);

#if defined DEBUG_MSG_CONSOLE_ON
    MSG("out : ");
    print_message(m);
#endif
    return 0;					// OK
}


/***************************************************************************/
UNS8 LIBAPI canChangeBaudRate_driver(CAN_HANDLE fd, char* baud)
{
    printf("canChangeBaudRate not yet supported by this driver\n");
    return 0;
}


/***************************************************************************/
CAN_HANDLE LIBAPI canOpen_driver(s_BOARD * board)
{
int ret;
int i;

  if(!initialisedQ)
  {
    memset(Q_DATA,0,sizeof(Q_DATA));
    initialisedQ = 1;
  }

  i = strtol(board->busname,NULL,0);			// Get slot name
  //printf ("Board Busname=%d.\n",strtol(board->busname, &pEnd,0));
  if(i<SLAVE_COUNT && i>=0)
  {
    Q_DATA[i].board = board;    
      //printf ("First Board selected\n");
    if(Q_DATA[i].hEventx==NULL)				// Create local event
    {
      Q_DATA[i].hEventx = CreateEvent(NULL, FALSE, FALSE, "");
    }

    if(hEvent1!=NULL) return (CAN_HANDLE)board;		// Create global event, if needed

    ret = canInit(board);
    if(ret)
      return (CAN_HANDLE)board;
  }

  return NULL;
}


/***************************************************************************/
int LIBAPI canClose_driver(CAN_HANDLE fd0)
{
s_BOARD *x_board = NULL;
int ActiveBoards = 0;
int i;

  if((s_BOARD *)fd0==NULL) return 0;
  for(i=0; i<SLAVE_COUNT; i++)
  {
    if(Q_DATA[i].board == (s_BOARD *)fd0)
    {
      x_board = Q_DATA[i].board;
      Q_DATA[i].board = NULL;
      CloseHandle(Q_DATA[i].hEventx);
      Q_DATA[i].hEventx = NULL;
    }
    else
      ActiveBoards++;
  }

  if(ActiveBoards<=0)
  {				// No can device is used.
    CAN_SetRcvEvent(NULL);
    CAN_Close();    
    if(hEvent1) 
    {
      SetEvent(hEvent1);
      CloseHandle(hEvent1);
      hEvent1 = NULL;
    }
  }
  else    
    SetEvent(hEvent1);

  return 0;
}