#include <stdio.h>
#include "BenthicBioSampler.h"
#include "DmErrno.h"
extern "C" 
{
# include "usrTime.h"
};
  
BenthicBioSampler::BenthicBioSampler(const char *name, BenthicBioApp *app)
  : IbcMicroSampler(name, app)
{
  Int16 board;
  
  _benthicBioApp = app;

  addCallback((float)10.0, 
	      (CallbackMethod) BenthicBioSampler::updateDigitalInputs);
  
  Errno err;
  char errorBuf[errorMsgSize];

  MBool debug = TRUE;

  //Start Provider for Digital Inputs  
  if ((err = _benthicBioApp->dmItems->ssIndexSense->startProvide(DM_ASYNC,
      ExclusiveProvider)) != SUCCESS)
  {	
      sprintDmError(err, "ssIndexSense->startProvide()", errorBuf);
      fprintf(stderr, "%s\n", errorBuf);
  }

  if ((err = _benthicBioApp->dmItems->swingArmSense->startProvide(DM_ASYNC,
      ExclusiveProvider)) != SUCCESS)
  {	
      sprintDmError(err, "swingArmSense->startProvide()", errorBuf);
      fprintf(stderr, "%s\n", errorBuf);
  }

  if ((err = _benthicBioApp->dmItems->compLevelSense->startProvide(DM_ASYNC,
      ExclusiveProvider)) != SUCCESS)
  {	
      sprintDmError(err, "compLevelSense->startProvide()", errorBuf);
      fprintf(stderr, "%s\n", errorBuf);
  }

  if ((err = _benthicBioApp->dmItems->spareDigitalInput->startProvide(DM_ASYNC,
      ExclusiveProvider)) != SUCCESS)
  {	
      sprintDmError(err, "spareDigitalInput->startProvide()", errorBuf);
      fprintf(stderr, "%s\n", errorBuf);
  }

  initDigitalInputs();
}


BenthicBioSampler::~BenthicBioSampler()
{
}



void BenthicBioSampler::updateDigitalInputs()
{ 
  Word portWord;
  MBool ssIndexSense;
  MBool swingArmSense;
  MBool compLevelSense;
  MBool spareDigitalInput;
   
  MBool debug = FALSE;
  
  dprintf("BenthicBioSampler::updateDigitalInputs()\n");
  
  Int16   cmdStatus;		    /* Command Status                       */
  Word    reply[2];               /* Command Result from microcontroller  */

                                    /* Send command to VME IBC micro        */
  if ( sendMicroCommand(_benthicBioApp->device->channel(),
			&reply, sizeof(Word) * 2,
			2, IBC_CPU_APP | ISO_IO_READ_PORT, 
			ISOLATED_IO_BOARD_ADDR) == ERROR )
  {  
    logMsg("BenthicBioSampler::updateDigitalInputs() "
	   "- sendMicroCommand() failed\n");

    return;
  }
    
  /* Check if command result is CMD OK    */
  if (wordFromBuf(&reply, 0) != CMD_OK) 
  {
    logMsg("BenthicBioSampler::updateDigitalInputs() "
	   "- wordFromBuf() failed\n");
    return;
  }

  /* Command OK so update thresholds      */
  wordsFromBuf(reply, 2, &cmdStatus, &portWord);
   
  ssIndexSense = ((portWord & SSIndexSenseBit) ? TRUE : FALSE);    
  dprintf("updateDigitalInputs(): ssIndexSense=%d\n", ssIndexSense);

  swingArmSense = ((portWord & SwingArmSenseBit) ? TRUE : FALSE);    
  dprintf("updateDigitalInputs(): swingArmSense=%d\n", swingArmSense);

  compLevelSense = ((portWord & CompLevelSenseBit) ? TRUE : FALSE);    
  dprintf("updateDigitalInputs(): compLevelSense=%d\n", compLevelSense);

  spareDigitalInput = ((portWord & SpareDigitalInputBit) ? TRUE : FALSE);    
  dprintf("updateDigitalInputs(): spareDigitalInput=%d\n", spareDigitalInput);
  
  if (ssIndexSense != lastSSIndexSense)
  {
    if(writeDmItem(_benthicBioApp->dmItems->ssIndexSense->getHandle(),
                   &ssIndexSense, sizeof(ssIndexSense))==ERROR)
    {
      logMsg("BenthicBioSampler::updateDigitalInput() IndexSense "
	     "- writeDmItem() failed\n");
      return;
    }
    lastSSIndexSense = ssIndexSense;
  }

  if (swingArmSense != lastSwingArmSense)
  {
    if(writeDmItem(_benthicBioApp->dmItems->swingArmSense->getHandle(),
                   &swingArmSense, sizeof(swingArmSense))==ERROR)
    {
      logMsg("BenthicBioSampler::updateDigitalInput() SwingArmSense "
	     "- writeDmItem() failed\n");
      return;
    }
    lastSwingArmSense = swingArmSense;
  }

  if (compLevelSense != lastCompLevelSense)
  {
    if(writeDmItem(_benthicBioApp->dmItems->compLevelSense->getHandle(),
                   &compLevelSense, sizeof(compLevelSense))==ERROR)
    {
      logMsg("BenthicBioSampler::updateDigitalInput() Comp Level Sense "
	     "- writeDmItem() failed\n");
      return;
    }
    lastCompLevelSense = compLevelSense;
  }

  if (spareDigitalInput != lastSpareDigitalInput)
  {
    if(writeDmItem(_benthicBioApp->dmItems->spareDigitalInput->getHandle(),
                   &spareDigitalInput, sizeof(spareDigitalInput))==ERROR)
    {
      logMsg("BenthicBioSampler::updateDigitalInput() Spare Digital Input "
	     "- writeDmItem() failed\n");
      return;
    }
    lastSpareDigitalInput = spareDigitalInput;
  }

  return;
} 


void BenthicBioSampler::initDigitalInputs()
{ 
  Word portWord;
  MBool ssIndexSense;
  MBool swingArmSense;
  MBool compLevelSense;
  MBool spareDigitalInput;
 
  Int16   cmdStatus;		    /* Command Status                       */
  Word    reply[2];               /* Command Result from microcontroller  */

                                    /* Send command to VME IBC micro        */
  if ( sendMicroCommand(_benthicBioApp->device->channel(), 
			&reply, sizeof(Word) * 2,
			2, IBC_CPU_APP | ISO_IO_READ_PORT, 
			ISOLATED_IO_BOARD_ADDR) == ERROR )
  {
    return;//ERROR
  }
    
  /* Check if command result is CMD OK    */
  if (wordFromBuf(&reply, 0) != CMD_OK) 
    return; //ERROR		    /* Command failed so return ERROR       */

  /* Command OK so update thresholds      */
  wordsFromBuf(reply, 2, &cmdStatus, &portWord);
   
  ssIndexSense = ((portWord & SSIndexSenseBit) ? TRUE : FALSE);    
  swingArmSense = ((portWord & SwingArmSenseBit) ? TRUE : FALSE);
  compLevelSense = ((portWord & CompLevelSenseBit) ? TRUE : FALSE);
  spareDigitalInput = ((portWord & SpareDigitalInputBit) ? TRUE : FALSE);

  writeDmItem(_benthicBioApp->dmItems->ssIndexSense->getHandle(),
	      &ssIndexSense, sizeof(ssIndexSense));
  lastSSIndexSense = ssIndexSense;

  writeDmItem(_benthicBioApp->dmItems->swingArmSense->getHandle(),
	      &swingArmSense, sizeof(swingArmSense));
  lastSwingArmSense = swingArmSense;

  writeDmItem(_benthicBioApp->dmItems->compLevelSense->getHandle(),
	      &compLevelSense, sizeof(compLevelSense));
  lastCompLevelSense = compLevelSense;

  writeDmItem(_benthicBioApp->dmItems->spareDigitalInput->getHandle(),
	      &spareDigitalInput, sizeof(spareDigitalInput));
  lastSpareDigitalInput = spareDigitalInput;
  
   return;
}