#include <stdio.h>
#include <stdlib.h>
#include "emulator.h"

short etching_zone = 0, galvanizing_zone = 0;

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

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

// TODO обновить метод для работы с двумя роботами
// вернет можно ли ехать и главное куда ехать, если можно (нельзя если вернулось значение < 0)
// -1 вернет что перемещать нельзя
// -2 вернет если требуется атомарная операция пассивации
short can_move(struct barrel* bar) {
    // сразу отсекаем варианты, при которых невозможно переместить барабан
    if (!bar->flags.is_exist) {
        return -1;
    }

    if (bar->software_timer > 0) {
        return -1;
    }

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

    if (!schedulerOneRobotMode) {
        printf("WARMING: нет проверки того, что для перемещения барабана не мешает второй робот\n");
    }

    // дальше нужно проверить, что можно передвигать бочку

    switch (bar->zone) {
        case ZONE_LOAD_2:
            // загрузка 2, только в нее можно грузить новые барабаны, нужно обезжиривание
            if (!zone_is_busy(ZONE_DEGREASING)) {
                return ZONE_DEGREASING;
            }
            break;

        case ZONE_DEGREASING:
            // обезжиривание, нужна промывка 1А
            if (!zone_is_busy(ZONE_WASHING_1A)) {
                return ZONE_WASHING_1A;
            }
            break;

        case ZONE_WASHING_1A:
            // промывка 1А, нужна промывка 1Б
            if (!zone_is_busy(ZONE_WASHING_1B)) {
                return ZONE_WASHING_1B;
            }
            break;

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

        case ZONE_ETCHING_1:
        case ZONE_ETCHING_2:
            // травление, нужна промывка 2А
            if (!zone_is_busy(ZONE_WASHING_2A)) {
                return ZONE_WASHING_2A;
            }
            break;

        case ZONE_WASHING_2A:
            // промывка 2А, нужна промывка 2Б
            if (!zone_is_busy(ZONE_WASHING_2B)) {
                return ZONE_WASHING_2B;
            }
            break;

        case ZONE_WASHING_2B:
            // промывка 2Б, нужно цинкование (зоны 9-16)
            if (!zone_is_busy(ZONE_GALVANIZING_1 + galvanizing_zone)) {
                return ZONE_GALVANIZING_1 + galvanizing_zone;
            }
            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:
            // цинкование, требуется чтобы в зонах 17-22 было максимум 2 барабана (3 барабана для этой части линии - максимум)
            if (!zone_is_busy(ZONE_WASHING_3A)) {
                short count = 0;
                // если зона 17 свободна, то диапазон начнется с 3Б
                for (short i = ZONE_WASHING_3B; i <= ZONE_UNLOAD; i++) {
                    if (zone_is_busy(i)) {
                        count++;
                    }
                }
                if (count < 3) {
                    return ZONE_WASHING_3A;
                }
            }
            break;

        case ZONE_WASHING_3A:
            // промывка 3А, перекладываем в промывку 3Б
            if (!zone_is_busy(ZONE_WASHING_3B)) {
                return ZONE_WASHING_3B;
            }
            break;

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

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

        case ZONE_WASHING_4A:
            // промывка 4А, перекладываем в промывку 4Б
            if (!zone_is_busy(ZONE_WASHING_4B)) {
                return ZONE_WASHING_4B;
            }
            break;

        case ZONE_WASHING_4B:
            // процесс пассивации, нужна промывка 4B (зона 21) (потому что сейчас я в 4A)
            if (!zone_is_busy(ZONE_UNLOAD)) {
                return ZONE_UNLOAD;
            }
            break;

        case ZONE_UNLOAD:
            // последняя промывка, нужно разрешение на выгрузку
            if (schedulerOneRobotMode && schedulerUnloadButton) {
                // нужно промывку загрузку 0
                if (!zone_is_busy(ZONE_LOAD_1)) {
                    return ZONE_LOAD_1;
                }
            }
            break;
    }
    return -1;
}


struct scheduler_task {
    short start_zone; // стартовая зона
    short dest_zone; // конечная зона
    short priority; // приоритет, чем больше тем выше, по умолчанию 0
};

// выставляет приоритет операции
short get_operation_priority(short barrel_id) {
    // сделать приоритет на барабан, который больше всего ждет

//    task->priority = 0;
//    switch (barrel_process) {
//        case PROCESS_RETURN_2:
//            break;
//    }

    return 0;
}


struct barrel makeBarrel(short flags, short zone, short timer) {
    struct barrel b;
    b.flags.raw_word = flags;
    b.zone = zone;
    b.software_timer = timer;
    b.time_degreasing = 12;
    b.time_washing_1a = 4;
    b.time_washing_1b = 6;
    b.time_etching = 16;
    b.time_washing_2a = 6;
    b.time_washing_2b = 8;
    b.time_galvanizing = 30;
    b.time_washing_3a = 8;
    b.time_washing_3b = 10;
    b.time_passivation = 3;
    b.time_washing_4a = 12;
    b.time_washing_4b = 14;
    return b;
}


static short scheduler_find_task(struct scheduler_task* tasks, const short curr_pos) {
    // ищем первый барабан слева, и ближайший справа
    short left = -1, right = -1;
    for (short i = 0; i < BARRELS_COUNT; i++) {
        short target = tasks[i].start_zone; // фактическая зона откуда тащить барабан
        if (tasks[i].dest_zone == -2) {
            target = 18;
        } else if (tasks[i].dest_zone < 0) {
            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 debug_print_robot1_code() {
//    printf("INFO: code length is %d\n", cmd_index);
    printf("Code for R0, B%d:\n", robot1_code.barrel_id);
    for (int i = 0; i < 16; i++) {
        const short cmd_arg = (short)(robot1_code.code[i] & (short)(~ROBOT_CMD_MASK));

        printf("    %3d: 0x%04X", i, robot1_code.code[i] & 0xFFFF);

        if ((robot1_code.code[i] & ROBOT_CMD_MASK) == ROBOT_CMD_END_code) {
            printf("    END\n");
            break;
        }

        switch ((short)(robot1_code.code[i] & (short)ROBOT_CMD_MASK)) {
            case ROBOT_CMD_MOVE_TO_ZONE_code:
                printf("    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:
                if (cmd_arg) {
                    printf("    move to offset pos\n");
                } else {
                    printf("    move to accurate pos\n");
                }
                break;

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

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

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

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

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

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

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

            default:
                printf("    UNKNOWN: 0x%04X\n", robot1_code.code[i] & 0xFFFF);
        }
    }
}


void schedule_robot_1() {
    // формируем список задач
    struct scheduler_task tasks[BARRELS_COUNT];
    for (short i = 0; i < BARRELS_COUNT; i++) {
        // для каждой задачи:
        tasks[i].start_zone = barrels[i].zone;
        // определяем можно ли ее выполнить и что вообще нужно выполнить
        tasks[i].dest_zone = can_move(barrels + i);
        if (tasks[i].dest_zone >= 0) {
            tasks[i].priority = get_operation_priority(i);
        }
    }

    // TODO добавить вставку lock-point'ов на задачах на несколько роботов
    // найти подходящую задачу
    if (schedulerOneRobotMode) {
        short target_task = scheduler_find_task(tasks, robot1.dx.current_zone);

        if (target_task >= 0) {
            // создаем код транзакции, пока обычный
            robot1_code.barrel_id = target_task;
            short cmd_index = 0;

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

            if (tasks[target_task].dest_zone == ZONE_PASSIVATION) {
                // пассивация, тут все просто
                if (robot1.dx.current_zone != tasks[target_task].start_zone) {
                    robot1_code.code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE(ZONE_WASHING_3B);
                }

                robot1_code.code[cmd_index++] = ROBOT_CMD_UP_WITH_BARREL();
                robot1_code.code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE_WITH_BARREL(ZONE_PASSIVATION);
                robot1_code.code[cmd_index++] = ROBOT_CMD_DOWN_WITH_BARREL();

                robot1_code.code[cmd_index++] = ROBOT_CMD_TMR_SET(barrels[target_task].time_passivation);

                robot1_code.code[cmd_index++] = ROBOT_CMD_UP_WITH_BARREL();
                robot1_code.code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE_WITH_BARREL(ZONE_WASHING_4A);
                robot1_code.code[cmd_index++] = ROBOT_CMD_DOWN_WITH_BARREL();

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

                robot1_code.code[cmd_index++] = ROBOT_CMD_UP_WITH_BARREL();

                // теперь надо определиться с тем, сколько ждать скапывания
                switch (tasks[target_task].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:
                        // время скапывания реактивов
                        // TODO добавить переменные времен скапывания
                        robot1_code.code[cmd_index++] = ROBOT_CMD_WAIT(30);
                        break;

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

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

                robot1_code.code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE_WITH_BARREL(tasks[target_task].dest_zone);

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

                robot1_code.code[cmd_index++] = ROBOT_CMD_DOWN_WITH_BARREL();

                if (tasks[target_task].dest_zone == 22) {
                    robot1_code.code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE(21);
                    // NOTE старая механика не позволяет просто опустить траверсу до конца, для новой изменить поведение
                    robot1_code.code[cmd_index++] = ROBOT_CMD_UP();
                    robot1_code.code[cmd_index++] = ROBOT_CMD_DOWN();
                } else {

                    if (tasks[target_task].dest_zone != 0) {

                        // установка времени ожидания барабана
                        short tmp = -1;
                        switch (tasks[target_task].dest_zone) {
                            case ZONE_DEGREASING:
                                tmp = barrels[target_task].time_degreasing;
                                break;

                            case ZONE_ETCHING_1:
                            case ZONE_ETCHING_2:
                                tmp = barrels[target_task].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[target_task].time_galvanizing;
                                break;

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

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

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

            robot1_code.code[cmd_index++] = ROBOT_CMD_END();
            robot1_code.PC = 0;

            printf("INFO: code length is %d\n", cmd_index);
            debug_print_robot1_code();
        }
    } else {
        printf("WARMING: support only one robot mode\n");
    }
}

// вернет false если удалось вставить барабан, иначе true
char create_barrel_in_load(short zone) {
    for (int i = 0; i < BARRELS_COUNT; i++) {
        if (!barrels[i].flags.is_exist) {
            barrels[i] = makeBarrel(1, zone, 0);
            return 0;
        }
    }
    return 1;
}

/*
=== ЦИКЛОГРАММА ПЕРЕТАСКИВАНИЯ БАРАБАНА ===
// NOTE первой командой на любую транзакцию должна стоять команда опустить траверсу (в 22 зоне мы никогда не закончим, за нее не беспокоится)
* опустить траверсу
если зона изъятия != промывка 3б
	если текущая зона != зона иъятия
		если зона изъятия == 22
			* встать на смещенную
			* поднять траверсу
			* съебать в 21 зону
			* встать на смещенную
			* опустить траверсу не до конца
			* съебать в 22 зону
		иначе
			* съебать в зону изъятия
	* поднять траверсу с барабаном
	если зона изъятия != 22 и зона изъятия != 1:
		* ждать скапывания (зависит от зоны)
	* ехать в зону назначения
	если зона назначения == 22
		* опустить траверсу не до конца с барабаном
		* съебать в 21 зону
		если ЭНКОДЕРЫ СТАРЫЕ (по умолчанию)
			* поднять траверсу
        * опустить траверсу
	иначе
		* опустить траверсу с барабаном
		если зона назначения != 0
            * установить время ожидания барабана (зависит от зоны)
иначе
	если текущая зона != промывка 3б
		* съебать в промывку 3б
	* поднять траверсу с барабаном
	* съебать в пассивацию
	* опустить траверсу с барабаном
	* поставить время ожидания барабана в <время пассивации>
	* поднять траверсу с барабаном
	* съебать в зону промывка 4а
	* опустить траверсу с барабаном
	* установить время ожидания барабана (для промывки 4а)
 */

void scheduler_main() {
    // тут должно быть удаление барабана из зоны 1, если он там есть
//    if (schedulerLoadButton1) {
//        schedulerLoadButton1 = create_barrel_in_load(0);
//    }

    // тут возможна только вставка барабанов
    if (schedulerLoadButton2) {
        schedulerLoadButton2 = create_barrel_in_load(1);
    }

    if (schedulerSoftwareTimer) {
        schedulerSoftwareTimer = 0;
        for (int i = 0; i < 10; i++) {
            if (barrels[i].software_timer > -9999) {
                barrels[i].software_timer--;
            }
        }
    }
    if (robot1_code.PC < 0) {
        schedule_robot_1();
    }

    // пока без второго робота
//    if (robot2_cmd.cmd == 0) {
//        robot2_cmd.cmd = 2; // пиздуем в зону
//        robot2_cmd.args[0] = rand() % 23;
//    }
}