/*
 * EEPROM Write
 *
 * Stores values read from analog input 0 into the EEPROM.
 * These values will stay in the EEPROM when the board is
 * turned off and may be retrieved later by another sketch.
 */

#include <EEPROM.h>
#include <Wire.h>

#define Addr 0x1E               // 7-bit address of HMC5883 compass


// the current address in the EEPROM (i.e. which byte
// we're going to write to next)
int addr = 0;
#define trigPin 7
#define echoPin 6
float distance = 0;

// Assign LED Output PWM Pins
int Red = 11;
int Green = 10;
int Blue = 9;

void setup(){
  
    pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  
    Serial.begin(9600);
      for (int i = 0; i < 512; i++) {
      Serial.print(i);
  Serial.print("\t");
  Serial.print(i, DEC);
  Serial.println();
  EEPROM.write(i, 100);
  }
  
  Wire.begin();
  
  // Set operating mode to continuous
  Wire.beginTransmission(Addr); 
  Wire.write(byte(0x02));
  Wire.write(byte(0x00));
  Wire.endTransmission();
  for (int i = 0; i < 512; i++) {
  byte  value = EEPROM.read(i);

  Serial.print(i);
  Serial.print("\t");
  Serial.print(value, DEC);
  Serial.println();
  }
}

void loop()
{
//  // need to divide by 4 because analog inputs range from
//  // 0 to 1023 and each byte of the EEPROM can only hold a
//  // value from 0 to 255.
//  int val = analogRead(0) / 4;
//
//
//
//  // write the value to the appropriate byte of the EEPROM.
//  // these values will remain there when the board is
//  // turned off.
//  EEPROM.write(addr, val);
//
//  // advance to the next address.  there are 512 bytes in
//  // the EEPROM, so go back to 0 when we hit 512.
//  addr = addr + 1;
//  if (addr == 512)
//    addr = 0;
//
//  delay(100);

//  long duration, distance;
//  digitalWrite(trigPin, LOW); 
//  delayMicroseconds(2);
//  digitalWrite(trigPin, HIGH);
//  delayMicroseconds(10);
//  digitalWrite(trigPin, LOW);
//  duration = pulseIn(echoPin, HIGH);
//  distance = (duration/2) / 29.1;

  readCompass();

  readPresence();
  readSerial();
  if (distance > 0) {
      Serial.print(distance);
    Serial.println(" cm");
    
    if (distance < 100) {
      //analogWrite(3, 255 - distance * 2.5);
    }
  } else {
    // analogWrite(3, 0);
  }
  
  delay(100);
}

void readPresence() {
  digitalWrite(trigPin, LOW); //Low high and low level take a short time to trigPin pulse
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  float cm = pulseIn(echoPin, HIGH) / 58.0; //Echo time conversion into cm
  // cm = (int(cm * 100.0)) / 100.0; //Keep two decimal places
  
  //distance = cm * 100;
  distance = cm;
}

void readSerial() {
    // First we check to see if incoming data is available:

    while (Serial.available() > 0)
    {
      // If it is, we'll use parseInt() to pull out any numbers:

      int speed = Serial.parseInt();

      // Because analogWrite() only works with numbers from
      // 0 to 255, we'll be sure the input is in that range:

      speed = constrain(speed, 0, 255);

      // We'll print out a message to let you know that the
      // number was received:

      Serial.print("Setting speed to ");
      Serial.println(speed);

      // And finally, we'll set the speed of the motor!

      analogWrite(3, speed);
    }
}

void readCompass() {
  int x, y, z;

  // Initiate communications with compass
  Wire.beginTransmission(Addr);
  Wire.write(byte(0x03));       // Send request to X MSB register
  Wire.endTransmission();

  Wire.requestFrom(Addr, 6);    // Request 6 bytes; 2 bytes per axis
  if(Wire.available() <=6) {    // If 6 bytes available
    x = Wire.read() << 8 | Wire.read();
    z = Wire.read() << 8 | Wire.read();
    y = Wire.read() << 8 | Wire.read();
  }
  
  // Print raw values
  Serial.print("X="); Serial.print(x);
  //Serial.print(min(255, 255.0 * (512 + x) / 1024.0));
  Serial.print(", Y="); Serial.print(y);
  //Serial.print(min(255, 255.0 * (512 + y) / 1024.0));
  Serial.print(", Z="); Serial.println(z);
  //Serial.println(min(255, 255.0 * (512 + z) / 1024.0));
  
 // EEPROM.write(addr, x);

          
    analogWrite(Red, min(255, 255.0 * (512 + x) / 1024.0));
    analogWrite(Green, min(255, 255.0 * (512 + y) / 1024.0));
    analogWrite(Blue, min(255, 255.0 * (512 + z) / 1024.0));
}