#ifdef EMULATOR
#include "emulator.h"
#endif


struct scheduler_task {
    short start_zone; // стартовая зона
    short dest_zone; // конечная зона
    short priority; // приоритет, чем больше тем выше, по умолчанию 0
};


short scheduler_find_task(const struct scheduler_task* tasks, const short curr_pos) {
    // для начала надо найти максимальный приоритет у операций
    short max_priority = -1;
    for (short i = 0; i < BARRELS_COUNT; i++) {
        if (tasks[i].dest_zone >= 0) {
            if (tasks[i].priority > max_priority) {
                max_priority = tasks[i].priority;
            }
        }
    }

    if (max_priority < 0) {
        return -1; // тасков нет
    }

    // ищем первый барабан слева, и ближайший справа
    short left = -1, right = -1;
    for (short i = 0; i < BARRELS_COUNT; i++) {
        short target = tasks[i].start_zone; // фактическая зона откуда тащить барабан
        if (tasks[i].dest_zone < 0) {
            continue;
        }

        // чтобы не получилось перемещать барабаны с приоритетом ниже
        if (tasks[i].priority < max_priority) {
            continue;
        }

        if (curr_pos <= target) {
            // это таск справа, надо найти ближайший
            if (right == -1) {
                right = i;
            } else {
                if (barrels[right].zone > target) {
                    right = i;
                }
            }
        } else {
            // таск слева, ищем максимально дальний (с минимальной зоной)
            if (left == -1) {
                left = i;
            } else {
                if (barrels[left].zone > target) {
                    left = i;
                }
            }
        }
    }

    // итого есть результат: есть ли таски, которые надо тащить вперед (и если надо то какой ближний), и есть первый таск
    if (left < 0) {
        return right; // вернем таск справа (если его нет, в переменной будет -1)
    }
    if (right < 0) {
        return left; // если вдруг задачи справа не оказалось, вернем задачу слева если есть
    }

    // вычисляем что ближе
    short ld = curr_pos - left; // левая дельта
    short rd = right - curr_pos; // правая дельта

    // дальше сравниваем дельты
    // по идее если они равны то с большим приоритетом робот поедет в левую часть,
    // а левую дельту вообще уменьшу на 1, чтобы цель слева казалась ближе
    if (rd > ld - 1) {
        return left;
    } else {
        return right;
    }
}


void schedule_one_robot(const struct scheduler_task* tasks, const struct robot_regs* r, struct robot_code* code, const short robot_id) {
    // формируем список задач
    short target_task = scheduler_find_task(tasks, r->dx.current_zone);

    if (target_task >= 0) {
        create_operation(code, target_task, tasks[target_task].start_zone, tasks[target_task].dest_zone,
                         r->dx.current_zone, robot_id);
    }
}


#ifdef EMULATOR
void scheduler_main()
#endif
{
    if (scheduler_start_signal) {
        for (short i = 0; i < BARRELS_COUNT; i++) {
            // после рестарта планировщика надо всем барабаном убрать робота,
            // всех кто сверху удалить,
            barrels[i].flags.robot = 0;
            if (barrels[i].flags.is_up) {
                barrels[i].flags.is_up = 0;
                barrels[i].flags.is_exist = 0;
            }
        }
        scheduler_start_signal = 0;
    }

    if (scheduler_en) {

        // программный таймер, применяется ко всем существующим барабанам
        if (_scheduler_software_timer) {
            _scheduler_software_timer = 0;
            for (int i = 0; i < 10; i++) {
                if (barrels[i].flags.is_exist && barrels[i].software_timer > -9999) {
                    barrels[i].software_timer--;
                }
            }
        }

        // кнопки на линии

        // кнопка загрузки барабана
        if (button_load) {
            if (!zone_is_busy(1)) {
                for (int i = 0; i < BARRELS_COUNT; i++) {
                    if (!barrels[i].flags.is_exist) {
                        barrels[i].flags.raw_word = 1; // только is_exist
                        barrels[i].zone = 1;
                        barrels[i].software_timer = -1;
                        barrels[i].time_degreasing = hla_time_degreasing;
                        barrels[i].time_washing_1a = hla_time_washing_1a;
                        barrels[i].time_washing_1b = hla_time_washing_1b;
                        barrels[i].time_etching = hla_time_etching;
                        barrels[i].time_washing_2a = hla_time_washing_2a;
                        barrels[i].time_washing_2b = hla_time_washing_2b;
                        barrels[i].time_galvanizing = hla_time_galvanizing;
                        barrels[i].time_washing_3a = hla_time_washing_3a;
                        barrels[i].time_washing_3b = hla_time_washing_3b;
                        barrels[i].time_passivation = hla_time_passivation;
                        barrels[i].time_washing_4a = hla_time_washing_4a;
                        barrels[i].time_washing_4b = hla_time_washing_4b;
                        button_load = 0;
                        break;
                    }
                }
            }
        }

        // кнопка выгрузки
        if (button_unload_end) {
            button_unload_end = 0;
            remove_barrel_from_zone(0);
        }

        // кнопка удаления барабана из 22 зоны
        if (button_unload_remove) {
            button_unload_remove = 0;
            remove_barrel_from_zone(22);
        }

        if (hla_correct_command) {
            if (hla_robot2_en && (!robot2.mx.correct_sensor || !robot2.mz.correct_status)) {
                if (robot2_code.PC < 0) {
                    robot2_code.barrel_id = -1;
                    robot2_code.code[0] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_Z);
                    robot2_code.code[0] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_X);
                    robot2_code.code[2] = ROBOT_CMD_END();
                    robot2_code.PC = 0;
                }
            } else if (hla_robot1_en && (!robot1.mx.correct_sensor || !robot1.mz.correct_status)) {
                if (robot1_code.PC < 0) {
                    robot1_code.barrel_id = -1;
                    robot1_code.code[0] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_Z);
                    robot1_code.code[0] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_X);
                    robot1_code.code[2] = ROBOT_CMD_END();
                    robot1_code.PC = 0;
                }
            } else {
                hla_correct_command = 0;
            }
        } else if (!auto_mode_pause) {
            if ((hla_robot1_en && robot1_code.PC < 0) || (hla_robot2_en && robot2_code.PC < 0)) {
                struct scheduler_task tasks[BARRELS_COUNT];

                if (one_robot_mode) {
                    // режим одного робота
                    char robot_id = 0;
                    if (hla_robot1_en && robot1_code.PC < 0) {
                        robot_id = 1;
                    } else if (robot2_code.PC < 0 && hla_robot2_en) {
                        robot_id = 2;
                    }

                    if (robot_id != 0) {
                        for (short i = 0; i < BARRELS_COUNT; i++) {
                            // для каждой задачи:
                            tasks[i].start_zone = barrels[i].zone;
                            // определяем можно ли ее выполнить и что вообще нужно выполнить
                            tasks[i].dest_zone = can_move(barrels + i, robot_id);
                            if (tasks[i].dest_zone >= 0) {
                                tasks[i].priority = get_operation_priority(i);
                            }
                        }

                        if (robot_id == 1) {
                            schedule_one_robot(tasks, &robot1, &robot1_code, 1);
                        } else {
                            schedule_one_robot(tasks, &robot2, &robot2_code, 2);
                        }
                    }
                } else {
                    // а вот для режима двух роботов все интересно
                    // для каждого робота нужно получить свой список задач
                    // и надо еще сделать так, чтобы роботы не столкнулись
                    char cmd_en = 0;

                    // добавляем ночной режим: в режиме двух роботов требуется чтобы во время ночного режима робот 2 стоял на парковке
                    if (hla_night_mode) {
                        if (!robot2.mx.correct_sensor) {
                            // уводим робота в парковку
                            if (robot2_code.PC < 0) {
                                robot2_code.barrel_id = -1;
                                robot2_code.code[0] = ROBOT_CMD_DOWN();
                                robot2_code.code[1] = ROBOT_CMD_MOVE_TO_PARKING();
                                robot2_code.code[2] = ROBOT_CMD_END();
                                robot2_code.PC = 0;
                            }
                        } else {
                            cmd_en = 1;
                        }
                    } else {
                        // логика для того, чтобы роботы не столкнулись в начале
                        if (robot1.dx.current_zone < ZONE_GALVANIZING_1) {
                            // начальная позиция робота 1 - промывка 3Б
                            robot2_lock_zone = -1;
                            robot1_lock_zone = ZONE_WASHING_3B;
                            if (robot1_code.PC < 0) {
                                robot1_code.barrel_id = -1;
                                robot1_code.code[0] = ROBOT_CMD_DOWN();
                                robot1_code.code[1] = ROBOT_CMD_MOVE_TO_ZONE(ZONE_WASHING_3B);
                                robot1_code.code[2] = ROBOT_CMD_END();
                                robot1_code.PC = 0;
                            }
                        } else if (robot2.dx.current_zone < ZONE_DEGREASING) {
                            // начальная позиция робота 1 - обезжир
                            robot2_lock_zone = ZONE_WASHING_2A;
                            if (robot2_code.PC < 0) {
                                robot2_code.barrel_id = -1;
                                robot2_code.code[0] = ROBOT_CMD_DOWN();
                                robot2_code.code[1] = ROBOT_CMD_MOVE_TO_ZONE(ZONE_DEGREASING);
                                robot2_code.code[2] = ROBOT_CMD_END();
                                robot2_code.PC = 0;
                            }
                        } else {
                            cmd_en = 1;
                        }
                    }

                    if (cmd_en) {
                        // отдельно просчитаем все для первого робота
                        if (robot1_code.PC < 0) {
                            for (short i = 0; i < BARRELS_COUNT; i++) {
                                // для каждой задачи:
                                tasks[i].start_zone = barrels[i].zone;
                                // определяем можно ли ее выполнить и что вообще нужно выполнить
                                tasks[i].dest_zone = can_move(barrels + i, 1);
                                if (tasks[i].dest_zone >= 0) {
                                    tasks[i].priority = get_operation_priority(i);
                                }
                            }

                            schedule_one_robot(tasks, &robot1, &robot1_code, 1);
                        }

                        // и отдельно для второго (только если не в ночном режиме)
                        if (robot2_code.PC < 0) {
                            for (short i = 0; i < BARRELS_COUNT; i++) {
                                // для каждой задачи:
                                tasks[i].start_zone = barrels[i].zone;
                                // определяем можно ли ее выполнить и что вообще нужно выполнить
                                tasks[i].dest_zone = can_move(barrels + i, 2);
                                if (tasks[i].dest_zone >= 0) {
                                    tasks[i].priority = get_operation_priority(i);
                                }
                            }

                            schedule_one_robot(tasks, &robot2, &robot2_code, 2);
                        }
                    }
                }
            }
        }
    }
}