/*-----------------------------------------------------------------------
 A couple of utility functions for handling a list of MotReg
 structures . This is a helper module for the anticollider collision
 control system. See anticollider.tex for more details.

 copyright: see file copyright

 Mark Koennecke, August 2002
-------------------------------------------------------------------------*/
#include <stdlib.h>
#include <assert.h>
#include "fortify.h"
#include "lld.h"
#include "motreglist.h"

/*-----------------------------------------------------------------------*/
int MakeMotList()
{
  return LLDcreate(sizeof(pMotReg));
}

/*----------------------------------------------------------------------*/
pMotReg FindMotEntry(int iList, char *name)
{
  int iRet;
  pMotReg pMot = NULL;

  iRet = LLDnodePtr2First(iList);
  while (iRet != 0) {
    LLDnodeDataTo(iList, &pMot);
    if (pMot != NULL) {
      if (RegMotMatch(pMot, name)) {
        return pMot;
      }
    }
    iRet = LLDnodePtr2Next(iList);
  }
  return NULL;
}

/*-----------------------------------------------------------------------*/
pMotReg FindMotFromDataStructure(int iList, void *pData)
{
  int iRet;
  pMotReg pMot = NULL;

  iRet = LLDnodePtr2First(iList);
  while (iRet != 0) {
    LLDnodeDataTo(iList, &pMot);
    if (pMot != NULL) {
      if (pMot->motorData == pData) {
        return pMot;
      }
    }
    iRet = LLDnodePtr2Next(iList);
  }
  return NULL;
}

/*----------------------------------------------------------------------*/
int CheckAllMotors(int iList, SConnection * pCon)
{
  int iRet, count = 0;
  pMotReg pMot = NULL;

  iRet = LLDnodePtr2First(iList);
  while (iRet != 0) {
    LLDnodeDataTo(iList, &pMot);
    if (pMot != NULL) {
      CheckRegMot(pMot, pCon);
      if (pMot->iActive) {
        count++;
      }
    }
    iRet = LLDnodePtr2Next(iList);
  }
  return count;
}

/*----------------------------------------------------------------------*/
void StopAllMotors(int iList)
{
  int iRet, count = 0;
  pMotReg pMot = NULL;
  pIDrivable pDriv;

  iRet = LLDnodePtr2First(iList);
  while (iRet != 0) {
    LLDnodeDataTo(iList, &pMot);
    if (pMot != NULL) {
      if (pMot->iActive) {
        pDriv = (pIDrivable) GetDrivableInterface(pMot->motorData);
        if (pDriv) {
          pDriv->Halt(pMot->motorData);
        }
        pMot->iActive = 0;
      }
    }
    iRet = LLDnodePtr2Next(iList);
  }
}

/*----------------------------------------------------------------------*/
void DeactivateAllMotors(int iList)
{
  int iRet, count = 0;
  pMotReg pMot = NULL;
  pIDrivable pDriv;

  iRet = LLDnodePtr2First(iList);
  while (iRet != 0) {
    LLDnodeDataTo(iList, &pMot);
    if (pMot != NULL) {
      pMot->iActive = 0;
    }
    iRet = LLDnodePtr2Next(iList);
  }
}

/*----------------------------------------------------------------------*/
void KillMotList(int iList)
{
  int iRet;
  pMotReg pMot = NULL;

  iRet = LLDnodePtr2First(iList);
  while (iRet != 0) {
    LLDnodeDataTo(iList, &pMot);
    if (pMot != NULL) {
      KillRegMot(pMot);
    }
    iRet = LLDnodePtr2Next(iList);
  }
  LLDdelete(iList);
}