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

#include "robot.h"

static short get_barrel(char robot_id) {
    for (short i = 0; i < BARRELS_COUNT; i++) {
        if (barrels[i].flags.robot == robot_id) {
            return i;
        }
    }
    return -1;
}

// true означает что движение закончено
static bool robot_move(robot& r, int target, char robot_id) {
    if (r.curr_zone == target) {
        return true;
    }

    if (r.curr_zone < target) {
        r.curr_zone++;
    } else {
        r.curr_zone--;
    }
    return false;
}

static void emulate_robot(robot_cmd& cmd, robot& r, char robot_id) {
    auto barrel = get_barrel(robot_id);
    switch (cmd.cmd) {
        case 1:
            // команда просто уехать
            switch (cmd.step) {
                case 0:
                    // двигаемся в сторону цели
                    if (robot_move(r, cmd.args[0], robot_id)) {
                        cmd.step++;
                    }
                    break;

                default:
                    cmd.cmd = 0;
                    cmd.step = 0;
            }
            break;

        case 2:
            // команда взять барабан и увезти его куда положено
            switch (cmd.step) {
                case 0:
                    // двигаемся в сторону барабана
                    if (robot_move(r, cmd.args[0], robot_id)) {
                        cmd.step++;
                    }
                    break;

                case 1:
                    // поднимаем траверсу
                    r.mz.is_up = 1;
                    if (barrel != -1) {
                        barrels[barrel].flags.is_up = 1;
                    }
                    cmd.step++;
                    break;

                case 2:
                    // двигаемся в сторону выгрузки
                    if (robot_move(r, cmd.args[1], robot_id)) {
                        cmd.step++;
                    }
                    break;

                case 3:
                    // опускаем траверсу
                    r.mz.is_up = 0;
                    if (barrel != -1) {
                        barrels[barrel].flags.is_up = 0;
                    }
                    // ну и тут же конец

                default:
                    cmd.cmd = 0;
                    cmd.step = 0;
            }
            break;

        case 3:
            // команда пассивация
            switch (cmd.step) {
                case 0:
                    // двигаемся в 18 зону
                    if (robot_move(r, 18, robot_id)) {
                        cmd.step++;
                    }
                    break;

                case 1:
                    // поднимаем траверсу
                    r.mz.is_up = 1;
                    if (barrel != -1) {
                        barrels[barrel].flags.is_up = 1;
                    }
                    cmd.step++;
                    break;

                case 2:
                    // двигаемся в 19 зону
                    if (robot_move(r, 19, robot_id)) {
                        cmd.step++;
                    }
                    break;

                case 3:
                    // опускаем траверсу
                    r.mz.is_up = 0;
                    if (barrel != -1) {
                        barrels[barrel].flags.is_up = 0;
                        barrels[barrel].software_timer = barrels[barrel].time_passivation;
                    }
                    cmd.step++;
                    break;

                case 4:
                    // ждем пока барабан отстоит таймер в пассивации
                    if (barrel != -1) {
                        if (barrels[barrel].software_timer <= 0) {
                            cmd.step++;
                        }
                    }
                    break;

                case 5:
                    // поднимаем траверсу
                    r.mz.is_up = 1;
                    if (barrel != -1) {
                        barrels[barrel].flags.is_up = 1;
                    }
                    cmd.step++;
                    break;

                case 6:
                    // двигаемся в 20 зону
                    if (robot_move(r, 20, robot_id)) {
                        cmd.step++;
                    }
                    break;

                case 7:
                    // опускаем траверсу
                    r.mz.is_up = 0;
                    if (barrel != -1) {
                        barrels[barrel].flags.is_up = 0;
                        // и тут же ставим время промывки барабана
                        barrels[barrel].software_timer = barrels[barrel].time_washing_4a;
                    }
                    // ну и тут же конец

                default:
                    cmd.cmd = 0;
                    cmd.step = 0;
            }
            break;


        default:
            cmd.cmd = 0;
            cmd.step = 0;
    }

    if (r.mz.is_up) {
        if (barrel != -1) {
            barrels[barrel].zone = r.curr_zone;
        }
    }
}

void robot_main() {
    emulate_robot(robot1_cmd, robot1, 1);
    emulate_robot(robot2_cmd, robot2, 2);
}