//sudo mount -t vboxsf share_d share/
//$CC main.c config_qn.c -o ectest_mc  -L ./  -lethercat -lpthread
//scp ectest_mc root@192.168.58.1:/home/mc

#include "config_qn.h"

int main(void) {
    /* Lock memory */
    if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) {
        perror("mlockall failed");
        return -1;
    }
    int ret = 0;
    master = ecrt_request_master(0);
    if (!master) {
        fprintf(stderr, "Failed to request master.\n");
        return -1;
    }
    printf("step1:request_master sucess\n");
    check_master_state();

    printf("step2:master get slave config\n");
    sc_cooldrive0 = ecrt_master_slave_config(master, CoolDrivePos0, CoolDrive);
    sc_cooldrive1 = ecrt_master_slave_config(master, CoolDrivePos1, CoolDrive);  
    
    printf("step2.1:remap qn pdo.\n");
    remap_qn_pdo(sc_cooldrive0);
    remap_qn_pdo(sc_cooldrive1);
    /**************
    printf("step2.2:configure SYNC signals.\n");
    ecrt_slave_config_dc(sc_cooldrive0, 0x0300, 4000000, 1000000, 0, 0);
    ecrt_slave_config_dc(sc_cooldrive1, 0x0300, 4000000, 1000000, 0, 0);
    **************************/

    printf("step2.2:configure free runmode.\n");
    /**
    ecrt_slave_config_sdo8(sc_cooldrive0, 0x6060, 0x00, 0x01);
    ecrt_slave_config_sdo8(sc_cooldrive0, 0x6860, 0x00, 0x01);
    ecrt_slave_config_sdo8(sc_cooldrive1, 0x6060, 0x00, 0x01);
    ecrt_slave_config_sdo8(sc_cooldrive1, 0x6860, 0x00, 0x01);
    *****/
    ecrt_slave_config_sdo16(sc_cooldrive0, 0x1c32, 0x01, 0x00);
    ecrt_slave_config_sdo16(sc_cooldrive1, 0x1c32, 0x01, 0x00);

    printf("step2.3:master_reset,Retry configuring slaves.\n");
    ecrt_master_reset(master);
    sleep(5);
    printf("step2.4:check salve config states.\n");
    check_slave0_config_states();
    check_slave1_config_states();

    printf("step3:create domain.\n");
    domainServoOutput = ecrt_master_create_domain(master);
    if (!domainServoOutput) {
        return -1;
    }
   // domainServoOutput = (ec_domain_t*)malloc(LIST_INIT_SIZE * sizeof(ec_domain_t));
    domainServoInput = ecrt_master_create_domain(master);
    if (!domainServoInput) {
        return -1;
    }
   // domainServoInput = (ec_domain_t*)malloc(LIST_INIT_SIZE * sizeof(ec_domain_t));
    
    printf("step3.1:domain reg pdo entry.\n");
    /*****
    if (ecrt_domain_reg_pdo_entry_list(domainServoOutput, domainServoOutput_regs)) {
        fprintf(stderr, "PDO entry registration failed!\n");
        return -1;
    }
    if (ecrt_domain_reg_pdo_entry_list(domainServoInput, domainServoInput_regs)) {
        fprintf(stderr, "PDO entry registration failed!\n");
        return -1;
    }
    /******/
    //qn_pdo_reg(sc_cooldrive0);
    ecrt_slave_config_reg_pdo_entry_pos(sc_cooldrive0, 2, 0, 0, domainServoOutput, NULL);
    ecrt_slave_config_reg_pdo_entry_pos(sc_cooldrive1,2, 0, 0, domainServoOutput, NULL);
    printf("drive0 reg done\n");
    //qn_pdo_reg(sc_cooldrive1);
    ecrt_slave_config_reg_pdo_entry_pos(sc_cooldrive0, 3, 0, 0, domainServoInput, NULL);
    ecrt_slave_config_reg_pdo_entry_pos(sc_cooldrive1, 3, 0, 0, domainServoInput, NULL);
    printf("drive1 reg done\n");
    /***************
    ret = ecrt_master_select_reference_clock(master, sc_cooldrive0);
    if (ret < 0) {
        fprintf(stderr, "Failed to select reference clock: %s\n",
            strerror(-ret));
        return ret;
    }
   ****************/
   
    printf("xenomai Activating master...\n");
    if (ecrt_master_activate(master)) {
        printf("xenomai Activating master...failed\n");
        return -1;
    }
   
    /********************/
    if (!(domainInput_pd = ecrt_domain_data(domainServoInput))) {
        fprintf(stderr, "xenomai Failed to get domain_in data pointer.\n");
        return -1;
    }
    if (!(domainOutput_pd = ecrt_domain_data(domainServoOutput))) {
        fprintf(stderr, "xenomai Failed to get domain_out data pointer.\n");
        return -1;
    }
    printf("xenomai Activating master...success\n");   
   
    /* Set priority */

    struct sched_param param = {};
    param.sched_priority = sched_get_priority_max(SCHED_FIFO);

    printf("Using priority %i.", param.sched_priority);
    if (sched_setscheduler(0, SCHED_FIFO, &param) == -1) {
        perror("sched_setscheduler failed");
    }

    printf("Starting cyclic function.\n");
    //data_cycle_task();
    cyclic_task();
    //positon_control();
    printf("release_master\n");
    ecrt_release_master(master);
    return 0;
    
}