#include "microperiodichandler.h"
#include "ostime.h"
#include "debug.h"
#include "exceptions.h"
#include "tasksynchronizer.h"

Nat32 MicroPeriodicHandler::_clksPerSec = Clock::getSystemClkTck();
#define MAXINT 99999

MicroPeriodicHandler::MicroPeriodicHandler(const char *name, TaskSynchronizer *tasksync)
:_wakeupSem(SEM_FULL, 0)
{
  DPRINTF_L3("MicroPeriodicHandler::MicroPeriodicHandler()\n");

  _name = strdup(name);
  _sampleTicks = 1;
  _maxTicks = 0;
  _accumulatedTicks = 0; 
  _taskSync = tasksync;
  _callbackLists.clear();	//d.cline added

  if(tasksync == NULL)
	  _THROW_(NullArg(2));
}


MicroPeriodicHandler::~MicroPeriodicHandler()
{
  free((void *)_name);

  for (int i = 0; i < _callbackLists.size(); i++)
  {
    CallbackList *list;
    _callbackLists.get(i, (void **)&list);
    delete list;
  }


}
int MicroPeriodicHandler::shutdown(int waitmillisecs)
{
	_taskSync->shutdown();	
	_wakeupSem.semGive();
	// Wait up to waitmillisecs for task to exit
	_taskSync->waitForShutdown(MicroPeriodicTask, waitmillisecs);
	if(exitCode() != TASK_READY)
		return 0;
	return -1;
}

void MicroPeriodicHandler::reset()
{
	_wakeupSem.semGive();
}
void *MicroPeriodicHandler::run()
{
  DPRINTF("MicroPeriodicHandler::run()\n");

  while (!_taskSync->_shutdown)
  {	  
    // Wait for EventHandler to finish poweron/reset processing
    _taskSync->setTaskReset(MicroPeriodicTask);

    DPRINTF_L3("MicroPeriodicHandler::run() - wait for MicroEventHandler\n");
    _taskSync->semTake(MicroPeriodicTask, WAIT_FOREVER);     

	DPRINTF_L3("MicroPeriodicHandler::run() - start sampling loop\n");

	while(!_taskSync->_reset && !_taskSync->_shutdown)
	{		
		DPRINTF_L4("MicroPeriodicHandler::run() - top of sampling loop; sampleTicks=%d\n",
			_sampleTicks);	

		_wakeupSem.semTake(_sampleTicks*1000/_clksPerSec);

		if(_taskSync->_reset)
			break;

		_taskSync->_mutex.lock();	//Protect task from deletion

		// Invoke user defined callbacks
		invokeCallbacks();	

		_taskSync->_mutex.unlock();	

	}//end while
  }
   
  _taskSync->setTaskShutdown(MicroPeriodicTask);	
  return 0;
}

void MicroPeriodicHandler::addCallback(Nat32 millisec, CallbackObject *callback)
{
  float hertz = 1000.f / (float) millisec;
  
  addCallback(hertz, callback);
}

void MicroPeriodicHandler::enableCallback(CallbackObject *callback)
{
	if(callback != 0)
		callback->enable();
}
void MicroPeriodicHandler::disableCallback(CallbackObject *callback)
{
	if(callback != 0)
		callback->disable();
}
void MicroPeriodicHandler::addCallback(float sampleHertz, CallbackObject *callbackObj)
{
  int sampleTicks = (int) (Clock::getSystemClkTck()/sampleHertz + 0.5); 

  DPRINTF_L3("MicroPeriodicHandler::addCallback(), hertz=%.1f, ticks=%d\n", 
	  sampleHertz, sampleTicks);
  
  CallbackList *list;
  Nat16 sz = _callbackLists.size(); //d.cline added

  for (Nat16 i = 0; i < sz; i++)
  { 
    _callbackLists.get(i, (void **)&list);
    if (list->sampleTicks == sampleTicks)
    {
      // Check to see if callback is already assigned for this interval
      MBool found = FALSE;
      for (int j = 0; j < list->callbacks.size(); j++)
      {
	CallbackObject *cbObj;
	list->callbacks.get(j, (void **)&cbObj);	
	if (cbObj == callbackObj) //d.cline added
	{
	  found = TRUE;
	  break;
	}
      }  
      if (!found)
      {
		// Add callback to this interval
		DPRINTF_L3("Assign new callback for %d interval\n", sampleTicks);        
		list->callbacks.add((void **)&callbackObj); //d.cline added
        return;
      }
      else 
      {
	DPRINTF_L3("Already assigned this callback for %d interval\n", 
		sampleTicks);

	return;
      }
    }
  }

  // New sampling interval
  DPRINTF_L3("New sampling interval; %d\n", sampleTicks);

  list = new CallbackList();
  list->sampleTicks = sampleTicks;  
  list->callbacks.add((void **)&callbackObj);

  _callbackLists.add((void **)&list);

  // Compute minimum sampling interval needed
  computeSampleTicks();
}


int MicroPeriodicHandler::computeSampleTicks()
{
  DPRINTF_L3("MicroPeriodicHandler::computeSampleTicks()\n");

  int minTicks = MAXINT;
  int i;
  CallbackList *list;
  
  if (_callbackLists.size() == 0)
    return _sampleTicks;
  
  for (i = 0; i < _callbackLists.size(); i++)
  {
    _callbackLists.get(i, (void **)&list);
  
    if (list->sampleTicks < minTicks)
      minTicks = list->sampleTicks;
    
    if (list->sampleTicks > _maxTicks)
      _maxTicks = list->sampleTicks;
  }

  // Find greatest integer by which all intervals are divisible
  _sampleTicks = 1;
  for (int gcd = 2; gcd <= minTicks; gcd++)
  {
    MBool allDivisible = TRUE;
    
    for (i = 0; i < _callbackLists.size(); i++)
    {
      _callbackLists.get(i, (void **)&list);
      if (list->sampleTicks % gcd)
      {
	allDivisible = FALSE;
	break;
      }
    }
    if (allDivisible)
      _sampleTicks = gcd;
  }
  DPRINTF_L4("MicroPeriodicHandler::computeSampleTicks(), minTicks=%d, maxTicks=%d, "
	  " _sampleTicks=%d\n", minTicks, _maxTicks, _sampleTicks);

  return _sampleTicks;
}


int MicroPeriodicHandler::invokeCallbacks()
{
  DPRINTF_L4("MicroPeriodicHandler::invokeCallbacks()\n");

  _accumulatedTicks += _sampleTicks;
  
  for (int i = 0; i < _callbackLists.size(); i++)
  {
    CallbackList *list;
    _callbackLists.get(i, (void **)&list);
    
    if (!(_accumulatedTicks % list->sampleTicks))
    {
      for (int j = 0; j < list->callbacks.size(); j++)
      {	
		CallbackObject *callbackObj; //d.cline added
	
		list->callbacks.get(j, (void **)&callbackObj);	//d.cline added

		if(callbackObj->enabled()) //d.cline added
		{
			DPRINTF_L4("MicroPeriodicHandler::invokeCallbacks(), invoke %d tick callback...",
				list->sampleTicks);        
			callbackObj->call(); //d.cline added
			//TODO: need metrics on how long callbacks & jitter
			// add comments on rules for callbacks to meet scheduled sampleTicks
			DPRINTF_L4("done\n");
		}
	
      }
    }
  }

  if (_accumulatedTicks > _maxTicks)
    _accumulatedTicks = 0;

  return 0;//d.cline added
}