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

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

bool robot1_offset_pos = false;
bool robot2_offset_pos = false;


// true означает что движение закончено
static bool robot_move(robot_regs& r, short target, char robot_id) {
    // после перемещения мы явно в точной позиции
    if (robot_id == 1) {
        robot1_offset_pos = false;
    } else {
        robot2_offset_pos = false;
    }

    // перемещения в зоне парковки
    if (target < 0 && r.dx.current_zone == 0) {
        r.mx.correct_sensor = true;
        return true;
    }

    if (r.mx.correct_sensor) {
        r.dx.current_zone = 0;
        r.mx.correct_sensor = false;
        return target == 0;
    }

    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_code &code, robot_regs& r, char robot_id) {
    if (code.PC < 0) {
        return;
    }

    if (code.barrel_id >= BARRELS_COUNT) {
        code.barrel_id = -1;
    }

    // приватизируем бочку
    if (code.barrel_id >= 0) {
        barrels[code.barrel_id].flags.robot = robot_id;
    }

    const auto cmd_arg = (short)(code.code[code.PC] & (~ROBOT_CMD_MASK));

    switch (code.code[code.PC] & (short)ROBOT_CMD_MASK) {
        case ROBOT_CMD_MOVE_TO_ZONE_code:
            // двигаемся в сторону цели
            {
                short target = cmd_arg;
                if (target & ROBOT_ZONE_PARKING) {
                    target = -1;
                } else {
                    target &= 0x7F;
                }
                if (robot_move(r, target, robot_id)) {
                    code.PC++;
                }
            }

            if (cmd_arg & ROBOT_WITH_BARREL && code.barrel_id >= 0) {
                barrels[code.barrel_id].zone = r.dx.current_zone;
            }
            break;

        case ROBOT_CMD_MOVE_OFF_code:
            if (robot_id == 1) {
                robot1_offset_pos = true;
            } else {
                robot2_offset_pos = true;
            }
            std::cout << "robot " << robot_id << " move to offset pos" << std::endl;
            code.PC++;
            break;

        case ROBOT_CMD_MOVE_ACCURATE_code:
            if (robot_id == 1) {
                robot1_offset_pos = false;
            } else {
                robot2_offset_pos = false;
            }
            std::cout << "robot " << robot_id << " move to accurate position" << std::endl;
            code.PC++;
            break;

        case ROBOT_CMD_UP_code:
            if (code.barrel_id >= 0 && cmd_arg) {
                // не давать ехать перед тем как истечет таймер
                if (barrels[code.barrel_id].software_timer > 0) {
                    break;
                }
                barrels[code.barrel_id].flags.is_up = true;
                r.mz.is_up = true;
            }
            code.PC++;
            break;

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

                if (r.dx.current_zone == ZONE_UNLOAD) {
                    barrels[code.barrel_id].flags.is_empty = true;
                }
            }
            code.PC++;
            break;

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

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

        case ROBOT_CMD_SET_LOCK_ZONE_code:
            if (robot_id == 1) {
                robot1_lock_zone = cmd_arg;
            } else {
                robot2_lock_zone = cmd_arg;
            }
            code.PC++;
            break;

        case ROBOT_CMD_CORRECT_AXIS_code:
            if (cmd_arg == ROBOT_AXIS_X) {
                std::cout << "robot " << robot_id << " correct axis X..." << std::endl;
                r.dz.current_zone = 0;
                r.mx.correct_status = true;
            } else if (cmd_arg == ROBOT_AXIS_Z) {
                std::cout << "robot " << robot_id << " correct axis Z..." << std::endl;
                r.mz.is_up = true;
                r.mz.correct_status = true;
            } else {
                std::cout << "ERROR: R" << robot_id << " AXIS CORRECT - INVALID ARGUMENT VALUE " << cmd_arg << std::endl;
            }

            code.PC++;
            break;

        case ROBOT_CMD_INC_ZONE_code:
            if (!increment_zone(cmd_arg)) {
                std::cout << "WARM: R" << robot_id << " increment zone failed (cmd_arg=" << cmd_arg << ")" << std::endl;
            }

            code.PC++;
            break;

        case ROBOT_CMD_END_code:
            code.PC = -1;
            // де-приватизируем бочку
            if (code.barrel_id >= 0) {
                barrels[code.barrel_id].flags.robot = 0;
            }
            break;

        default:
            printf("ERROR: R%d INVALID INSTRUCTION: PC=%d CODE=0x%04X\n", robot_id, code.PC, code.code[code.PC] & 0xFFFF);
            code.PC = -1;
            // де-приватизируем бочку
            if (code.barrel_id >= 0) {
                barrels[code.barrel_id].flags.robot = 0;
            }
    }
}


void robot_main() {
    if (hla_robot1_en) {
        emulate_robot(robot1_code, robot1, 1);
    }

    if (hla_robot2_en) {
        emulate_robot(robot2_code, robot2, 2);
    }
}