#include <Wire.h>
#include <LedControl.h>
#include <MPU6050.h>
#include <MD_Parola.h>
#include <MD_MAX72xx.h>
#include <SPI.h>

#define HARDWARE_TYPE MD_MAX72XX::PAROLA_HW
#define MAX_DEVICES 4
#define CS_PIN 53
#define DATA_PIN 51
#define CLK_PIN 52
#define HALL_SENSOR_PIN 3

MD_Parola myDisplay = MD_Parola(HARDWARE_TYPE, CS_PIN, MAX_DEVICES);
MPU6050 mpu;

LedControl lc = LedControl(DATA_PIN, CLK_PIN, CS_PIN, MAX_DEVICES);

int pinSTOP = 3;
bool STOPdisplayWasNotCleared;
bool leftRightDisplayWasNotCleared;

// Put values in arrays
byte invader1a[] =
    {
        B00010010, // First frame of invader #1
        B00100100,
        B01001001,
        B10010010,
        B10010010,
        B01001001,
        B00100100,
        B00010010};

byte invader1b[] =
    {
        B00000000, // Second frame of invader #1
        B00000000,
        B00000000,
        B00000000,
        B00000000,
        B00000000,
        B00000000,
        B00000000};

byte invader2a[] =
    {
        B00010010, // First frame of invader #1
        B00100100,
        B01001001,
        B10010010,
        B10010010,
        B01001001,
        B00100100,
        B00010010};

byte invader2b[] =
    {
        B00000000, // Second frame of invader #2 (flipped horizontally)
        B00000000,
        B00000000,
        B00000000,
        B00000000,
        B00000000,
        B00000000,
        B00000000};

byte invader3a[] =
    {
        B00010010, // First frame of invader #1
        B00100100,
        B01001001,
        B10010010,
        B10010010,
        B01001001,
        B00100100,
        B00010010};

byte invader3b[] =
    {
        B00000000, // First frame of invader #2 (flipped horizontally)
        B00000000,
        B00000000,
        B00000000,
        B00000000,
        B00000000,
        B00000000,
        B00000000};

byte invader4a[] =
    {
        B00010010, // First frame of invader #1
        B00100100,
        B01001001,
        B10010010,
        B10010010,
        B01001001,
        B00100100,
        B00010010};

byte invader4b[] =
    {
        B00000000, // Second frame of invader #2
        B00000000,
        B00000000,
        B00000000,
        B00000000,
        B00000000,
        B00000000,
        B00000000};

// Put values in arrays
byte invader1c[] =
    {
        B01001000, // First frame of invader #1
        B00100100,
        B10010010,
        B01001001,
        B01001001,
        B10010010,
        B00100100,
        B01001000};

byte invader1d[] =
    {
        B00000000, // Second frame of invader #1
        B00000000,
        B00000000,
        B00000000,
        B00000000,
        B00000000,
        B00000000,
        B00000000};

byte invader2c[] =
    {
        B01001000, // First frame of invader #1
        B00100100,
        B10010010,
        B01001001,
        B01001001,
        B10010010,
        B00100100,
        B01001000};

byte invader2d[] =
    {
        B00000000, // Second frame of invader #2 (flipped horizontally)
        B00000000,
        B00000000,
        B00000000,
        B00000000,
        B00000000,
        B00000000,
        B00000000};

byte invader3c[] =
    {
        B01001000, // First frame of invader #1
        B00100100,
        B10010010,
        B01001001,
        B01001001,
        B10010010,
        B00100100,
        B01001000};

byte invader3d[] =
    {
        B00000000, // First frame of invader #2 (flipped horizontally)
        B00000000,
        B00000000,
        B00000000,
        B00000000,
        B00000000,
        B00000000,
        B00000000
    };
        
byte invader4c[] =
    {
        B01001000, // First frame of invader #1
        B00100100,
        B10010010,
        B01001001,
        B01001001,
        B10010010,
        B00100100,
        B01001000};

byte invader4d[] =
    {
        B00000000, // Second frame of invader #2
        B00000000,
        B00000000,
        B00000000,
        B00000000,
        B00000000,
        B00000000,
        B00000000};

void report(int16_t ax) {
  const uint32_t interval = 500;
  static uint32_t last = 0;
  uint32_t now = millis();
  if (now - last >= interval) {
    last += interval; // shows that acceleration animation takes up to 7ms
    //last = now;
  
    Serial.print("ms:");
    Serial.print(now);
    Serial.print(" Hall:");
    Serial.print(digitalRead(pinSTOP));
    Serial.print(" ");
    Serial.print(" ax:");
    Serial.print(ax);
    Serial.println();
  }
}

void setup() {
  Serial.begin(115200);
  pinMode(pinSTOP, INPUT_PULLUP);
  myDisplay.begin();
  myDisplay.setIntensity(8);
  myDisplay.displayClear();

  for (int i = 0; i < 4; i++) {
    lc.shutdown(i, false); // wake up displays
    lc.setIntensity(i, 15); // set intensity levels
    lc.clearDisplay(i); // clear displays
  }
}

void loop() {
  int16_t ax, ay, az;
  mpu.getAcceleration(&ax, &ay, &az);

  if (ax > 4000 && !STOPdisplayWasNotCleared) {
    tiltRightAnimation_OneStep();
    leftRightDisplayWasNotCleared = 1; // enable display clearing
  } else if (ax < -4000 && !STOPdisplayWasNotCleared) {
    tiltLeftAnimation_OneStep();
    leftRightDisplayWasNotCleared = 1; // enable display clearing
  } else if (leftRightDisplayWasNotCleared && !STOPdisplayWasNotCleared) {
    for (int i = 0; i < 4; i++) {
      lc.clearDisplay(i); // clear displays
    }
    leftRightDisplayWasNotCleared = 0; // disable display clearing
  }

  bool hallEffectDetected = digitalRead(HALL_SENSOR_PIN);
  if (hallEffectDetected) {
    displayStopMessage();
    STOPdisplayWasNotCleared = 1; // enable display clearing
  } else {
    if (STOPdisplayWasNotCleared) {
      myDisplay.displayClear();
      STOPdisplayWasNotCleared = 0; // disable display clearing, not wasting CPU time
    }
  }

  report(ax); // Pass the value of ax to the report() function
}

void displayStopMessage() {
  if (myDisplay.displayAnimate()) {
    myDisplay.displayReset();
    myDisplay.setTextAlignment(PA_CENTER);
    myDisplay.setSpeed(30);
    myDisplay.displayScroll(" STOP ", PA_CENTER, PA_FADE, 20);
  }
}

void tiltRightAnimation_OneStep() {
  static int frameIndex = 0; // Static variable to keep track of frame index

  // Define the array of animation frames
  byte* frames[] = {invader1a, invader2a, invader3a, invader4a, invader1b, invader2b, invader3b, invader4b};

  // Display the current frame
  for (int i = 0; i < 8; i++) {
    lc.setRow(frameIndex / 2, i, frames[frameIndex][i]);
  }

  // Increment frame index
  frameIndex++;
  if (frameIndex >= 8) {
    frameIndex = 0; // Reset frame index if it exceeds the number of frames
  }
}

void tiltLeftAnimation_OneStep() {
  static int frameIndex = 0; // Static variable to keep track of frame index

  // Define the array of animation frames
  byte* frames[] = {invader4c, invader3c, invader2c, invader1c, invader4d, invader3d, invader2d, invader1d};

  // Display the current frame
  for (int i = 0; i < 8; i++) {
    lc.setRow(frameIndex / 2, i, frames[frameIndex][i]);
  }

  // Increment frame index
  frameIndex++;
  if (frameIndex >= 8) {
    frameIndex = 0; // Reset frame index if it exceeds the number of frames
  }
}