/*-----------------------------------------------------------------------
 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);
}