#include<Wire.h>
const int MPU_addr = 0x68; // I2C address of the MPU-6050
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;

#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#define OLED_RESET 23
Adafruit_SSD1306 display(OLED_RESET);

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BME280.h>
#define SEALEVELPRESSURE_HPA (1013.25)


#define Buzzer A1
#define LED1 22 //dekat xbee
#define LED2 24
#define LED3 26
#define LED4 28
#define LED5 30
#define LED6 32
#define LED7 34
#define LED8 36
#define LED9 38
#define LED10 40


#define btnDown 2
#define btnRight 3
#define btnUp 4
#define btnLeft 5
#define btnCenter 6

#define sw1 A2 //dekat header
#define sw2 A3
#define sw3 A4
#define sw4 A5
const int
PWM_A   = 9,
PWM_B   = 12,
DIR_A1   = 7,
DIR_A2   = 8,
DIR_B1   = 10,
DIR_B2   = 11;

int  barLED[] = {22, 24, 26, 28, 30, 32, 34, 36, 38, 40};

int mspeed;
// prototipe function here
void testBuzzer(void);
void GPIOinit(void);
void testBarLED(void);
void OLEDinit(void);
void printBME280Value(void);


Adafruit_BME280 bme; // I2C

void setup()
{
  GPIOinit();
  OLEDinit();
    // by default, we'll generate the high voltage from the 3.3v line internally! (neat!)
  Serial.begin(9600);
  Serial.println("1-Test buzzer:\n");
  Serial.println("Please Listen the test  manually:\n");
  display.setCursor(0,10);
  display.println("Please use Serial ");
  display.setCursor(0,20);
  display.println("Port Terminal");
  display.display();
  testBuzzer();
  Serial.println("1-Test BarLED:\n");
  Serial.println("Please see the result manually:\n");
  testBarLED();

  Serial.println("2-Motor shield DC motor Test and Btn:\n");
  //gy-88 init***
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  //gy-88 end of init

    bool status;
    status = bme.begin();  
    if (!status) {
        Serial.println("Could not find a valid BME280 sensor, check wiring!");
        while (1);
    }
   delay(100); // let sensor boot up  
   printBME280Value();
}

// the loop function runs over and over again forever
void loop()
{

  mspeed = analogRead(A0);
  mspeed = map(mspeed, 0, 705, 0, 255); //map the pot. output in range of 0-255
  Serial.println(mspeed);

  digitalWrite(DIR_A1, HIGH);
  //  digitalWrite(DIR_A2, LOW);
  //  analogWrite(PWM_A, mspeed);
  //
  //  digitalWrite(DIR_B1, HIGH);
  //  digitalWrite(DIR_B2, LOW);
  //  analogWrite(PWM_B, mspeed);

  if (digitalRead(btnDown) == LOW)
    digitalWrite(LED1, HIGH);
  else
    digitalWrite(LED1, LOW);
    
  if (digitalRead(btnRight) == LOW)
    digitalWrite(LED2, HIGH);
  else
    digitalWrite(LED2, LOW);

  if (digitalRead(btnUp) == LOW)
    digitalWrite(LED3, HIGH);
  else
    digitalWrite(LED3, LOW);

  if (digitalRead(btnLeft) == LOW)
    digitalWrite(LED4, HIGH);
  else
    digitalWrite(LED4, LOW);

  if (digitalRead(btnCenter) == LOW)
    digitalWrite(LED5, HIGH);
  else
    digitalWrite(LED5, LOW);

  if (digitalRead(sw1) == LOW)
    digitalWrite(LED6, HIGH);
  else
    digitalWrite(LED6, LOW);

  if (digitalRead(sw2) == LOW)
    digitalWrite(LED7, HIGH);
  else
    digitalWrite(LED7, LOW);

  if (digitalRead(sw3) == LOW)
    digitalWrite(LED8, HIGH);
  else
    digitalWrite(LED8, LOW);

  if (digitalRead(sw4) == LOW)
    digitalWrite(Buzzer, HIGH);
  else
    digitalWrite(Buzzer, LOW);

  //gy-88 test***
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 14, true); // request a total of 14 registers
  AcX = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  AcY = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp = Wire.read() << 8 | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyX = Wire.read() << 8 | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY = Wire.read() << 8 | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ = Wire.read() << 8 | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  Serial.print("AcX = ");
  Serial.print(AcX);
  Serial.print(" | AcY = ");
  Serial.print(AcY);
  Serial.print(" | AcZ = ");
  Serial.print(AcZ);
  Serial.print(" | Tmp = ");
  Serial.print(Tmp / 340.00 + 36.53); //equation for temperature in degrees C from datasheet
  Serial.print(" | GyX = ");
  Serial.print(GyX);
  Serial.print(" | GyY = ");
  Serial.print(GyY);
  Serial.print(" | GyZ = ");
  Serial.println(GyZ);
  delay(333);
  //end test of gy-88
}

void GPIOinit(void)
{
  for (int i = 0; i < 10; i++) {
    pinMode(barLED[i], OUTPUT);
  }
  pinMode(Buzzer , OUTPUT);
  pinMode(btnDown, INPUT_PULLUP);
  pinMode(btnRight, INPUT_PULLUP);
  pinMode(btnUp, INPUT_PULLUP);
  pinMode(btnLeft, INPUT_PULLUP);
  pinMode(btnCenter, INPUT_PULLUP);
  pinMode(sw1, INPUT_PULLUP);
  pinMode(sw2, INPUT_PULLUP);
  pinMode(sw3, INPUT_PULLUP);
  pinMode(sw4, INPUT_PULLUP);

  pinMode(DIR_A1, OUTPUT);
  pinMode(DIR_A2, OUTPUT);
  pinMode(DIR_B1, OUTPUT);
  pinMode(DIR_B2, OUTPUT);
}

void OLEDinit(void){
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);  // initialize with the I2C addr 0x3D (for the 128x64)
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  // init done

  display.setCursor(0,0);
  display.println("Demo WSN Board v1.0!");
  display.display();
  delay(300);
}

void testBuzzer(void)
{
  for (int i = 0; i < 5; i++) {
    digitalWrite(Buzzer, HIGH);   // turn the LED on (HIGH is the voltage level)
    delay(100);              // wait for a second
    digitalWrite(Buzzer , LOW);    // turn the LED off by making the voltage LOW
    delay(100);              // wait for a second
  }
}

void testBarLED(void) {
  for (int i = 0; i < 3; i++)   {
    for (int i = 0; i < 10; i++) {    
      digitalWrite(barLED[i], HIGH);
      delay(300);
    }
    for (int i = 0; i < 10; i++) {
      digitalWrite(barLED[i], LOW);
    }
  }
}

void printBME280Value(void) {
      Serial.println(F("BME280 test"));
    Serial.print("Temperature = ");
    Serial.print(bme.readTemperature());
    Serial.println(" *C");

    Serial.print("Pressure = ");

    Serial.print(bme.readPressure() / 100.0F);
    Serial.println(" hPa");

    Serial.print("Approx. Altitude = ");
    Serial.print(bme.readAltitude(SEALEVELPRESSURE_HPA));
    Serial.println(" m");

    Serial.print("Humidity = ");
    Serial.print(bme.readHumidity());
    Serial.println(" %");

    Serial.println();
}