//head files for kernel socket 
#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/init.h>
#include <linux/in.h>
#include <linux/inet.h>
#include <linux/socket.h>
#include <net/sock.h>


#include "rtapi.h"			/* RTAPI realtime OS API */
#include "rtapi_app.h"		/* RTAPI realtime module decls */
#include "hal.h"			/* HAL public API decls */

#include <float.h>
#include "rtapi_math.h"

#define MAX_CHAN 16
#define MAX_CYCLE 18
#define USER_STEP_TYPE 13

/* module information */
MODULE_AUTHOR("paul");
MODULE_DESCRIPTION("PulseCard for EMC HAL");
MODULE_LICENSE("GPL");


/***********************************************************************
*                STRUCTURES AND GLOBAL VARIABLES                        *
************************************************************************/

/** This structure contains the runtime data for a single pulsecard. */
typedef struct {
    
    hal_bit_t *enable;		/* pin for enable PulseCard */
	hal_bit_t *estop;
	hal_bit_t *spindlecw;

	//pins for home signal
	hal_bit_t *homex;
	hal_bit_t *homey;
	hal_bit_t *homez;
	hal_bit_t *homea;
	hal_bit_t *homeb;
	hal_bit_t *homec;

	//pins for limit signal
	hal_bit_t *xlimpos;
	hal_bit_t *ylimpos;
	hal_bit_t *zlimpos;
	hal_bit_t *alimpos;
	hal_bit_t *blimpos;
	hal_bit_t *climpos;

	hal_bit_t *xlimneg;
	hal_bit_t *ylimneg;
	hal_bit_t *zlimneg;
	hal_bit_t *alimneg;
	hal_bit_t *blimneg;
	hal_bit_t *climneg;

	//pins for position 
    hal_float_t *xposcmd;	
	hal_float_t *yposcmd;
	hal_float_t *zposcmd;
	hal_float_t *aposcmd;
	hal_float_t *bposcmd;
	hal_float_t *cposcmd;
} PulseCard_t;

/* ptr to array of PulseCard_t structs in shared memory, 1 per channel */
static PulseCard_t *PulseCard_array;




/* ----------------------------GLOBAL VARIABLES FOR COMPONENT------------------------------------- */

static int comp_id;				/* component ID */
static int num_chan = 1;		/* number of step pulsecard configured */



/* ----------------------------GLOBLA VARIABLES FOR ETH BUS------------------------------------- */

static struct socket *sock;						//handle for socket in kernel
static struct sockaddr_ll sa;					//socket addr 


#define EtherLength  200								//the length of send/receive buffer for ethernet bus
static unsigned char SndEtherData[EtherLength];			//packet for send 
static unsigned char RcvEtherData[EtherLength];			//packet for receive

//global variables for functions which used for calculating pulse count in axis
static hal_float_t remain_dis_x = 0;
static hal_float_t remain_dis_y = 0;
static hal_float_t remain_dis_z = 0;
static hal_float_t remain_dis_a = 0;
static hal_float_t remain_dis_b = 0;

static hal_float_t poscmd_old_x = 0;
static hal_float_t poscmd_old_y = 0;
static hal_float_t poscmd_old_z = 0;
static hal_float_t poscmd_old_a = 0;
static hal_float_t poscmd_old_b = 0;


double pulse_count_x = 0;
double pulse_count_y = 0;
double pulse_count_z = 0;
double pulse_count_a = 0;
double pulse_count_b = 0;
double pulse_count_c = 0;

//struct kvec vec_Send;
//struct msghdr msg_Send;
/***********************************************************************
*                    本地函数声明                         *
************************************************************************/

static int export_PulseCard(int num, PulseCard_t * addr, int step_type, int pos_mode);
//static void make_pulses(void *arg, long period);
//static void update_freq(void *arg, long period);
static void update_pos(void *arg, long period);


//function for send data 
static int Mac_SendTo(struct socket *sock, void * buff, size_t len, 
                unsigned flags, struct sockaddr *addr, int addr_len);
//function for receive data			
static int Mac_RecvFrom(struct socket *sock, void * buff, size_t len, 
				unsigned flags, struct sockaddr *addr, int addr_len);
//initial function,for the initial of socket and the relational buffers 
static int Init(void);

//add head information for send packet
static void CfgEtherPacketHead(unsigned char * EtherPacket);

//add position information in send packet 
static void AddNodePacket(unsigned char * EtherPacket);

//calculate pulse counts in axis (AxisNum:0~4 A~E)
static double CalcPulseCount(hal_float_t *poscmd,hal_float_t *remain_dis,hal_float_t *poscmd_old);

/***********************************************************************
*                       INIT AND EXIT CODE                              *
************************************************************************/

int rtapi_app_main(void)
{
    int n, retval;


    comp_id = hal_init("PulseCard");
    if (comp_id < 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
			"PulseCard: ERROR: hal_init() failed\n");
	return -1;
    }
	
    /* allocate shared memory for counter data */
    PulseCard_array = hal_malloc(num_chan * sizeof(PulseCard_t));
    if (PulseCard_array == 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
			"PulseCard: ERROR: hal_malloc() failed\n");
	hal_exit(comp_id);
	return -1;
    }

    /* export all the variables for each pulsecard */
    for (n = 0; n < num_chan; n++) {
	/* export all vars */
		retval = export_PulseCard(n, &(PulseCard_array[n]),0, true);
		if (retval != 0) {
			rtapi_print_msg(RTAPI_MSG_ERR,
			"PulseCard: ERROR: PulseCard %d var export failed,line %d error!\n", n,retval);
			hal_exit(comp_id);
			return -1;
		}
		else{
			rtapi_print_msg(RTAPI_MSG_ERR,"PulseCard:PulseCard var export successed!\n");
		}
    }

	


	/* export functions */
    retval = hal_export_funct("PulseCard.update_pos", update_pos,
	PulseCard_array, 1, 0, comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	 "PulseCard: ERROR: pos update funct export failed\n");
	hal_exit(comp_id);
	return -1;
    }
	else{
		rtapi_print_msg(RTAPI_MSG_ERR,"PulseCard:function update_pos export successed!\n");
	}
		
    rtapi_print_msg(RTAPI_MSG_ERR,
	"PulseCard: installed %d step pulse generators\n", num_chan);
    hal_ready(comp_id);

	Init();
	rtapi_print_msg(RTAPI_MSG_ERR,"function Init execute successed!\n");
    return 0;
}

void rtapi_app_exit(void)
{
	rtapi_print_msg(RTAPI_MSG_ERR,"enter into sock_release fuction \n");
	sock_release(sock); //release sock

	rtapi_print_msg(RTAPI_MSG_ERR,"successed for sock_release function ,enter into hal_exit function \n");
    hal_exit(comp_id);

	rtapi_print_msg(RTAPI_MSG_ERR,"successed for hal_exit function \n");
}
/***********************************************************************
*             REALTIME  FUNCTIONS FOR PULSECARD			  	             *
************************************************************************/




static void update_pos(void *arg, long period)
{


	//build send packet
	AddNodePacket(SndEtherData);					

	//send the packet by socket 
	Mac_SendTo(sock,SndEtherData,EtherLength,0,(struct sockaddr *)&sa,sizeof(sa));		


}


/*
	add head information for send packet 
*/
static void CfgEtherPacketHead(unsigned char * EtherPacket)
{
	EtherPacket[0] = 0xf0;
	EtherPacket[1] = 0xde;
	EtherPacket[2] = 0xf1;
	EtherPacket[3] = 0x7b;
	EtherPacket[4] = 0x06;
	EtherPacket[5] = 0xa6; 	
	EtherPacket[6] = 0x00;
	EtherPacket[7] = 0x1B;
	EtherPacket[8] = 0xEB;
	EtherPacket[9] = 0x22;
	EtherPacket[10] = 0x04;
	EtherPacket[11] = 0x09;
	EtherPacket[12] = 0x83;
	EtherPacket[13] = 0x82;						
	EtherPacket[14] = 0x00;
	EtherPacket[15] = 0x00;
	EtherPacket[16] = 0x20;
	
	EtherPacket[20] = 0x00;	//motor A
	EtherPacket[21] = 0x04;
	EtherPacket[22] = 0x04;	
	
	EtherPacket[27] = 0x00;	//motor B
	EtherPacket[28] = 0x06;
	EtherPacket[29] = 0x04;	
	
	EtherPacket[34] = 0x00;	//motor C
	EtherPacket[35] = 0x08;
	EtherPacket[36] = 0x04;	
	
	EtherPacket[41] = 0x00;	//motor D
	EtherPacket[42] = 0x10;
	EtherPacket[43] = 0x04;	
	
	
	EtherPacket[48] = 0x00;	//motor E
	EtherPacket[49] = 0x12;
	EtherPacket[50] = 0x04;	
	
	
	EtherPacket[70] = 0x00;	//IO data
	EtherPacket[71] = 0x30;						
	EtherPacket[72] = 0x05;

}

//add motor and IO data for send packet
static void AddNodePacket(unsigned char * EtherPacket)
{


	//calculate pulsecount for axis
	pulse_count_x = CalcPulseCount(PulseCard_array->xposcmd,&remain_dis_x,&poscmd_old_x);
	//pulse_count_y = CalcPulseCount(PulseCard_array->yposcmd,&remain_dis_y,&poscmd_old_y);
	//pulse_count_z = CalcPulseCount(PulseCard_array->zposcmd,&remain_dis_z,&poscmd_old_z);
	//pulse_count_a = CalcPulseCount(PulseCard_array->aposcmd,&remain_dis_a,&poscmd_old_a);
	//pulse_count_b = CalcPulseCount(PulseCard_array->bposcmd,&remain_dis_b,&poscmd_old_b);
	//pulse_count_c = CalcPulseCount(PulseCard_array->xposcmd,&remain_dis_x,&poscmd_old_x);
		
//	rtapi_print_msg(RTAPI_MSG_ERR,"successed for pulse_count,enter into add motor information  \n");
	//inject motor and IO data into send packet
	EtherPacket[26] = (long)pulse_count_x;
	EtherPacket[25] = (long)(pulse_count_x) >> 8;
	EtherPacket[24] = (long)(pulse_count_x) >> 16;
	EtherPacket[23] = (long)(pulse_count_x) >> 24;
/*
	EtherPacket[33] = (long)pulse_count_y;
	EtherPacket[32] = (long)(pulse_count_y) >> 8;
	EtherPacket[31] = (long)(pulse_count_y) >> 16;
	EtherPacket[30] = (long)(pulse_count_y) >> 24;

	EtherPacket[40] = (long)pulse_count_z;
	EtherPacket[39] = (long)(pulse_count_z) >> 8;
	EtherPacket[38] = (long)(pulse_count_z) >> 16;
	EtherPacket[37] = (long)(pulse_count_z) >> 24;

	EtherPacket[47] = (long)pulse_count_a;
	EtherPacket[46] = (long)(pulse_count_a) >> 8;
	EtherPacket[45] = (long)(pulse_count_a) >> 16;
	EtherPacket[44] = (long)(pulse_count_a) >> 24;

	EtherPacket[54] = (long)pulse_count_b;
	EtherPacket[53] = (long)(pulse_count_b) >> 8;	
	EtherPacket[52] = (long)(pulse_count_b) >> 16;
	EtherPacket[51] = (long)(pulse_count_b) >> 24;
*/	
//	rtapi_print_msg(RTAPI_MSG_ERR,"successed for add motor information \n");

}
/***********************************************************************
*                   LOCAL FUNCTIONS DEFINITION                          *
************************************************************************/

static int export_PulseCard(int num, PulseCard_t * addr, int step_type, int pos_mode)
{
    int retval, msg;

    /* This function exports a lot of stuff, which results in a lot of
       logging if msg_level is at INFO or ALL. So we save the current value
       of msg_level and restore it later.  If you actually need to log this
       function's actions, change the second line below */
	   
    msg = rtapi_get_msg_level();
    rtapi_set_msg_level(RTAPI_MSG_WARN);
	
    /* export pins for poscmd */
	retval = hal_pin_float_newf(HAL_IN, &(addr->xposcmd), comp_id, "PulseCard.%d.xposition-cmd", num);
    if (retval != 0) { return 1; }

	retval = hal_pin_float_newf(HAL_IN, &(addr->yposcmd), comp_id, "PulseCard.%d.yposition-cmd", num);                                                 
	if (retval != 0) { return 2; }

    retval = hal_pin_float_newf(HAL_IN, &(addr->zposcmd), comp_id, "PulseCard.%d.zposition-cmd", num);                                                    
	if (retval != 0) { return 3; }

    retval = hal_pin_float_newf(HAL_IN, &(addr->aposcmd), comp_id, "PulseCard.%d.aposition-cmd", num);    
	if (retval != 0) { return 4; }
	
    retval = hal_pin_float_newf(HAL_IN, &(addr->bposcmd), comp_id, "PulseCard.%d.bposition-cmd", num);
	if (retval != 0) { return 5; }

	retval = hal_pin_float_newf(HAL_IN, &(addr->cposcmd), comp_id, "PulseCard.%d.cposition-cmd", num);                                                
	if (retval != 0) { return 6; }


    /* export pin for enable  */
    retval = hal_pin_bit_newf(HAL_IN, &(addr->enable), comp_id,"PulseCard.%d.enable", num);
    if (retval != 0) { return 7; }

	/* export pin for spindle CW	*/
	retval = hal_pin_bit_newf(HAL_IN, &(addr->spindlecw), comp_id,"PulseCard.%d.spindlecw", num);
	if (retval != 0) { return 9; }
	
	
	/* export pin for E-STOP */
	retval = hal_pin_bit_newf(HAL_IN, &(addr->estop), comp_id,"PulseCard.%d.estop", num);
	if (retval != 0) { return 8; }

	
    /* export pins for home signal */
    retval = hal_pin_bit_newf(HAL_OUT, &(addr->homex), comp_id,"PulseCard.%d.home-x", num);
    if (retval != 0) { return 10; }
	
    retval = hal_pin_bit_newf(HAL_OUT, &(addr->homey), comp_id,"PulseCard.%d.home-y", num);       
	if (retval != 0) { return 11; }

	retval = hal_pin_bit_newf(HAL_OUT, &(addr->homez), comp_id,"PulseCard.%d.home-z", num);    
	if (retval != 0) { return 12; }

    retval = hal_pin_bit_newf(HAL_OUT, &(addr->homea), comp_id,"PulseCard.%d.home-a", num);
    if (retval != 0) { return 13; }

	retval = hal_pin_bit_newf(HAL_OUT, &(addr->homeb), comp_id,"PulseCard.%d.home-b", num);
	if (retval != 0) { return 14; }

	retval = hal_pin_bit_newf(HAL_OUT, &(addr->homec), comp_id,"PulseCard.%d.home-c", num);
	if (retval != 0) { return 15; }


	// export pins 
    retval = hal_pin_bit_newf(HAL_OUT, &(addr->xlimpos), comp_id,"PulseCard.%d.xlimpos", num);
    if (retval != 0) { return 17; }

    retval = hal_pin_bit_newf(HAL_OUT, &(addr->xlimneg), comp_id,"PulseCard.%d.xlimneg", num);
    if (retval != 0) { return 18; }

    retval = hal_pin_bit_newf(HAL_OUT, &(addr->ylimpos), comp_id,"PulseCard.%d.ylimpos", num);
    if (retval != 0) { return 19; }

    retval = hal_pin_bit_newf(HAL_OUT, &(addr->ylimneg), comp_id,"PulseCard.%d.ylimneg", num);
    if (retval != 0) { return 20; }

    retval = hal_pin_bit_newf(HAL_OUT, &(addr->zlimpos), comp_id,"PulseCard.%d.zlimpos", num);
    if (retval != 0) { return 21; }

    retval = hal_pin_bit_newf(HAL_OUT, &(addr->zlimneg), comp_id,"PulseCard.%d.zlimneg", num);
    if (retval != 0) { return 22; }


	//set default values
    *(addr->enable) = 0;

    *(addr->xposcmd) = 0.0;
    *(addr->yposcmd) = 0.0;
    *(addr->zposcmd) = 0.0;
    *(addr->aposcmd) = 0.0;
    *(addr->bposcmd) = 0.0;
    *(addr->cposcmd) = 0.0;
 
	*(addr->homex) = 0; 
	*(addr->homey) = 0;
	*(addr->homez) = 0;
	*(addr->homea) = 0;
	*(addr->homeb) = 0;
	*(addr->homec) = 0;
        
    *(addr->xlimpos) = 0; 
	*(addr->xlimneg) = 0;
     
    *(addr->ylimpos) = 0; 
	*(addr->ylimneg) = 0;
     
    *(addr->zlimpos) = 0; 
	*(addr->zlimneg) = 0;

	Init();
 
    /* restort msg_level */
    rtapi_set_msg_level(msg);
    return 0;
}

static int Init(void){

	struct net_device *dev;

	memset(SndEtherData,0,sizeof(SndEtherData));
	memset(RcvEtherData,0,sizeof(RcvEtherData));

/*1、init netcard 
-----------------------*/

	//get netcard information
	dev = dev_get_by_name(&init_net,"eth0");		//根据名字获取设备句柄

	//init socket
	memset(&sa,0,sizeof(sa));
	sa.sll_family = AF_PACKET;
	sa.sll_ifindex = dev->ifindex;
	sa.sll_protocol = htons(ETH_P_ALL);

	
	sock_create_kern(PF_PACKET,SOCK_RAW,htons(ETH_P_ALL),&sock);
	
/*2、init send buffer
--------------------------*/	

	//add head information
    CfgEtherPacketHead(SndEtherData);	

	 return 0;	
}


/* 
	send function
*/
static int Mac_SendTo(struct socket *sock, void * buff, size_t len, 
                unsigned flags, struct sockaddr *addr, int addr_len) 
{ 
        struct kvec vec_Send; 
		
        struct msghdr msg_Send; 

        vec_Send.iov_base=buff; 
        vec_Send.iov_len=len;

        memset(&msg_Send, 0x00, sizeof(msg_Send)); 
        msg_Send.msg_name=addr; 
        msg_Send.msg_namelen=addr_len; 
        msg_Send.msg_flags = flags | MSG_DONTWAIT; 

        return kernel_sendmsg(sock, &msg_Send, &vec_Send, 1, len); 
} 


/* 
	receive function
*/
static int Mac_RecvFrom(struct socket *sock, void * buff, size_t len, 
                unsigned flags, struct sockaddr *addr, int addr_len) 
{ 
        static struct kvec vec_Rcv; 
        static struct msghdr msg_Rcv; 

        vec_Rcv.iov_base=buff; 
        vec_Rcv.iov_len=len;

        memset(&msg_Rcv, 0x00, sizeof(msg_Rcv)); 
        msg_Rcv.msg_name=addr; 
        msg_Rcv.msg_namelen=addr_len; 
        msg_Rcv.msg_flags = flags | MSG_DONTWAIT; 

        return kernel_recvmsg(sock, &msg_Rcv, &vec_Rcv, 1, len,MSG_DONTWAIT); 
} 


/*
	pulse count function
*/

static double CalcPulseCount(hal_float_t *poscmd,hal_float_t *remain_dis,hal_float_t *poscmd_old){
	
	hal_float_t theory_dis = 0;
	hal_float_t  real_dis = 0;
	int sign = 0;
	
	theory_dis = ((*poscmd)-(*poscmd_old))*100.0 + *(remain_dis);//*10000+(*remain_dis);	//理论的相对距离 unit:count
	sign = theory_dis >= 0 ? 1:-1;
	real_dis = (int)(fabs(theory_dis) +0.5)*sign;					


	(*remain_dis) = theory_dis - real_dis;
	(*poscmd_old) = (*poscmd);

	return real_dis;  
}