#include <bccandrivertinycan.h>


//#define UNLIMITED_SPEED_VALUE			70 /* Km/h */
//#define UNLIMITED_MIN_SPEED_VALUE		30 /* Km/h */
//#define MAX_THROTTLE_SPEED_VALUE		70 /* Km/h */

#include <QDebug>


#include <bccandrivertinycan.h>


#include <can_drv.h>
//#include "mhstcan.h"

BCCanDriverTinyCan::BCCanDriverTinyCan( QObject* parent )
  : BCCanDriver(parent )
{

}


BCCanDriver::DriverState BCCanDriverTinyCan::loadDriver()
{

  //qDebug() << "CanBusControl " << cbc::Version <<  '\n' << "based on BigXionFlasher (c) 2011-2013 by Thomas Koenig <info@bigxionflasher.org> - www.bigxionflasher.org";

  struct ::TDeviceStatus status;

  if( ::LoadDriver( NULL ) < 0 )
    throw BCException( "Driver Error: 'LoadDriver'" );

   setState(BCCanDriver::DriverState::Loaded);

  if( ::CanInitDriver( NULL ) < 0 )
    throw BCException( "Driver Error: 'InitDriver'" );

  if( ::CanDeviceOpen( 0, NULL ) < 0 )
    throw  std::runtime_error( "Driver Error: 'DeviceOpen'" );

  ::CanSetSpeed( 0, CAN_125K_BIT );
  ::CanSetMode( 0, OP_CAN_START, CAN_CMD_ALL_CLEAR );
  ::CanGetDeviceStatus( 0, &status );

  setState(BCCanDriver::DriverState::Initialized);

  if( status.DrvStatus < DRV_STATUS_CAN_OPEN )
    throw BCException( "Driver Error: could not open device." );

  if( status.CanStatus == CAN_STATUS_BUS_OFF )
  {
    ::CanSetMode( 0, OP_CAN_RESET, CAN_CMD_NONE );
    throw BCException( "Driver Error: CAN Status 'BusOff'" );
  }

  setState(BCCanDriver::DriverState::Ready);

  return BCCanDriver::DriverState::Ready;

}


/*
    try
    {
        loadDriver();
        initBCDevice::ID::Console();
    }

    catch( std::exception& except )
    {
        ::CanDownDriver();
        ::UnloadDriver();

        // re-throw
        throw BCException( except.what() );
    }
    */


BCCanDriver::DriverState BCCanDriverTinyCan::initDriver()
{

  qDebug() << "XXX BCCanDriverTinyCan::Driver Init: putting BCDevice::ID::Console in slave mode ... ";
  // BCDevice::ID::Console already in slave mode. good!
  if( readRawValue( BCDevice::ID::Console, BC::ID::Cons_Status_Slave ) )
    return DriverState::Ready;

  qDebug() << "BCCanDriverTinyCan::BCCanDriverTinyCan::XXX Driver Init: putting BCDevice::ID::Console in slave mode ... ";

  unsigned int retry = _timeOuts;
  emit statusHint( "Driver Init: putting BCDevice::ID::Console in slave mode ... " );

  bcdata_t isSlave = 0;
  do
  {
    writeRawValue( BCDevice::ID::Console, BC::ID::Cons_Status_Slave, 1 );
    isSlave = readRawValue( BCDevice::ID::Console, BC::ID::Cons_Status_Slave );
    bc::delay_millis( 200 );

  } while( retry-- && !isSlave );

  bc::delay_millis( 500 ); // give the Console some time to settle
  //if( !isSlave )
  emit statusHint( QString("putting BCDevice::ID::Console in slave mode ") + (isSlave ? "done" : "failed") );

  // ist das jetzt irgendwie schlimm, wenn wir keine slave BCDevice::ID::Console haben
  //return isSlave ? BCCanDriver::connected

  return DriverState::Ready;


}



bcdata_t BCCanDriverTinyCan::readRawValue( BCDevice::ID deviceID, BC::ID registerID ) const
{

  if( getState() != DriverState::Ready)
    throw BCException( "readRawValue error: driver not loaded." );

  TCanMsg msg;

  uint32_t device = static_cast<uint32_t>(deviceID);
  uint8_t  reg    = static_cast<uint8_t> (registerID);

  // msg verpacken
  msg.MsgFlags = 0L;
  msg.Id = device;
  msg.MsgLen = 2;
  msg.MsgData[0] = 0x00;
  msg.MsgData[1] = reg;

  // msg verschicken
  ::CanTransmit( 0, &msg, 1 );

  int retries = _retries; // 5?
  // _timeOuts (== 20) mal cTIMEOUT_MS (== 10 ms ) Versuche ...
  int timeOuts = _timeOuts; // 20 ?

  // ... warten bis der Sendepuffer leer ist
  while( timeOuts-- && ::CanTransmitGetCount( 0 ) )
    bc::delay_millis( cTIMEOUT_MS );

  if( timeOuts == -1 )
    throw BCException( "readRawValue error: could not send value" );

retry:

  // _timeOuts (== 20) mal cTIMEOUT_MS (== 10 ms ) Versuche ...
  timeOuts = _timeOuts;
  // ... warten, bis der Empfangspuffer nicht mehr leer ist
  while( timeOuts-- && !::CanReceiveGetCount( 0 ) )
    bc::delay_millis( cTIMEOUT_MS );

  if( timeOuts == -1 )
    throw BCException( "getValue error: no response from node" );

  // message empfangen
  int err = ::CanReceive( 0, &msg, 1 );
  qDebug() << "HÄÄ ?" << err << "reg: "<< reg <<"  timeOuts: " << timeOuts;

  if( err < 0 )
    //throw BCException( "getValue error: could not receive value" );
    qDebug( "getValue error: could not receive value" );

  qDebug() << "HÄÄ 2" <<msg.Id;
  qDebug() << "HÄÄ 2" <<msg.MsgLen;
  qDebug() << "HÄÄ 2" <<msg.MsgData[1];

  //if( err > 0 )
  if( --retries && ( msg.Id != BIB || msg.MsgLen != 4 || msg.MsgData[1] != reg ) )
    goto retry;

  if( !timeOuts )
    throw BCException( "CAN --response errror" );

  return (bcdata_t) msg.MsgData[3];

}


// void BCCanDriverTinyCan::setValue( unsigned char receipient, unsigned char reg, unsigned char value )
void BCCanDriverTinyCan::writeRawValue( BCDevice::ID deviceID, BC::ID registerID, bcdata_t value ) const
{

  if( getState() != DriverState::Ready)
    throw BCException( "writeRawValue error: driver not loaded." );

  qDebug() << " --- BCCanDriverTinyCan writeRawValue: " << value;

  uint32_t device = static_cast<uint32_t>(deviceID);
  uint8_t  reg    = static_cast<uint8_t> (registerID);

  struct TCanMsg msg;
  int timeout_count = cTIMEOUT_COUNT;

  msg.MsgFlags = 0L;
  msg.Id = device;
  msg.MsgLen = 4;
  msg.MsgData[0] = 0x00;
  msg.MsgData[1] = reg;
  msg.MsgData[2] = 0x00;
  msg.MsgData[3] = value;

  ::CanTransmit( 0, &msg, 1 );

  while( timeout_count-- && ::CanTransmitGetCount( 0 ) )
    bc::delay_millis( cTIMEOUT_MS );

  if( timeout_count == -1 )
    emit statusHint( QString( "error: could not send value to %1" ).arg( device ) );

}