//
// Created by Владислав Остапов on 13.12.2022.
//

#ifdef EMULATOR
#include <stdio.h>
#include "emulator.h"
#else
#include "utils.h"
#endif


#define LOCK_ZONE_BORDER 1


const short NIGHT_ZONES[9] = {
        ZONE_LOAD_1,
        ZONE_WASHING_1A, ZONE_WASHING_1B,
        ZONE_WASHING_2A, ZONE_WASHING_2B,
        ZONE_WASHING_3A, ZONE_WASHING_3B,
        ZONE_WASHING_4A, ZONE_WASHING_4B
};


char zone_is_busy(short zone) {
    for (short i = 0; i < BARRELS_COUNT; i++) {
        if (barrels[i].flags.is_exist && barrels[i].zone == zone && !barrels[i].flags.is_up) {
            return 1;
        }
    }
    return 0;
}


short get_first_night_zone() {
    short min_normal_barrel = -1;
    for (short i = 0; i < BARRELS_COUNT; i++) {
        // ищем все не ночные, не пустые, и которые внизу барабаны
        if (barrels[i].flags.is_exist && !barrels[i].flags.is_empty  && !barrels[i].flags.is_night && !barrels[i].flags.is_up) {
            // нужен барабан, у которого нет
            if (min_normal_barrel < 0 || barrels[i].zone < min_normal_barrel) {
                min_normal_barrel = barrels[i].zone;
            }
        }
    }

    // всего зон, куда можно сныкать барабаны (всего 9 мест: загрузка 1 и 8 промывок)
    for (short nz = 0; nz < 9; nz++) {
        char found = 0;
        for (short i = 0; i < BARRELS_COUNT; i++) {
            if (barrels[i].flags.is_exist && barrels[i].zone == NIGHT_ZONES[nz] && barrels[i].flags.is_night) {
                found = 1;
            }
            if (NIGHT_ZONES[nz] == ZONE_LOAD_1 && barrels[i].flags.is_exist && barrels[i].zone == NIGHT_ZONES[nz]) {
                found = 1;
            }
        }

        if (found == 0) {
            if (min_normal_barrel < 0 || NIGHT_ZONES[nz] < min_normal_barrel) {
                return NIGHT_ZONES[nz];
            } else {
                break;
            }
        }
    }

    return -1;
}


short _get_dest_zone(struct barrel *bar, char robot_id) {
    if (bar->flags.is_night) {
        if (one_robot_mode) {
            return ZONE_LOAD_1;
        } else {
            if (robot_id == ROBOT_1) {
                // робот 1 может выкидывать барабаны только до зоны обмена
                if (bar->zone >= hla_exchange_zone) {
                    return hla_exchange_zone;
                } else {
                    return -1;
                }
            } else if (robot_id == ROBOT_2) {
                // робот 2 всегда кидает барабаны в загрузку 1, но только дальше зоны обмена он не имеет права брать
                if (bar->zone <= hla_exchange_zone) {
                    return ZONE_LOAD_1;
                } else {
                    return -1;
                }
            } else {
                return ZONE_LOAD_1;
            }
        }
    }

    // если это зона обмена и барабан пустой
    if (bar->zone == hla_exchange_zone && bar->flags.is_empty) {
        if (hla_night_mode) {
            return get_first_night_zone();
        } else {
            return ZONE_LOAD_1;
        }
    }


    switch (bar->zone) {
        case ZONE_LOAD_2:
            // загрузка 2, только в нее можно грузить новые барабаны, по умолчанию следующая зона - обезжиривание
            // при повторном цинковании это кислота
            if (hla_zinc_again) {
                if (etching_zone < 0) {
                    return -1;
                } else {
                    return (short)(ZONE_ETCHING_1 + etching_zone);
                }
            } else {
                return ZONE_DEGREASING;
            }

        case ZONE_DEGREASING:
            // обезжиривание, нужна промывка 1А
            return ZONE_WASHING_1A;

        case ZONE_WASHING_1A:
            // промывка 1А, нужна промывка 1Б
            return ZONE_WASHING_1B;

        case ZONE_WASHING_1B:
            // промывка 1Б, нужно травление (зоны 5-6)
            if (etching_zone < 0) {
                return -1;
            }
            return (short)(ZONE_ETCHING_1 + etching_zone);

        case ZONE_ETCHING_1:
        case ZONE_ETCHING_2:
            // травление, нужна промывка 2А
            return ZONE_WASHING_2A;

        case ZONE_WASHING_2A:
            // промывка 2А, нужна промывка 2Б
            return ZONE_WASHING_2B;

        case ZONE_WASHING_2B:
            // промывка 2Б, нужно цинкование (зоны 9-16)
            if (galvanizing_zone < 0) {
                return -1;
            }
            return (short)(ZONE_GALVANIZING_1 + galvanizing_zone);

        case ZONE_GALVANIZING_1:
        case ZONE_GALVANIZING_2:
        case ZONE_GALVANIZING_3:
        case ZONE_GALVANIZING_4:
        case ZONE_GALVANIZING_5:
        case ZONE_GALVANIZING_6:
        case ZONE_GALVANIZING_7:
        case ZONE_GALVANIZING_8:
            // цинкование, требуется чтобы в зонах 17-22 было максимум 2 барабана (3 барабана для этой части линии - максимум)
            if (!zone_is_busy(ZONE_WASHING_3A)) {
                short count = 0;
                // если зона 17 свободна, то диапазон начнется с 3Б
                for (short i = ZONE_WASHING_3B; i <= (short)ZONE_UNLOAD; i++) {
                    if (zone_is_busy(i)) {
                        count++;
                    }
                }
                if (count < 3) {
                    return ZONE_WASHING_3A;
                }
            }
            return -1;

        case ZONE_WASHING_3A:
            // промывка 3А, перекладываем в промывку 3Б
            return ZONE_WASHING_3B;

        case ZONE_WASHING_3B:
            // это перед пассивацией, требует свободную промывку 4А и на всякий случай свободную пассивацию
            if (!zone_is_busy(ZONE_WASHING_4A)) {
                return ZONE_PASSIVATION;
            }
            // это атомарная операция, по идее вносить барабан в пассивацию нельзя
            return -1;

        case ZONE_PASSIVATION:
            // процесс пассивации, нужна промывка 4A
            // чисто теоретически сюда никогда не попадем, но если вдруг выстрелит пусть будет
            return ZONE_WASHING_4A;

        case ZONE_WASHING_4A:
            // промывка 4А, перекладываем в промывку 4Б
            return ZONE_WASHING_4B;

        case ZONE_WASHING_4B:
            // последняя промывка в процессе
            return ZONE_UNLOAD;

        case ZONE_UNLOAD:
            // последняя промывка, нужно разрешение на выгрузку
            if (button_unload) {
                if (hla_night_mode) {
                    short nz = get_first_night_zone();
                    if (nz < hla_exchange_zone) {
                        if (one_robot_mode && nz >= 0) {
                            return nz;
                        } else {
                            return hla_exchange_zone;
                        }
                    } else {
                        return nz;
                    }
                } else {
                    if (one_robot_mode) {
                        return ZONE_LOAD_1;
                    } else {
                        return hla_exchange_zone;
                    }
                }
            }
            break;
    }

    return -1;
}


short can_move(struct barrel *bar, char robot_id) {
    if (robot_id != ROBOT_NONE && robot_id != ROBOT_1 && robot_id != ROBOT_2) {
        return -3;
    }
    // сразу отсекаем варианты, при которых невозможно переместить барабан
    if (!bar->flags.is_exist) {
        return -4;
    }

    if (bar->flags.robot != 0) {
        return -5;
    }

    // проверка ночного режима
    if (hla_night_mode) {
        if (bar->flags.is_night) {
            return -1;
        }
    }

    // тут просчет примерного времени, в течении которого можно доехать до барабана
    // !учитывается только с привязкой к роботу
    if (!bar->flags.is_empty && robot_id != ROBOT_NONE) {
        short approximate_time;
        if (robot_id == 1) {
            approximate_time = bar->zone - robot1.dx.current_zone;
        } else {
            approximate_time = bar->zone - robot2.dx.current_zone;
        }

        // abs(approximate_time)
        if (approximate_time < 0) {
            approximate_time = -approximate_time;
        }

#ifndef EMULATOR
        if (approximate_time > 1) {
            approximate_time = ((approximate_time - 1) * 3) + 4;
        } else if (approximate_time == 1) {
            approximate_time = 5;
        }
#endif

        if (bar->software_timer > approximate_time) {
            return -2;
        }
    }

    // дальше нужно проверить, что можно передвигать бочку
    short dest_zone = _get_dest_zone(bar, robot_id);

    if (zone_is_busy(dest_zone)) {
        return -1;
    }

    // учитывать границы только в режиме двух роботов и с привязкой к роботу
    if (!one_robot_mode && dest_zone >= 0 && robot_id != ROBOT_NONE) {
        if (robot_id == 1) {
            // если робот 1, то это старый, который ближе к концу линии.
            // Ему нельзя ехать если хоть одна из зон <= max(r2_pos, r2_lock) + кол-во пограничных зон
            short border = robot2.dx.current_zone;
            if (border < robot2_lock_zone) {
                border = robot2_lock_zone;
            }
            if (border < ZONE_ETCHING_2) {
                border = ZONE_ETCHING_2;
            }

            border += LOCK_ZONE_BORDER;

            if (bar->zone <= border || dest_zone <= border) {
                dest_zone = -6;
            }
        } else {
            // если робот 2, то это новый, который ближе к началу линии.
            // Ему нельзя ехать если хоть одна из зон >= max(r2_pos, r2_lock) - кол-во пограничных зон
            short border = robot1.dx.current_zone;
            if (border > robot1_lock_zone) {
                border = robot1_lock_zone;
            }
            if (border > ZONE_PASSIVATION) {
                border = ZONE_PASSIVATION;
            }

            border -= LOCK_ZONE_BORDER;

            if (bar->zone >= border || dest_zone >= border) {
                dest_zone = -6;
            }
        }
    }

    return dest_zone;
}


short get_operation_priority(short barrel_id) {
    // если барабан пустой и не в ночном режиме, приоритет ему 2
    if (barrels[barrel_id].flags.is_empty && !barrels[barrel_id].flags.is_night) {
        return 2;
    }

    // если барабан в ночном режиме, то приоритет 1 самому левому, остальным 0
    if (barrels[barrel_id].flags.is_night) {
        short min_zone = barrels[barrel_id].zone;

        for (short i = 0; i < BARRELS_COUNT; i++) {
            if (i == barrel_id) {
                continue;
            }
            if (barrels[i].flags.is_exist && barrels[i].flags.is_night && barrels[i].flags.robot == 0 && !barrels[i].flags.is_up) {
                if (can_move(barrels + i, ROBOT_NONE) >= 0) {
                    // чем больше у барабана время ожидания тем меньше у него число
                    if (barrels[i].zone < min_zone) {
                        min_zone = barrels[i].zone;
                    }
                }
            }
        }

        return (min_zone == barrels[barrel_id].zone) ? 1 : 0;
    }

    // сделать приоритет на барабан, который больше всего ждет
    if (barrels[barrel_id].zone >= ZONE_GALVANIZING_1 && barrels[barrel_id].zone <= ZONE_GALVANIZING_8) {
        // теперь надо выяснить, есть ли барабаны с большим временем ожидания
        // тут возможны несколько случаев:
        //     1) когда барабан один такой (больше в цинковании нет барабанов чтобы их изъять),
        //     2) когда барабанов несколько, соответственно если есть барабан с наибольшим временем, то надо ему дать приоритет 1, а остальным 0,

        char is_not_one = 0;
        char is_with_max_time = 1;

        for (short i = 0; i < BARRELS_COUNT; i++) {
            if (i == barrel_id) {
                continue;
            }
            if (barrels[i].flags.is_exist && barrels[i].zone >= ZONE_GALVANIZING_1 && barrels[i].zone <= ZONE_GALVANIZING_8) {
                if (can_move(barrels + i, ROBOT_NONE) >= 0) {
                    is_not_one = 1;
                    // чем больше у барабана время ожидания тем меньше у него число
                    if (barrels[i].software_timer < barrels[barrel_id].software_timer) {
                        is_with_max_time = 0;
                        break;
                    }
                }
            }
        }

        if (is_not_one == 0) {
            return 1;
        }
        return is_with_max_time;
    }


    // теперь то же самое, только для травления (только тут задача проще потому что травления всего 2 зоны, и надо только один барабан если он есть)
    if (barrels[barrel_id].zone >= ZONE_ETCHING_1 && barrels[barrel_id].zone <= ZONE_ETCHING_2) {

        // если в травлении барабан лежит больше 40 минут, ему присвоить высокий приоритет
        if (barrels[barrel_id].software_timer < (-1 * 60 * 40)) {
            return 2;
        }

        for (short i = 0; i < BARRELS_COUNT; i++) {
            if (i == barrel_id) {
                continue;
            }

            if (barrels[i].flags.is_exist && barrels[i].zone >= ZONE_ETCHING_1 && barrels[i].zone <= ZONE_ETCHING_2) {
                if (can_move(barrels + i, ROBOT_NONE) >= 0) {
                    if (barrels[i].software_timer < barrels[barrel_id].software_timer) {
                        return 0; // у этого барабана больше время ожидания (число меньше), значит приоритет 0
                    } else {
                        return 1; // поскольку у нашего барабана максимальное время ожидания
                    }
                }
            }
        }
        // барабан не найден, приоритет ему 1
        return 1;
    }

    return 1; // 1 - нормальный приоритет
}


char remove_barrel_from_zone(short zone) {
    for (short i = 0; i < 10; i++) {
        if (barrels[i].flags.is_exist && barrels[i].flags.robot == 0 && barrels[i].zone == zone) {
            barrels[i].flags.is_exist = 0;
            return 1;
        }
    }
    return 0;
}


#ifdef EMULATOR
void debug_print_robot_code(const struct robot_code *code, const short robot_id, int fd) {
    dprintf(fd, "Code for R%d, B%d:\n", robot_id, code->barrel_id);
    for (int i = 0; i < 16; i++) {
        const short cmd_arg = (short)(code->code[i] & (short)(~ROBOT_CMD_MASK));

        if (code->PC == i) {
            dprintf(fd, "==>%2d 0x%04X", i, code->code[i] & 0xFFFF);
        } else {
            dprintf(fd, "   %2d 0x%04X", i, code->code[i] & 0xFFFF);
        }

        if ((code->code[i] & ROBOT_CMD_MASK) == ROBOT_CMD_END_code) {
            dprintf(fd, "    END\n");
            break;
        }

        switch (code->code[i] & ROBOT_CMD_MASK) {
            case ROBOT_CMD_MOVE_TO_ZONE_code:
                if (cmd_arg & ROBOT_ZONE_PARKING) {
                    dprintf(fd, "    move to parking (with barrel: %d)\n", (cmd_arg & ROBOT_WITH_BARREL) != 0);
                } else {
                    dprintf(fd, "    move to zone %d (with barrel: %d)\n",
                            cmd_arg & (~ROBOT_WITH_BARREL), (cmd_arg & ROBOT_WITH_BARREL) != 0);
                }
                break;

            case ROBOT_CMD_MOVE_OFF_code:
                dprintf(fd, "    move to offset pos\n");
                break;

            case ROBOT_CMD_MOVE_ACCURATE_code:
                dprintf(fd, "    move to accurate pos\n");
                break;

            case ROBOT_CMD_UP_code:
                dprintf(fd, "    up (with barrel: %d)\n", (cmd_arg & ROBOT_WITH_BARREL) != 0);
                break;

                // в эмуляторе не важно где я, поэтому тут обе команды вниз обрабатываются одинаково
            case ROBOT_CMD_DOWN_code:
                dprintf(fd, "    down (with barrel: %d)\n", (cmd_arg & ROBOT_WITH_BARREL) != 0);
                break;

            case ROBOT_CMD_WAIT_code:
                dprintf(fd, "    wait %d secs\n", cmd_arg);
                break;

            case ROBOT_CMD_TMR_SET_code:
                dprintf(fd, "    set barrel timer %d secs\n", cmd_arg);
                break;

            case ROBOT_CMD_SET_LOCK_ZONE_code:
                dprintf(fd, "    set lock zone %d\n", cmd_arg);
                break;

            case ROBOT_CMD_CORRECT_AXIS_code:
                if (cmd_arg == ROBOT_AXIS_X) {
                    dprintf(fd, "    correct axis: X\n");
                } else if (cmd_arg == ROBOT_AXIS_Z) {
                    dprintf(fd, "    correct axis: Z\n");
                } else {
                    dprintf(fd, "    correct axis: INVALID (%d)\n", cmd_arg);
                }
                break;

            case ROBOT_CMD_INC_ZONE_code:
                if (cmd_arg == ROBOT_ZONE_GAL) {
                    dprintf(fd, "    increment zone: galvanic\n");
                } else if (cmd_arg == ROBOT_ZONE_ETCH) {
                    dprintf(fd, "    increment zone: etching\n");
                } else {
                    dprintf(fd, "    increment zone: INVALID (0x%4X)\n", cmd_arg);
                }
                break;

            default:
                dprintf(fd, "    UNKNOWN: 0x%04X\n", code->code[i] & 0xFFFF);
        }
    }
}
#endif


void create_operation(short barrel_id, const short start_zone, const short dest_zone, const short robot_id) {
    short current_zone;
    struct robot_code *code;
    if (robot_id == ROBOT_1) {
        current_zone = robot1.dx.current_zone;
        code = &robot1_code;
    } else if (robot_id == ROBOT_2) {
        current_zone = robot2.dx.current_zone;
        code = &robot2_code;
    } else {
        return;
    }

    // создаем код транзакции, пока обычный
    if (barrel_id >= BARRELS_COUNT) {
        barrel_id = -1;
    }
    code->barrel_id = barrel_id;

    if (barrel_id >= 0) {
        barrels[barrel_id].flags.robot = robot_id;
        if (hla_night_mode && barrels[barrel_id].flags.is_empty && dest_zone == get_first_night_zone()) {
            barrels[barrel_id].flags.is_night = -1;
        }

        // сброс флага повторного цинкования во время создания задания
        if (start_zone == ZONE_LOAD_2) {
            hla_zinc_again = 0;
        }
    }

    short cmd_index = 0;

    if (!one_robot_mode) {
        if (robot_id == 1) {
            short tmp = dest_zone;
            if (start_zone < dest_zone) {
                tmp = start_zone;
            }
            if (tmp > ZONE_PASSIVATION) {
                tmp = ZONE_PASSIVATION;
            }
            robot1_lock_zone = tmp;
        } else {
            short tmp = dest_zone;
            if (start_zone > dest_zone) {
                tmp = start_zone;
            }
            if (tmp < ZONE_ETCHING_2) {
                tmp = ZONE_ETCHING_2;
            }
            robot2_lock_zone = tmp;
        }
    }

    // первым делом добавляем команду опустить траверсу
    code->code[cmd_index++] = ROBOT_CMD_DOWN();

    if (dest_zone == ZONE_PASSIVATION) {
        // пассивация, тут все просто
        if (current_zone != start_zone) {
            code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE(ZONE_WASHING_3B);
        }
        if (!one_robot_mode) {
            code->code[cmd_index++] = ROBOT_CMD_SET_LOCK_ZONE(ZONE_PASSIVATION);
        }
        code->code[cmd_index++] = ROBOT_CMD_UP_WITH_BARREL();
        code->code[cmd_index++] = ROBOT_CMD_WAIT(hla_time_washing_2);
        code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE_WITH_BARREL(ZONE_PASSIVATION);
        code->code[cmd_index++] = ROBOT_CMD_DOWN_WITH_BARREL();

        // NOTE таймер робота работает точнее чем таймер барабанов
        code->code[cmd_index++] = ROBOT_CMD_TMR_SET(barrels[barrel_id].time_passivation);
//        code->code[cmd_index++] = ROBOT_CMD_WAIT(barrels[barrel_id].time_passivation);

        code->code[cmd_index++] = ROBOT_CMD_UP_WITH_BARREL();
        code->code[cmd_index++] = ROBOT_CMD_WAIT(hla_time_digging);
        code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE_WITH_BARREL(ZONE_WASHING_4A);
        code->code[cmd_index++] = ROBOT_CMD_DOWN_WITH_BARREL();

        code->code[cmd_index++] = ROBOT_CMD_TMR_SET(barrels[barrel_id].time_washing_4a);
    } else {
        // любой другой случай
        if (current_zone != start_zone) {
            if (start_zone != 22) {
                code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE(start_zone);
            } else {
                code->code[cmd_index++] = ROBOT_CMD_MOVE_OFF();
                code->code[cmd_index++] = ROBOT_CMD_UP();
                code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE(22);
                code->code[cmd_index++] = ROBOT_CMD_MOVE_OFF();
                code->code[cmd_index++] = ROBOT_CMD_DOWN();
                code->code[cmd_index++] = ROBOT_CMD_MOVE_ACCURATE();
            }
        }

        // теперь обновляем LOCK-зону
        if (!one_robot_mode) {
            if (robot_id == 1) {
                // ставим lock-зону только если она ближе к концу линии
                short tmp = dest_zone;

                // lock-зона этого робота должна быть такой, чтобы была свободна промывка 3а
                if (tmp > ZONE_PASSIVATION) {
                    tmp = ZONE_PASSIVATION;
                }

                // в любом случае lock-зону нельзя двигать к началу линии
                if (tmp > robot1_lock_zone) {
                    code->code[cmd_index++] = ROBOT_CMD_SET_LOCK_ZONE(tmp);
                }
            } else {
                // ставим lock-зону только если она ближе к началу линии
                short tmp = dest_zone;

                // lock-зона этого робота не может выходить за промывку 3А, потому что это не имеет смысла
                if (tmp < ZONE_ETCHING_2) {
                    tmp = ZONE_ETCHING_2;
                }

                // в любом случае lock-зону нельзя двигать к концу линии
                if (tmp < robot2_lock_zone) {
                    code->code[cmd_index++] = ROBOT_CMD_SET_LOCK_ZONE(tmp);
                }
            }
        }

        code->code[cmd_index++] = ROBOT_CMD_UP_WITH_BARREL();

        // теперь надо определиться с тем, сколько ждать скапывания
        switch (start_zone) {
            case ZONE_DEGREASING:
            case ZONE_ETCHING_1:
            case ZONE_ETCHING_2:
            case ZONE_GALVANIZING_1:
            case ZONE_GALVANIZING_2:
            case ZONE_GALVANIZING_3:
            case ZONE_GALVANIZING_4:
            case ZONE_GALVANIZING_5:
            case ZONE_GALVANIZING_6:
            case ZONE_GALVANIZING_7:
            case ZONE_GALVANIZING_8:
                // время скапывания реактивов, оно не должно работать только в зоне обмена
                if (start_zone != hla_exchange_zone) {
                    code->code[cmd_index++] = ROBOT_CMD_WAIT(hla_time_reagent);
                }
                break;

            case ZONE_WASHING_1A:
            case ZONE_WASHING_2A:
            case ZONE_WASHING_3A:
            case ZONE_WASHING_4A:
                // время скапывания первого каскада промывок
                code->code[cmd_index++] = ROBOT_CMD_WAIT(hla_time_washing_1);
                break;

            case ZONE_WASHING_1B:
            case ZONE_WASHING_2B:
            case ZONE_WASHING_3B:
            case ZONE_WASHING_4B:
                // время скапывания второго каскада промывок
                code->code[cmd_index++] = ROBOT_CMD_WAIT(hla_time_washing_2);
                break;
        }

        code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE_WITH_BARREL(dest_zone);

        if (barrel_id >= 0) {
            if (!barrels[barrel_id].flags.is_empty) {
                // инкремент зоны (если травление или цинкование)
                if (dest_zone == ZONE_ETCHING_1 || dest_zone == ZONE_ETCHING_2) {
                    code->code[cmd_index++] = ROBOT_CMD_INC_ZONE(ROBOT_ZONE_ETCH);
                } else if (dest_zone >= ZONE_GALVANIZING_1 && dest_zone <= ZONE_GALVANIZING_8) {
                    code->code[cmd_index++] = ROBOT_CMD_INC_ZONE(ROBOT_ZONE_GAL);
                }
            }
        }

        code->code[cmd_index++] = ROBOT_CMD_DOWN_WITH_BARREL();

        if (dest_zone == 22) {
            code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE(21);
            // NOTE старая механика не позволяет просто опустить траверсу до конца, для новой изменить поведение
            code->code[cmd_index++] = ROBOT_CMD_UP();
            code->code[cmd_index++] = ROBOT_CMD_DOWN();
        } else {
            // установка времени ожидания барабана
            short tmp = -1;
            if (barrel_id >= 0) {
                if (!barrels[barrel_id].flags.is_empty) {
                    switch (dest_zone) {
                        case ZONE_DEGREASING:
                            tmp = barrels[barrel_id].time_degreasing;
                            break;

                        case ZONE_ETCHING_1:
                        case ZONE_ETCHING_2:
                            tmp = barrels[barrel_id].time_etching;
                            break;

                        case ZONE_GALVANIZING_1:
                        case ZONE_GALVANIZING_2:
                        case ZONE_GALVANIZING_3:
                        case ZONE_GALVANIZING_4:
                        case ZONE_GALVANIZING_5:
                        case ZONE_GALVANIZING_6:
                        case ZONE_GALVANIZING_7:
                        case ZONE_GALVANIZING_8:
                            tmp = barrels[barrel_id].time_galvanizing;
                            break;

                        case ZONE_WASHING_1A:
                            tmp = barrels[barrel_id].time_washing_1a;
                            break;
                        case ZONE_WASHING_2A:
                            tmp = barrels[barrel_id].time_washing_2a;
                            break;
                        case ZONE_WASHING_3A:
                            tmp = barrels[barrel_id].time_washing_3a;
                            break;
                        case ZONE_WASHING_4A:
                            tmp = barrels[barrel_id].time_washing_4a;
                            break;

                        case ZONE_WASHING_1B:
                            tmp = barrels[barrel_id].time_washing_1b;
                            break;
                        case ZONE_WASHING_2B:
                            tmp = barrels[barrel_id].time_washing_2b;
                            break;
                        case ZONE_WASHING_3B:
                            tmp = barrels[barrel_id].time_washing_3b;
                            break;
                        case ZONE_WASHING_4B:
                            tmp = barrels[barrel_id].time_washing_4b;
                            break;
                    }
                }
            }

            if (tmp >= 0) {
                if (tmp > 8000) {
                    tmp = 8000;
                }
                code->code[cmd_index++] = ROBOT_CMD_TMR_SET(tmp);
            }
        }
    }

    if (!one_robot_mode) {
        if (robot_id == 2) {
            // 2-му роботу нужно держать всегда свободной зону промывки 2б
            if (dest_zone > ZONE_ETCHING_2) {
                code->code[cmd_index++] = ROBOT_CMD_SET_LOCK_ZONE(ZONE_ETCHING_2);
                code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE(ZONE_ETCHING_2);
            } else if (dest_zone < ZONE_WASHING_1A) {
                // на случай если робот уехал в выгрузку или обезжир, надо его отогнать в зону промывки 1а
//                code->code[cmd_index++] = ROBOT_CMD_SET_LOCK_ZONE(ZONE_WASHING_1A);
                code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE(ZONE_WASHING_1A);
            }
        } else if (robot_id == 1) {
            // 2-му роботу нужно держать всегда свободной зону промывки 3а
            if (dest_zone < ZONE_PASSIVATION) {
                // чтобы из этой зоны можно было переложить барабан первому роботу
                code->code[cmd_index++] = ROBOT_CMD_SET_LOCK_ZONE(ZONE_PASSIVATION);
                code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE(ZONE_PASSIVATION);
            }
        }
    } else {
        // автоматический отгоняем робота в зону 3, чтоб не мешался
        if (dest_zone < ZONE_WASHING_1A) {
            code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE(ZONE_WASHING_1A);
        }
    }

    code->code[cmd_index++] = ROBOT_CMD_END();
#ifdef EMULATOR
    printf("INFO: code length is %d\n", cmd_index);
    debug_print_robot_code(code, robot_id, 0);
#endif
    code->PC = 0;
}


char is_accessible_zone(short zone) {
    if (zone == ROBOT_ZONE_ETCH) {
        if (etching_zone < 0) {
            return 0;
        }

        return (hla_disabled_zones & (DISABLED_ETCH_1 << etching_zone)) == 0;
    } else if (zone == ROBOT_ZONE_GAL) {
        if (galvanizing_zone < 0) {
            return 0;
        }

        return (hla_disabled_zones & (DISABLED_GAL_1 << galvanizing_zone)) == 0;
    } else {
        // неверный аргумент
        return 0;
    }
}


char increment_zone(short zone) {
    if (zone == ROBOT_ZONE_ETCH) {
        if ((hla_disabled_zones & DISABLED_ETCH) == DISABLED_ETCH) {
            etching_zone = -1;
            return 0;
        } else {
            if ((hla_disabled_zones & DISABLED_ETCH) == 0) {
                // если обе зоны активны
                if (etching_zone < 0) {
                    etching_zone = 0;
                } else {
                    if (etching_zone == 0) {
                        etching_zone = 1;
                    } else {
                        etching_zone = 0;
                    }
                }
            } else {
                // если только одна зона
                if (hla_disabled_zones & DISABLED_ETCH_1) {
                    etching_zone = 1;
                } else {
                    etching_zone = 0;
                }
            }

            return 1;
        }
    } else if (zone == ROBOT_ZONE_GAL) {
        // с зонами цинкования все несколько сложнее...
        // сначала надо посчитать кол-во включенных зон
        short enabled_zones = 0;
        for (short z = DISABLED_GAL_1; z & DISABLED_GAL; z <<= 1) {
            if ((hla_disabled_zones & z) == 0) {
                enabled_zones++;
            }
        }

        if (enabled_zones == 0) {
            // таких зон нет...
            galvanizing_zone = -1;
            return 0;
        } else if (enabled_zones == 1) {
            // зона одна, ее только найти надо и установить
            for (short z = 0; z < 8; z++) {
                if ((hla_disabled_zones & (DISABLED_GAL_1 << z)) == 0) {
                    galvanizing_zone = z;
                    break;
                }
            }
        } else {
            // зон несколько, поэтому берем текущую, пробегаемся по битам и ищем другую
            // всего может быть 7 зон, куда мы еще посмотрим
            short bit_id = galvanizing_zone + 1;
            if (galvanizing_zone < 0) {
                bit_id = 0;
            }
            for (short i = 0; i < 7 + (galvanizing_zone < 0 ? 1 : 0); i++) {
                if (bit_id >= 8) {
                    bit_id = 0;
                }
                if ((hla_disabled_zones & (DISABLED_GAL_1 << bit_id)) == 0) {
                    // свободная зона
                    galvanizing_zone = bit_id;
                    break;
                }

                bit_id++;
            }
        }

        return 1;
    } else {
        // неверный аргумент
        return 0;
    }
}