#include <Wire.h> 

int compassAddress = 0x32 >> 1;  // From datasheet compass address is 0x42 
// shift the address 1 bit right, the Wire library only needs the 7 
// most significant bits for the address 
int reading = 0; 

#define SensorPin1      0
#define filterSamples   21              // filterSamples should  be an odd number, no smaller than 3
int sensSmoothArray1 [filterSamples];   // array for holding raw sensor values for sensor1 

int rawData1, smoothData1;  // variables for sensor1 data

void setup() 
{ 
  Wire.begin();                // join i2c bus (address optional for master) 
  Serial.begin(9600);          // start serial communication at 9600bps 
  pinMode(48, OUTPUT); 
  digitalWrite(48, HIGH); 
  pinMode(1, OUTPUT); 
  pinMode(2, OUTPUT); 
  pinMode(3, OUTPUT); 
  pinMode(4, OUTPUT); 
  pinMode(6, OUTPUT);   
  pinMode(7, OUTPUT);   
  pinMode(8, OUTPUT); 
  DDRD = DDRD | B11111100;
  PORTD = B00000000;      
  digitalWrite(8, LOW);      
} 

void myPulse()
{
  digitalWrite(7, LOW);
  delay(5);                  // wait for half a second 
  digitalWrite(7, HIGH);
  delay(5);                  // wait for half a second 
  digitalWrite(7, LOW);
}

void loop() 
{ 
  // step 1: instruct sensor to read echoes 
  Wire.beginTransmission(compassAddress); // transmit to device 
  // the address specified in the datasheet is 66 (0x42) 
  // but i2c adressing uses the high 7 bits so it's 33 
  Wire.send(0x50);             // command sensor to measure angle  
  Wire.endTransmission();     // stop transmitting 

  // step 2: wait for readings to happen 
  delay(10);                   // datasheet suggests at least 6000 microseconds 

  // step 3: request reading from sensor 
  Wire.requestFrom(compassAddress, 6);    // request 2 bytes from slave device #33 
  // step 4: receive reading from sensor 
  if(6 <= Wire.available())    // if two bytes were received 
  { 
    reading = Wire.receive();  // receive high byte (overwrites previous reading) 
    reading = reading << 8;    // shift high byte to be high 8 bits 
    reading += Wire.receive(); // receive low byte as lower 8 bits 
    reading /= 10; 

    rawData1=reading;
    Serial.println(reading);
    smoothData1 = digitalSmooth(rawData1, sensSmoothArray1);
    Serial.print(rawData1);
    Serial.print("   ");
    Serial.println(smoothData1);


    if(smoothData1 >= 348 || smoothData1 < 12)
    {
      Serial.println("B00000000");   // print the smoothData1 
      PORTD = B00000000;           
      myPulse();
    }
    else if(smoothData1 >= 12 && smoothData1 < 36)
    {
      Serial.println("B00000100");   // print the smoothData1 
      PORTD = B00000100;           
      myPulse();
    }
    else if(smoothData1 >= 36 && smoothData1 < 60)
    {
      Serial.println("B00001000");   // print the smoothData1 
      PORTD = B00001000;           
      myPulse();
    }
    else if(smoothData1 >= 60 && smoothData1 < 84)
    {
      Serial.println("B00001100");   // print the smoothData1 
      PORTD = B00001100;           
      myPulse();
    }
    else if(smoothData1 >= 84 && smoothData1 < 108)
    {
      Serial.println("B00010000");   // print the smoothData1 
      PORTD = B00010000;           
      myPulse();
    }
    else if(smoothData1 >= 108 && smoothData1 < 132)
    {
      Serial.println("B00010100");   // print the smoothData1 
      PORTD = B00010100;           
      myPulse();
    }
    else if(smoothData1 >= 132 && smoothData1 < 156)
    {
      Serial.println("B00011000");   // print the smoothData1 
      PORTD = B00011000;           
      myPulse();
    }
    else if(smoothData1 >= 156 && smoothData1 < 180)
    {
      Serial.println("B00011100");   // print the smoothData1 
      PORTD = B00011100;           
      myPulse();
    }
    else if(smoothData1 >= 180 && smoothData1 < 204)
    {
      Serial.println("B00100000");   // print the smoothData1 
      PORTD = B00100000;           
      myPulse();
    }
    else if(smoothData1 >= 204 && smoothData1 < 228)
    {
      Serial.println("B00100100");   // print the smoothData1 
      PORTD = B00100100;           
      myPulse();
    }
    else if(smoothData1 >= 228 && smoothData1 < 252)
    {
      Serial.println("B00101000");   // print the smoothData1 
      PORTD = B00101000;           
      myPulse();
    }
    else if(smoothData1 >= 252 && smoothData1 < 276)
    {
      Serial.println("B00101100");   // print the smoothData1 
      PORTD = B00101100;           
      myPulse();
    }
    else if(smoothData1 >= 276 && smoothData1 < 300)
    {
      Serial.println("B00110000");   // print the smoothData1 
      PORTD = B00110000;           
      myPulse();
    }
    else if(smoothData1 >= 300 && smoothData1 < 324)
    {
      Serial.println("B00110100");   // print the smoothData1 
      PORTD = B00110100;           
      myPulse();
    }
    else if(smoothData1 >= 324 && smoothData1 < 348)
    {
      Serial.println("B00111000");   // print the smoothData1 
      PORTD = B00111000;           
      myPulse();
    }

    delay(50);                  // wait for half a second 
  }
}

int digitalSmooth(int rawIn, int *sensSmoothArray){     // "int *sensSmoothArray" passes an array to the function - the asterisk indicates the array name is a pointer
  int j, k, temp, top, bottom;
  long total;
  static int i;
  // static int raw[filterSamples];
  static int sorted[filterSamples];
  boolean done;

  i = (i + 1) % filterSamples;    // increment counter and roll over if necc. -  % (modulo operator) rolls over variable
  sensSmoothArray[i] = rawIn;                 // input new data into the oldest slot

  // Serial.print("raw = ");

  for (j=0; j<filterSamples; j++){     // transfer data array into anther array for sorting and averaging
    sorted[j] = sensSmoothArray[j];
  }

  done = 0;                // flag to know when we're done sorting              
  while(done != 1){        // simple swap sort, sorts numbers from lowest to highest
    done = 1;
    for (j = 0; j < (filterSamples - 1); j++){
      if (sorted[j] > sorted[j + 1]){     // numbers are out of order - swap
        temp = sorted[j + 1];
        sorted [j+1] =  sorted[j] ;
        sorted [j] = temp;
        done = 0;
      }
    }
  }

  /*
  for (j = 0; j < (filterSamples); j++){    // print the array to debug
   Serial.print(sorted[j]); 
   Serial.print("   "); 
   }
   Serial.println();
   */

  // throw out top and bottom 15% of samples - limit to throw out at least one from top and bottom
  bottom = max(((filterSamples * 15)  / 100), 1); 
  top = min((((filterSamples * 85) / 100) + 1  ), (filterSamples - 1));   // the + 1 is to make up for asymmetry caused by integer rounding
  k = 0;
  total = 0;
  for ( j = bottom; j< top; j++){
    total += sorted[j];  // total remaining indices
    k++; 
    // Serial.print(sorted[j]); 
    // Serial.print("   "); 
  }

  //  Serial.println();
  //  Serial.print("average = ");
  //  Serial.println(total/k);
  return total / k;    // divide by number of samples
}