#include "liftlibcontrol.h"
#include "liftlibio.h"

/* Global variables */
int lift_levelPos[16];
int lift_one_level;

/**
   Is the counter valid for level positioning?
*/

// Wasm loop bounds

__attribute__((import_module("__pragma"), import_name("loopbound"))) extern void
__pragma_loopbound(unsigned int min_bound, unsigned int max_bound);

int lift_cntValid;

/**
   Position absolute or relative<.
*/
int lift_cnt;

/**
   Last stoped level (1..13) if position is absolute else 0.
*/
int lift_level;

/**
   load position in level, 0 means we don't know
*/
int lift_loadLevel;

/**
   we're going TOP or BOTTOM, but stop at load position.
*/
int lift_loadPending;

/**
   we're waiting for the load sensor to go.
*/
int lift_loadSensor;

/**
   cmd keeps the value of the command until the command is finished.
   It is only updated by the switches if it's current value is CMD_NONE.
*/
int lift_cmd;

int lift_timMotor;

int lift_timImp;

/**
   Remember last direction for impuls count after motor off;
*/
int lift_directionUp;

/**
   Last value of impuls sensor.
*/
int lift_lastImp;

int lift_dbgCnt;

/**
   stop value for the counter.
*/
int lift_endCnt;

__attribute__((always_inline)) static inline void
lift_ctrl_init() {
    int i;
    lift_checksum = 0;

    lift_io_init();
    lift_cntValid = 0;
    lift_cnt = 0;
    lift_cmd = lift_CMD_NONE;
    lift_timMotor = 0;
    lift_timImp = 0;
    lift_directionUp = 1;
    lift_lastImp = 0;
    lift_loadLevel = 0;
    lift_loadPending = 0;
    lift_loadSensor = 0;
    i = 0;
    lift_levelPos[i++] = 0;
    lift_levelPos[i++] = 58;
    lift_levelPos[i++] = 115;
    lift_levelPos[i++] = 173;
    lift_levelPos[i++] = 230;
    lift_levelPos[i++] = 288;
    lift_levelPos[i++] = 346;
    lift_levelPos[i++] = 403;
    lift_levelPos[i++] = 461;
    lift_levelPos[i++] = 518;
    lift_levelPos[i++] = 576;
    lift_levelPos[i++] = 634;
    lift_levelPos[i++] = 691;
    lift_levelPos[i++] = 749;
    lift_levelPos[i++] = 806;
    lift_levelPos[i++] = 864;
    lift_one_level = lift_levelPos[1];
}

__attribute__((always_inline)) static inline void
lift_ctrl_loop() {
    if (lift_cmd == lift_CMD_NONE)
        lift_check_cmd();
    else {
        lift_do_impulse(lift_ctrl_io_in[lift_SENS_IMPULS],
                        lift_ctrl_io_out[lift_MOTOR_ON],
                        lift_ctrl_io_in[lift_SENS_BOTTOM]);
        lift_do_cmd();
    }
    lift_check_level();
    lift_ctrl_io_led[13] = (lift_dbgCnt & 0x80) != 0;
    ++lift_dbgCnt;
}

__attribute__((always_inline)) static inline void
lift_check_level() {
    int i;
    int middle = lift_one_level >> 2;
    if (lift_cntValid) {
        __pragma_loopbound(14, 14);
        for (lift_level = 1; lift_level < 14; ++lift_level) {
            if (lift_cnt < lift_levelPos[lift_level] - middle)
                break;
        }
    } else
        lift_level = 0;
    __pragma_loopbound(14, 14);
    for (i = 0; i < 14; ++i)
        lift_ctrl_io_led[i] = (i == lift_level - 1);
}

__attribute__((always_inline)) static inline void
lift_check_cmd() {
    if (lift_loadPending) {
        if (lift_ctrl_io_in[lift_SENS_BOTTOM])
            lift_cmd = lift_CMD_TOP;
    } else if (lift_ctrl_io_in[lift_GO_UP]) {
        if (!lift_ctrl_io_in[lift_SENS_TOP] && lift_level != 14)
            lift_cmd = lift_CMD_UP;
    } else if (lift_ctrl_io_in[lift_GO_DOWN]) {
        if (!lift_ctrl_io_in[lift_SENS_BOTTOM] && lift_level != 1)
            lift_cmd = lift_CMD_DOWN;
    } else if (lift_ctrl_io_in[lift_GO_LOAD]) {
        if (lift_loadLevel != 0 && lift_level < lift_loadLevel)
            lift_cmd = lift_CMD_TOP;
        else
            lift_cmd = lift_CMD_BOTTOM;
        lift_loadPending = 1;
        lift_loadSensor = 0;
    } else if (lift_ctrl_io_in[lift_GO_TOP]) {
        if (!lift_ctrl_io_in[lift_SENS_TOP])
            lift_cmd = lift_CMD_TOP;
    } else if (lift_ctrl_io_in[lift_GO_BOTTOM]) {
        if (!lift_ctrl_io_in[lift_SENS_BOTTOM])
            lift_cmd = lift_CMD_BOTTOM;
    }
    if (lift_cmd != lift_CMD_NONE)
        lift_timMotor = 50;
}

__attribute__((always_inline)) static inline void
lift_do_impulse(int val, int motor, int reset) {
    if (val && !lift_lastImp) {
        if (motor || lift_timImp > 0) {
            if (lift_directionUp)
                ++lift_cnt;
            else
                --lift_cnt;
        }
    }
    if (reset) {
        lift_cnt = 0;
        lift_cntValid = 1;
    }
    lift_lastImp = val;
    if (lift_timImp > 0) {
        --lift_timImp;
        if (lift_timImp == 0 && lift_cmd != lift_CMD_NONE)
            lift_cmd = lift_CMD_NONE;
    }
}

__attribute__((always_inline)) static inline void
lift_do_cmd() {
    int run = 0;
    if (lift_timMotor > 0)
        lift_wait_for_motor_start();
    else {
        run = lift_check_run();
        if (lift_ctrl_io_out[lift_MOTOR_ON] && !run) {
            /* motor stopped: */
            lift_cmd = 99;
            lift_timImp = 50;
        }
        lift_ctrl_io_out[lift_MOTOR_ON] = run;
    }
}

__attribute__((always_inline)) static inline void
lift_wait_for_motor_start() {
    int newLevel = 0;
    --lift_timMotor;
    lift_directionUp = (lift_cmd == lift_CMD_UP || lift_cmd == lift_CMD_TOP);
    lift_ctrl_io_out[lift_MOTOR_UP] = lift_directionUp;
    if (!lift_cntValid) {
        lift_cnt = 0; /* use relative counter */
        if (lift_cmd == lift_CMD_UP)
            lift_endCnt = lift_one_level;
        else
            lift_endCnt = -lift_one_level;
    } else {
        lift_endCnt = lift_cnt;
        newLevel = -99;
        if (lift_cmd == lift_CMD_UP)
            newLevel = lift_level + 1;
        else if (lift_cmd == lift_CMD_DOWN)
            newLevel = lift_level - 1;
        --newLevel; /* lift_level is one based */
        if (newLevel >= 0 && newLevel < 14)
            lift_endCnt = lift_levelPos[newLevel];
    }
}

__attribute__((always_inline)) static inline int
lift_check_run() {
    if (lift_cmd == lift_CMD_UP) {
        if (lift_cnt < lift_endCnt - 1 && !lift_ctrl_io_in[lift_SENS_TOP])
            return 1;
    } else if (lift_cmd == lift_CMD_DOWN) {
        if (lift_cnt > lift_endCnt + 1 && !lift_ctrl_io_in[lift_SENS_BOTTOM])
            return 1;
    } else if (lift_cmd == lift_CMD_TOP) {
        if (lift_loadPending && lift_ctrl_io_in[lift_SENS_LOAD]) {
            /* we are at lift_load position */
            lift_loadLevel = lift_level;
            lift_loadPending = 0;
            return 0;
        }
        if (!lift_ctrl_io_in[lift_SENS_TOP])
            return 1;
        /* safe fallback if lift_load sensor does not work */
        lift_loadPending = 0;
    } else if (lift_cmd == lift_CMD_BOTTOM) {
        if (lift_loadPending) {
            if (lift_loadSensor) {
                if (!lift_ctrl_io_in[lift_SENS_LOAD]) {
                    lift_loadSensor = 0;
                    /* we are at lift_load position */
                    lift_loadPending = 0;
                    lift_loadLevel = lift_level;
                    return 0;
                }
            }
            lift_loadSensor = lift_ctrl_io_in[lift_SENS_LOAD];
        }
        if (!lift_ctrl_io_in[lift_SENS_BOTTOM])
            return 1;
    }
    return 0;
}