#include "conveyorBeltControlUnit.h"

// 202行记得修改
// x86 无法运行

// 定义状态
const int MODE_INIT = 0;          // 初始状态
const int MODE_FRUN = 1;          // 正转运行状态
const int MODE_RRUN = 2;          // 反转运行状态
const int MODE_CLOSING_STEP1 = 3; // 进入关闭状态步骤1
const int MODE_CLOSING_STEP2 = 4; // 进入关闭状态步骤2
const int MODE_STOP = 5;
int ConveyorBeltControlUnit::getWaitCount() {
    return 2 * (0.8 / speed) / 0.1;
}

std::string ConveyorBeltControlUnit::modeToString() {
    switch (currentMode) {
    case MODE_INIT: return "MODE_INIT";
    case MODE_FRUN: return "MODE_FRUN";
    case MODE_RRUN: return "MODE_RRUN";
    case MODE_CLOSING_STEP1: return "MODE_CLOSING_STEP1";
    case MODE_CLOSING_STEP2: return "MODE_CLOSING_STEP2";
    case MODE_STOP: return "MODE_STOP";
    default: return "INVALID";
    }
}

void ConveyorBeltControlUnit::action(int event) {
    /* int destMode = modeSwitchMap[currentMode][event];
    if (destMode == currentMode) { return; }

    switch (destMode) {
    case MODE_INIT:
        digitalWrite(OUT_FRUN, OUT_OFF);
        digitalWrite(OUT_RRUN, OUT_OFF);
        break;
    case MODE_FRUN:
        digitalWrite(OUT_FRUN, OUT_ON);
        digitalWrite(OUT_RRUN, OUT_OFF);
        break;
    case MODE_RRUN:
        digitalWrite(OUT_FRUN, OUT_OFF);
        digitalWrite(OUT_RRUN, OUT_ON);
        break;
    case MODE_CLOSING_STEP1: break;
    case MODE_CLOSING_STEP2:
        digitalWrite(OUT_FRUN, OUT_OFF);
        digitalWrite(OUT_RRUN, OUT_ON);
        break;
    case MODE_STOP:
        digitalWrite(OUT_FRUN, OUT_OFF);
        digitalWrite(OUT_RRUN, OUT_OFF);
        workState = STANDBY;
        break;
    default: break;
    }
    currentMode = destMode;
    LOG(INFO) << GREEN << "now mode is " << modeToString() << EMPTY; */
}

void ConveyorBeltControlUnit::conveyorBeltWorkingProcess() {
   /*  workState = RUNNING;
    int waitCount = 0;
    int count = 0;
    while (workState == RUNNING) {
        waitCount = getWaitCount();
        if (currentMode == MODE_CLOSING_STEP1 || currentMode == MODE_CLOSING_STEP2) {
            count--;
            if (count < 0) { action(EVENT_RSENSOR_OBSTRUCT); }
        }
        else { count = waitCount; }

        if (current_start_signal == 1) {
            LOG(INFO) << YELLOW << "find a start signal" << EMPTY;
            current_start_signal = 0;
            action(EVENT_START_RISING);
        }

        if (digitalRead(INPUT_FSENSOR) == IN_LOGIC_1) {
            // LOG(INFO) << YELLOW << "find a Fsensor signal" << EMPTY;
            action(EVENT_FSENSOR_OBSTRUCT);
        }

        if (digitalRead(INPUT_RSENSOR) == IN_LOGIC_1) {
            // LOG(INFO) << YELLOW << "find a Rsensor signal" << EMPTY;
            action(EVENT_RSENSOR_OBSTRUCT);
        }

        if (current_stop_signal == 1) {
            LOG(INFO) << YELLOW << "find a stop signal" << EMPTY;
            LOG(INFO) << YELLOW << "wait count is " << waitCount << " last wait count left "
                       << count << EMPTY;
            current_stop_signal = 0;
            action(EVENT_STOP_PUSH);
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    } */
}

int ConveyorBeltControlUnit::init() {
    current_start_signal = 0;
    current_stop_signal = 0;
    currentMode = MODE_INIT;
    speed = 0;
    workState = STANDBY;
    return 0;
}

int ConveyorBeltControlUnit::setSpeed(float speed) {
    // if (workState == RUNNING)
    // {
    //     return -1;
    // }
    // 原本已经有的注释

    /* int regValue = speed * 62500;
    modbus_t* ctx = modbus_new_rtu("/dev/ttyUSB0", 9600, 'N', 8, 2);
    modbus_set_slave(ctx, 1);
    if (modbus_connect(ctx) == -1) {
        fprintf(stderr, "Connection failed:%s\n", modbus_strerror(errno));
        modbus_free(ctx);
        return -1;
    }
    LOG(INFO) << YELLOW << "set speed:" << speed << " set register:" << regValue << EMPTY;
    int result = modbus_write_register(ctx, 4096, regValue);
    if (result == -1) {
        fprintf(stderr, "write failed:%s\n", modbus_strerror(errno));
        modbus_free(ctx);
        return -1;
    }
    LOG(INFO) << YELLOW << "get result from com" << ":" << result << EMPTY;
    uint16_t* speedBuffer = (uint16_t*)malloc(sizeof(uint16_t));
    int getBytes = modbus_read_registers(ctx, 4096, 2, speedBuffer);
    LOG(INFO) << YELLOW << "get result from com : " << getBytes << " register:" << *speedBuffer
               << EMPTY;
    this->speed = speed;
    modbus_free(ctx); */
    return 0;
}

int ConveyorBeltControlUnit::setDirection(int direction) {
    if (workState == RUNNING) { return -1; }

    if (direction == 0) {
        LOG(INFO) << YELLOW << "set direction to foreward" << EMPTY;
        INPUT_FSENSOR = 28;
        INPUT_RSENSOR = 29;
        OUT_FRUN = 14;
        OUT_RRUN = 10;
    }
    else if (direction == 1) {
        LOG(INFO) << YELLOW << "set direction to reversal" << EMPTY;
        INPUT_FSENSOR = 29;
        INPUT_RSENSOR = 28;
        OUT_FRUN = 10;
        OUT_RRUN = 14;
    }
    else { return -1; }
    return 0;
}

int ConveyorBeltControlUnit::startConveyorBelt() {
    current_start_signal = 1;
    return 0;
}

int ConveyorBeltControlUnit::stopConveyorBelt() {
    current_stop_signal = 1;
    return 0;
}

int ConveyorBeltControlUnit::getWorkState() {
    if (workState == RUNNING) { return 0; }
    else { return -1; }
}

ConveyorBeltControlUnit::ConveyorBeltControlUnit(/* args */) {
    modeSwitchMap[MODE_INIT]
        = {{EVENT_START_RISING, MODE_FRUN},
           {EVENT_FSENSOR_OBSTRUCT, MODE_INIT},
           {EVENT_RSENSOR_OBSTRUCT, MODE_INIT},
           {EVENT_STOP_PUSH, MODE_INIT}};
    modeSwitchMap[MODE_FRUN]
        = {{EVENT_START_RISING, MODE_FRUN},
           {EVENT_FSENSOR_OBSTRUCT, MODE_RRUN},
           {EVENT_RSENSOR_OBSTRUCT, MODE_FRUN},
           {EVENT_STOP_PUSH, MODE_CLOSING_STEP1}};
    modeSwitchMap[MODE_RRUN]
        = {{EVENT_START_RISING, MODE_RRUN},
           {EVENT_FSENSOR_OBSTRUCT, MODE_RRUN},
           {EVENT_RSENSOR_OBSTRUCT, MODE_FRUN},
           {EVENT_STOP_PUSH, MODE_CLOSING_STEP2}};
    modeSwitchMap[MODE_CLOSING_STEP1]
        = {{EVENT_START_RISING, MODE_CLOSING_STEP1},
           {EVENT_FSENSOR_OBSTRUCT, MODE_CLOSING_STEP2},
           {EVENT_RSENSOR_OBSTRUCT, MODE_STOP},
           {EVENT_STOP_PUSH, MODE_CLOSING_STEP1}};
    modeSwitchMap[MODE_CLOSING_STEP2]
        = {{EVENT_START_RISING, MODE_CLOSING_STEP2},
           {EVENT_FSENSOR_OBSTRUCT, MODE_CLOSING_STEP2},
           {EVENT_RSENSOR_OBSTRUCT, MODE_STOP},
           {EVENT_STOP_PUSH, MODE_CLOSING_STEP2}};
// x86 无法运行
/*     wiringPiSetup();
    pinMode(BTN_START, INPUT);
    pinMode(BTN_DIRSWITCH, INPUT);
    pinMode(BTN_STOP, INPUT);
    pinMode(INPUT_FSENSOR, INPUT);
    pinMode(INPUT_RSENSOR, INPUT);
    pullUpDnControl(BTN_START, PUD_UP);
    pullUpDnControl(BTN_DIRSWITCH, PUD_UP);
    pullUpDnControl(BTN_STOP, PUD_UP);
    pullUpDnControl(INPUT_FSENSOR, PUD_UP);
    pullUpDnControl(INPUT_RSENSOR, PUD_UP);
    pinMode(OUT_RRUN, OUTPUT);
    pinMode(OUT_FRUN, OUTPUT);
    digitalWrite(OUT_FRUN, OUT_OFF);
    digitalWrite(OUT_RRUN, OUT_OFF); */

}

ConveyorBeltControlUnit::~ConveyorBeltControlUnit() {}