#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_GFX.h>     //OLEDのGFXライブラリ
#include <Adafruit_SSD1306.h> //OLEDのライブラリ
#include <Adafruit_AHTX0.h>   //AHT20のライブラリ
//Wokwiの超音波センサーは、逆算すると15℃の時の音速 340 m/sに設定されていると思われる。
//また、測定対象との距離が近い場合、送信部と受信部の間隔が無視できなくなるが、
//間隔を無視して単に音速から求めた距離による時間間隔を出力していると思われる。

// 発信を５番ピン、受信を１８番ピンに設定
const int trigPin = 5;
const int echoPin = 18;

//音速の変数を宣言
float snd_speed;
float cal_speed1;
float cal_speed2;

//温度と湿度の変数を定義
float tempAHT;
float humiAHT;

//受信した時間間隔の変数を設定
long duration;

//OLEDの初期設定
Adafruit_SSD1306 display(128, 64, &Wire, -1);

//AHT20の定義 使用する際には下記のコメントを外す
//Adafruit_AHTX0 aht;

void setup() {
 Serial.begin(115200);
  //OLEDの初期設定
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { 
    Serial.println(F("SSD1306 allocation failed"));
    while(0);
  }
  //AHT20の初期化
  /*AHT20を使う場合にはコメントを外す
  if (! aht.begin()) {
    Serial.println(F("AHT20 allocation failed"));
    while (0);
  }
  */
  // 超音波センサーの接続されたピンの送信、受信の設定
  pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
  pinMode(echoPin, INPUT); // Sets the echoPin as an Input
  
  //LEDの接続されたピンを出力に設定する。
  pinMode(15, OUTPUT); //Sets the 15 pin as an R-LED
  pinMode(16, OUTPUT); //Sets the 15 pin as an G-LED
  pinMode(17, OUTPUT); //Sets the 15 pin as an B-LED
  
  //OLEDの初期化
  display.clearDisplay();
  display.setTextColor(WHITE);

}
void loop() {
  //トリガーピンを2 μsローレベルにする（初期化の代わり）。
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  //トリガーピンを10 μsハイレベルにする。
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  

  //受信したパルスの時間をμs単位で測定する。
  duration = pulseIn(echoPin, HIGH);

  //温度を格納する変数を設定（温度センサーがあれば、そこから取得した温度を格納するようにするとよい。）
  float tempAHT = 20.0; // set room temp in deg C.
  float humiAHT = 50.0;  // set room humidity in %
  
  //AHT20を接続して、温度、湿度データを取得する場合にはコメントを外す
  /*sensors_event_t humidity, temp;
  aht.getEvent(&humidity, &temp);
  tempAHT = temp.temperature;
  humiAHT = humidity.relative_humidity;
  */

  //補正なしの音速 (m/s)
  float snd_speed = 340;
  //温度補正した音速 (m/s)
  float cal_speed1 = (331.5+0.6*tempAHT);
  //温度・湿度補正した音速 (m/s)
  float cal_speed2 = (331.4+(0.606*tempAHT)+(0.0124*humiAHT));

  //補正なしの音速 (cm/μs)
  float snd_speedCm = (0.034);
  //温度補正した音速 (cm/μs)
  float cal_speedCm1 = (331.5+0.6*tempAHT)/10000;
  //温度・湿度補正した音速 (cm/μs)
  float cal_speedCm2 = (331.4+(0.606*tempAHT)+(0.0124*humiAHT))/10000;
  
  //斜辺(cm)の算出
  float hypotenuseCm = duration/2 * snd_speedCm;
  float hypotenuseCm1 = duration/2 * cal_speedCm1;
  float hypotenuseCm2 = duration/2 * cal_speedCm2;
  
  //超音波センサーの中心からの距離(cm)を定義
  float uspitch = 1.3;

  //ピタゴラスの定理より、センサー距離と斜辺の長さより距離を算出する場合にはコメントを外す
  float distanceCm = sqrt(pow(hypotenuseCm,2)-pow(uspitch,2));
  float distanceCm1= sqrt(pow(hypotenuseCm1,2)-pow(uspitch,2));
  float distanceCm2= sqrt(pow(hypotenuseCm2,2)-pow(uspitch,2));

  //シリアルモニタに測定時間を表示
  Serial.print("Duration: "); Serial.print(duration, 1); Serial.println(" μs");
  //温度と湿度を表示
  Serial.printf("Temp.: %2.1f C\n", tempAHT);
  Serial.printf("Humi.: %2.1f %%\n", humiAHT);
  //各音速を表示
  Serial.printf("Snd-Speed: %2.1f m/s\n", snd_speed);
  Serial.printf("Cal-Snd-Speed1 %2.1f m/s\n", cal_speed1);
  Serial.printf("Cal-Snd-Speed2 %2.1f m/s\n", cal_speed2);
  //斜辺の長さを表示
  Serial.printf("Hypo-Dist.: %2.1f cm\n", hypotenuseCm);
  Serial.printf("Hypo-Dist.: %2.1f cm\n", hypotenuseCm1);
  Serial.printf("Hypo-Dist.: %2.1f cm\n", hypotenuseCm2);
  //距離を表示
  Serial.printf("Cal-dist.: %2.1f cm\n", distanceCm);
  Serial.printf("Cal-dist.: %2.1f cm\n", distanceCm1);
  Serial.printf("Cal-dist.: %2.1f cm\n", distanceCm2);
  
  //OLEDの表示 ８行までしか表示できないので、適宜取捨選択すること。
  display.clearDisplay();
  display.setCursor(0,0);
  display.setTextSize(1);
  //温度と湿度を表示
  display.printf("Temp.:%2.1f ", tempAHT); display.printf("Humi.:%2.1f\n", humiAHT);
  //シリアルモニタに測定時間を表示
  display.print("Duration:   "); display.print(duration, 1); display.println(" us");
  //補正した音速を表示（最初が温度のみの補正、次が温度と湿度による補正）
  display.printf("S-Spd(T):   %2.1f m/s\n",cal_speed1); 
  display.printf("S-Spd(T-H): %2.1f m/s\n", cal_speed2);
  
  //OLEDの表示 ８行までしか表示できないので、適宜取捨選択すること。
  //斜辺の長さを表示（表示する場合にはコメントを外す。）
  //display.printf("Hypo-Dist.:  %2.1f cm\n", hypotenuseCm);
  //display.printf("Hypo-Dist1.: %2.1f cm\n", hypotenuseCm1);
  //display.printf("Hypo-Dist2.: %2.1f cm\n", hypotenuseCm2);
  //距離を表示（表示する場合にはコメントを外す。）
  //display.printf("Cal-dist.:   %2.1f cm\n", distanceCm);
  //display.printf("Cal-dist1.:  %2.1f cm\n", distanceCm1);
  //display.printf("Cal-dist2.:  %2.1f cm\n", distanceCm2);
  //バッファに格納したデータを表示
  display.display();

  //距離に応じてLEDの色や点滅間隔を変える。
  if (distanceCm <= 10){
  //LED-Res on off
  digitalWrite(15, HIGH);
  delay(250);
  digitalWrite(15,LOW);
  delay(250);
  digitalWrite(15, HIGH);
  delay(250);
  digitalWrite(15,LOW);
  delay(250);
  }
  else if (distanceCm <= 20){
  //LED-Red & Blue on off
  digitalWrite(15, HIGH);
  digitalWrite(17, HIGH);
  delay(500);
  digitalWrite(15, LOW);
  digitalWrite(17, LOW);
  delay(500);
  }
   else if (distanceCm <= 30){
  //LED-Red & Green on off
  digitalWrite(15, HIGH);
  digitalWrite(16, HIGH);
  delay(500);
  digitalWrite(15, LOW);
  digitalWrite(16, LOW);
  delay(500);
  }
   else if (distanceCm <= 50){
  //LED-Green & Blue on off
  digitalWrite(16, HIGH);
  digitalWrite(17, HIGH);
  delay(500);
  digitalWrite(16, LOW);
  digitalWrite(17, LOW);
  delay(500);
  }
  else{
  //LED-Blue on off
  digitalWrite(17, HIGH);
  delay(500);
  digitalWrite(17,LOW);
  delay(500);
  }

}