////////////////////////////////////////////////////////////////////
// Author: RSP @KMUTNB
// Date: 2022-02-11
////////////////////////////////////////////////////////////////////

// Add the LCD_I2C library for Arduino
#include <LCD_I2C.h> // see: https://github.com/blackhack/LCD_I2C
#include<Arduino_FreeRTOS.h>

// see also: https://docs.wokwi.com/parts/wokwi-hc-sr04
#define TRIG_PIN        (5)     // Arduino pin for the TRIG signal
#define ECHO_PIN        (2)     // Arduino pin for the ECHO signal
#define TIMEOUT_USEC    (30000) // timeout in usec

#define SOUND_SPEED     (343)   // in msec per sec
#define USEC_TO_CM(x)   ((SOUND_SPEED*100UL)*(x)/1000000)

#define RED_PIN         (10)    // Arduino pin for Red   LED pin
#define GREEN_PIN       (9)     // Arduino pin for Green LED pin
#define BLUE_PIN        (8)     // Arduino pin for Blue  LED pin
#define BUZZER_PIN      (11)

// use the default address of most PCF8574_LCD16x2 module
// create a LCD_I2C object:
// I2C address, no. of characters per line and no. of lines
LCD_I2C lcd( 0x27, 16, 2 ); 
char sbuf[2][20]; // string buffer for two text lines of 20 chars max.

// note: The RGB LED is active-low.
void write_leds( bool r, bool g, bool b) {
  digitalWrite(   RED_PIN, r ? LOW : HIGH );  
  digitalWrite( GREEN_PIN, g ? LOW : HIGH );  
  digitalWrite(  BLUE_PIN, b ? LOW : HIGH );  
}

void setup() {
  Serial.begin( 115200 );            // set baudrate to 115200
  pinMode( ECHO_PIN, INPUT_PULLUP ); // set ECHO pin as output
  pinMode( TRIG_PIN, OUTPUT );       // set TRIG pin as input
  digitalWrite( TRIG_PIN, LOW );     // output LOW to TRIG pin
  
  pinMode( BUZZER_PIN, OUTPUT );
  analogWrite( BUZZER_PIN, 0 );      // turn off alarm buzzer

  pinMode(    RED_PIN, OUTPUT );
  pinMode(  GREEN_PIN, OUTPUT );
  pinMode(   BLUE_PIN, OUTPUT );
  write_leds(false, false, false);   // turn off all RGB LEDs

  lcd.begin();                       // initialize the LCD screen
  lcd.backlight();                   // turn on the LCD backlight


  // create a new task
  xTaskCreate( task1, "Task1", 128, NULL , tskIDLE_PRIORITY+1, NULL ); //for Serial
  xTaskCreate( task2, "Task2", 128, NULL, tskIDLE_PRIORITY+1, NULL ); //for LCD
  xTaskCreate( task3, "Task3", 128, NULL, tskIDLE_PRIORITY+1, NULL ); //for LED
}

void inputPW() {
  // Step 1) send a short pulse (e.g. 20 usec) on TRIGGER pin.
  digitalWrite( TRIG_PIN, HIGH );
  delayMicroseconds( 20 );
  digitalWrite( TRIG_PIN, LOW );

  // Step 2) busy-wait to read the pulse width of the signal
  // on the ECHO pin with timeout (30 msec max.). 
  uint32_t pw = pulseIn( ECHO_PIN, HIGH, TIMEOUT_USEC );

  // Step 3) compute the distance from the obstacle
  uint16_t distance_cm = USEC_TO_CM(pw/2);

  // Step 4) send text messages to Serial
  sprintf( sbuf[0], " ECHO [us] %5u", pw );
  sprintf( sbuf[1], "Range [cm] %5u", distance_cm );

  delay(200);

  return distance_cm;
}

void loop() {}

void task1( void *pvParameters ) {
  while(1){
    inputPW();
    // Step 4) send text messages to Serial
    Serial.println( sbuf[0] );
    Serial.println( sbuf[1] );
  }
}

void task2( void *pvParameters ) {
  while(1){
    inputPW();
    // Step 5) update the LCD screen
    lcd.clear();           // clear LCD screen
    lcd.setCursor(0,0);    // goto the begin of the first line
    lcd.print( sbuf[0] );  // write the first line to LCD screen
    lcd.setCursor(0,1);    // goto the begin of the second line
    lcd.print( sbuf[1] );  // write the second line to LCD screen
  }
}

void task3( void *pvParameters ) {
  while(1){
    uint16_t distance_cm = inputPW;
    //uint16_t dist_cm = pvParameters;
    // Step 6) show light and sound indicator 
    if ( distance_cm < 50) {
      // red on, blue and green off
      write_leds(true, false, false);
      // turn on BUZZER (generate a PWM signal 50% duty cycle, ~500Hz)
      analogWrite( BUZZER_PIN, 127 );
    } else if ( distance_cm < 100 ) {
      // red and green on (=yellow), blue off
      write_leds(true, true, false);
    } else if ( distance_cm < 300 ) {
      // green on, but red and blue off
      write_leds(false, true, false);
    } else {
      // blue on, but red and green off
      write_leds(false, false, true);
    }
    delay(10);
    // turn off BUZZER (if duty cycle=0%, output is LOW)
    analogWrite( BUZZER_PIN, 0 );
  }
}