/*
HM5833L with Teensy I2C example

 We use the compass difference with a given direction to make a sound
 Plays a pitch that changes based on a changing analog input
 Also, the delay changes as the tone gets higher
 In addition to the tone, a LED flashes for a short duration

 This program is a rewrite of an earlier program of mine, "CMPS_03_tone_button" 
 rewritten for the HNMC5883L compass.
 Based on https://www.loveelectronics.co.uk/Tutorials/8/hmc5883l-tutorial-and-arduino-library

 The SCL line is on pin 5 of the Teensy 2.0 and is conected to SCL pin of HMC5883L.
 The SDA line is on pin 6 of the Teensy 2.0 and is connected to SDCA pin of HMC5883L.
 Both SDA and SCL are also connected to the +5v via a couple of 10k resistors.

 Sound circuit:
 * 8 Ohm small speaker connected to SOUND_PIN via a 100 Ohm resistor 
 (based on http://arduino.cc/en/Tutorial/tone) 
 Sound circuit is based on the program "Pitch Follower" by Tom Igoe
 http://arduino.cc/en/Tutorial/Tone2
 
 Created, combined and adapted by Gidi van Liempd (gidi@geedesign.com)
 This example code is in the public domain. 

*/
// Reference the I2C Library
#include <Wire.h>
// Reference the HMC5883L Compass Library
#include <HMC5883L.h>

// Store our compass as a variable.
HMC5883L compass;
// Record any errors that may occur in the compass.
int error = 0;

int target_direction = 0;      // (will be determined in INIT_STATE)

#define COMPASS_OFFSET  (90)   // must at least be >= 0
#define INIT_STATE  0          // initial state, will be used to set the direction of search
#define SEEK_STATE  1          // seek state, a sound is played depending on the direction
int actual_state = 0;  

#define BUTTON_PIN 2          // button to toggle state
#define SIGNAL_LED_PIN 3      // indicate flashing signal  
#define STATE_LED_PIN  11     // indicate state (we use the internal LED)  
#define SOUND_PIN (9)         // output pin for speaker 

boolean button_reset = true;  // checks if button press is processed or not

#define MIN_SENSOR (0)       // minimum value to compute a sound for
#define MAX_SENSOR (180)
#define LOW_TONE (1000)
#define HIGH_TONE (5000)
#define DURATION (50)        // length of tone

#define MIN_DELAY (0)        // min interval between sounds
#define MAX_DELAY (950)      // max interval between sounds

void setup(){
  pinMode(BUTTON_PIN, INPUT_PULLUP);
  pinMode(STATE_LED_PIN, OUTPUT);
  pinMode(SIGNAL_LED_PIN, OUTPUT);

  Wire.begin(); //conects I2C interface
  Serial.begin(9600);
  Serial.println("Starting ...");

  // Initialize the serial port.
  Serial.begin(9600);

  Serial.println("Starting the I2C interface.");
  Wire.begin(); // Start the I2C interface.

  // Initialize compass
  Serial.println("Constructing new HMC5883L");
  compass = HMC5883L(); // Construct a new HMC5883 compass.
    
  Serial.println("Setting scale to +/- 1.3 Ga");
  error = compass.SetScale(1.3); // Set the scale of the compass.
  if(error != 0) // If there is an error, print it out.
    Serial.println(compass.GetErrorText(error));
  
  Serial.println("Setting measurement mode to continous.");
  error = compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous
  if(error != 0) // If there is an error, print it out.
    Serial.println(compass.GetErrorText(error));
  
}

void loop(){
  int newbearing;       // new bearing from the compass
  int absdiff;          // absolute value of difference
  
  newbearing = GetBearing();
  
  process_input();	// check if the user has pressed a button
  
  // do what has to be done based on the actual state
  switch (actual_state) {
    case INIT_STATE:
      // In the init state, we determine a new target direction
      digitalWrite(STATE_LED_PIN, HIGH);
      Serial.println(newbearing); 
      target_direction = newbearing + COMPASS_OFFSET;
      if (target_direction > 360) {
          target_direction -= 360;
      };
      delay(100);  // check direction every 0.1 sec
      break;
    case SEEK_STATE:
      digitalWrite(STATE_LED_PIN, LOW);
      // In the seek state, we make a sound based on the difference in direction
      // compute the closest way to turn    
      absdiff = abs(ClosestDiff( target_direction, newbearing));  
      Serial.print(target_direction);
      Serial.print("\t");    // prints a tab
      Serial.print(newbearing);
      Serial.print("\t");    // prints a tab
      Serial.println(absdiff); 
 
      // map the pitch to the range of the analog input.
      int thisPitch = map(absdiff, MIN_SENSOR, MAX_SENSOR, HIGH_TONE, LOW_TONE);
      // use the analog input also to map the delay
      int mydelay = map(absdiff, MIN_SENSOR, MAX_SENSOR, MIN_DELAY, MAX_DELAY);
      // play the pitch:
      tone(SOUND_PIN, thisPitch, DURATION);
      // show the led
      digitalWrite(SIGNAL_LED_PIN, HIGH);
      delay(DURATION);
      digitalWrite(SIGNAL_LED_PIN, LOW);     
      // additonal delay
      delay(mydelay);
      break;
  }

}

// handle button press
void process_input(void) {
  int new_state;
  
    if (digitalRead(BUTTON_PIN) == HIGH) {  // Button is not pressed
        button_reset = true;
    } 
    else {                                // Button is pressed
      if (button_reset) {  // button press was not yet processed
        // process the button press
        switch (actual_state) {
          case INIT_STATE:
            new_state = SEEK_STATE;
            break;
          case SEEK_STATE:
            new_state = INIT_STATE;            
            break;
        };
        actual_state = new_state;  // change the state
        button_reset = false;  // 
      };  // if button not reset, the button press is already handled
   }
}

// return the current bearing from the compass in degrees (0 - 359)
int GetBearing(void) {
  // Retrieve the scaled values from the compass (scaled to the configured scale).
  MagnetometerScaled scaled = compass.ReadScaledAxis();
  
  // Calculate heading when the magnetometer is level, then correct for signs of axis.
  float heading = atan2(scaled.YAxis, scaled.XAxis);
  
  // Once you have your heading, you must then add your 'Declination Angle', which is the 'Error' of the magnetic field in your location.
  // Find yours here: http://www.magnetic-declination.com/
  // Mine is: 2� 37' W, which is 2.617 Degrees, or (which we need) 0.0456752665 radians, I will use 0.0457
  // If you cannot find your Declination, comment out these two lines, your compass will be slightly off.
  //float declinationAngle = 0.0457;
  //heading += declinationAngle;
  
  // Correct for when signs are reversed.
  if(heading < 0)
    heading += 2*PI;
    
  // Check for wrap due to addition of declination.
  if(heading > 2*PI)
    heading -= 2*PI;
   
  // Convert radians to degrees for readability.
  float headingDegrees = heading * 180/M_PI; 

  return ((int) headingDegrees);
}

/*
* Compute the least number of degrees to turn (and direction)
* olddir and newdir are taken to be angles 0<= angle <= 360
*/
int ClosestDiff(int olddir, int newdir) {
  int diff1, diff2;
  
  if (newdir > olddir) {  // newdir is a higher value than olddir
    diff1 = newdir - olddir;          // (positive value)
    diff2 = (olddir + 360) - newdir;  // (positive value)
    if (diff1 < diff2) {
      return diff1;  // turn right (increase degrees)
    }
    else {
      return (-diff2); // turn left
    }  
  }
  else {
    diff1 = olddir - newdir;          // (positive value)
    diff2 = (newdir + 360) - olddir;  // (positive value)
    if (diff1 < diff2) {
      return (-diff1);  // turn left (decrease degrees)
    }
    else {
      return diff2; // turn right
    }  
  }
}