/*
 * ctrl_process.c
 *
 *  Created on: May 4, 2015
 *      Author: root
 */
#include "common.h"
#include <stdlib.h>
#include "thread.h"
#include <pthread.h>
#include "msq.h"
#include "serial.h"
#include "ysp_debug.h"
#include <time.h>

#define MAX_SEND_TIME_CURVE 5
#define MAX_SAVE_TEMP       256

mqd_t ctrl_tx_mq;//

extern int ctrl_fd;//控制信号发送串口句柄 

COM_HEADER ctrl_tx_header;
COM_HEADER ctrl_rx_header;

OUTPUT_TYPE_DEF ctrl_tx_data;

unsigned char ctrl_tx_buf[128];//串口发送缓冲区
unsigned char ctrl_rx_buf[128];//

unsigned char ctrl_tx_mq_buf[CTRL_TX_MESSAGE_SIZE];//消息队列接收缓冲区

static const unsigned int timeout_curve[MAX_SEND_TIME_CURVE]={
200000,300000,500000,1000000,2000000
};//发送间隔


float latest_spz_temp[MAX_SAVE_TEMP];
unsigned char spz_save_index=0;
/*
static const char *relay_name[]={
"LJ_FAN",
"SAMPLE_FAN",
"YV6",
"YV4",
"no_use",
"no_use",
"YV7",
"YV14",
"YV13",
"YV10",
"YV11",
"YV12",
"no",
"no",
"no",
"YV16",
"YV17",
"YV18",
"YV19",
"YV20",
"YV21",
"NO",
"NO",
"NO",
"NO",
"NO",
"NO",
"NO",
"NO",
"NO",
"NO",
};*/


void arm_var_f32(float *pSrc,unsigned int blockSize,float *pResult)
{
  float sum = (float) 0.0;               
  float meanOfSquares, mean, in, squareOfMean;      
  unsigned int blkCnt;                               
  float *pIn;                                
  pIn = pSrc;
  blkCnt = blockSize>>2u;
  while(blkCnt>0u)
  {
    in = *pSrc++;
    sum += in * in;
    in = *pSrc++;
    sum += in * in;
    in = *pSrc++;
    sum += in * in;
    in = *pSrc++;
    sum += in * in;
    blkCnt--;
  }
  blkCnt = blockSize % 0x4u;

  while(blkCnt>0u)
  {
    in = *pSrc++;
    sum += in * in;
    blkCnt--;
  }
  meanOfSquares = sum/((float) blockSize-1.0f);

  sum = 0.0f;


  blkCnt = blockSize>>2u;

  pSrc = pIn;

  while(blkCnt>0u)
  {
    sum += *pSrc++;
    sum += *pSrc++;
    sum += *pSrc++;
    sum += *pSrc++;
    blkCnt--;
  }

  blkCnt = blockSize%0x4u;

  while(blkCnt>0u)
  {
    sum += *pSrc++;
    blkCnt--;
  }
  mean = sum/(float)blockSize;

  squareOfMean = (mean * mean) *(((float)blockSize)/((float)blockSize-1.0f));
  *pResult = meanOfSquares - squareOfMean;
}

void save_spz_temp(float *temp)
{
 latest_spz_temp[spz_save_index++]=*temp;
}

int get_spz_stable(float *delt)
{
  lock_input_data();
  arm_var_f32(latest_spz_temp,sizeof(latest_spz_temp)/sizeof(float),delt);
  unlock_input_data();
  /*if(*delt<SPZ_TEMP_DELT)
    return 0;
  return 1;	*/
  return 0;
}

int set_gpio_output(int index,int val)
{
  static int iofd[4]={-1,-1,-1,-1};
  char c;
  if(iofd[index]<0)
   {
     if((iofd[index]=open("/sys/class/leds/mcu_intr/brightness",O_WRONLY))==-1)//打开设备 
	  {
	  	#ifdef DEBUG
	     printf("open %s failed \n",io_dev_name[index]);
	    #endif
	    return -1;
	  }
   }
   c = val?'1':'0';
  return write(iofd[index],&c,1);//写数据到设备 
}

int send_ctrl_frame(int dev_fd,COM_HEADER *head,void *data,unsigned int data_length)
{
  unsigned int data_bytes;
  int ret;
  data_bytes=sizeof(COM_HEADER)+data_length;
  ctrl_tx_buf[0]=0xAA;
  ctrl_tx_buf[1]=0x1;//address
  ctrl_tx_buf[2]=(unsigned char)data_bytes;
  ctrl_tx_buf[3]=(unsigned char)(data_bytes>>8);
  ctrl_tx_buf[4]=0x01;//cmd
  memcpy(&ctrl_tx_buf[5],head,sizeof(COM_HEADER));//拷贝信息头 
  memcpy(&ctrl_tx_buf[5+sizeof(COM_HEADER)],data,data_length);//拷贝数据 
  ctrl_tx_buf[5+data_bytes]=cal_sum(ctrl_tx_buf,5+data_bytes);
  ctrl_tx_buf[6+data_bytes]=0x55;
  set_gpio_output(0,1);
  ret=SerialWrite(dev_fd,ctrl_tx_buf,7+data_bytes);
  //usleep(10000);
  set_gpio_output(0,0);
  return ret;
}


//控制数据重发 
int ctrl_data_retrans(void)
{
  static mqd_t ctrl_tx_slave_mq=(mqd_t)-1;
  unsigned char dumy_msg[16];
  if(ctrl_tx_slave_mq==-1)
   {
	 if((ctrl_tx_slave_mq= open_mq(CTRL_TX_MQ))==((mqd_t)-1))
	    return -1;
   }
  if(mq_send(ctrl_tx_slave_mq,(const char *)dumy_msg,sizeof(dumy_msg),CTRL_MQ_PRIO)==-1)
	 return -2;
 
  return 0;
}

void show_hex(unsigned char *buf,unsigned int len)
{
 unsigned int i;
 for(i=0;i<len;i++)
	{
	 printf("0x%2x ",*buf++);
	 if((i+1)%16==0)
	 {
	   printf("\r\n");
	 }
	}
  printf("\r\n\r\n");
}


//获取IO输入状态 
unsigned int io_get(unsigned int i)
{
  unsigned int val;
  lock_input_data(); 
  memcpy(&val,&my_input_data.io_input,sizeof(val));
  unlock_input_data();
  if((val&(1<<i))!=0)
    return ON;
  return OFF; 
} 


int get_pres_vol(unsigned int ch,float *fval)
{
  lock_input_data(); 
  *fval=my_input_data.PresVolt[ch%5];
  unlock_input_data();
  return 0;	
}

//获取通道温度 
int get_temperature(unsigned int ch,float *fval)
{
  if(ch>=5)
    return -1;
  if((my_input_data.work_state&(1<<ch))!=0)
    return -2;
  lock_input_data(); 
  *fval=my_input_data.temperature[ch%5];
  unlock_input_data();
  return 0;
}

/*
//电流转换成压力 
void CurrCnvToPres(float *raw_data,float *fval)
{
  float temp;
  temp=(0.82846*(*raw_data))/3000;
  *fval=(0.2*temp-0.1);  
}

//获取电流通道压力 
void CurrCnvToPresEx(unsigned int ch,float *fval)
{
  float temp;
  get_curr(ch,&temp);
  CurrCnvToPres(&temp,fval);
}*/

//电压转换成压力 
/*/void VoltCnvToPres(unsigned int ch,float *fval)
{
  if(fval!=NULL)
    *fval=(0.4*(*fval)); 
}*/

//获取通道压力 
int VoltCnvToPresEx(unsigned int ch,float *fval)
{
  float temp;
  get_pres_vol(ch,&temp);
  //VoltCnvToPres(&temp);
  if(fval!=NULL)
    *fval=(my_para_data.mach_run_para.pres_fac[ch].a)*temp+(my_para_data.mach_run_para.pres_fac[ch].b);
  //*fval=temp;
  return 0;
}

//获取通道压力
int get_presure(unsigned int ch,float *fval)
{
  if(ch>=5)
    return -1;
  //if((my_input_data.work_state&(1<<ch))!=0)
   // return -2;
  //lock_input_data(); 
  VoltCnvToPresEx(ch,fval);
  //unlock_input_data();
  return 0;
}


void relay_clr(unsigned int mask)
{
 lock_output_data();
 my_output_data.relay_ctrl&=mask;
 unlock_output_data();
 ctrl_data_retrans();	
}

void set_man_out(unsigned int ch,unsigned int val)
{
 lock_output_data();
 my_output_data.man_output[ch]=val;
 unlock_output_data();
 //ctrl_data_retrans();	
}

void set_pid_ch(unsigned int ch,unsigned int en_dis,unsigned int mode)
{
  //unsigned int val;
  lock_output_data();
  ch=ch%4;
  if(my_output_data.pid_en[ch]!=en_dis)
    {
   	  if(mode)
        ctrl_data_retrans();	
    }
  my_output_data.pid_en[ch]=en_dis; 
  unlock_output_data();	
}

//设置目标温度 
void set_pid_temp(unsigned int ch,float *temp,unsigned int mode)
{
  lock_output_data();
  ch&=3;
  my_output_data.temperature[ch]=*temp;
  if(mode)
   {
     ctrl_data_retrans();
   }   	
  unlock_output_data();		
}

void relay_set(unsigned int i,unsigned char val,unsigned int mode)
{
	unsigned int tmp;
	unsigned int pos=(1<<i);
	int retrans_flg=0;
	if(i>31)
		return;
    lock_output_data();
	tmp=my_output_data.relay_ctrl;
	if(val!=OFF)//=ON
	{
	  tmp|=pos;
	}
	else//=OFF
	{
	  tmp&=(~pos);
	}
	if(tmp!=my_output_data.relay_ctrl)
	{
	  my_output_data.relay_ctrl=tmp;
	  retrans_flg=1;   
	}
    unlock_output_data();
    if(retrans_flg)
      {
      	//printf("relay=%x %x %x %x\n",my_output_data.relay_ctrl[0],my_output_data.relay_ctrl[1],my_output_data.relay_ctrl[2],my_output_data.relay_ctrl[3]);
  	   if(mode)
	    ctrl_data_retrans();
      }
    //show_hex(my_output_data.relay_ctrl,8);
}

void relay_set_all(unsigned int val,unsigned int mode)
{
	unsigned int tmp;
	int retrans_flg=0;

    lock_output_data();
	tmp=my_output_data.relay_ctrl;

	if(tmp!=val)
	{
	  my_output_data.relay_ctrl=val;
	  retrans_flg=1;   
	}
    unlock_output_data();
    if(retrans_flg)
    {      
     if(mode)
       ctrl_data_retrans();
    }
}

unsigned int relay_get(unsigned int i,unsigned char *val)
{
  unsigned int pos=(1<<i);
  unsigned int ret;
  lock_input_data(); 
  if((my_input_data.relay_state&pos)!=0)
    {
      ret=ON;	
    }
  else
    {
      ret=OFF;	
    }
  if(val!=NULL)
    *val=ret;
  unlock_input_data();
 return ret;	
}

unsigned int relay_get_all(unsigned int *val)
{
  unsigned int ret;
  lock_input_data();
  ret=my_input_data.relay_state; 
  if(val!=NULL)
    *val=ret;
  unlock_input_data();
 return ret;	
}

void ctrl_tx_routine(void *arg)
{
  unsigned int send_index=0;
  unsigned int curr_timeout;

  
  if((ctrl_tx_mq=create_mq(CTRL_TX_MQ,CTRL_TX_MAX_MESSAGE,CTRL_TX_MESSAGE_SIZE))<0)
   {
   	 printf("create ctrl tx message queue failed\n");
   	 exit(1);
   }

  ctrl_tx_header.stNum=0;
  ctrl_tx_header.sqNum=0;
  while(1)
   {
     curr_timeout=timeout_curve[send_index%MAX_SEND_TIME_CURVE];
     if(recv_mq_wait(ctrl_tx_mq,(void *)ctrl_tx_mq_buf,sizeof(ctrl_tx_mq_buf),curr_timeout)>0)
	   {
   		 //清空消息队列?
	     ++ctrl_tx_header.stNum;
         if(ctrl_tx_header.stNum==0)
           ++ctrl_tx_header.stNum;	  //skip over 0
         ctrl_tx_header.sqNum = 0;	  //reset sqNum to 0 on each state change
   	     send_index=0;
   	     //show_time("tx");
   	     //printf("state change:stNum=%d sqNum=%d\r\n",ctrl_tx_header.stNum,ctrl_tx_header.sqNum);
   	     //LOG_DEBUG(COMUNICATION_TRACE_DEBUG,"state change:stNum=%d sqNum=%d\n",ctrl_tx_header.stNum,ctrl_tx_header.sqNum);
		 usleep(10000);
	   }
	 else
	   {
	     if(send_index<(MAX_SEND_TIME_CURVE-1))
	       {
   		     send_index++;
       	   }
       	 ++ctrl_tx_header.sqNum;
       	//printf("stNum=%d sqNum=%d\n",ctrl_tx_head.stNum,ctrl_tx_head.sqNum);
   	   }
     //发送数据
     lock_output_data();
     memcpy(&my_output_data.pid_para[0],&my_para_data.mach_run_para.pid_type_def[PID_SPZ],sizeof(PID_TYPE_DEF));
     memcpy(&my_output_data.pid_para[1],&my_para_data.mach_run_para.pid_type_def[PID_LJ],sizeof(PID_TYPE_DEF));
     memcpy(&my_output_data.pid_para[2],&my_para_data.mach_run_para.pid_type_def[PID_SAMPLE],sizeof(PID_TYPE_DEF));
     my_output_data.temperature[TEMP_SPZ]=my_para_data.mach_run_para.temperature[TEMP_SPZ];
     my_output_data.temperature[TEMP_LJ]=my_para_data.mach_run_para.temperature[TEMP_LJ];
     my_output_data.temperature[TEMP_SAM]=my_para_data.mach_run_para.temperature[TEMP_SAM];
	 //memcpy(my_output_data.pid_en,my_para_data.mach_run_para.pid_en,sizeof(my_output_data.pid_en));
	 my_output_data.man_output[0]=(unsigned short)my_para_data.mach_run_para.volt_output[0]; 
	 my_output_data.man_output[2]=0;
	 my_output_data.man_output[3]=0;
	 unlock_output_data();
	 if(send_ctrl_frame(ctrl_fd,&ctrl_tx_header,&my_output_data,sizeof(my_output_data))<(sizeof(my_output_data)+7))
	  {
	    LOG_DEBUG(COMUNICATION_EEROR_DEBUG,"send data failed\n");
	  }
   }
}


int process_message(unsigned char *msg_buf,unsigned int msg_length)
{
  static unsigned int spz_index=0;
  //unsigned int i=0;
  unsigned int length;
  COM_HEADER received_header;
  if(msg_length<(7+sizeof(COM_HEADER)))
    return -1;
  if(msg_buf[0]!=0xAA)
    return -2;
  if(msg_buf[msg_length-1]!=0x55)
    return -3;
  length=(unsigned int)256*msg_buf[3]+msg_buf[2];
  if(length!=(msg_length-7))
    return -4;
  if(chk_sum(msg_buf,msg_length-2,msg_buf[msg_length-2])!=0)
    return -5;
  memcpy(&received_header,msg_buf+5,sizeof(COM_HEADER));
  if(received_header.stNum!=ctrl_rx_header.stNum)
    {
   	  //数据有变化 
      //printf("state change stNum=%d sqNum=%d\n",received_header.stNum,received_header.sqNum);
    }
  else if(received_header.sqNum!=(ctrl_rx_header.sqNum+1))
    {
      //丢帧了
	  printf("lost frame old_sq=%d new_sq=%d\n",ctrl_rx_header.sqNum,received_header.sqNum);	
    }
  memcpy(&ctrl_rx_header,&received_header,sizeof(COM_HEADER));
  //获取信号量
  lock_input_data();
  //拷贝数据到全局缓冲   
  memcpy(&my_input_data,&msg_buf[5+sizeof(COM_HEADER)],sizeof(my_input_data));
  //保存最近的色谱柱温度,用于分析温度是否稳定
  spz_index++;
  if((spz_index&3)==0)
    {
      save_spz_temp(&my_input_data.temperature[TEMP_SPZ]);   	
    }
  //printf("temp=%f %f %f %f pres=%f %f\n",my_input_data.temperature[0],my_input_data.temperature[1],my_input_data.temperature[2],my_input_data.temperature[3],my_input_data.PresVolt[6],my_input_data.Current[0]);
  //释放信号量 
  unlock_input_data();
  //printf("io_input=0x%2x 0x%2x\n",my_input_data.io_input[0],my_input_data.io_input[1]);
  return 0;
}


//接收控制MCU发送的信息,并存入共享内存
void ctrl_rx_routine(void *arg)
{
 
  int curr_length,last_length,rx_length;
  int recv_error_tick=0;
  pthread_t ctrl_tx_thread;

  rx_length=0;  //本次接收的字节长度
  last_length=0;//上一次接收的字节长度
    //打开全局设备
  if((ctrl_fd=SerialOpen(CTRL_SERIAL_DEV,CTRL_SERIAL_SPEED))==-1)
   {
     LOG_DEBUG(COMUNICATION_EEROR_DEBUG,"open ctrl serial device failed\n");
     pthread_exit(1);
   }
  if((ctrl_tx_thread=task_create(ctrl_tx_routine,NULL,0,0))==-1)
   {
     LOG_DEBUG(COMUNICATION_EEROR_DEBUG,"create ctrl_tx_routine failed\n");
   	 close(ctrl_fd);
   	 pthread_exit(2);
   }
  my_input_data.com_flg=1;
  while(1)
  {
    usleep(30000);//wait 30ms
    curr_length=read(ctrl_fd,ctrl_rx_buf+rx_length,sizeof(ctrl_rx_buf)-rx_length);
    if(curr_length>0)
     {
   	  //存入缓冲区
 	  rx_length+=curr_length;
      last_length=curr_length;
 	  if(rx_length>=sizeof(ctrl_rx_buf))
 	   {
 	   	 //printf("code1=%d rx_length=%d\n",process_message(ctrl_rx_buf,rx_length),rx_length);//处理消息
 	   	 if(process_message(ctrl_rx_buf,rx_length)==0)
 	   	   {
 	   	   	 if(recv_error_tick>200)
 	   	   	   {
   	   	   	 	 LOG_DEBUG(ERROR_DEBUG,"ctrl mcu recovery\n");
   	   	   	   }
   	   	 	 recv_error_tick=0;
   	   	 	 if(comunication_flg==0)
   	   	 	   {
   	 	   	   	ysp_glb_data.MoDevConf.v.stVal=0;
       	  	    ysp_glb_data.MoDevConf.q=0;
       	  	    ysp_glb_data.MoDevConf.t.secs=(unsigned int)time(NULL);
   	 	   	   }
   	   	 	 comunication_flg=1;
   	   	   }
		 rx_length=0;
 	   	 memset(ctrl_rx_buf,0,sizeof(ctrl_rx_buf));
       }
     }
    else
     {
      if(last_length>0)
 	   {
         //printf("code2=%d rx_length=%d\n",process_message(ctrl_rx_buf,rx_length),rx_length);//处理消息
         if(process_message(ctrl_rx_buf,rx_length)==0)
           {
      	     if(recv_error_tick>200)
   	   	      {
   	   	   	 	 LOG_DEBUG(ERROR_DEBUG,"ctrl mcu recovery\n");
   	   	      }
           	 recv_error_tick=0;
           	  if(comunication_flg==0)
   	   	 	   {
   	 	   	   	ysp_glb_data.MoDevConf.v.stVal=0;
       	  	    ysp_glb_data.MoDevConf.q=0;
       	  	    ysp_glb_data.MoDevConf.t.secs=time(NULL);
   	 	   	   }
           	 comunication_flg=1;
           }
		 memset(ctrl_rx_buf,0,sizeof(ctrl_rx_buf));
  	   }
      else
       {
       	 //出错处理
       	 recv_error_tick++;
       	 if(recv_error_tick>256)
       	  {
       	  	comunication_flg=0;       	  	
  	        if(recv_error_tick==257)
			  {
			  	ysp_glb_data.MoDevConf.v.stVal=1;
       	  	    ysp_glb_data.MoDevConf.q=0;
       	  	    ysp_glb_data.MoDevConf.t.secs=(unsigned int)time(NULL);
  			    LOG_DEBUG(ERROR_DEBUG,"ctrl mcu lost\n");	
  			  }	
          }
       }
 	  last_length=0;
 	  rx_length=0;
     }
  }
 //wait ctrl_tx_thread exit
 close(ctrl_fd);
}

int ctrl_init(void)
{
  pthread_t ctrl_rx_thread;
  if((ctrl_rx_thread=task_create(ctrl_rx_routine,NULL,0,0))==-1)
   {
	 LOG_DEBUG(COMUNICATION_EEROR_DEBUG,"create ctrl_tx_routine failed\n");
	 return 1;
   }
  return 0;
}