#include "config_qn.h"
//EtherCAT
ec_master_t* master = NULL;
ec_master_state_t master_state = {};

ec_domain_t* domainServoInput = NULL;
ec_domain_state_t domainServoInput_state = {};
ec_domain_t* domainServoOutput = NULL;
ec_domain_state_t domainServoOutput_state = {};

uint8_t* domainOutput_pd = NULL;
uint8_t* domainInput_pd = NULL;

ec_slave_config_t* sc_cooldrive0 = NULL;
ec_slave_config_state_t sc_cooldrive0_state = {};
ec_slave_config_t* sc_cooldrive1 = NULL;
ec_slave_config_state_t sc_cooldrive1_state = {};
/****************************************************************************/
 ec_pdo_entry_reg_t domainServoInput_regs[] = {
    {CoolDrivePos0, CoolDrive, 0x6064, 0x00, &A1.actpos, NULL},
    {CoolDrivePos0, CoolDrive, 0x6041, 0x00, &A1.status, NULL},
    {CoolDrivePos0, CoolDrive, 0x6864, 0x00, &A2.actpos, NULL},
    {CoolDrivePos0, CoolDrive, 0x6841, 0x00, &A2.status, NULL},
    {CoolDrivePos1, CoolDrive, 0x6064, 0x00, &A3.actpos, NULL},
    {CoolDrivePos1, CoolDrive, 0x6041, 0x00, &A3.status, NULL},
    {CoolDrivePos1, CoolDrive, 0x6864, 0x00, &A4.actpos, NULL},
    {CoolDrivePos1, CoolDrive, 0x6841, 0x00, &A4.status, NULL},
    {}
};
 ec_pdo_entry_reg_t domainServoOutput_regs[] = {
    {CoolDrivePos0, CoolDrive, 0x607a, 0x00, &A1.tarpos, NULL},
    {CoolDrivePos0, CoolDrive, 0x6040, 0x00, &A1.cntlwd, NULL},
    {CoolDrivePos0, CoolDrive, 0x687a, 0x00, &A2.tarpos, NULL},
    {CoolDrivePos0, CoolDrive, 0x6840, 0x00, &A2.cntlwd, NULL},
    {CoolDrivePos1, CoolDrive, 0x607a, 0x00, &A3.tarpos, NULL},
    {CoolDrivePos1, CoolDrive, 0x6040, 0x00, &A3.cntlwd, NULL},
    {CoolDrivePos1, CoolDrive, 0x687a, 0x00, &A4.tarpos, NULL},
    {CoolDrivePos1, CoolDrive, 0x6840, 0x00, &A4.cntlwd, NULL},
    {}
};

/*****************************************************************/

const struct timespec cycletime = { 0, PERIOD_NS };

void check_master_state(void)
{
    ec_master_state_t ms;

    ecrt_master_state(master, &ms);

    if (ms.slaves_responding != master_state.slaves_responding) {
        printf("found %u slave(s).\n", ms.slaves_responding);
    }
    if (ms.al_states != master_state.al_states) {
        printf("AL states: 0x%02X.\n", ms.al_states);
    }
    if (ms.link_up != master_state.link_up) {
        printf("Link is %s.\n", ms.link_up ? "up" : "down");
    }

    master_state = ms;
}

/*****************************************************************************/

void check_slave0_config_states(void)
{
    ec_slave_config_state_t s;

    ecrt_slave_config_state(sc_cooldrive0, &s);

    if (s.al_state != sc_cooldrive0_state.al_state) {
        printf("CoolDrive0: State 0x%02X.\n", s.al_state);
    }
    if (s.online != sc_cooldrive0_state.online) {
        printf("CoolDrive0: %s.\n", s.online ? "online" : "offline");
    }
    if (s.operational != sc_cooldrive0_state.operational) {
        printf("CoolDrive0: %soperational.\n", s.operational ? "" : "Not ");
    }

    sc_cooldrive0_state = s;
}
/*****************************************************************************/

void check_slave1_config_states(void)
{
    ec_slave_config_state_t s;

    ecrt_slave_config_state(sc_cooldrive1, &s);

    if (s.al_state != sc_cooldrive1_state.al_state) {
        printf("CoolDrive1: State 0x%02X.\n", s.al_state);
    }
    if (s.online != sc_cooldrive1_state.online) {
        printf("CoolDrive1: %s.\n", s.online ? "online" : "offline");
    }
    if (s.operational != sc_cooldrive1_state.operational) {
        printf("CoolDrive1: %soperational.\n", s.operational ? "" : "Not ");
    }

    sc_cooldrive1_state = s;
}
/*****************************************************************************/

void check_domainInput_state(void)
{
    ec_domain_state_t ds;

    ecrt_domain_state(domainServoInput, &ds);

    if (ds.working_counter != domainServoInput_state.working_counter) {
        printf("Domain1: WC %u.\n", ds.working_counter);
    }
    if (ds.wc_state != domainServoInput_state.wc_state) {
        printf("Domain1: State %u.\n", ds.wc_state);
    }

    domainServoInput_state = ds;
}
/*****************************************************************************/

void check_domainOutput_state(void)
{
    ec_domain_state_t ds;

    ecrt_domain_state(domainServoOutput, &ds);

    if (ds.working_counter != domainServoOutput_state.working_counter) {
        printf("Domain1: WC %u.\n", ds.working_counter);
    }
    if (ds.wc_state != domainServoOutput_state.wc_state) {
        printf("Domain1: State %u.\n", ds.wc_state);
    }

    domainServoOutput_state = ds;
}
/*****************************************************************************/

int qn_pdo_reg(ec_slave_config_t* sc) {
    ecrt_slave_config_reg_pdo_entry_pos(sc, 2, 0, 0, domainServoOutput, NULL);
    ecrt_slave_config_reg_pdo_entry_pos(sc, 2, 0, 1, domainServoOutput, NULL);
   // ecrt_slave_config_reg_pdo_entry_pos(sc, 2, 0, 2, domainServoOutput, NULL);
   // ecrt_slave_config_reg_pdo_entry_pos(sc, 2, 0, 3, domainServoOutput, NULL);
    ecrt_slave_config_reg_pdo_entry_pos(sc, 2, 1, 0, domainServoOutput, NULL);
    ecrt_slave_config_reg_pdo_entry_pos(sc, 2, 1, 1, domainServoOutput, NULL);
    // ecrt_slave_config_reg_pdo_entry_pos(sc, 2, 1, 2, domainServoOutput, NULL);
    // ecrt_slave_config_reg_pdo_entry_pos(sc, 2, 1, 3, domainServoOutput, NULL);

    ecrt_slave_config_reg_pdo_entry_pos(sc, 3, 0, 0, domainServoInput, NULL);
    ecrt_slave_config_reg_pdo_entry_pos(sc, 3, 0, 1, domainServoInput, NULL);
   // ecrt_slave_config_reg_pdo_entry_pos(sc, 3, 0, 2, domainServoInput, NULL);
  //  ecrt_slave_config_reg_pdo_entry_pos(sc, 3, 0, 3, domainServoInput, NULL);
    ecrt_slave_config_reg_pdo_entry_pos(sc, 3, 1, 0, domainServoInput, NULL);
    ecrt_slave_config_reg_pdo_entry_pos(sc, 3, 1, 1, domainServoInput, NULL);
    // ecrt_slave_config_reg_pdo_entry_pos(sc, 3, 1, 2, domainServoInput, NULL);
   //  ecrt_slave_config_reg_pdo_entry_pos(sc, 3, 1, 3, domainServoInput, NULL);

}
void remap_qn_pdo(ec_slave_config_t* sc) {
    ecrt_slave_config_pdo_assign_clear(sc, 2);
    ecrt_slave_config_pdo_assign_add(sc, 2, 0x1600);
    ecrt_slave_config_pdo_mapping_clear(sc, 0x1600);
    ecrt_slave_config_pdo_mapping_add(sc, 0x1600, 0x6040, 0x00, 16);
    ecrt_slave_config_pdo_mapping_add(sc, 0x1600, 0x607a, 0x00, 32);
   // ecrt_slave_config_pdo_mapping_add(sc, 0x1600, 0x6060, 0x00, 8);
   // ecrt_slave_config_pdo_mapping_add(sc, 0x1600, 0x27fe, 0x00, 8);//假字节数据
    ecrt_slave_config_pdo_assign_add(sc, 2, 0x1610);
    ecrt_slave_config_pdo_mapping_clear(sc, 0x1610);
    ecrt_slave_config_pdo_mapping_add(sc, 0x1610, 0x6840, 0x00, 16);
    ecrt_slave_config_pdo_mapping_add(sc, 0x1610, 0x687a, 0x00, 32);
   // ecrt_slave_config_pdo_mapping_add(sc, 0x1610, 0x6860, 0x00, 8);
  //  ecrt_slave_config_pdo_mapping_add(sc, 0x1610, 0x2ffe, 0x00, 8);//假字节数据




    ecrt_slave_config_pdo_assign_clear(sc, 3);
    ecrt_slave_config_pdo_assign_add(sc, 3, 0x1a00);
    ecrt_slave_config_pdo_mapping_clear(sc, 0x1a00);
    ecrt_slave_config_pdo_mapping_add(sc, 0x1a00, 0x6041, 0x00, 16);
    ecrt_slave_config_pdo_mapping_add(sc, 0x1a00, 0x6064, 0x00, 32);
   // ecrt_slave_config_pdo_mapping_add(sc, 0x1a00, 0x6061, 0x00, 8);
   // ecrt_slave_config_pdo_mapping_add(sc, 0x1a00, 0x27ff, 0x00, 8);//假字节数据
    ecrt_slave_config_pdo_assign_add(sc, 3, 0x1a10);
    ecrt_slave_config_pdo_mapping_clear(sc, 0x1a10);
    ecrt_slave_config_pdo_mapping_add(sc, 0x1a10, 0x6841, 0x00, 16);
    ecrt_slave_config_pdo_mapping_add(sc, 0x1a10, 0x6864, 0x00, 32);
   // ecrt_slave_config_pdo_mapping_add(sc, 0x1a10, 0x6861, 0x00, 8);
   // ecrt_slave_config_pdo_mapping_add(sc, 0x1a10, 0x2fff, 0x00, 8);//假字节数据

    ecrt_slave_config_sync_manager(sc, 0, EC_DIR_OUTPUT, EC_WD_DISABLE);
    ecrt_slave_config_sync_manager(sc, 1, EC_DIR_INPUT, EC_WD_DISABLE);
    ecrt_slave_config_sync_manager(sc, 2, EC_DIR_OUTPUT, EC_WD_ENABLE);
    ecrt_slave_config_sync_manager(sc, 3, EC_DIR_INPUT, EC_WD_DISABLE);


}
void domain_init() {
    //Aix1

    EC_WRITE_U16(domainOutput_pd, 0x05);
    EC_WRITE_U32(domainOutput_pd + 2, 20561);
    EC_WRITE_S8(domainOutput_pd + 6, 0x07);
    //EC_WRITE_U16(domainOutput_pd+8,0x00);
    //EC_WRITE_U16(domainOutput_pd+10,0x00);
    /****
    //Aix2
    EC_WRITE_U16(domainOutput_pd+8,0x07);
    EC_WRITE_U32(domainOutput_pd+8+2,0x6e1303);
    EC_WRITE_U8(domainOutput_pd+8+6,0x08);
    //EC_WRITE_U16(domainOutput_pd+12+8,0x00);
    //EC_WRITE_U16(domainOutput_pd+12+10,0x00);
    //Aix3
    EC_WRITE_U16(domainOutput_pd+16,0x07);
    EC_WRITE_U32(domainOutput_pd+16+2,0x6e1303);
    EC_WRITE_U8(domainOutput_pd+16+6,0x08);
    //EC_WRITE_U16(domainOutput_pd+24+8,0x00);
   // EC_WRITE_U16(domainOutput_pd+24+10,0x00);
    //Aix4
    EC_WRITE_U16(domainOutput_pd+24,0x07);
    EC_WRITE_U32(domainOutput_pd+24+2,0x6e1303);
    EC_WRITE_U8(domainOutput_pd+24+6,0x08);
   // EC_WRITE_U16(domainOutput_pd+36+8,0x00);
   // EC_WRITE_U16(domainOutput_pd+36+10,0x00);
   ****/
}

void cyclic_task() {
    ecrt_master_set_send_interval(master,1000);
    ecrt_master_receive(master);
    printf("1 ok?\n");
    ecrt_domain_process(domainServoInput);
   // ecrt_domain_process(domainServoOutput);
    printf("2 ok?\n");
    check_domainInput_state();
    check_domainOutput_state();
    check_master_state();
    check_slave0_config_states();
    check_slave1_config_states();
    //A1_data.status=EC_READ_U32(domainOutput_pd);     
    //printf("A1.status=%d",A1_data.status);

    //EC_WRITE_U16(domainInput_pd,0x45);     	
    // send process data
    //domain_init();
   // clock_gettime(CLOCK_REALTIME, &tv);
    // write application time to master
   // ecrt_master_application_time(master, EC_TIMEVAL2NANO(tv));
   // ecrt_master_sync_reference_clock(master);
   // ecrt_master_sync_slave_clocks(master);

    printf("3 ok?\n");
    ecrt_domain_queue(domainServoOutput);
    //ecrt_domain_queue(domainServoInput);
    printf("ok?\n");
    pause();
    ecrt_master_set_send_interval(master,1000);
    ecrt_master_send(master);


}

void data_cycle_task() {
    struct timespec wakeupTime, time;
    // get current time
    clock_gettime(CLOCK_TO_USE, &wakeupTime);
    int ret = 0;
    while (1) {
        wakeupTime = timespec_add(wakeupTime, cycletime);
        clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL);

        // Write application time to master
        //
        // It is a good idea to use the target time (not the measured time) as
        // application time, because it is more stable.
        //
        ecrt_master_application_time(master, TIMESPEC2NS(wakeupTime));
        // receive process data
        ecrt_master_receive(master);
        ecrt_domain_process(domainServoInput);
        ecrt_domain_process(domainServoOutput);

        // check process data state (optional)
        check_domainInput_state();
        check_domainOutput_state();

        if (counter) {
            counter--;
        }
        else { // do this at 1 Hz
            counter = FREQUENCY;

            // check for master state (optional)
            check_master_state();
            // calculate new process data
            
        }
       // domain_init();
        if (sync_ref_counter) {
            sync_ref_counter--;
        }
        else {
            sync_ref_counter = 1; // sync every cycle

            clock_gettime(CLOCK_TO_USE, &time);
           //ecrt_master_sync_reference_clock_to(master, TIMESPEC2NS(time));
            ecrt_master_application_time(master, TIMESPEC2NS(time));
            ecrt_master_sync_reference_clock(master);
        }
        ecrt_master_sync_slave_clocks(master);
        // send process data
        ecrt_domain_queue(domainServoOutput);
        ecrt_domain_queue(domainServoInput);
       // pause();
        ecrt_master_send(master);
    }
}

struct timespec timespec_add(struct timespec time1, struct timespec time2)
{
    struct timespec result;

    if ((time1.tv_nsec + time2.tv_nsec) >= NSEC_PER_SEC) {
        result.tv_sec = time1.tv_sec + time2.tv_sec + 1;
        result.tv_nsec = time1.tv_nsec + time2.tv_nsec - NSEC_PER_SEC;
    }
    else {
        result.tv_sec = time1.tv_sec + time2.tv_sec;
        result.tv_nsec = time1.tv_nsec + time2.tv_nsec;
    }

    return result;
}

void positon_control() {
    A1_data.ok = 0;
    //int cntwrd,pos;
    printf("please input cntwrd and position(r/min)\n");
    scanf("%d %d", &A1_data.cntlwd_d, &A1_data.tarpos_d);
    printf("cntwrd:%d;position:%d\n", A1_data.cntlwd_d, A1_data.tarpos_d);

    while (!A1_data.ok) {
        if (A1_data.ok) {
            printf("status:%d;actpos:%d\n", A1_data.status_d, A1_data.actpos_d);
            break;
            A1_data.ok = 0;
        }

        sleep(1);
        printf("wait data ok\n");

    }

}

void motor_enable() {

    do {
        A1_data.cntlwd_d = 0x10;//enable operation
        sleep(0.1);
        A1_data.cntlwd_d = 0x06;//shut down
        sleep(0.1);
        A1_data.cntlwd_d = 0x07;//disable voltage,quik stop
        sleep(0.1);
        A1_data.cntlwd_d = 0xf;// fault reset
        sleep(0.1);
    } while (!((A1_data.status_d & 0x06f) == 0x027));

    printf("enable servo success!\n");

}

void motor_disable() {

    do {
        A1_data.cntlwd_d = 0x05;//disable operation
        sleep(0.1);
        A1_data.cntlwd_d = 0x06;//shut down
        sleep(0.1);
        A1_data.cntlwd_d = 0x07;//disable voltage,quik stop
        sleep(0.1);
        A1_data.cntlwd_d = 0xf;//fault reset
        sleep(0.1);
    } while ((A1_data.status_d & 0x06f) == 0x027);

    printf("disable servo success!\n");

}