#include "math.h"
#include "powerAlloc.h"
#include "powerDM.h"
#include "BenthicBioMonitor.h"
#include "Toolsled.h"
#include "ECUBoard.h"
#include "time.h"
#include "usrTime.h"

BenthicBioMonitor::BenthicBioMonitor(const char *name, BenthicBioApp *app)
  : IbcDmMonitor(name, app)
{
  _benthicBioApp = app;
  
  _autoIndexState = FALSE;
 
  bbsledName = name;
  
  addMonitoredItem(_benthicBioApp->dmItems->ssPumpSpeed);
  addMonitoredItem(_benthicBioApp->dmItems->spareAnalogOutput);

  addMonitoredItem(_benthicBioApp->dmItems->ssPumpDirection);
  addMonitoredItem(_benthicBioApp->dmItems->ssAutoIndex);
  addMonitoredItem(_benthicBioApp->dmItems->ssIndexSense);
}


BenthicBioMonitor::~BenthicBioMonitor()
{
}


int BenthicBioMonitor::handleResetSemaphore()
{
  IbcDmMonitor::handleResetSemaphore();
    
  Int16 initPumpSpeed = 0;
  Int32 initPumpDir   = PumpStop;
  Int16 initAnalogOutputValue = 0;
  switchStatus initSwitchState = SWITCH_OFF;

  //Initialize SS Pump speed and direction
  urgentWriteDmItem(_benthicBioApp->dmItems->ssPumpSpeed->getHandle(),
                    &initPumpSpeed,
                    sizeof(initPumpSpeed)); 

  urgentWriteDmItem(_benthicBioApp->dmItems->ssPumpDirection->getHandle(),
                    &initPumpDir,
                    sizeof(initPumpDir));
  //Initialize Spare Analog Output value
  urgentWriteDmItem(_benthicBioApp->dmItems->spareAnalogOutput->getHandle(),
                    &initAnalogOutputValue,
                    sizeof(initAnalogOutputValue)); 

}


int BenthicBioMonitor::cmdGroupCallback()
{
  IbcDmMonitor::cmdGroupCallback();
    
  MBool debug = TRUE;
  
  MBool autoIndexSense;
  Int16 pumpSpeed;
  Int32 pumpDir;
  Int16 spareAnalogOutputValue;
  SwitchStatus valveSwitchState;

  if (_benthicBioApp->ssPump->newRequest(_monitoredGroup))  
  {
    dprintf("BenthicBioMonitor::processDmInput() - req for ssPump\n");
    pumpSpeed = SSPumpStopValue;

    // Write speed Value to micro
    setAnalogValue((Nat16 )pumpSpeed, PumpSpeedChannel);

    _benthicBioApp->ssPump->processSwitchRequest();
  }

  if (_benthicBioApp->indexFwd->newRequest(_monitoredGroup))
  {
    dprintf("BenthicBioMonitor::processDmInput() - req for indexFwd\n");
    _benthicBioApp->indexRev->switchOff(BypassSwitchRequest);
    _benthicBioApp->indexFwd->processSwitchRequest();
  }
  
  if (_benthicBioApp->indexRev->newRequest(_monitoredGroup))
  {
    dprintf("BenthicBioMonitor::processDmInput() - req for indexRev\n");
    _benthicBioApp->indexFwd->switchOff(BypassSwitchRequest);
    _benthicBioApp->indexRev->processSwitchRequest();
  }

  if (_benthicBioApp->valvePack->newRequest(_monitoredGroup))
  {
    dprintf("BenthicBioMonitor::processDmInput() - req for valvePack\n");
    _benthicBioApp->valvePack->processSwitchRequest();

    taskDelay(CLOCKS_PER_SEC / (USECS_PER_SEC / POWER_DELAY));

    valveSwitchState = _benthicBioApp->valvePack->status();
    
    if(valveSwitchState == SWITCH_ON)    
      _benthicBioApp->ecuBoard->initECUDigitalOutputs();

  }


  if (_monitoredGroup->itemChanged(_benthicBioApp->dmItems->ssPumpSpeed))
  {   
    dprintf("BenthicBioMonitor::processDmInput() - input for ssPumpSpeed\n");
    _benthicBioApp->dmItems->ssPumpSpeed->read(&pumpSpeed, 
					     (DM_Time *) NULL); 
    
    _benthicBioApp->dmItems->ssPumpDirection->read(&pumpDir, 
						 (DM_Time *) NULL);
      
    if (pumpDir == PumpForward)
      pumpSpeed = (((pumpSpeed * SSPumpStopValue)/100) + SSPumpStopValue); 

    else if (pumpDir == PumpReverse)
      pumpSpeed = abs((((pumpSpeed * SSPumpStopValue)/100) - SSPumpStopValue));

    else if (pumpDir == PumpStop)
      pumpSpeed = SSPumpStopValue;

    // Write speed Value to micro
    setAnalogValue((Nat16 )pumpSpeed, PumpSpeedChannel);
  }

  if (_monitoredGroup->itemChanged(_benthicBioApp->dmItems->ssPumpDirection))
  {   
    dprintf("BenthicBioMonitor::processDmInput() - input for ssPumpDir\n");

    _benthicBioApp->dmItems->ssPumpSpeed->read(&pumpSpeed, 
					     (DM_Time *) NULL); 
    
    _benthicBioApp->dmItems->ssPumpDirection->read(&pumpDir, 
						 (DM_Time *) NULL);
      
    if (pumpDir == PumpForward)
      pumpSpeed = (((pumpSpeed * SSPumpStopValue)/100) + SSPumpStopValue); 

    else if (pumpDir == PumpReverse)
    {
      pumpSpeed = 
	abs((((pumpSpeed * SSPumpStopValue)/100) - SSPumpStopValue)); 
    }
    else if (pumpDir == PumpStop)
      pumpSpeed = SSPumpStopValue;

    //Write speed Value to micro
    setAnalogValue((Nat16 )pumpSpeed, PumpSpeedChannel);
  }

  if (_monitoredGroup->itemChanged(_benthicBioApp->dmItems->spareAnalogOutput))
  {   
    dprintf("BenthicBioMonitor::processDmInput() - input for spareAnalogOutput\n");
    _benthicBioApp->dmItems->spareAnalogOutput->read(&spareAnalogOutputValue, 
					     (DM_Time *) NULL); 
    
    // Write Analog Value to micro
    setAnalogValue((Nat16 )spareAnalogOutputValue, SpareAnalogOutputChannel);
  }

  if (_monitoredGroup->itemChanged(_benthicBioApp->dmItems->ssAutoIndex))
  {
    dprintf("BenthicBioMonitor::processDmInput() - input for ssAutoIndex\n");    
    _benthicBioApp->dmItems->ssAutoIndex->read(&_autoIndexState,
                                             (DM_Time *) NULL);  
  }
  
  if (_monitoredGroup->itemChanged(_benthicBioApp->dmItems->ssIndexSense))
  {
    dprintf("BenthicBioMonitor::processDmInput() - input for ssIndexSense\n");
     _benthicBioApp->dmItems->ssIndexSense->read(&autoIndexSense, 
                                               (DM_Time *) NULL);
     if (autoIndexSense)
     {
       if(_autoIndexState)        
       { 
	 // Write ss Index switches to Off
	 _benthicBioApp->indexFwd->switchOff(BypassSwitchRequest);
	 _benthicBioApp->indexRev->switchOff(BypassSwitchRequest);
       }
     } 
  }
}

STATUS BenthicBioMonitor::setAnalogValue(Nat16 level, Nat16 chan)
{
  return IBC_ISO_IO_SetDacLevel(_benthicBioApp->device->channel(),
				ISO_IO_0_ADDR,
				chan,
				level);
}