#include <Wire.h>
#include <Servo.h>

// Pins
#define SDA 4 // analog
#define SCL 5 // analog
#define servoPointerPin 10

// Controllers
Servo servoPointer;

void setup()
{
  Serial.begin( 4800 );
  delay( 500 );
  setupCompass();
  setupServo();
  Serial.println( "Setup Complete" );
}

void setupCompass()
{
  Wire.begin();
}

void setupServo()
{
  servoPointer.attach( servoPointerPin );
  positionServo( &servoPointer, 50 );
}

void positionServo( Servo* pServo, int position )
{
  int currentPosition = pServo->read();
  if( currentPosition != position )
    pServo->write( position );
}

int lastHeading = -1;

bool getCurrentHeading( int& reading )
{
  int compassAddress = 0x42 >> 1;
  Wire.beginTransmission( compassAddress );
  Wire.send( 'A' );
  Wire.endTransmission();

  delay(10);  
  Wire.requestFrom( compassAddress, 2 );
  while( Wire.available() < 2 )
    delay(10);

  int newReading = Wire.receive();
  newReading = newReading << 8;
  newReading += Wire.receive();
  newReading /= 10;
  bool result = ( lastHeading == -1 ) ? true : ( abs( newReading - lastHeading ) > 2 );
  reading = newReading;
  if( result )
    lastHeading = reading;  
  return result;
}

void loop()
{
  int heading = 0;
  if( getCurrentHeading(heading) )
    PositionNeedle( heading );
}

void PositionNeedle( const int currentHeading )
{
  int bearing = -currentHeading;
  if( bearing < 0 )
    bearing += 360;

  Serial.print( "Bearing=" );
  Serial.print( bearing, DEC );
  float minSweep = 80.0f;
  float sweepRange = 145.0f - minSweep;
  int servoPosition = (int)(sweepRange - (bearing / 360.0f * sweepRange) + minSweep);
  Serial.print( " Servo=" );
  Serial.println( servoPosition, DEC );
  positionServo( &servoPointer, servoPosition );
}