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


struct scheduler_task {
    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 = barrels[i].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 short robot_id) {
    struct scheduler_task tasks[BARRELS_COUNT];

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

    // узнаем текущую зону робота
    short current_zone;
    if (robot_id == ROBOT_1) {
        current_zone = robot1.dx.current_zone;
    } else {
        current_zone = robot2.dx.current_zone;
    }

    // формируем список задач
    short target_task = scheduler_find_task(tasks, current_zone);

    if (target_task >= 0) {
        if (robot_id == ROBOT_1) {
            create_operation(target_task, barrels[target_task].zone, tasks[target_task].dest_zone, robot_id);
        } else {
            create_operation(target_task, barrels[target_task].zone, tasks[target_task].dest_zone, robot_id);
        }
    }
}


#ifdef EMULATOR
void scheduler_main()
#endif
{
    if (scheduler_start_signal) {
        robot1_code.PC = -1;
        robot2_code.PC = -1;

        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;
        scheduler_stage = 0;
    }

    // программный таймер, применяется ко всем существующим барабанам во всех режимах
    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 (hla_correct_command) {
        if (one_robot_mode) {
            switch (scheduler_stage) {
                case 0:
                    if (hla_robot2_en) {
                        if (robot2_code.PC < 0) {
                            robot2_code.barrel_id = -1;
                            robot2_code.code[0] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_Z);
                            robot2_code.code[1] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_X);
                            robot2_code.code[2] = ROBOT_CMD_END();
                            robot2_code.PC = 0;

                            scheduler_stage++;
                        }
                    } else {
                        if (robot1_code.PC < 0) {
                            robot1_code.barrel_id = -1;
                            robot1_code.code[0] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_Z);
                            robot1_code.code[1] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_X);
                            robot1_code.code[2] = ROBOT_CMD_MOVE_TO_PARKING();
                            robot1_code.code[3] = ROBOT_CMD_END();
                            robot1_code.PC = 0;

                            scheduler_stage++;
                        }
                    }
                    break;

                case 1:
                    if (robot2_code.PC < 0 && robot1_code.PC < 0) {
                        scheduler_stage++;
                    }
                    break;

                default:
                    scheduler_stage = 0;
                    hla_correct_command = 0;
            }
        } else {
            switch (scheduler_stage) {
                case 0:
                    // сначала робота 2 отправляем на коррекцию
                    if (robot2_code.PC < 0) {
                        robot2_code.barrel_id = -1;
                        robot2_code.code[0] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_Z);
                        robot2_code.code[1] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_X);
                        robot2_code.code[2] = ROBOT_CMD_END();
                        robot2_code.PC = 0;

                        scheduler_stage++;
                    }
                    break;

                case 1:
                    if (robot2_code.PC < 0) {
                        scheduler_stage++;
                    }
                    break;

                case 2:
                    // потом робот 1
                    if (robot1_code.PC < 0) {
                        robot1_code.barrel_id = -1;
                        robot1_code.code[0] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_Z);
                        robot1_code.code[1] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_X);
                        robot1_code.code[2] = ROBOT_CMD_MOVE_TO_PARKING();
                        robot1_code.code[3] = ROBOT_CMD_END();
                        robot1_code.PC = 0;

                        scheduler_stage++;
                    }
                    break;

                case 3:
                    if (robot1_code.PC < 0) {
                        scheduler_stage++;
                    }
                    break;

                default:
                    scheduler_stage = 0;
                    hla_correct_command = 0;
            }
        }
    } else if (scheduler_en) {

        // автоматический инкремент зон травления и цинкования
        if (!is_accessible_zone(ROBOT_ZONE_ETCH)) {
            increment_zone(ROBOT_ZONE_ETCH);
        }
        if (!is_accessible_zone(ROBOT_ZONE_GAL)) {
            increment_zone(ROBOT_ZONE_GAL);
        }

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

                    switch (scheduler_stage) {
                        case 0:
                            if ((robot_id == ROBOT_1 && robot1.dx.current_zone < ZONE_WASHING_1A) ||
                                (robot_id == ROBOT_2 && robot2.dx.current_zone < ZONE_WASHING_1A)) {
                                if (robot_id == ROBOT_1 && 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_1A);
                                    robot1_code.code[2] = ROBOT_CMD_END();
                                    robot1_code.PC = 0;
                                    scheduler_stage++;
                                } else 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_WASHING_1A);
                                    robot2_code.code[2] = ROBOT_CMD_END();
                                    robot2_code.PC = 0;
                                    scheduler_stage++;
                                }
                            } else {
                                scheduler_stage += 2;
                            }
                            break;

                        case 1:
                            if ((robot_id == ROBOT_1 && robot1_code.PC < 0) || (robot_id == ROBOT_2 && robot2_code.PC < 0)) {
                                scheduler_stage++;
                            }
                            break;

                        case 2:
                            schedule_one_robot(robot_id);
                            break;

                        default:
                            scheduler_stage = 0;
                    }
                } else {
                    switch (scheduler_stage) {
                        case 0:
                            // посылаем робота 1 в жопу линии, если он не в жопе
                            if (robot1.dx.current_zone < ZONE_PASSIVATION) {
                                if (robot1_code.PC < 0) {
                                    robot2_lock_zone = -1;
                                    robot1_lock_zone = ZONE_PASSIVATION;
                                    robot1_code.barrel_id = -1;
                                    robot1_code.code[0] = ROBOT_CMD_DOWN();
                                    robot1_code.code[1] = ROBOT_CMD_MOVE_TO_ZONE(ZONE_PASSIVATION);
                                    robot1_code.code[2] = ROBOT_CMD_END();
                                    robot1_code.PC = 0;
                                    scheduler_stage++;
                                }
                            } else {
                                scheduler_stage++;
                            }
                            break;

                        case 1:
                            // ждем пока робот съебет в зону дальше, чем зона 8
                            if (robot1.dx.current_zone > ZONE_WASHING_2A + 1 && robot1_lock_zone > ZONE_WASHING_2A + 1) {
                                robot2_lock_zone = ZONE_ETCHING_2;
                                scheduler_stage++;
                            }
                            break;

                        case 2:
                            // отправляем второго робота в зону 3, если он левее
                            if (robot2.dx.current_zone < ZONE_WASHING_1A) {
                                if (robot2_code.PC < 0) {
                                    robot2_lock_zone = ZONE_ETCHING_2;
                                    robot2_code.barrel_id = -1;
                                    robot2_code.code[0] = ROBOT_CMD_DOWN();
                                    robot2_code.code[1] = ROBOT_CMD_MOVE_TO_ZONE(ZONE_WASHING_1A);
                                    robot2_code.code[2] = ROBOT_CMD_END();
                                    robot2_code.PC = 0;
                                    scheduler_stage++;
                                }
                            } else {
                                scheduler_stage++;
                            }
                            break;

                        case 3:
                            // ну а теперь конечная стадия - собсна раздача команд

                            // сначала выдаем команды первому роботу
                            if (robot1_code.PC < 0) {
                                schedule_one_robot(ROBOT_1);
                            }

                            // потом второму
                            if (robot2_code.PC < 0) {
                                if (robot2.dx.current_zone < ZONE_WASHING_1A) {
                                    // то же самое что и во второй стадии, только без инкремента стадии
                                    if (robot2_code.PC < 0) {
                                        robot2_lock_zone = ZONE_ETCHING_2;
                                        robot2_code.barrel_id = -1;
                                        robot2_code.code[0] = ROBOT_CMD_DOWN();
                                        robot2_code.code[1] = ROBOT_CMD_MOVE_TO_ZONE(ZONE_WASHING_1A);
                                        robot2_code.code[2] = ROBOT_CMD_END();
                                        robot2_code.PC = 0;
                                    }
                                } else {
                                    schedule_one_robot(ROBOT_2);
                                }
                            }

                            break;

                        default:
                            scheduler_stage = 0;
                    }
                }
            }
        }
    }
}