#include <IRremote.hpp>
#include <LedControl.h>

// ===================== Пины по схеме =====================

// IR
const int IR_PIN = 2;

// Датчики линии
const int LINE_L_PIN = 3;
const int LINE_R_PIN = 4;

// MX1508, мотор/канал 1
const int M1_IN1 = 5;
const int M1_IN2 = 6;

// Ультразвуковой датчик
const int US_TRIG = 7;
const int US_ECHO = 8;

// MX1508, мотор/канал 2
const int M2_IN1 = 9;
const int M2_IN2 = 10;

// Дополнительные входы
const int EXTRA_D11 = 11;
const int EXTRA_D12 = 12;

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

// Дополнительные аналоговые входы
const int EXTRA_A3 = A3;
const int EXTRA_A4 = A4;
const int EXTRA_A5 = A5;
const int EXTRA_A6 = A6;
const int EXTRA_A7 = A7;

// MAX7219: DIN, CLK, CS, количество матриц
LedControl matrix = LedControl(MATRIX_DIN, MATRIX_CLK, MATRIX_CS, 1);

// ===================== Настройки =====================

const int MOTOR_SPEED = 150;      // 0-255
const int MOTOR_TEST_TIME = 800;  // мс

unsigned long lastSensorPrint = 0;
unsigned long lastMatrixUpdate = 0;

int matrixStep = 0;

// ===================== SETUP =====================

void setup() {
  Serial.begin(9600);
  delay(500);

  Serial.println();
  Serial.println(F("========================================"));
  Serial.println(F(" ТЕСТ РОБОТА НА ARDUINO NANO"));
  Serial.println(F(" Проверка всех подключенных элементов"));
  Serial.println(F("========================================"));
  Serial.println();

  // IR
  IrReceiver.begin(IR_PIN, ENABLE_LED_FEEDBACK);
  Serial.println(F("[OK] IR приемник запущен на D2"));

  // Датчики линии
  pinMode(LINE_L_PIN, INPUT_PULLUP);
  pinMode(LINE_R_PIN, INPUT_PULLUP);

  // MX1508
  pinMode(M1_IN1, OUTPUT);
  pinMode(M1_IN2, OUTPUT);
  pinMode(M2_IN1, OUTPUT);
  pinMode(M2_IN2, OUTPUT);

  stopMotors();

  // УЗ датчик
  pinMode(US_TRIG, OUTPUT);
  pinMode(US_ECHO, INPUT);

  // Дополнительные входы
  pinMode(EXTRA_D11, INPUT_PULLUP);
  pinMode(EXTRA_D12, INPUT_PULLUP);

  // A3-A5 можно использовать как аналоговые входы
  pinMode(EXTRA_A3, INPUT);
  pinMode(EXTRA_A4, INPUT);
  pinMode(EXTRA_A5, INPUT);

  // A6 и A7 только analogRead, pinMode для них не нужен

  // MAX7219
  matrix.shutdown(0, false);
  matrix.setIntensity(0, 6);   // яркость 0-15
  matrix.clearDisplay(0);

  Serial.println(F("[OK] MAX7219 запущен: DIN=A0, CLK=A1, CS=A2"));

  Serial.println();
  Serial.println(F("Сейчас будет тест моторов."));
  Serial.println(F("Поставь робота так, чтобы колеса свободно крутились."));
  delay(2000);

  testMotors();

  Serial.println();
  Serial.println(F("Основной тест запущен."));
  Serial.println(F("Открой Монитор порта на скорости 9600."));
  Serial.println(F("Нажимай кнопки на ИК-пульте, закрывай датчики линии, меняй расстояние перед УЗ датчиком."));
  Serial.println();
}

// ===================== LOOP =====================

void loop() {
  checkIR();

  if (millis() - lastSensorPrint >= 700) {
    lastSensorPrint = millis();
    printSensors();
  }

  if (millis() - lastMatrixUpdate >= 300) {
    lastMatrixUpdate = millis();
    testMatrixAnimation();
  }
}

// ===================== IR TEST =====================

void checkIR() {
  if (IrReceiver.decode()) {
    Serial.println(F("----- IR СИГНАЛ -----"));

    Serial.print(F("Protocol: "));
    Serial.println(IrReceiver.getProtocolString());

    Serial.print(F("Command HEX: 0x"));
    Serial.println(IrReceiver.decodedIRData.command, HEX);

    Serial.print(F("Raw HEX: 0x"));
    Serial.println(IrReceiver.decodedIRData.decodedRawData, HEX);

    Serial.println(F("---------------------"));
    Serial.println();

    IrReceiver.resume();
  }
}

// ===================== SENSOR TEST =====================

void printSensors() {
  int lineL = digitalRead(LINE_L_PIN);
  int lineR = digitalRead(LINE_R_PIN);

  int d11 = digitalRead(EXTRA_D11);
  int d12 = digitalRead(EXTRA_D12);

  int a3 = analogRead(EXTRA_A3);
  int a4 = analogRead(EXTRA_A4);
  int a5 = analogRead(EXTRA_A5);
  int a6 = analogRead(EXTRA_A6);
  int a7 = analogRead(EXTRA_A7);

  long distance = readUltrasonicCM();

  Serial.println(F("========== ДАННЫЕ ДАТЧИКОВ =========="));

  Serial.print(F("Линия L D3: "));
  Serial.print(lineL);
  Serial.print(F(" | Линия R D4: "));
  Serial.println(lineR);

  Serial.print(F("УЗ расстояние D7/D8: "));
  if (distance <= 0) {
    Serial.println(F("нет сигнала"));
  } else {
    Serial.print(distance);
    Serial.println(F(" см"));
  }

  Serial.print(F("D11: "));
  Serial.print(d11);
  Serial.print(F(" | D12: "));
  Serial.println(d12);

  Serial.print(F("A3: "));
  Serial.print(a3);
  Serial.print(F(" | A4: "));
  Serial.print(a4);
  Serial.print(F(" | A5: "));
  Serial.println(a5);

  Serial.print(F("A6: "));
  Serial.print(a6);
  Serial.print(F(" | A7: "));
  Serial.println(a7);

  Serial.println(F("====================================="));
  Serial.println();
}

// ===================== ULTRASONIC TEST =====================

long readUltrasonicCM() {
  digitalWrite(US_TRIG, LOW);
  delayMicroseconds(2);

  digitalWrite(US_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(US_TRIG, LOW);

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

  if (duration == 0) {
    return -1;
  }

  long distance = duration / 58;
  return distance;
}

// ===================== MOTOR TEST =====================

void testMotors() {
  Serial.println(F("========== ТЕСТ МОТОРОВ =========="));

  Serial.println(F("Мотор/канал 1 вперед: D5/D6"));
  motor1Forward(MOTOR_SPEED);
  delay(MOTOR_TEST_TIME);
  stopMotors();
  delay(400);

  Serial.println(F("Мотор/канал 1 назад: D5/D6"));
  motor1Backward(MOTOR_SPEED);
  delay(MOTOR_TEST_TIME);
  stopMotors();
  delay(400);

  Serial.println(F("Мотор/канал 2 вперед: D9/D10"));
  motor2Forward(MOTOR_SPEED);
  delay(MOTOR_TEST_TIME);
  stopMotors();
  delay(400);

  Serial.println(F("Мотор/канал 2 назад: D9/D10"));
  motor2Backward(MOTOR_SPEED);
  delay(MOTOR_TEST_TIME);
  stopMotors();
  delay(400);

  Serial.println(F("Оба мотора вперед"));
  motor1Forward(MOTOR_SPEED);
  motor2Forward(MOTOR_SPEED);
  delay(MOTOR_TEST_TIME);
  stopMotors();
  delay(400);

  Serial.println(F("Оба мотора назад"));
  motor1Backward(MOTOR_SPEED);
  motor2Backward(MOTOR_SPEED);
  delay(MOTOR_TEST_TIME);
  stopMotors();

  Serial.println(F("Тест моторов завершен."));
  Serial.println(F("=================================="));
}

// Мотор 1
void motor1Forward(int speedValue) {
  analogWrite(M1_IN1, speedValue);
  analogWrite(M1_IN2, 0);
}

void motor1Backward(int speedValue) {
  analogWrite(M1_IN1, 0);
  analogWrite(M1_IN2, speedValue);
}

// Мотор 2
void motor2Forward(int speedValue) {
  analogWrite(M2_IN1, speedValue);
  analogWrite(M2_IN2, 0);
}

void motor2Backward(int speedValue) {
  analogWrite(M2_IN1, 0);
  analogWrite(M2_IN2, speedValue);
}

void stopMotors() {
  analogWrite(M1_IN1, 0);
  analogWrite(M1_IN2, 0);
  analogWrite(M2_IN1, 0);
  analogWrite(M2_IN2, 0);
}

// ===================== MATRIX TEST =====================

void testMatrixAnimation() {
  matrix.clearDisplay(0);

  // бегущая точка по строкам
  int row = matrixStep / 8;
  int col = matrixStep % 8;

  matrix.setLed(0, row, col, true);

  matrixStep++;

  if (matrixStep >= 64) {
    matrixStep = 0;
  }
}