#include <stdio.h>

#include "pci_errno.h"
#include "pci_zecm.h"
#include "pci_dbg.h"

#ifdef VXWORKS
#define main SYNC_CONTROL_TEST
#endif

#define RETURN_IF_FAILED(expr)      { int ret = (expr); if (ret != 0) { _ERR_(#expr " failed: err=%d", ret); return -1; } else { _INF_(#expr " succeeded"); } }

/**************************motor config*************************/
#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   // 红色按钮

typedef  uint32_t   U32;
typedef  uint16_t   U16;
typedef  uint8_t    U8;
typedef  int32_t    I32;
typedef  int16_t    I16;

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,
};
static U32 g_op_mode = OP_MODE_CSP;
static U32 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; // 延时再错误恢复
    float zero_p;          // 零点位置
    float offset_p;        // 偏移输出位置
    I32 out_p;             // 输出位置
    I32 out_v;             // 输出转速
	I16 out_t;             // 输出扭矩
	I32 out_acc;             // 输出扭矩
	I32 out_dec;             // 输出扭矩
    bool reverse;          // 当前方向
}servos_t;

static servos_t __servos[NUM_SERVOS] = {0};

#ifdef _WIN32
#include <windows.h>
void usleep(int64_t usec){
    HANDLE timer;
    LARGE_INTEGER ft;
    ft.QuadPart = -(10*usec);
    timer = CreateWaitableTimer(NULL, TRUE, NULL);
    SetWaitableTimer(timer, &ft, 0, NULL, NULL, 0);
    WaitForSingleObject(timer, INFINITE);
    CloseHandle(timer);
}
#else
#include <unistd.h>
#endif

/************************ motor config end ***************************/
static bool motor_control(int axis, servo_stat_word *s0, const servo_stat_word s1, servo_ctrl_word *c, bool show)
{
    servos_t *servo = &__servos[axis];
    do {
        c->v = 0;
        c->b.enable_voltage = 1;
        c->b.quick_stop = 1;
        c->b.enable_operation = s1.b.switched_on || s1.b.operation_enabled;
        c->b.switch_on = s1.b.ready_to_switch_on;

        if (s1.b.fault && !servo->fault_reset_delay) 
        {
            // 延时1秒再尝试错误恢复，防止伺服在错误和恢复中反复快速切换损坏电路
            _DBG_("axis %d: delay fault reset, ctrl:%d, stat:%x", axis, c->v, (int)s1.v);
            servo->fault_reset_delay = 2000; // 2s(即2000个周期)
            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);
                c->b.fault_reset = 1;
            }
            break;
        }

    } while (0);
    
    s0->v = s1.v;

    return s1.b.operation_enabled;
}

typedef struct {
	int16_t slave;
	int16_t index;
	int8_t subindex;
	int32_t len;
	void* val;
} ecat_sdo_t;

static int motor_config(ECAT_HANDLE ecat_dev)
{
	_INF_("motor_config Enter\r\n");
	ECAT_RESULT ret = ECAT_S_OK;
	uint32_t len;
	unsigned i;
	int cnt = 0;
	ecat_sdo_t items[] = {
		{ 0, 0x6060, 0x00, 1, &g_op_mode }, // 控制模式
	};
	for (uint16_t s = 0; s < NUM_SERVOS; s++)
	{
		_INF_("motor_config slave: %d\r\n", s + 1);

		for (i = 0; i < sizeof(items) / sizeof(ecat_sdo_t); i++)
		{
			_INF_("ecat_sdo_t: %d\r\n", i);
			ecat_sdo_t *item = &items[i];
			len = item->len;
			item->slave = s + 3; // 前面接了3个电机
			_INF_("ecat_sdo_t line: %d\r\n", __LINE__);
			ret = EcatCoeSDODownload(
				ecat_dev,
				item->slave,
				item->index,
				item->subindex,
				len,
				(void*)item->val,
				5000);
			_INF_("ecat_sdo_t line: %d\r\n", __LINE__);
			if (ret != ECAT_S_OK) {
				_DBG_("EcatCoeSDODownload failed: ret=%d, slave=%d, index=0x%04x, subindex=0x%02x, value=0x%08x(%d)", ret, item->slave, item->index, item->subindex, *(uint16_t*)item->val, *(uint16_t*)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, *(uint16_t*)item->val, *(uint16_t*)item->val);
		}
	}
	return ret;
}

int main(int argc, char* argv[])
{
    char index_buff[10];
    uint32_t index = 0;
    uint64_t loops = 0;
    uint32_t channel = 0;
    char *eni_file;
    ECAT_HANDLE ecat_dev;

    int btn_run = 0;
    int btn_stop = 0;
    int btn_close = 0;

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

    if (argc != 4)
    {
       _INF_("usage: ./program encoder_id chnnal eni.xml\r\n");
       return 0;
    }

    index = atoi(argv[1]);
    channel = atoi(argv[2]);
    if(channel > 1) channel = 1;
    eni_file = argv[3];
  
    //PCIe-2E 只有一个通道,PCIe-4E有两个通道
    RETURN_IF_FAILED(EcatOpen(&ecat_dev, BOARD_ALIAS(index_buff, index), channel));
    
    RETURN_IF_FAILED(EcatBusRun(ecat_dev, eni_file));

    // 请求切换状态
    uint8_t state;
    RETURN_IF_FAILED(EcatRequestMasterState(ecat_dev, EcatStateO));
    for (uint32_t i = 0; ; i++)
    {
        RETURN_IF_FAILED(EcatGetMasterState(ecat_dev, &state));
        _INF_("request is: 8, curr is: %d", state);
		usleep(500*1000);
        if (state == EcatStateO) break;
    }

	_INF_("EcatRequestMasterState done");

	RETURN_IF_FAILED(motor_config(ecat_dev));

    // 清空并预计填入数据
    proc_img_i *pi;
    proc_img_o *po;
    RETURN_IF_FAILED(EcatPINMap(ecat_dev, PI_AREA_LOCAL_INPUT, (void **)&pi));
	_INF_("EcatPINMap done");

    RETURN_IF_FAILED(EcatPINMap(ecat_dev, PI_AREA_LOCAL_OUTPUT, (void **)&po));
	_INF_("EcatPINMap done");

	_INF_("pi: 0x%llx", pi);
	_INF_("po: 0x%llx", po);

    for (uint32_t i = 0; i < 2; i++)
    {
        EcatPIOutputQueuePush(ecat_dev, false, 100);
    }
	_INF_("EcatPINMap done");

    RETURN_IF_FAILED(EcatPIEnable(ecat_dev));

    while (1)
    {
        while (EcatPIInputQueuePop(ecat_dev, false, 100) != 0);

        // 绿色按钮按下时，才启动
        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);      // 红灯熄灭
        }

        loops++;
        for (int i = 0; i < NUM_SERVOS; i++)
        {
            if(halt_cmd | stop_cmd) {
                // 电机halt
                po->servos[i].ctrl.b.halt = 1;
            }
            else {
                po->servos[i].ctrl.b.halt = 0;
            }

            do {
                servos_t *servo = &__servos[i];
                if(!run_cmd || (i > 0 && run[i-1] == 0)) {
                    break;
                }

                run[i] = motor_control(i, &servo->ss, pi->servos[i].stat, &servo->sc, false);

                if (pi->servos[i].stat.b.fault) { // 检测到报错
                    _DBG_("Motor fault, servo[%d] error code = 0x%04X", i, pi->servos[i].error);
                }

                if (loops % 1000 == 0) 
                {
                    _INF_("servo[%d]: p=%10d, ctrl:%08x stat=%08x", i, pi->servos[i].p, po->servos[i].ctrl.v, pi->servos[i].stat.v);
                }

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

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

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

                    switch (g_op_mode)
                    {
                    case OP_MODE_CSP:
                        {
                            // 不同轴单位可能不一样，下表定义各轴位置范围及增量
                            I32 range_min[] = { -50000 };
                            I32 range_max[] = {  50000 };
                            float step_size[] = { 50.f };
                            if (servo->offset_p < range_min[0]) servo->reverse = false;
                            if (servo->offset_p > range_max[0]) servo->reverse = true;
                            servo->offset_p += (servo->reverse ? -1 : 1) * step_size[0];
                            servo->out_p = (I32)(servo->zero_p + servo->offset_p);
                        }
                        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);

        }

        if (EcatPIOutputQueuePush(ecat_dev, 0, 100) != 0)
        {
            _ERR_("push pi output error");
            break;
        }
        
    }
	RETURN_IF_FAILED(EcatBusStop(ecat_dev));
	RETURN_IF_FAILED(EcatClose(ecat_dev));
	return 0;
}
