/*
 * MotorKnob
 *
 * A stepper motor follows the turns of a potentiometer
 * (or other sensor) on analog input 0.
 *
 * http://www.arduino.cc/en/Reference/Stepper
 * This example code is in the public domain.
 */
 /*
 VIN - this is the power in pin, give it 3-5VDC. It is reverse-polarity protected. Use the same voltage as you do for the controlling microcontroller's logic. For an Arduino, 5V is best.
 GND - this is the common power and data ground pin
 3vo - This is the 3.3V output from the onboard regulator. It will be at 3.3V and you can grab up to 100mA from this to power other items
 SDA and SCL - these are the I2C data and clock pins used to send and receive data from the module to your microcontroller. There are 10K pullups on these pins to the VIN pin. You can connect these pins to 5V I2C lines, there are level shifters on board to safely bring the pins down to 3V
 RDY - this it the 'data ready' pin output. If you want to stream data at high speed (higher than 100 times a second) you may want to listen to this pin for when data is ready to be read. Check the datasheet for more details about using the RDY pin, we don't use it because we don't read that fast!
 */
 /*
 WIRING FOR PHOTON
 VIN -> 3V3 (regulated)
 GND -> GND
 3v0 -> NONE
 SDA -> D0 - YOU MUST USE THIS PIN
 SCL -> D1 - YOU MUST USE THIS PIN
 RDY -> NONE
 */
#include <Adafruit_Sensor.h>
#include <Adafruit_HMC5883.h>
Adafruit_HMC5883_Unified mag = Adafruit_HMC5883_Unified(12345);
int sPin = D2;
Servo myServo;
float angle = 0;
int ledPin = A1;
int switchPin = D3;
bool shouldToggleLed = false;
void setup() {
  Serial.begin(9600);
  myServo.attach(sPin);
  Serial.println("HMC5883 Magnetometer Test"); Serial.println("");
  /* Initialise the sensor */
  if(!mag.begin())
  {
    /* There was a problem detecting the HMC5883 ... check your connections */
    Serial.println("Ooops, no HMC5883 detected ... Check your wiring!");
    while(1);
  }
  /* Display some basic information on this sensor */
  displaySensorDetails();
  pinMode( switchPin , INPUT_PULLUP); // sets pin as input for switch
  pinMode( ledPin , OUTPUT ); // sets pin as output for LED
  Particle.subscribe("diot2017/mashedpotatoes/panda/LedOn", Led);
}
void loop() {
  if( shouldToggleLed )
  {
     digitalWrite( ledPin, HIGH );
     shouldToggleLed = false;
  }
  else
  {
     digitalWrite( ledPin, LOW );
  }
  /* Get a new sensor event */
   sensors_event_t event;
   mag.getEvent(&event);
   /* Display the results (magnetic vector values are in micro-Tesla (uT)) */
   Serial.print("X: "); Serial.print(event.magnetic.x); Serial.print("  ");
   Serial.print("Y: "); Serial.print(event.magnetic.y); Serial.print("  ");
   Serial.print("Z: "); Serial.print(event.magnetic.z); Serial.print("  ");Serial.println("uT");
   // Hold the module so that Z is pointing 'up' and you can measure the heading with x&y
   // Calculate heading when the magnetometer is level, then correct for signs of axis.
   float heading = atan2(event.magnetic.y, event.magnetic.x);
   // 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: -13* 2' W, which is ~13 Degrees, or (which we need) 0.22 radians
   // If you cannot find your Declination, comment out these two lines, your compass will be slightly off.
   float declinationAngle = 0.22;
   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;
   Serial.print("Heading (degrees): "); Serial.println(headingDegrees);
   delay(500);
   //Panda Rotation
   if ( headingDegrees > 180) {
     angle = 360 - headingDegrees;
     }
   else {
     angle = headingDegrees;
     }
  myServo.write (angle);
  Serial.print("Rotation Angle (degrees): "); Serial.println(angle);
  delay(1000);
  //20 and 60 for arm
  //acknowledgement
  int buttonState = digitalRead(switchPin);
  if( buttonState == LOW )
  {
     Particle.publish("diot2017/mashedpotatoes/panda/nod");
     Serial.println ("It should nod");
  }
}
void displaySensorDetails(void)
{
  sensor_t sensor;
  mag.getSensor(&sensor);
  Serial.println("------------------------------------");
  Serial.print  ("Sensor:       "); Serial.println(sensor.name);
  Serial.print  ("Driver Ver:   "); Serial.println(sensor.version);
  Serial.print  ("Unique ID:    "); Serial.println(sensor.sensor_id);
  Serial.print  ("Max Value:    "); Serial.print(sensor.max_value); Serial.println(" uT");
  Serial.print  ("Min Value:    "); Serial.print(sensor.min_value); Serial.println(" uT");
  Serial.print  ("Resolution:   "); Serial.print(sensor.resolution); Serial.println(" uT");
  Serial.println("------------------------------------");
  Serial.println("");
  delay(500);
}
void Led (const char *event,  const char *data)
{
  Serial.println(  "LED Event triggered");
    // turn the LED On
  //  analogWrite( ledPin, HIGH);
  shouldToggleLed = true;
}
/*void Rotate()
{
  delay()
}*/