#include "dummyLibs.h"

#include "module_setup.h"

Module module;                                       // Объект для работы с параметрами модуля
Device device;                                       // Объект для работы с копией устройства
KTANEBusSlave busSlave(&device, &module, I2C_ADDR);  // Объект для работы с KTANEBus как слейв

uint8_t wiresNumber = 0;                               // Количество подключенных проводов
uint8_t wireIndexToCut = 0;                            // Индекс провода в массиве который нужно перерезать
Color initialWires[WIRES_NUMBER] = { 0 };              // Начальная комбинация проводов
uint8_t connectedInputsNumbers[WIRES_NUMBER] = { 0 };  // Номера подключенных входов

void setup() {
  // Конфигурируем порты на выход
  pinMode(RGB_LED_G, OUTPUT);
  pinMode(RGB_LED_R, OUTPUT);

  module.status = NO_INIT;
  wiresInit();

  /*************************************** Debug *******************************************************/
  Serial.begin(115200);
}

void loop() {
  readInputs();
  handleMistakes();
  handleModuleStatus();
  writeOutputs();

  /*************************************** Debug *******************************************************/
  printDebugInfo();
}

// Инициализация проводов
inline void wiresInit() {
  readInputs();
  // Заполняем начальную комбинацию, проверяем на правильность подключения,
  // определяем число проводов и заполням массив подключенных входов
  uint8_t connectedInputsIndex = 0;

  for (uint8_t i = 0; i < WIRES_NUMBER; i++) {
    initialWires[i] = inputs.wires[i];
    if (inputs.wires[i] < WHITE) {
      module.status = ERROR;
      return;
    }
    if (inputs.wires[i] != NOT_CONNECTED) {
      wiresNumber++;
      connectedInputsNumbers[connectedInputsIndex] = i;
      connectedInputsIndex++;
    }
  }
  if (wiresNumber < 3) {
    module.status = ERROR;
    return;
  }

  wireIndexToCut = calculateWireIndexToCut();
}

// Возвращает индекс провода для перерезания
uint8_t calculateWireIndexToCut() {
  switch (wiresNumber) {
    case 3:  // 3 провода
      // Если нет красных проводов, режьте второй.
      if (countWiresColorNumber(RED) == 0) return connectedInputsNumbers[2 - 1];
      // Иначе, если последний провод белый, режьте последний.
      else if (initialWires[connectedInputsNumbers[wiresNumber - 1]] == WHITE) return connectedInputsNumbers[wiresNumber - 1];
      // Иначе, если синих проводов больше, чем один, режьте последний синий.
      else if (countWiresColorNumber(BLUE) > 1) return countLastColorWireIndex(BLUE);
      // Иначе режьте последний.
      else return connectedInputsNumbers[wiresNumber - 1];
      break;

    case 4:  // 4 провода
      // Если красных проводов больше, чем один, и последняя цифра серийного номера нечетная, режьте последний красный.
      if (countWiresColorNumber(RED) > 1 && device.serialNumberIsOdd()) return countLastColorWireIndex(RED);
      // Иначе, если последний провод желтый, а красных нет, режьте первый.
      else if (initialWires[connectedInputsNumbers[wiresNumber - 1]] == YELLOW) return connectedInputsNumbers[1 - 1];
      // Иначе, если синий провод всего и только один, режьте первый.
      else if (countWiresColorNumber(BLUE) == 1) return connectedInputsNumbers[1 - 1];
      // Иначе, если желтых проводов больше, чем один, режьте последний.
      else if (countWiresColorNumber(YELLOW) > 1) return connectedInputsNumbers[wiresNumber - 1];
      // Иначе режьте второй.
      else return connectedInputsNumbers[2 - 1];
      break;

    case 5:  // 5 проводов
      // Если последний провод черный, и последняя цифра серийного номера нечетная, режьте четвертый.
      if (initialWires[connectedInputsNumbers[wiresNumber - 1]] == BLACK) return connectedInputsNumbers[4 - 1];
      // Иначе, если красный провод всего и только один, а желтых больше, чем один, режьте первый.
      else if (countWiresColorNumber(RED) == 1 && countWiresColorNumber(YELLOW) > 1) return connectedInputsNumbers[1 - 1];
      // Иначе, если черных проводов нет, режьте второй.
      else if (countWiresColorNumber(BLACK) == 0) return connectedInputsNumbers[2 - 1];
      // Иначе режьте первый.
      else return connectedInputsNumbers[1 - 1];
      break;

    case 6:  // 6 проводов
      // Если желтых проводов нет, и последняя цифра серийного номера нечетная, режьте третий.
      if (countWiresColorNumber(YELLOW) == 0 && device.serialNumberIsOdd()) return connectedInputsNumbers[3 - 1];
      // Иначе, если желтый провод всего и только один, а белых проводов больше, чем один, режьте четвертый.
      else if (countWiresColorNumber(YELLOW) == 1 && countWiresColorNumber(WHITE) > 1) return connectedInputsNumbers[4 - 1];
      // Иначе, если красных проводов нет, режьте последний.
      else if (countWiresColorNumber(RED) == 0) connectedInputsNumbers[wiresNumber - 1];
      // Иначе режьте четвертый.
      else return connectedInputsNumbers[4 - 1];
      break;
  }
}

// Возвращает количество проводов определенного цвета
uint8_t countWiresColorNumber(Color color) {
  uint8_t result = 0;
  for (uint8_t i = 0; i < WIRES_NUMBER; i++) {
    if (initialWires[i] == color) result++;
  }
  return result;
}

// Возвращает индекс последнего провода с заданным цветом
uint8_t countLastColorWireIndex(Color color) {
  for (uint8_t i = wiresNumber; i > 0; i--) {
    if (initialWires[connectedInputsNumbers[i - 1]] == color) return connectedInputsNumbers[i - 1];
  }
}

// Читаем входы (АЦП)
inline void readInputs() {
  for (uint8_t i = 0; i < WIRES_NUMBER; i++) {
    inputs.wires[i] = getWireColor(analogRead(WIRES_PINS[i]));
  }
}

// Обрабатываем ошибки решения
inline void handleMistakes() {
  uint32_t currentTime = millis();
  static uint32_t prevTime = 0;
  static uint8_t prevMistakesNumber = 0;

  if (module.mistakesNumber > prevMistakesNumber) {
    outputs.rgbLedRed = true;
    prevTime = currentTime;
  }

  if (currentTime - prevTime >= MISTAKE_FLASH_DURATION) {
    outputs.rgbLedRed = false;
  }

  prevMistakesNumber = module.mistakesNumber;
}

// Обрабатывем статусы модуля
inline void handleModuleStatus() {
  outputs.rgbLedRed = false;
  outputs.rgbLedGreen = false;
  switch (module.status) {
    case SOLVED:
      outputs.rgbLedGreen = true;
      break;

    case ERROR:
      outputs.rgbLedGreen = true;
      outputs.rgbLedRed = true;
      break;
  }
}

// Записываем состояния выходов из переменных
inline void writeOutputs() {
  digitalWrite(RGB_LED_G, outputs.rgbLedGreen);
  digitalWrite(RGB_LED_R, outputs.rgbLedRed);
}

// Возвращае цвет по значению АЦП
inline Color getWireColor(uint16_t adcValue) {
  for (uint8_t i = 0; i < WIRES_COLORS_NUMBER; i++) {
    if (adcValue < WIRES_COLORS_ADC[i] + ADC_TOLERANCE) {
      return (Color)i;
    }
  }
}

/*************************************** Debug *******************************************************/
void printDebugInfo() {
  static const char* const WIRES_COLORS[WIRES_COLORS_NUMBER] = {
    "shorted",
    "red & blue",
    "red & white",
    "blue & white",
    "white",
    "blue",
    "yellow",
    "red",
    "black",
    "not connected",
  };

  uint32_t currentTime = millis();
  static uint32_t prevTime = 0;

  if (currentTime - prevTime > 3000) {
    for (uint8_t i = 0; i < WIRES_NUMBER; i++) {
      Serial.print(F("Wire["));
      Serial.print(i);
      Serial.print(F("] is "));
      Serial.println(WIRES_COLORS[inputs.wires[i]]);
    }

    Serial.print(F("connectedInputsNumbers: "));
    for (uint8_t i = 0; i < wiresNumber; i++) {

      Serial.print(connectedInputsNumbers[i]);
      Serial.print(F(" "));
    }
    Serial.println();

    Serial.print(F("Wires number "));
    Serial.println(wiresNumber);
    Serial.print(F("Wire to cut index "));
    Serial.println(wireIndexToCut);
    Serial.println(F("-----------------------"));
    prevTime = currentTime;
  }
}