#include<stdio.h>
#include <errno.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <sys/resource.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
#include <time.h> /* clock_gettime() */
#include <sys/mman.h> /* mlockall() */
#include <sched.h> /* sched_setscheduler() */
#include <pthread.h>
#include <math.h>

#include "ecrt.h"

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

// EtherCAT
extern ec_master_t* master;
extern ec_master_state_t master_state;

extern ec_domain_t* domainServoInput;
extern ec_domain_state_t domainServoInput_state;
extern ec_domain_t* domainServoOutput;
extern ec_domain_state_t domainServoOutput_state;

extern uint8_t* domainOutput_pd;
extern uint8_t* domainInput_pd;

extern ec_slave_config_t* sc_cooldrive0;
extern ec_slave_config_state_t sc_cooldrive0_state;
extern ec_slave_config_t* sc_cooldrive1;
extern ec_slave_config_state_t sc_cooldrive1_state;
/*************************************************************************/
//self define fx
#define CoolDrivePos0  0, 0
#define CoolDrivePos1  0, 1
#define PI 3.1415926535898
#define CoolDrive 0x00000748, 0x0000000a
typedef struct {
	int cntlwd_d;
	int tarpos_d;
	int status_d;
	int actpos_d;
	int ok;

}Aix_data;
typedef struct {
	 unsigned int cntlwd;
	 unsigned int tarpos;
	 unsigned int status;
	 unsigned int actpos;
	
}Aix_reg;

Aix_data A1_data;
Aix_reg A1,A2,A3,A4;
extern ec_pdo_entry_reg_t domainServoInput_regs[];
extern ec_pdo_entry_reg_t domainServoOutput_regs[];


pthread_t id;
void remap_qn_pdo(ec_slave_config_t* sc);
int qn_pdo_reg(ec_slave_config_t* sc);
void domain_init();
void cyclic_task();
void data_cycle_task();
void positon_control();
void motor_enable();
void motor_disable();

// Application parameters
#define FREQUENCY 1000
#define CLOCK_TO_USE CLOCK_MONOTONIC
#define MEASURE_TIMING

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

#define NSEC_PER_SEC (1000000000L)
#define PERIOD_NS (NSEC_PER_SEC / FREQUENCY)

#define DIFF_NS(A, B) (((B).tv_sec - (A).tv_sec) * NSEC_PER_SEC + \
        (B).tv_nsec - (A).tv_nsec)

#define TIMESPEC2NS(T) ((uint64_t) (T).tv_sec * NSEC_PER_SEC + (T).tv_nsec)

/****************************************************************************/
static unsigned int counter = 0;
static unsigned int blink = 0;
static unsigned int sync_ref_counter = 0;
const struct timespec cycletime;
struct timespec timespec_add(struct timespec time1, struct timespec time2);
/*************************************************************************/
//check state
void check_master_state(void);
void check_slave0_config_states(void);
void check_slave1_config_states(void);

void check_domainInput_state(void);
void check_domainOutput_state(void);
/*************************************************************************/
//time

struct timeval tv;