#include <MD_Parola.h>
#include <MD_MAX72xx.h>
#include <SPI.h>

int ledR = 14;
int ledG = 27;
int ledY = 26;


#define USE_UI_CONTROL 0

#if USE_UI_CONTROL
#include <MD_UISwitch.h>
#endif

#define DEBUG 0

#if DEBUG
#define PRINT(s, x) { Serial.print(F(s)); Serial.print(x); }
#define PRINTS(x) Serial.print(F(x))
#define PRINTX(x) Serial.println(x, HEX)
#else
#define PRINT(s, x)
#define PRINTS(x)
#define PRINTX(x)
#endif

#define HARDWARE_TYPE MD_MAX72XX::PAROLA_HW
#define MAX_DEVICES 11
#define CLK_PIN   18
#define DATA_PIN  23
#define CS_PIN    15

#if USE_UI_CONTROL
const uint8_t SPEED_IN = A5;
const uint8_t DIRECTION_SET = 8;  
const uint8_t INVERT_SET = 9;   

const uint8_t SPEED_DEADBAND = 5;
#endif
MD_Parola P = MD_Parola(HARDWARE_TYPE, CS_PIN, MAX_DEVICES);
uint8_t scrollSpeed = 25;
textEffect_t scrollEffect = PA_SCROLL_LEFT;
textPosition_t scrollAlign = PA_LEFT; 
uint16_t scrollPause = 500;

#define  BUF_SIZE  75  // Maximum of 75 characters
char curMessage[BUF_SIZE] = { "" };
char newMessage1[BUF_SIZE] = { "Waspada" };
bool newMessage1Available = true;
char newMessage2[BUF_SIZE] = { "Amaran" };
bool newMessage2Available = true;
char newMessage3[BUF_SIZE] = { "Bahaya" };
bool newMessage3Available = true;

#define trigPin  13
#define echoPin  12

char testStr[10] = { "" };
String heightStr = "";
long duration;
int distance;
int height;

#if USE_UI_CONTROL

MD_UISwitch_Digital uiDirection(DIRECTION_SET);
MD_UISwitch_Digital uiInvert(INVERT_SET);

void doUI(void)
{
  {
    int16_t speed = map(analogRead(SPEED_IN), 0, 1023, 10, 150);

    if ((speed >= ((int16_t)P.getSpeed() + SPEED_DEADBAND)) ||
      (speed <= ((int16_t)P.getSpeed() - SPEED_DEADBAND)))
    {
      P.setSpeed(speed);
      scrollSpeed = speed;
      PRINT("\nChanged speed to ", P.getSpeed());
    }
  }

  if (uiDirection.read() == MD_UISwitch::KEY_PRESS) 
  {
    PRINTS("\nChanging scroll direction");
    scrollEffect = (scrollEffect == PA_SCROLL_LEFT ? PA_SCROLL_RIGHT : PA_SCROLL_LEFT);
    P.setTextEffect(scrollEffect, scrollEffect);
    P.displayClear();
    P.displayReset();
  }

  if (uiInvert.read() == MD_UISwitch::KEY_PRESS)  // INVERT MODE
  {
    PRINTS("\nChanging invert mode");
    P.setInvert(!P.getInvert());
  }
}
#endif // USE_UI_CONTROL


void setup() {
  pinMode(trigPin, OUTPUT); 
  pinMode(echoPin, INPUT); 
  pinMode(ledR,OUTPUT);
  pinMode(ledG,OUTPUT);
  pinMode(ledY,OUTPUT);
  Serial.begin(57600); 
 #if USE_UI_CONTROL
  uiDirection.begin();
  uiInvert.begin();
  pinMode(SPEED_IN, INPUT);

  doUI();
#endif // USE_UI_CONTROL

  P.begin();
  P.displayText(curMessage, scrollAlign, scrollSpeed, scrollPause, scrollEffect, scrollEffect);
}

void loop() {

  
  measure();
 #if USE_UI_CONTROL
  doUI();
 #endif

  if (distance >=71 && distance < 100) {
    digitalWrite(ledG,HIGH);
    digitalWrite(ledR,LOW);
    digitalWrite(ledY,LOW);
    if (P.displayAnimate())
  {
    if (newMessage1Available)
    {
      strcpy(curMessage, newMessage1);
      newMessage1Available = false;
    }
    P.displayReset();
   }
  } 
  
 if (distance > 30 && distance <=70) {
    digitalWrite(ledG,LOW);
    digitalWrite(ledR,LOW);
    digitalWrite(ledY,HIGH);
    if (P.displayAnimate())
  {
    if (newMessage2Available)
    {
      strcpy(curMessage, newMessage2);
      newMessage2Available = false;
    }
    P.displayReset();
   }
 }
   else if (distance <= 29 ) {
    digitalWrite(ledG,LOW);
    digitalWrite(ledR,HIGH);
    digitalWrite(ledY,LOW);
    if (P.displayAnimate())
  {
    if (newMessage3Available)
    {
      strcpy(curMessage, newMessage3);
      newMessage3Available = false;
    }
    P.displayReset();
     }
   }
  // delay(500);
 }

void measure() {

  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.017;

}