/*********************************************************************
//  created:    2008/2/11 - 11:59
//  filename:   Win32CanInterface.cpp
//
//  author:     Gerald Dherbomez
//              Copyright Heudiasyc UMR UTC/CNRS 6599
// 
//  version:    $Id: $
//
//  purpose:    Windows specific management of the Can Interface
//
*********************************************************************/

#include "Win32CanInterface.h"

#include "Pacpus/PacpusTools/ShMem.h"

namespace pacpus {

using namespace std;

/************************************************************************/
/// Constructor
Win32CanInterface::Win32CanInterface()
{
  continue_ = true;
  counter_ = 0;
  receivedFramesArraySize_ = 0;
}

/************************************************************************/
/// Destructor
Win32CanInterface::~Win32CanInterface()
{
}

/************************************************************************/
/// Opens the CAN interface of the number given in argument 
bool Win32CanInterface::openInterface(const int number, const unsigned int speed)
{
  canDriver_ = new CanDriver(number, speed);
            
  //connection to CAN	bus 
  if(canDriver_->initPort() != 0)
  {
    cout << "CAN connection fails" << endl;
    return false;
  }
  else
    return true;
}
/************************************************************************/
/// Open the CAN interface of the number given in argument
bool  Win32CanInterface::openInterface(char * port, char * accessMode)
{
  canDriver_ = new CanDriver(port, accessMode);
  //connection to CAN	bus
  if(canDriver_->initPort() != 0)
  {
    cout << "CAN connection fails" << endl;
    return false;
  }
  else{
    return true;
    }
}

/************************************************************************/
/// Close the CAN interface
/// todo: close only the port identified by number 
///       make a function that close the driver or close the driver only if 
///       there is no more pending connections
bool Win32CanInterface::closeInterface(const int /*number*/)
{
  if(canDriver_->cleanUpPort() != 0)
  {
    cout << "CAN disconnection fails" << endl;
    delete canDriver_;
    return false;
  }
  else
  {
    delete canDriver_;
    return true;
  }
}

/************************************************************************/
/// The main loop of the class
void Win32CanInterface::run()
{
  continue_ = true;
  counter_ = 0;

  if (!receivedFramesArraySize_)
    qFatal("receivedFramesArraySize_ not initialized, division by zero may occur if you continue. Context:%s:L%d",__FILE__, __LINE__);  

  cout << "Win32CanInterface starts" << endl;

  switch (source_)
  {
  case VectorCard:
    vectorLoop();
    break;
  case SharedMemory:
    shMemLoop();
    break;
  case PeakCard:
    peakLoop();
    break;
  case XLVectorCard:
    vectorXlLoop();
    break;
  default: 
    break;
  }
  counter_ = 0;
  cout << "Win32CanInterface thread stopped...\n" << endl;
}


/************************************************************************/
/// The loop used for waiting CAN data from Vector card
void Win32CanInterface::vectorLoop()
{
  while (continue_)
  {
    // Wait incoming data from the CAN bus 
    if (canDriver_->receiveFrame(frame_) == 0) {
      receivedFrames_[counter_].time = road_time();
      receivedFrames_[counter_].timerange = 0;
      memcpy(&(receivedFrames_[counter_].frame), &frame_, sizeof(CanFrame) );
      semaphore_->release();
      counter_++; 
      counter_ = counter_ % receivedFramesArraySize_;
    }
  }
}

/************************************************************************/
/// The loop used for waiting CAN data from XL Vector card
void Win32CanInterface::vectorXlLoop()
{
  while(continue_) {
    // Wait incoming data from the CAN bus 
    if (canDriver_->receiveFrame(frame_) == 0) {
      receivedFrames_[counter_].time = road_time();
      receivedFrames_[counter_].timerange = 0;
      memcpy(&(receivedFrames_[counter_].frame), &frame_, sizeof(CanFrame));
      semaphore_->release();
      counter_++; 
      counter_ = counter_ % receivedFramesArraySize_;
    }
  }
}

/************************************************************************/
/* The loop used for waiting CAN data from a shared memory
/************************************************************************/
void Win32CanInterface::shMemLoop()
{
  shMem_ = new ShMem("CARMEN_CAN_2200", sizeof(TimestampedCanFrame));
  while (continue_)
  {
    // Wait incoming data from the shared memory
    if ( shMem_->wait(100) )
    {
      TimestampedCanFrame* ptr = (TimestampedCanFrame*)(shMem_->read());
      memcpy(&frame_, &(ptr->frame), sizeof(CanFrame));
      receivedFrames_[counter_].time = ptr->time;
      receivedFrames_[counter_].timerange = ptr->timerange;
      memcpy(&(receivedFrames_[counter_].frame), &frame_, sizeof(CanFrame) );
      semaphore_->release();
      counter_++; 
      counter_ = counter_ % receivedFramesArraySize_;
    }
  } // END while (continue_)
  
  delete shMem_;
}

/************************************************************************/
/// The loop used for waiting CAN data from Peak card
void Win32CanInterface::peakLoop()
{
  std::cout << "In peak loop" << std::endl;
  while(continue_)
  {
    // Wait incoming data from the CAN bus
    if ( canDriver_->receiveFrame(frame_) == 0 ) {
      receivedFrames_[counter_].time = road_time();
      receivedFrames_[counter_].timerange = 0;
      memcpy(&(receivedFrames_[counter_].frame), &frame_, sizeof(CanFrame) );
      semaphore_->release();
      counter_++;
      counter_ = counter_ % receivedFramesArraySize_;
    }
  }
}

/************************************************************************/
/// Stops the thread
void Win32CanInterface::stop()
{
  continue_ = false;
}

/************************************************************************/
/// Defines the place where the incoming frames will be copied
void Win32CanInterface::setExchangeBuffer(TimestampedCanFrame * framesArray, const int framesArraySize)
{ 
  receivedFrames_ = framesArray;
  receivedFramesArraySize_ = framesArraySize;
}

void Win32CanInterface::setSource(DataSource source)
{
  source_ = source;
}

} // namespace pacpus