#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <unistd.h>
#include <time.h>
#include <math.h>
#include <pthread.h>
#include "ecat/zecm_zh.h"

////////////////////////////////////////////////////////////////////////////////
#define _MSG_(flt,fmt,args...)  printf(flt ": %s<%d>: " fmt "\n", __PRETTY_FUNCTION__, __LINE__, ##args)
#define _INF_(fmt,args...)  _MSG_("INF", fmt, ##args)
#define _WRN_(fmt,args...)  _MSG_("WRN", fmt, ##args)
#define _ERR_(fmt,args...)  _MSG_("ERR", fmt, ##args)
#define _DBG_(fmt,args...)  _MSG_("DBG", fmt, ##args)
#define BREAK_IF_FAILED(ret, expr)  { ret = (expr); if (ECAT_FAILED(ret)) { _ERR_(#expr " failed: err=%d", ret); break; } else { _INF_(#expr " succeeded"); } }
#define LOG_RET(ret, expr)          { ret = (expr); if (ECAT_FAILED(ret)) { _ERR_(#expr " failed: err=%d", ret);        } else { _INF_(#expr " succeeded"); } }
#define ARRAY_SIZE(a)  (sizeof(a) / sizeof(a[0]))
#define BIT_STR(v,i)  ((v).b.i ? (" " #i) : "")

////////////////////////////////////////////////////////////////////////////////
// 全局配置参数
#define ECAT_ETH0           "zecm0"
#define RPC_SERVER_PORT     5000                // 开放与上位机通讯的端口号
#define NUM_SERVOS          1                   // 总电机数
#define NUM_DO              1  
#define NUM_DI              1

#define LED_GREEN           0                   // 绿灯
#define LED_RED             1                   // 红灯

#define BTN_CLOSE           0                   // 接近传感器
#define BTN_RUN             1                   // 绿色按钮
#define BTN_STOP            2                   // 红色按钮

enum {
	OP_MODE_PP = 1,
	OP_MODE_PV = 3,
	OP_MODE_PT = 4,
	OP_MODE_HM = 6,
	OP_MODE_CSP = 8,
	OP_MODE_CSV = 9,
	OP_MODE_CST = 10,
};
uint32_t g_op_mode = OP_MODE_CSP;
uint32_t g_run_task = 1;

////////////////////////////////////////////////////////////////////////////////
// 过程映像
#pragma pack(push, 1)

typedef union {
    struct {
        uint16_t switch_on: 1;
        uint16_t enable_voltage: 1;
        uint16_t quick_stop: 1;
        uint16_t enable_operation: 1;
        uint16_t pad0: 3;
        uint16_t fault_reset: 1;
        uint16_t halt: 1;
        uint16_t pad1: 1;
        uint16_t pad2: 6;
    } b;
    uint16_t v;
} servo_ctrl_word;

typedef union {
    struct {
        uint16_t ready_to_switch_on: 1;
        uint16_t switched_on: 1;
        uint16_t operation_enabled: 1;
        uint16_t fault: 1;
        uint16_t voltage_enabled: 1;
        uint16_t quick_stop: 1;
        uint16_t switch_on_disabled: 1;
        uint16_t warning: 1;
        uint16_t pad0: 1;
        uint16_t remote: 1;
        uint16_t target_reached: 1;
        uint16_t internal_limit_active: 1;
        uint16_t pad1: 4;
    } b;
    uint16_t v;
} servo_stat_word;

typedef struct {
    servo_ctrl_word ctrl;
    int32_t p;
    uint8_t pad[6];
} servo_ctrl;

typedef struct {
    servo_stat_word stat;
    int32_t p;
    uint16_t error;
    uint32_t inputs;
} servo_stat;

typedef struct {
    servo_ctrl servos[NUM_SERVOS];
    uint16_t outputs[NUM_DO];
} proc_img_o;

typedef struct {
    servo_stat servos[NUM_SERVOS];
    uint16_t inputs[NUM_DI];
} proc_img_i;

#pragma pack(pop)

////////////////////////////////////////////////////////////////////////////////
// 伺服驱动
typedef struct {
    servo_ctrl_word sc;    // 新控制字
    servo_stat_word ss;    // 旧状态字
    int run_delay;         // 延时启动
    int fault_reset_delay; // 延时再错误恢复
    int32_t out_p;         // 输出位置
    int32_t out_v;         // 输出转速
    int16_t out_t;         // 输出扭矩
    bool reverse;          // 当前方向
} servo_t;

bool servo_control(servo_t *servo, int axis, servo_stat_word *s1, bool show)
{
    // 402状态机处理
    do {
        servo->sc.v = 0;
        servo->sc.b.enable_voltage = 1;
        servo->sc.b.quick_stop = 1;
        servo->sc.b.enable_operation = s1->b.switched_on || s1->b.operation_enabled;
        servo->sc.b.switch_on = s1->b.ready_to_switch_on;

        // 伺服状态字检测到错误
        if (s1->b.fault && !servo->fault_reset_delay) {
            // 延时1秒再尝试错误恢复，防止伺服在错误和恢复中反复快速切换损坏电路
            _DBG_("axis %d: delay fault reset", axis);
            servo->fault_reset_delay = 1000; // 1s(即1000个周期)
            servo->run_delay = 2000; // 当错误恢复成功伺服出力后，延时2s再开始加速(根据抱闸动作时间适当调整)
            break;
        }

        // 伺服发生错误，且在延时等待处理中
        if (servo->fault_reset_delay) {
            servo->fault_reset_delay--;
            if (!servo->fault_reset_delay) {
                // 延时减到0，尝试进行错误恢复
                _DBG_("axis %d: start fault reset", axis);
                servo->sc.b.fault_reset = 1;
            }
            break;
        }
    } while (0);

    // 忽略一些状态以减少log
    s1->b.target_reached = 0;
    s1->b.pad0 = 0;
    s1->b.pad1 = 0;

    if (servo->ss.v != s1->v)
        show = true;
    if (show) {
        _DBG_("+++ axis=%d", axis);
        _DBG_("old_stat:%s%s%s%s%s%s%s%s%s%s%s%s%s",
            BIT_STR(servo->ss, ready_to_switch_on),
            BIT_STR(servo->ss, switched_on),
            BIT_STR(servo->ss, operation_enabled),
            BIT_STR(servo->ss, fault),
            BIT_STR(servo->ss, voltage_enabled),
            BIT_STR(servo->ss, quick_stop),
            BIT_STR(servo->ss, switch_on_disabled),
            BIT_STR(servo->ss, warning),
            BIT_STR(servo->ss, pad0),
            BIT_STR(servo->ss, remote),
            BIT_STR(servo->ss, target_reached),
            BIT_STR(servo->ss, internal_limit_active),
            BIT_STR(servo->ss, pad1)
        );
        _DBG_("new_stat:%s%s%s%s%s%s%s%s%s%s%s%s%s",
            BIT_STR(*s1, ready_to_switch_on),
            BIT_STR(*s1, switched_on),
            BIT_STR(*s1, operation_enabled),
            BIT_STR(*s1, fault),
            BIT_STR(*s1, voltage_enabled),
            BIT_STR(*s1, quick_stop),
            BIT_STR(*s1, switch_on_disabled),
            BIT_STR(*s1, warning),
            BIT_STR(*s1, pad0),
            BIT_STR(*s1, remote),
            BIT_STR(*s1, target_reached),
            BIT_STR(*s1, internal_limit_active),
            BIT_STR(*s1, pad1)
        );
        _DBG_("ctrl:%s%s%s%s%s%s%s%s%s",
            BIT_STR(servo->sc, switch_on),
            BIT_STR(servo->sc, enable_voltage),
            BIT_STR(servo->sc, quick_stop),
            BIT_STR(servo->sc, enable_operation),
            BIT_STR(servo->sc, pad0),
            BIT_STR(servo->sc, fault_reset),
            BIT_STR(servo->sc, halt),
            BIT_STR(servo->sc, pad1),
            BIT_STR(servo->sc, pad2)
        );
        _DBG_("---");
    }
    servo->ss.v = s1->v;

    return s1->b.operation_enabled;
}

////////////////////////////////////////////////////////////////////////////////
// 主站应用
typedef struct {
    ECAT_SIMPLE_START_CONTEXT ctx;
    servo_t servos[NUM_SERVOS];
} ecat_master_t;

typedef struct {
    ECAT_WORD slave;
    ECAT_WORD index;
    ECAT_BYTE subindex;
    ECAT_DWORD len;
    ECAT_DWORD val;
} ecat_sdo_t;

bool ecat_master_request_state(ecat_master_t *master, ECAT_BYTE state, int retry)
{
    ECAT_RESULT ret;
    ECAT_BYTE query = EcatStateNotSet;

    LOG_RET(ret, EcatRequestMasterState(master->ctx.Master, state));
    if (ECAT_FAILED(ret)) return false;

    while (retry--) {
        LOG_RET(ret, EcatGetMasterState(master->ctx.Master, &query));
        if (ECAT_FAILED(ret)) break;
        _DBG_("request_state=%d, query_state=%d", state, query);
        if (query == state) return true;
        usleep(1000000);
    }
    return false;
}

bool ecat_master_config(ecat_master_t *master)
{
    ECAT_RESULT ret = ECAT_S_OK;
    ECAT_DWORD len;
    unsigned i;
    ecat_sdo_t items[] = {
        { 0, 0x6060, 0x00, 1, g_op_mode }, // 控制模式
    };
    
    for(uint16_t s = 0; s < NUM_SERVOS; s++ )
    {
        for (i = 0; i < ARRAY_SIZE(items); i++) 
        {
            ecat_sdo_t *item = &items[i];
            len = item->len;
            item->slave = s+3; // 电机前面接了3个模块
            ret = EcatCoeSDODownload(
                master->ctx.Master,
                item->slave,
                item->index,
                item->subindex,
                ECAT_FALSE,
                &len,
                (ECAT_BYTE*)&item->val,
                len,
                5000);
            if (ECAT_FAILED(ret)) {
                _DBG_("EcatCoeSDODownload failed: ret=%d, slave=%d, index=0x%04x, subindex=0x%02x, value=0x%08x(%d)", ret, item->slave, item->index, item->subindex, item->val, item->val);
                return ret;
            }
            _DBG_("EcatCoeSDODownload succeeded: slave=%d, index=0x%04x, subindex=0x%02x, value=0x%08x(%d)", item->slave, item->index, item->subindex, item->val, item->val);
        }
    }

    return ret;
}

static ECAT_RESULT ecat_master_task(ecat_master_t *m, proc_img_i *pi, proc_img_o *po)
{
    static uint64_t loops = 0;
    static int btn_run = 0;
    static int btn_stop = 0;
    static int btn_close = 0;

    static int run_cmd = 0;
    static int run[NUM_SERVOS] = {0};
    static int stop_cmd = 0;
    static int halt_cmd = 0;

    loops++;

    // 绿色按钮按下时，才启动
    btn_run = !((pi->inputs[0] >> BTN_RUN) & 0x1); // 输入低有效
    if(btn_run) {
        run_cmd = 1;
        stop_cmd = 0;
        halt_cmd = 0;
    }

    // 红色按钮按下，电机停止运行
    btn_stop = !((pi->inputs[0] >> BTN_STOP) & 0x1); // 输入低有效
    if(btn_stop) {
        stop_cmd = 1;
        run_cmd = 0;
    }
    else {
        stop_cmd = 0;
    }

    // 接近传感器
    btn_close = !((pi->inputs[0] >> BTN_CLOSE) & 0x1); // 输入低有效
    if(btn_close) {
        halt_cmd = 1;
        run_cmd = 0;
    }

    // 绿灯状态
    if(run_cmd) {
        // 常亮
        po->outputs[0] |= (1 << LED_GREEN);
    }
    else {
        // 闪烁 2Hz
        if (loops % 500 == 0) {
            po->outputs[0] ^= (1 << LED_GREEN);
        }
    }

    if(halt_cmd) {
        po->outputs[0] |= (1 << LED_RED);       // 红灯常亮
        po->outputs[0] &= ~(1 << LED_GREEN);    // 绿灯熄灭
    }
    else {
        po->outputs[0] &= ~(1 << LED_RED);      // 红灯熄灭
    }

    // 处理各轴输入输出变量
    for (int i = 0; i < NUM_SERVOS; i++) {
        if (loops % 1000 == 0) {
            _DBG_("servo[%d]: p=%10d", i, pi->servos[i].p);
        }

        if(halt_cmd | stop_cmd) {
            // 电机halt
            po->servos[i].ctrl.b.halt = 1;
        }
        else {
            po->servos[i].ctrl.b.halt = 0;
        }
        
        do {
            servo_t *servo = &m->servos[i];
            if(!run_cmd || (i > 0 && run[i-1] == 0)) {
                break;
            }
            // 根据驱动器状态更新控制字
            run[i] = servo_control(servo, i, &pi->servos[i].stat, false);

            // 伺服未出力，或已出力但延时未结束，应停止转动
            if (!run[i] || servo->run_delay != 0) {
                servo->out_p = pi->servos[i].p;
                servo->out_v = 0;
                servo->out_t = 0;
            }

            // 伺服已出力，延时计数递减
            if (run[i] && servo->run_delay)
                servo->run_delay--;

            // 伺服已出力，延时结束，进行正常控制
            if (run[i] && !servo->run_delay) {

                switch (g_op_mode) {
                case OP_MODE_CSP:
                {
                    // 不同轴单位可能不一样，下表定义各轴位置范围及增量
                    int32_t range_min[NUM_SERVOS] = { -50000 };
                    int32_t range_max[NUM_SERVOS] = { 50000 };
                    int32_t step_size[NUM_SERVOS] = { 50 };
                    if (servo->reverse  && servo->out_p < range_min[i]) servo->reverse = false;
                    if (!servo->reverse && servo->out_p > range_max[i]) servo->reverse = true;
                    servo->out_p += (servo->reverse ? -1 : 1) * step_size[i];
                }
                break;
                }
            }

            // 更新输出变量
            po->servos[i].ctrl = servo->sc;
            switch (g_op_mode)
            {
            case OP_MODE_CSP: po->servos[i].p = servo->out_p; break;
            }
        }while(0);
    }
    return 0;
}

////////////////////////////////////////////////////////////////////////////////
// 主函数
int main(int argc, char **argv)
{
    ECAT_RESULT ret;
    ecat_master_t m;

    // 命令参数
    if (argc > 2) g_run_task = atoi(argv[2]);
    if (g_op_mode < OP_MODE_CSP || g_op_mode > OP_MODE_CST)
        g_op_mode = OP_MODE_CSP;
    _DBG_("OP_MODE=%d, RUN_TASK=%d", g_op_mode, g_run_task);

    // 将当前线程转为实时线程，优先级为50
    struct sched_param sched;
    sched.sched_priority = 50;
    pthread_setschedparam(pthread_self(), SCHED_FIFO, &sched);

    // 初始化主站环境
    LOG_RET(ret, EcatMasterLibInit(ECAT_NULL));
    if (ECAT_FAILED(ret)) return -1;

    // 启动主站
    memset(&m, 0, sizeof(m));
    m.ctx.NIC0 = ECAT_ETH0;
    m.ctx.ENI = "test.xml";
    m.ctx.MainCycleTimeUs = 0;
    m.ctx.SubCycleTimeUs = 0;
    m.ctx.AutoRecoveryTimeoutMs = 0;
    m.ctx.RpcServerPort = RPC_SERVER_PORT;
    ret = EcatSimpleStart(&m.ctx);
    if (ECAT_FAILED(ret)) {
        _ERR_("EcatSimpleStart failed: ret=%d", ret);
        return -1;
    }

    // 不进入主循环
    if (g_run_task) {
        // 进入操作状态，设置回调控制电机
        if (!ecat_master_request_state(&m, EcatStateO, 30)) {
            _ERR_("ecat_master_request_state(EcatStateO) failed!");
            return -1;
        }

        // 配置电机寄存器
        if (ecat_master_config(&m)) {
            _ERR_("ecat_master_config failed!");
            return -1;
        }

        // 设置PDO回调
        EcatSetExtCtrlTaskHandler(m.ctx.Master, (EcatExtCtrlTaskHandler)ecat_master_task, &m);
    } else {
        // 进入初始状态，用于上位机进行描述设备
        if (!ecat_master_request_state(&m, EcatStateI, 30)) {
            _ERR_("ecat_master_request_state(EcatStateI) failed!");
            return -1;
        }
    }

    // 等待
    while (getchar() != 'e') usleep(50000);
    
    // 退出
    LOG_RET(ret, EcatSimpleStop(&m.ctx));   
    LOG_RET(ret, EcatMasterLibExit());
    return 0;
}

