---

#include <TM1637Display.h>
#include <LedControl.h>

const int LED_RED = 13;
const int LED_YELLOW = 12;
const int LED_GREEN = 11;

const int PED_RED = 10;
const int PED_GREEN = 9;

const int BUZZER = 3;

const int TRIG_PIN = 7;
const int ECHO_PIN = 6;

const int TM_CLK = 5;
const int TM_DIO = 4;

TM1637Display display(TM_CLK, TM_DIO);

const int MATRIX_DIN = A0;
const int MATRIX_CS = A1;
const int MATRIX_CLK = A2;

LedControl matrix = LedControl(MATRIX_DIN, MATRIX_CLK, MATRIX_CS, 1);

const int DETECT_DISTANCE = 20;
const int TIMEOUT_YELLOW = 1690;
const int TIMEOUT_FLASH_GREEN = 500;
const int PEDESTRIAN_TIME = 5;
const int SAFETY_DELAY = 500;

bool sensorReady = true;

byte standingPerson[8] =
{
  B00011000,
  B00011000,
  B00011000,
  B00111100,
  B00011000,
  B00011000,
  B00100100,
  B00100100
};

byte walkingPerson1[8] =
{
  B00011000,
  B00011000,
  B00011000,
  B00111100,
  B01011010,
  B00011000,
  B00101000,
  B01000100
};

byte walkingPerson2[8] =
{
  B00011000,
  B00011000,
  B00011000,
  B00111100,
  B01011010,
  B00011000,
  B00010100,
  B00100010
};

void showImage(byte image[])
{
  for (int i = 0; i < 8; i++)
  {
    matrix.setRow(0, i, image[i]);
  }
}

void showStandingPerson()
{
  showImage(standingPerson);
}

void showWalkingPerson(int frame)
{
  if (frame == 0)
  {
    showImage(walkingPerson1);
  }
  else
  {
    showImage(walkingPerson2);
  }
}

void setup()
{
  pinMode(LED_RED, OUTPUT);
  pinMode(LED_YELLOW, OUTPUT);
  pinMode(LED_GREEN, OUTPUT);

  pinMode(PED_RED, OUTPUT);
  pinMode(PED_GREEN, OUTPUT);

  pinMode(BUZZER, OUTPUT);

  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  display.setBrightness(7);
  display.clear();

  matrix.shutdown(0, false);
  matrix.setIntensity(0, 5);
  matrix.clearDisplay(0);

  digitalWrite(LED_RED, LOW);
  digitalWrite(LED_YELLOW, LOW);
  digitalWrite(LED_GREEN, HIGH);

  digitalWrite(PED_RED, HIGH);
  digitalWrite(PED_GREEN, LOW);

  showStandingPerson();
}

float getDistance()
{
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);

  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);

  digitalWrite(TRIG_PIN, LOW);

  long duration = pulseIn(ECHO_PIN, HIGH, 30000);

  if (duration == 0)
  {
    return 999;
  }

  return duration * 0.0343 / 2.0;
}

void pedestrianCycle()
{
  showStandingPerson();

  for (int i = 0; i < 3; i++)
  {
    digitalWrite(LED_GREEN, LOW);
    delay(TIMEOUT_FLASH_GREEN);

    digitalWrite(LED_GREEN, HIGH);
    delay(TIMEOUT_FLASH_GREEN);
  }

  digitalWrite(LED_GREEN, LOW);
  digitalWrite(LED_YELLOW, HIGH);

  delay(TIMEOUT_YELLOW);

  digitalWrite(LED_YELLOW, LOW);
  digitalWrite(LED_RED, HIGH);

  delay(SAFETY_DELAY);

  digitalWrite(PED_RED, LOW);
  digitalWrite(PED_GREEN, HIGH);

  for (int seconds = PEDESTRIAN_TIME; seconds > 0; seconds--)
  {
    display.showNumberDec(seconds);

    tone(BUZZER, 2000);
    showWalkingPerson(0);
    delay(150);
    noTone(BUZZER);

    showWalkingPerson(1);
    delay(212);

    showWalkingPerson(0);
    delay(212);

    showWalkingPerson(1);
    delay(212);

    showWalkingPerson(0);
    delay(212);
  }

  noTone(BUZZER);
  display.clear();

  digitalWrite(PED_GREEN, LOW);
  digitalWrite(PED_RED, HIGH);

  showStandingPerson();

  delay(SAFETY_DELAY);

  digitalWrite(LED_YELLOW, HIGH);

  delay(TIMEOUT_YELLOW);

  digitalWrite(LED_RED, LOW);
  digitalWrite(LED_YELLOW, LOW);
  digitalWrite(LED_GREEN, HIGH);
}

void loop()
{
  float distance = getDistance();

  if (sensorReady &&
      distance > 2 &&
      distance <= DETECT_DISTANCE)
  {
    sensorReady = false;
    pedestrianCycle();
  }

  if (!sensorReady && distance > 25)
  {
    sensorReady = true;
  }

  delay(100);
}