#include "QMC5883L.h"
struct QMCSTR QMC5883LStr;
static void QmcTest(void)
{
        static enum QMC_STATE QmcState = IDLE;
        static uint8_t buff[MagDataNum][6] = {0},qmc_dec_count = 0;
        uint32_t temp[6] = {0},i=0,j=0;
	switch(QmcState)
	{
		case qmc_init:
		{
                    if(Init_HMC5883())
                    {
                        QmcState = qmc_dec;
                        qmc_dec_count = 0;
                        ge_state = YES;
                    }
                    else
                    {
                        ge_state = NO;
                    }
                    break;
		}
		case qmc_dec:
		{
                      if((Single_Read_HMC5883(SLAVE_ADD,0x06))&0x01){;}
                      else
                        return;
                      Multiple_Read_HMC5883(SLAVE_ADD,buff[qmc_dec_count++]);
		      if(qmc_dec_count>=MagDataNum)
                      {
                          QMC5883LStr.DataOkFlag = TRUE;
                          for(j=0;j<6;j++)
                          {
                              for(i=1;i<MagDataNum-1;i++)temp[j] += buff[i][j];
                              temp[j] = temp[j]/(MagDataNum-2);
                          }
                          for(i=0;i<6;i++)QMC5883LStr.QMC_CUR_ROPORT[i] = temp[i];
                          for(i=0;i<3;i++)
                          {
                                QMC5883LStr.QMC_CUR[i] = QMC5883LStr.QMC_CUR_ROPORT[i*2+1];
                                QMC5883LStr.QMC_CUR[i]<<=8;
                                QMC5883LStr.QMC_CUR[i] += QMC5883LStr.QMC_CUR_ROPORT[i*2];
                          }
                          if(HC_DEBUG)UsartPrint(QMC5883LStr.QMC_CUR_ROPORT,6);
                          QmcState = qmc_lowpower;
                      }
                      break;
		}
		case qmc_lowpower:
		{
                      Single_Write_HMC5883(SLAVE_ADD,0x0a,0x80);
                      SCL_H;
                      SDA_H;
                      QmcState = IDLE;
                      break;
		}
                case IDLE:
                {
                    if(QMC5883LStr.DataOkFlag==0)QmcState = qmc_init;
                    break;
                }
		default :break;
	}

}

void QmcDeal(void)
{
	QmcTest();
}

////////////////////地磁检测车进入//////////////////////////////////////////
/***
输入：无
功能：地磁检测车进入
输出：无
***/
int geomag_car_in(void)
{
        static uint8_t stable_count = 0;
        static int tempX,tempY,tempZ,tempStableX,tempStableY,tempStableZ;
        tempX = abs(QMC5883LStr.QMC_CUR[0] - QMC5883LStr.QMC_BASE[0]);
        tempY = abs(QMC5883LStr.QMC_CUR[1] - QMC5883LStr.QMC_BASE[1]);
        tempZ = abs(QMC5883LStr.QMC_CUR[2] - QMC5883LStr.QMC_BASE[2]);
        tempStableX = abs(QMC5883LStr.QMC_CUR[0] - QMC5883LStr.QMC_BEFORE[0]);
        tempStableY = abs(QMC5883LStr.QMC_CUR[1] - QMC5883LStr.QMC_BEFORE[1]);
        tempStableZ = abs(QMC5883LStr.QMC_CUR[2] - QMC5883LStr.QMC_BEFORE[2]);
	if(( tempX > 2*SystemPara.QmcFixPara[2])||(tempZ > 2*SystemPara.QmcFixPara[2]))//已检测到车
	{
		//前后二次电磁值相比较，若大于阀值，则说明变化明显，有效
		if(tempStableX > 0x50||tempStableZ > 0x50)//车未停稳
		{
                        memcpy(QMC5883LStr.QMC_BEFORE,QMC5883LStr.QMC_CUR,3*sizeof(int16_t));
			stable_count = 0;
		}
		else //车稳了
		{
			stable_count++;
			if(stable_count >= 6)
			{
				stable_count = 6;
				memcpy(QMC5883LStr.QMC_STABLE,QMC5883LStr.QMC_CUR,3*sizeof(int16_t));
				return 1;
			}
		}
	}
        return 0;
}
/////////////////////////////地磁检测车离开/////////////////////////////
/***
输入：first_dec：对参数进行清零；next_dec：正常检测
功能：地磁检测车离开
输出：1：车离开；0：车未离开
***/
int geomag_car_out(int flag)
{
        int tempX,tempY,tempZ,tempStableX,tempStableY,tempStableZ;
        static uint8_t again_change_count = 0,again_change_count1 = 0;
        tempX = abs(QMC5883LStr.QMC_CUR[0] - QMC5883LStr.QMC_STABLE[0]);
        tempY = abs(QMC5883LStr.QMC_CUR[1] - QMC5883LStr.QMC_STABLE[1]);
        tempZ = abs(QMC5883LStr.QMC_CUR[2] - QMC5883LStr.QMC_STABLE[2]);
        tempStableX = abs(QMC5883LStr.QMC_CUR[0] - QMC5883LStr.QMC_BEFORE[0]);
        tempStableY = abs(QMC5883LStr.QMC_CUR[1] - QMC5883LStr.QMC_BEFORE[1]);
        tempStableZ = abs(QMC5883LStr.QMC_CUR[2] - QMC5883LStr.QMC_BEFORE[2]);


         /***有波动***/
        if((tempX > SystemPara.QmcFixPara[2])||(tempZ > SystemPara.QmcFixPara[2]))
	{
                if((tempStableX > 0x50)||(tempStableZ > 0x50))
                {
			again_change_count1 = 0;
		}
		else
		{
			again_change_count1++;
			if(again_change_count>=SystemPara.QmcFixPara[1] )
			{
                                 geo_wave_flag = 1;//有波动
			}

		}

	}

	if((tempX > 2*SystemPara.QmcFixPara[2])||(tempZ > 2*SystemPara.QmcFixPara[2]))
	{
		if((tempStableX > 0x50)||(tempStableZ > 0x50))
		{
			memcpy(QMC5883LStr.QMC_BEFORE,QMC5883LStr.QMC_CUR,3*sizeof(int16_t));
			again_change_count = 0;
		}
		else
		{
			again_change_count++;
			if(again_change_count>=SystemPara.QmcFixPara[1] )
			{
                               return 1;
			}

		}
	}
	return 0;

}
/////////////////////////////地磁自适应////////////////////////////////
/***
功能：地磁自适应
***/
void geomag_self_adaption(void)
{
	//set_parking_space_status=0;
	//mag_time_count=0;
        int mag_x_OPT,mag_y_OPT;
        int mag_x_filter = QMC5883LStr.QMC_CUR[0],mag_y_filter = QMC5883LStr.QMC_CUR[2];
        int CARON_magX = QMC5883LStr.QMC_BASE[0],CARON_magY = QMC5883LStr.QMC_BASE[2];

	mag_x_OPT=abs(mag_x_filter-CARON_magX);
	mag_y_OPT=abs(mag_y_filter-CARON_magY);
	if(mag_x_OPT>0x90||mag_y_OPT>0x90)
	{
		if(mag_x_OPT>mag_y_OPT)
		{
			SystemPara.QmcFixPara[2]= mag_x_OPT/3;
		}
		else
			SystemPara.QmcFixPara[2]= mag_y_OPT/3;
	}
	else if(SystemPara.QmcFixPara[2]>=45)
	{
		SystemPara.QmcFixPara[2]=SystemPara.QmcFixPara[2]-0x10;
	}
}
//////////////////////////////
/***
功能：有车处理函数
***/
void have_car_send()
{
                memcpy(QMC5883LStr.QMC_STABLE,QMC5883LStr.QMC_CUR,3*sizeof(int16_t));
                get_photoresistor_ad();
                ph_stable_value = VAL_PHOTO;
                judge_day_night(first_dec);
                ph_car_out(first_dec);
                geomag_car_out(first_dec);                          
                BeepStart(4,0);
                CarRecDataStr.CarState = 1;
                CarRecDataStr.CmdSource = 3;
                SendData(HAVE_CAR);
}

/***
功能：无车处理函数
***/
void NO_car_deal(void)//无车处理
{
        mag_time_count = 0;
        BeepStart(1,1);
        CarRecDataStr.CarState = 0;
        CarRecDataStr.CmdSource = 2;
        no_car_count ++;
        if(no_car_count>=6)//无车自动升起只允许尝试5次
        {
              no_car_count = 6;
              return;
        }
	geomag_car_out(first_dec);//复位内部变量
	ph_car_out(first_dec);//复位内部变量
        CarAction(0,0,1,1);
        CarRecDataStr.CarState = 0;
        CarRecDataStr.CmdSource = 2;
        judge_day_night(first_dec);
        SendData(HAVE_CAR);
        
}
