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

#include "robot.h"
#include <iostream>

bool robot1_offset_pos = false;
bool robot2_offset_pos = false;


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_regs& r, int target, char robot_id) {
    // после перемещения мы явно в точной позиции
    if (robot_id == 1) {
        robot1_offset_pos = false;
    } else {
        robot2_offset_pos = false;
    }

    if (r.dx.current_zone == target) {
        return true;
    }

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

static void emulate_robot(robot_cmd& cmd, robot_regs& 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.dx.current_zone;
        }
    }
}


static void emulate_robot_v2(robot_code* code, robot_regs& r, char robot_id) {
    if (code->PC < 0) {
        return;
    }
    const short cmd_arg = code->code[code->PC] & (~ROBOT_CMD_MASK);

    switch (code->code[code->PC] & ROBOT_CMD_MASK) {
        case ROBOT_CMD_MOVE_TO_ZONE:
            // двигаемся в сторону цели
            if (robot_move(r, cmd_arg, robot_id)) {
                code->PC++;
            }
            break;

        case ROBOT_CMD_MOVE_OFF:
            if (robot_id == 1) {
                robot1_offset_pos = true;
            } else {
                robot2_offset_pos = true;
            }
            code->PC++;
            break;

        case ROBOT_CMD_UP:
            if (code->barrel_id >= 0) {
                barrels[code->barrel_id].flags.is_up = true;
                r.mz.is_up = 1;
            }
            code->PC++;
            break;

            // в эмуляторе не важно где я, поэтому тут обе команды вниз обрабатываются одинаково
        case ROBOT_CMD_DOWN:
        case ROBOT_CMD_DOWN_2:
            if (code->barrel_id >= 0) {
                barrels[code->barrel_id].flags.is_up = false;
                r.mz.is_up = 0;
            }
            code->PC++;
            break;

        case ROBOT_CMD_WAIT:
            std::cout << "robot " << robot_id << " wait " << cmd_arg << " secs..." << std::endl;
            code->PC++;
            break;

        case ROBOT_CMD_TMR_SET:
            if (code->barrel_id >= 0) {
                barrels[code->barrel_id].software_timer = code->code[code->PC + 1];
                r.mz.is_up = 0;
            }
            code->PC += 2;
            break;

        case ROBOT_CMD_SET_LOCK_ZONE:
            if (robot_id == 1) {
                robot1_lock_zone = cmd_arg;
            } else {
                robot2_lock_zone = cmd_arg;
            }
            code->PC += 2;
            break;

        case ROBOT_CMD_CORRECT_X:
            std::cout << "robot " << robot_id << " correct axis X..." << std::endl;
            r.dz.current_zone = 0;
            r.mx.correct_status = true;
            code->PC++;
            break;

        case ROBOT_CMD_CORRECT_Z:
            std::cout << "robot " << robot_id << " correct axis Z..." << std::endl;
            r.mz.is_up = true;
            r.mz.correct_status = true;
            code->PC++;
            break;

        case ROBOT_CMD_END:
        default:
            code->PC = -1;
    }
}


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