/* test program for the following EtherCAT configuration
Xenomai RTDM */

#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 <sys/mman.h>
#include <rtdm/rtdm.h>
#include <rtdk.h>
#include <task.h>
#include <sem.h>
#include <mutex.h>
#include <timer.h>
#include <pthread.h>

#include "ecrt.h"	   /* EtherCAT master application interface */

RT_TASK rt_task2;		/* estimation task info */
static RT_MUTEX mutex;		/* mutex to protect the access of some variable between the simulation and estimation tasks */
static int run = 1;		/* status variable of the program */
long int t_delta_estim = 1000;	// time step for the estimation in us
	    
int16_t actTorque;	     /* actual torque, 16 bit signed integer */
int16_t targetTorque;	     /* target torque, 16 bit signed integer */
int32_t actPos;	     /* actual position, 32 bit integer */

// EtherCAT
static ec_master_t *master = NULL;
static ec_master_state_t master_state = {};

static ec_domain_t *domain1 = NULL;
static ec_domain_state_t domain1_state = {};
static uint8_t *domain1_pd = NULL;
// slaves
static ec_slave_config_t *sc_motor = NULL;
static ec_slave_config_state_t sc_state_motor = {};

// offsets for PDO entries
static unsigned int actual_position;
static unsigned int actual_torque;
static unsigned int target_torque;

/* Alias and position */
#define Omronpos  0, 0

/* Vendor ID and Product code */
#define Omron_motor  0x00000083, 0x0000005b /* Omron EtherCAT motor */

/* Alias - Position - Vendor ID - Product code - PDO entry index - PDO entry subindex - offset */
const static ec_pdo_entry_reg_t domain1_regs[] = {
  {Omronpos, Omron_motor, 0x6064, 0x00, &actual_position}, /* Position actual value */
  {Omronpos, Omron_motor, 0x6071, 0x00, &target_torque}, /* Target torque */
  {Omronpos, Omron_motor, 0x6077, 0x00, &actual_torque}, /* Torque actual value */
  {}
};

/* Master 0, Slave 0, "R88D-KN20F-ECT      "
 * Vendor ID:       0x00000083
 * Product code:    0x0000005b
 * Revision number: 0x00020001
 */

ec_pdo_entry_info_t slave_0_pdo_entries[] = {
    {0x6040, 0x00, 16}, /* Controlword */
    {0x607a, 0x00, 32}, /* Target position */
    {0x60FF, 0x00, 32}, /* Target velocity */
    {0x6071, 0x00, 16}, /* Target torque */
    {0x6060, 0x00, 8}, /* Modes of operation: Torque control position control*/
    {0x60b8, 0x00, 16}, /* Touch probe function */
    {0x607F, 0x00 , 32}, /* Max profile velocity */
    {0x603f, 0x00, 16}, /* Error code */
    {0x6041, 0x00, 16}, /* Statusword */
    {0x6064, 0x00, 32}, /* Position actual value */
    {0x6077, 0x00, 16}, /* Torque actual value */
    {0x6061, 0x00, 8}, /* Modes of operation display */
    {0x60b9, 0x00, 16}, /* Touch probe status */
    {0x60ba, 0x00, 32}, /* Touch probe pos1 pos value */
    {0x60bc, 0x00, 32}, /* Touch probe pos2 pos value */
    {0x60fd, 0x00, 32}, /* Digital inputs */
};

ec_pdo_info_t slave_0_pdos[] = {
    {0x1702, 7, slave_0_pdo_entries + 0}, /* 258th receive PDO Mapping */
    {0x1b02, 9, slave_0_pdo_entries + 7}, /* 258th transmit PDO Mapping */
};

ec_sync_info_t slave_0_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 1, slave_0_pdos + 0, EC_WD_ENABLE},
    {3, EC_DIR_INPUT, 1, slave_0_pdos + 1, EC_WD_DISABLE},
    {0xff}
};


void rt_check_domain_state(void)
{
//     printf("start\n");
    ec_domain_state_t ds = {};
    ecrt_domain_state(domain1, &ds);
    if (ds.working_counter != domain1_state.working_counter) {
        printf("Domain1: WC %u.\n", ds.working_counter);
    }
    if (ds.wc_state != domain1_state.wc_state) {
        printf("Domain1: State %u.\n", ds.wc_state);
    }
    domain1_state = ds;
//     printf("finish\n");
}

void rt_check_master_state(void)
{
    ec_master_state_t ms;	/* master state */
    ecrt_master_state(master, &ms); /* Reads the current master state. Stores the master state information in the given state structure. */
    if (ms.slaves_responding != master_state.slaves_responding)
        printf("EtherCAT: %u slave(s).\n", ms.slaves_responding);
    if (ms.al_states != master_state.al_states)
        printf("EtherCAT: application Layer states: 0x%02X.\n", ms.al_states); // Bit 0: INIT, Bit 1: PREOP, Bit 2: SAFEOP, Bit 3: OP
    
    if (ms.link_up != master_state.link_up)
        printf("EtherCAT: link is %s.\n", ms.link_up ? "up" : "down");
	
    master_state = ms;
    
    
}
void rt_check_slave_config_states(void)
{
    ec_slave_config_state_t s;
    ecrt_slave_config_state(sc_motor, &s);
    if (s.al_state != sc_state_motor.al_state)
        printf("AnaIn: State 0x%02X.\n", s.al_state);
    
    if (s.online != sc_state_motor.online)
        printf("AnaIn: %s.\n", s.online ? "online" : "offline");
    if (s.operational != sc_state_motor.operational)
        printf("AnaIn: %soperational.\n", s.operational ? "" : "Not ");
    sc_state_motor = s;
}

void estimation_task(void *arg)
{
  rt_task_set_periodic(NULL, TM_NOW, t_delta_estim*1000); // ns
//     rt_check_slave_config_states(); 
   
    EC_WRITE_U8(domain1_pd + target_torque, 0x0000);
    ecrt_domain_queue(domain1);	/* queues all domain datagrams in the master's datagram queue.  */
    ecrt_master_send(master);	/* sends all datagrams in the queue.  */


  RT_TASK *curtask;
  RT_TASK_INFO curtaskinfo;
  RT_TIMER_INFO timer_info;
  unsigned long long int start = 0;
  curtask = rt_task_self();
  rt_task_inquire(curtask,&curtaskinfo);
  rt_task_set_periodic(NULL, TM_NOW, t_delta_estim*1000); // ns
  (void)rt_timer_inquire(&timer_info); /* Inquire about the Alchemy clock.  */
  start = timer_info.date/1000;	       /* start time in us */

  while (run) {
    // receive process data
    ecrt_master_receive(master); /* Fetches received frames from the hardware and processes the datagrams */
    ecrt_domain_process(domain1); /* Determines the states of the domain's datagrams */
    rt_check_domain_state();	// check process data state (optional)
    
    
    actTorque = EC_READ_S16(domain1_pd + actual_torque);
    actPos = EC_READ_S32(domain1_pd + actual_position);
    printf("Actual position: %u\n",actPos); /* read value from position */
    printf("Actual torque: %u\n",actTorque);

    // write process data
    EC_WRITE_S16(domain1_pd + target_torque, 0x0006); /* write an 16-bit signed value to motor */
    
    // send process data
    ecrt_domain_queue(domain1);	/* queues all domain datagrams in the master's datagram queue.  */
    ecrt_master_send(master);	/* sends all datagrams in the queue.  */
    
    rt_task_wait_period(NULL);	/* Wait for the next periodic release point.  */
  }
}

void signal_handler(int sig)
{
    run = 0;
}

int main(int argc, char *argv[])
{
    ec_slave_config_t *sc;
    int ret;

    /* Perform auto-init of rt_print buffers if the task doesn't do so */
    rt_print_auto_init(1);

    signal(SIGTERM, signal_handler);
    signal(SIGINT, signal_handler);

    mlockall(MCL_CURRENT | MCL_FUTURE);

    master = ecrt_request_master(0);
    if (!master) {
        return -1;
    }
    printf("Master requested.\n");

    domain1 = ecrt_master_create_domain(master);
    if (!domain1) {
        return -1;
    }
    printf("Master created\n");

    printf("EtherCAT: configuring PDOs.\n");
     
    /* Master 0, Slave 0, "Motor" */
    /* obtain slave configuration for motor */
    if (!(sc = ecrt_master_slave_config(master, Omronpos, Omron_motor))) {
        fprintf(stderr, "Failed to get slave/motor configuration.\n");
        return -1;
    }
    printf("EtherCAT: obtained slave configuration for Motor\n");

    /* Specify a complete PDO configuration for Omron_motor */
    if (ecrt_slave_config_pdos(sc, EC_END, slave_0_syncs)) {
        fprintf(stderr, "Failed to configure PDOs.\n");
        return -1;
    }
    
    /* Registers a bunch of PDO entries for a domain */
    if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {
        fprintf(stderr, "PDO entry registration failed!\n");
        return -1;
    }

    if (ecrt_master_activate(master)){
      rt_printf("Activation of master failed.\n");
      return -1;
    }
    if (!(domain1_pd = ecrt_domain_data(domain1))) {
        return -1;
    }
    printf("Master activated.\n");

    /* tasks creation */
    ret = rt_task_create(&rt_task2, "estimation task", 0, 80, 0);
    if (ret < 0) {
        fprintf(stderr, "Failed to create the estimation task: %s\n", strerror(-ret));
        return -1;
    }

    /* tasks startup */
    ret = rt_task_start(&rt_task2, &estimation_task, NULL);
	   if (ret < 0) {
        fprintf(stderr, "Failed to start the estimation task: %s\n", strerror(-ret));
        return -1;
    }
    printf("Estimation task started\n");

	while (run) {
		sched_yield();	/* release processor */
	}

    rt_task_delete(&rt_task2);
    printf("Estimation task deleted.\n");
    rt_mutex_delete(& mutex);
    printf("Mutex deleted.\n");

    printf("End of Program\n");
    ecrt_release_master(master);

    return 0;
}