#include "Compass.h"
Compass Compass::aCompass;

Compass::Compass()
{
  //to run the program on the EyeSim simulator the initialization must not take place
  //in the constructor
  //so we have to declare the method as public and call it in main before we start
  //the program
  //if running the program on real robots the initialization must be done in the
  //constructors (otherwise the camera initialization doesn't work, for example)
  //(for whatever reason)
  initialize();
}


Compass::~Compass()
{
	release();
}



Compass* Compass::getCompass ()
{
  return &aCompass;
}


bool Compass::initialize ()
{

  if (COMPASSInit(COMPASS) != 0)
  {
    LCDPutString("Compass init. error!");
    OSPanic(""); //put that here or in calling method??
  }
  COMPASSStart(true); //true means continuous measurement, as opposed to a single measurement


  return true;
}

bool Compass::release ()
{
  if (COMPASSRelease() != 0)
    return false;
  else
    return true;
}

int Compass::getHeading ()
{
  int heading = COMPASSGet();
  if (heading < 0)
    return 0; //really do that???
  else
    return heading;
}