#include <Arduino.h>
#include <HCSR04.h>
#include <SPI.h>
#include <Wire.h>
#include <ICM_20948.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BME280.h>


#include "freertos/semphr.h"



typedef struct
{
  uint8_t data_ready;
  SemaphoreHandle_t mutex;

} data_flag_t;





// =====================================================
// ULTRASONIC SENSORS
// =====================================================

UltraSonicDistanceSensor distanceSensor1(9, 10);
UltraSonicDistanceSensor distanceSensor2(20, 21);
UltraSonicDistanceSensor distanceSensor3(2, 1);


// =====================================================
// ICM-20948
// =====================================================

#define CS_PIN   8
#define MISO_PIN 7
#define SCLK_PIN 6
#define MOSI_PIN 5

#define AD0_VAL 1

ICM_20948_SPI myICM;


// =====================================================
// BME280
// =====================================================

#define I2C0_SCL 4
#define I2C0_SDA 3

#define BME280_ADDR 0x76

TwoWire I2CBus0(0);

Adafruit_BME280 bme;


// =====================================================
// SETUP
// =====================================================

void setup()
{
  Serial.begin(115200);

  delay(1000);

  Serial.println();
  Serial.println("================================");
  Serial.println("      START SENZOROV");
  Serial.println("================================");


  // ---------------------------------------------------
  // ICM-20948 SPI
  // ---------------------------------------------------

  Serial.println("Inicializujem ICM-20948...");

  SPI.begin(SCLK_PIN, MISO_PIN, MOSI_PIN);

  bool icmOK = false;

  while (!icmOK)
  {
    myICM.begin(CS_PIN, SPI);

    Serial.print("ICM-20948: ");
    Serial.println(myICM.statusString());

    if (myICM.status == ICM_20948_Stat_Ok)
    {
      icmOK = true;
      Serial.println("ICM-20948 OK!");
    }
    else
    {
      Serial.println("ICM-20948 chyba - skusam znova...");
      delay(1000);
    }
  }


  // ---------------------------------------------------
  // BME280 I2C
  // ---------------------------------------------------

  Serial.println();
  Serial.println("Inicializujem BME280...");

  I2CBus0.setPins(I2C0_SDA, I2C0_SCL);

  bool bmeOK = bme.begin(BME280_ADDR, &I2CBus0);

  if (bmeOK)
  {
    Serial.println("BME280 OK!");
  }
  else
  {
    Serial.println("BME280 CHYBA!");
  }
}


// =====================================================
// LOOP
// =====================================================

void loop()
{
  // ===================================================
  // ULTRASONIC
  // ===================================================

  float distance1 = distanceSensor1.measureDistanceCm();
  float distance2 = distanceSensor2.measureDistanceCm();
  float distance3 = distanceSensor3.measureDistanceCm();


  // ===================================================
  // ICM-20948
  // ===================================================

  if (myICM.dataReady())
  {
    myICM.getAGMT();
  }


  // ===================================================
  // BME280
  // ===================================================

  float temperatureBME = bme.readTemperature();
  float pressureBME = bme.readPressure() / 100.0F;
  float humidityBME = bme.readHumidity();


  // ===================================================
  // SERIAL OUTPUT
  // ===================================================

  Serial.println("--------------------------------");

  // Ultrasonic
  Serial.println("ULTRASONIC:");

  Serial.print("US1: ");
  Serial.print(distance1);
  Serial.println(" cm");

  Serial.print("US2: ");
  Serial.print(distance2);
  Serial.println(" cm");

  Serial.print("US3: ");
  Serial.print(distance3);
  Serial.println(" cm");


  // ICM-20948
  Serial.println();
  Serial.println("ICM-20948:");

  Serial.print("ACC [mg]  X: ");
  Serial.print(myICM.accX());
  Serial.print("  Y: ");
  Serial.print(myICM.accY());
  Serial.print("  Z: ");
  Serial.println(myICM.accZ());

  Serial.print("GYR [DPS] X: ");
  Serial.print(myICM.gyrX());
  Serial.print("  Y: ");
  Serial.print(myICM.gyrY());
  Serial.print("  Z: ");
  Serial.println(myICM.gyrZ());

  Serial.print("MAG [uT]  X: ");
  Serial.print(myICM.magX());
  Serial.print("  Y: ");
  Serial.print(myICM.magY());
  Serial.print("  Z: ");
  Serial.println(myICM.magZ());

  Serial.print("IMU TEMP: ");
  Serial.print(myICM.temp());
  Serial.println(" C");


  // BME280
  Serial.println();
  Serial.println("BME280:");

  Serial.print("Temperature: ");
  Serial.print(temperatureBME);
  Serial.println(" C");

  Serial.print("Pressure:    ");
  Serial.print(pressureBME);
  Serial.println(" hPa");

  Serial.print("Humidity:    ");
  Serial.print(humidityBME);
  Serial.println(" %");


  Serial.println("--------------------------------");

  delay(500);
}