#include <wire.h> ;

int ASCIIoutputOn =false; //Toggles Serial ouput of measurements, note that this uses pins 0 and 1
int AudioOutputOn =true; //Toggles audio ouput of measurements

int HMC6343Address = 0x32;
int slaveAddress; // This is calculated in the setup() function

byte inByte;

byte headingData[6];
int i;

long time = 0;

int headingValue =0;
int tiltValue =0;
int rollValue =0;

int audioPinR =9; // This is the pin for the right channel audio bit-banging
int audioPinL =10; // This is the pin for the right channel audio bit-banging
// Note that these should not be in output mode unless needed,
// so they are not included in setup()

void setup()
{

TCCR1B = 0x09; // Make PWM frequency more audio friendly on pins 9 and 10

// Shift the device's documented slave address (0x32) 1 bit right
// This compensates for how the TWI library only wants the
// 7 most significant bits (with the high bit padded with 0)

slaveAddress = HMC6343Address >> 1; // This results in the 7 bit address to pass to TWI

while( millis() < 500) { delay(10); } // The HMC6343 needs a half second to start from power up

Wire.begin();

if (ASCIIoutputOn)
{
Serial.begin(9600);
Serial.print(10,BYTE);
Serial.println("Ready to start!");
Serial.print(10,BYTE);
}
}

void getSensorData()
{

Wire.beginTransmission(slaveAddress);
Wire.send(0x50); // Send a "Get Data" (0x50) command to the HMC6343
Wire.endTransmission();

delay(2); // The HMC6343 needs at least a 1ms (microsecond) delay
// after this command. Using 2ms just makes it safe

// Read the 6 heading bytes, MSB first
// The resulting 3 16bit words are the compass heading, the tilt and roll in 10th's of a degree
// For example: a heading of 1345 would be 134.5 degrees

Wire.requestFrom(slaveAddress, 6); // Request the 6 bytes of data (MSB comes first)
i = 0;
while(Wire.available() && i < 6)
{
headingData[i] = Wire.receive();
i++;
}
}

void convertDataToValues()
{
headingValue = headingData[0]*256 + headingData[1]; // Put the MSB and LSB together
tiltValue = headingData[2]*256 + headingData[3] + 900; // Put the MSB and LSB together
rollValue = headingData[4]*256 + headingData[5] + 1800; // Put the MSB and LSB together
}

void sendSerialValues()
{
Serial.print("$HTR,");
Serial.print(int (headingValue / 10)); // The whole number part of the heading
Serial.print(".");
Serial.print(int (abs (headingValue % 10))); // The fractional part of the heading
Serial.print(",");
Serial.print(int (tiltValue / 10)); // The whole number part of the tilt
Serial.print(".");
Serial.print(int (abs (tiltValue % 10))); // The fractional part of the tilt
Serial.print(",");
Serial.print(int (rollValue / 10)); // The whole number part of the roll
Serial.print(".");
Serial.print(int (abs (rollValue % 10))); // The fractional part of the roll
//Serial.print(",");
//Serial.print(ratioPin); // The remainder of the division by the number of output pins, scaled to 256
Serial.print(",A ");
//Serial.print(10,BYTE);
Serial.print(13,BYTE);
}

void stereoSound(int freq, int t, int phase, boolean phaseSign, int volumeR, int volumeL)
// freq in hz, t in ms, phase in us, phaseSign true if positive, volume from 0 to 255
{
int hperiod; //calculate 1/2 period in us
int audioPin1 =audioPinL;
int audioPin2 =audioPinR;
int volume1 =volumeL;
int volume2 =volumeR;
long cycles, i;

//pinMode(audioPinR, OUTPUT); // turn on output pin, not needed for analogWrite
//pinMode(audioPinL, OUTPUT); // turn on output pin, not needed for analogWrite

hperiod = (500000 / freq) - 7; // subtract 7 us to make up for digitalWrite overhead

cycles = ((long)freq * (long)t) / 1000; // calculate cycles

phase = max(3,phase);

if (phaseSign)
{
audioPin1 =audioPinR;
audioPin2 =audioPinL;
volume1 =volumeR;
volume2 =volumeL;
}

analogWrite(audioPin1, volume1);
for (i=0; i<= cycles; i++){ // play note for t ms
delayMicroseconds(phase);
analogWrite(audioPin2, volume2);
delayMicroseconds(hperiod-phase);
analogWrite(audioPin1, 0);
delayMicroseconds(phase);
analogWrite(audioPin2, 0);
delayMicroseconds(hperiod -phase - 1); // - 1 to make up for fractional us
analogWrite(audioPin1, volume1);
}
delayMicroseconds(hperiod);
analogWrite(audioPin1, 0);

//pinMode(audioPinR, INPUT); // shut off pin to avoid noise from other operations, not needed for analogWrite
//pinMode(audioPinL, INPUT);

}

void delayLoop(int duration)
{
while( (millis() - time) < duration) { delay(1); }
time = millis();
}

void loop()
{
getSensorData();

convertDataToValues();

if (ASCIIoutputOn) { sendSerialValues(); }

if (AudioOutputOn)
{
stereoSound(
150, // frequency of beep
40, // duration of beep
abs(int(1200*sin(float(headingValue-1800)*0.001745))), // phase shift in us
(headingValue < 1800), // phase shift sign (true if right before left)
constrain(map(headingValue,2700,1800,0,255),0,255),
constrain(map(headingValue,1800,900,255,0),0,255)
);
}

delayLoop(200); // The HMC6343 needs 200ms between measurement
// this function will make sure loop() lasts at least that long