#ifdef UNIX
#  include <values.h>
#else
#  define MAXINT 99999
#endif

#include "MicroSampler.h"
#include "MicroApp.h"
#include "vxUtils.h"

MicroSampler::MicroSampler(const char *name, MicroApp *app)
{
  MBool debug = FALSE;
  dprintf("MicroSampler::MicroSampler()\n");

  _name = strdup(name);
  _app = app;
  _sampleTicks = 1;
  _maxTicks = 0;
  _accumulatedTicks = 0;
  
  if ((_wakeupSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == 0)
  {
    // Throw exception
  }
}


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

  if (_wakeupSem)
    semDelete(_wakeupSem);

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


void MicroSampler::run()
{
  MBool debug = TRUE;
  dprintf("MicroSampler::run()\n");

  // DEBUG DEBUG DEBUG
  // taskSuspend(0);
  
  startSampling();
  
  while (TRUE)
  {
    // Wait for DmMonitor to finish poweron/reset processing
    dprintf("MicroSampler::run() - wait for DmMonitor\n");
    _app->taskSync->semTake(MicroSamplerTask, WAIT_FOREVER);
    dprintf("MicroSampler::run() - start sampling loop\n");

    while (_app->taskSync->deviceLinkState() == DeviceInterface::LinkUp)
    {
      semTake(_wakeupSem, _sampleTicks);

      taskSafe();

      // Read micro data and process
      invokeCallbacks();	

      taskUnsafe();
    }
  }
}

void MicroSampler::addCallback(Nat32 millisec,
			       CallbackMethod callback)
{
  float hertz = 1000. / millisec;
  
  addCallback(hertz, callback);
}


void MicroSampler::addCallback(float sampleHertz,
			       CallbackMethod callback)
{
  MBool debug = FALSE;

  int sampleTicks = (int )(sysClkRateGet() / sampleHertz + 0.5);
  dprintf("MicroSampler::addCallback(), hertz=%.1f, ticks=%d\n", 
	  sampleHertz, sampleTicks);

  CallbackObj *callbackObj = new CallbackObj(callback);
  CallbackList *list;

  for (int i = 0; i < _callbackLists.size(); 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++)
      {
	CallbackObj *cbObj;
	list->callbacks.get(j, (void **)&cbObj);
	if (cbObj->_callback == callback)
	{
	  found = TRUE;
	  break;
	}
      }  
      if (!found)
      {
	// Add callback to this interval
	dprintf("Assign new callback for %d interval\n", sampleTicks);

        list->callbacks.add((void **)&callbackObj);
        return;
      }
      else 
      {
	dprintf("Already assigned this callback for %d interval\n", 
		sampleTicks);

	return;
      }
    }
  }

  // New sampling interval
  dprintf("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 MicroSampler::computeSampleTicks()
{
  MBool debug = FALSE;
  dprintf("MicroSampler::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("MicroSampler::computeSampleTicks(), minTicks=%d, maxTicks=%d, "
	  " _sampleTicks=%d\n", minTicks, _maxTicks, _sampleTicks);

  return _sampleTicks;
}


int MicroSampler::invokeCallbacks()
{
  MBool debug = FALSE;
  dprintf("MicroSampler::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++)
      {
	CallbackObj *callbackObj; 
	list->callbacks.get(j, (void **)&callbackObj);
	dprintf("MicroSampler::invokeCallbacks(), invoke %d tick callback...",
		list->sampleTicks);        

	(callbackObj->_callback)();
	dprintf("done\n");
      }
    }
  }

  if (_accumulatedTicks > _maxTicks)
    _accumulatedTicks = 0;
}


void MicroSampler::startSampling()
{
}