#include "includes.h"
#include  "math.h"


SUT_XINBIAO_PAGE sut_xinbiaopage[XINBIAO_NUM];
SUT_XINBIAO_PAGE New_xinbiaopage;
SUT_XINBIAO_INFO sut_xinbiao_info;

USERARR UserArr[MAX_USER];  //临时存储当前所有成员   //每次 86  都需要更新
LEADER leader[XINBIAO_NUM];  //根据 UserArr  每次更新 进表格  存入文件系统



void PushXinbiao(SUT_XINBIAO_PAGE*NewXinbiao);


/*
	获得新的信标页
	 callID    //是， 更新页  
	 	      //否，创建新页	 	      
	
*/

//+POC=8C00%08x%02x;%d;%d;%d;%d;%d;\r\n"
//+POC:8C00 934C0008 00 3B 


void Recv_PackNewxinbiaopage(char*msg)
{
	char i,j=0,z=0;
	char start=0;
	char *Pmsg=0;
	char data[500]={0};
	msg[17]=0;

		//处理完 清空
	memset(&New_xinbiaopage,0,sizeof(New_xinbiaopage));
		
	New_xinbiaopage.xb_CallID=htoi(msg+9);
	MSG_INFO(1,"xb_CallID===%x",New_xinbiaopage.xb_CallID);

	//find user name
	for(i=0;i<MAX_USER;i++){
		if(UserArr[i].id==New_xinbiaopage.xb_CallID){
		strcpy(New_xinbiaopage.xb_CallName,UserArr[i].UserName);
		}	
	}
	
	MSG_INFO(1,"New_xinbiaopage.xb_CallName===%s",New_xinbiaopage.xb_CallName);
	
	StrAsciiToHex(msg+19,data);
	Pmsg=data;
	
	MSG_INFO(1,"Pmsg===%s",data);
	z=strlen(Pmsg);
		
	for(i=0;i<z;i++){
		if(Pmsg[i]==';'){
			Pmsg[i]=0;
			switch(j){
				
				case 0:
					strcpy(New_xinbiaopage.xb_Firm_CallId,Pmsg+start);
					MSG_INFO(1,"step%02d=====%s",j,New_xinbiaopage.xb_Firm_CallId);
					break;
					
				case 1:
					New_xinbiaopage.xb_longitue=atoi(Pmsg+start);
					MSG_INFO(1,"step%02d=====%d",j,New_xinbiaopage.xb_longitue);
					break;

				case 2:
					New_xinbiaopage.xb_latitue=atoi(Pmsg+start);
					MSG_INFO(1,"step%02d=====%d",j,New_xinbiaopage.xb_latitue);
					break;

				case 3:
					New_xinbiaopage.xb_heading=atoi(Pmsg+start);
					MSG_INFO(1,"step%02d=====%d",j,New_xinbiaopage.xb_heading);//航向
					break;
					
				case 4:
					New_xinbiaopage.xb_headerSpeed=atoi(Pmsg+start);
					MSG_INFO(1,"step%02d=====%d",j,New_xinbiaopage.xb_headerSpeed);//航速
					break;

				case 5:
					New_xinbiaopage.xb_elevation=atoi(Pmsg+start);
					MSG_INFO(1,"step%02d=====%d",j,New_xinbiaopage.xb_elevation);//海拔
					break;
			}
			i++;
			start=i;
			j++;
		}

	}
	PushXinbiao(&New_xinbiaopage);
	sutPocStatus.newxinbiaoflag=1;
	sutPocStatus.newxinbiaoStanby=1;
	if(newPara.Xinbiao_LiveUpdate==1){
		if(sutUIstatus.Status==UIS_STANDBY||sutUIstatus.Status==UIS_CALL_WINDOW)uISetNextStatus(UIS_MENU_ONLINE_XINBIAO);
	}
														
}




const double EARTH_RADIUS = 6378.137;
#define PI 3.141592
#define RIGHT_ANGLE 90.0

static double Deg2Rad(double degree){ 
	return degree * PI / (RIGHT_ANGLE * 2);
	
}

 double GetDirectDistance(double srcLon, double srcLat, double destLon, double destLat){

		double radSrcLat = Deg2Rad(srcLat);	
		double radDestLat = Deg2Rad(destLat);
		double a = radSrcLat - radDestLat;
		double b = Deg2Rad(srcLon) - Deg2Rad(destLon); 
		double DirectDistance = 2 * asin(sqrt(pow(sin(a/2),2) +cos(radSrcLat)*cos(radDestLat)*pow(sin(b/2),2))); 
		DirectDistance = DirectDistance * EARTH_RADIUS;
		DirectDistance = round(DirectDistance * 10000) / 10000;
		return DirectDistance;		
}

void CalcDistance(SUT_XINBIAO_PAGE*NewXinbiao)
{
		double srcLon=sutGpsInfo.longitue; 
		double srcLat=sutGpsInfo.latitue;
		double destLon=NewXinbiao->xb_longitue;
		double destLat=NewXinbiao->xb_latitue;
		
		srcLon/=1000000;
		srcLat/=1000000;
		destLon/=1000000;
		destLat/=1000000;
		MSG_INFO(1,"srcLon==%lf,srcLat====%lf,destLon====%lf,destLat===%lf",srcLon,srcLat,destLon,destLat);
		NewXinbiao->xb_distance=GetDirectDistance(srcLon,srcLat,destLon,destLat);
		MSG_INFO(1,"xb_distance===%lf",NewXinbiao->xb_distance);
	
}





//根据角度计算弧度
double rad(double d)
{
    //const double PI = 3.1415926535898;
 	double num=0;
	num=d * PI/180.0;
	MSG_INFO(1,"num===%lf",num);
    return num;
}


//根据弧度计算角度
double Angle(double r)
{
   // const double PI = 3.1415926535898;
    return r * 180/PI;
}


//计算方位角
double GetBearing(double lat1, double lng1, double lat2, double lng2)
{
    double d = 0;
   
    double radLat1 = rad(lat1);//根据角度计算弧度
    double radLat2 = rad(lat2);
    double radLng1 = rad(lng1);
    double radLng2 = rad(lng2);
   
    d = sin(radLat1)*sin(radLat2)+cos(radLat1)*cos(radLat2)*cos(radLng2-radLng1);
    d = sqrt(1-d*d);
    d = cos(radLat2)*sin(radLng2-radLng1)/d;
    d = Angle(asin(d));
   
    return d;//返回方位角
}




void PushXinbiao(SUT_XINBIAO_PAGE*NewXinbiao)
{

	char i=0;
	
	double srcLon=sutGpsInfo.longitue; 
	double srcLat=sutGpsInfo.latitue;
	double destLon=NewXinbiao->xb_longitue;
	double destLat=NewXinbiao->xb_latitue;
	
	srcLat/=1000000;	
	srcLon/=1000000;
	
	destLat/=1000000;
	destLon/=1000000;

	
	NewXinbiao->xb_dir=GetBearing(srcLat,srcLon,destLat,destLon);

	MSG_INFO(1,"xb_dir=======%d",NewXinbiao->xb_dir);
	
	CalcDistance(NewXinbiao);
	for(i=0;i<sut_xinbiao_info.Max_id;i++){
		if(NewXinbiao->xb_CallID==sut_xinbiaopage[i].xb_CallID){
			NewXinbiao->xb_id=sut_xinbiaopage[i].xb_id;
			memcpy(&sut_xinbiaopage[i],NewXinbiao,sizeof(SUT_XINBIAO_PAGE));	
			return;
		}
	}
	  //否，创建新页	
	 sut_xinbiao_info.Max_id++;
	NewXinbiao->xb_id=sut_xinbiao_info.Max_id;
	memcpy(&sut_xinbiaopage[i],NewXinbiao,sizeof(SUT_XINBIAO_PAGE));	

 //


	
}




//

/*
信标协议 cmd  00  通用 
	        0a  摇晕  只收不发
	        a0  解除摇晕
		  0b	摇毙   不收不发
		  b0  解除摇毙
		  0c  强制打开GPS
*/
//AT+POC=150000 000be4e300
void XinbiaoPackSend(char cmd)
{

	char temp[100]={0};
	char buf[50];
	/*
	snprintf(buf,sizeof(buf),"AT+POC=150000%08x%02x",sutPocStatus.LocalGroup.ID);
	msgAtSend(buf);
	snprintf(buf,sizeof(buf),"%d;%d;%d;%d;%d;",cmd,sutGpsInfo.longitue\
		,sutGpsInfo.latitue,sutGpsInfo.aspect,sutGpsInfo.speed,sutGpsInfo.hight);
	*/
	snprintf(buf,sizeof(buf),"AT+POC=150000%08x%02x",sutPocStatus.LocalGroup.ID,cmd);
	msgAtSend(buf);
	snprintf(buf,sizeof(buf),"%s;%d;%d;%d;%d;%d;",sutApp.UserInfo.Firm_CallID,sutGpsInfo.longitue\
		,sutGpsInfo.latitue,sutGpsInfo.aspect,sutGpsInfo.speed,sutGpsInfo.hight);
	AscStrTurnHexStr(buf,temp);
	msgAtSend(temp);
	msgAtSend("\r\n");
	MSG_INFO(1,temp);
	
}



//定时发送信标
void Xinbiao_handler(unsigned int interval){
	static unsigned short cnt=0;
	//char Packdata[100]={0};
	if(sutApp.gtMode!=0) return;
	if(newPara.Xinbiao_SendMode==0)return;
	if(newPara.gpsEnable==0)return;
	if(++cnt<((newPara.Xinbiao_Time+1)*5*1000/interval) && gpsCheckCnt==0) return;
	cnt=0;
	gpsCheckCnt=0;
	if(sutGpsInfo.isGpsValid){
		XinbiaoPackSend(NULLCMD);
	}
}



void ReadXbFile()
{
	int fd,i=0;
	fd=LSAPI_FS_Open(XINBIAO_FILE, LSAPI_FS_O_RDWR,0);
	if(fd<0){
		MSG_ERR(1,"%s open read failed", XINBIAO_FILE);
		return;
	}
	LSAPI_FS_Seek(fd, 0L, LSAPI_FS_SEEK_SET);
	LSAPI_FS_Read(fd, (unsigned char *)&sut_xinbiaopage, sizeof(SUT_XINBIAO_PAGE)*XINBIAO_NUM);

	for(i=0;i<XINBIAO_NUM;i++){
		if(sut_xinbiaopage[i].xb_id==0){
			if(i!=0)sut_xinbiao_info.Max_id=sut_xinbiaopage[i-1].xb_id;
				break;
		}
	}
}

void WriteXbFile()
{
	int fd;
	MSG_INFO(1,"%s save start",XINBIAO_FILE);
	fd=LSAPI_FS_Open(XINBIAO_FILE, LSAPI_FS_O_RDWR,0x0);
	if(fd<0){
		MSG_ERR(1,"%s open failed",XINBIAO_FILE);
		return;
	}
	LSAPI_FS_Seek(fd, 0L, LSAPI_FS_SEEK_SET);
	LSAPI_FS_Write(fd, (unsigned char *)&sut_xinbiaopage, sizeof(SUT_XINBIAO_PAGE)*XINBIAO_NUM);
	LSAPI_FS_Close(fd);
	MSG_INFO(1,"%s save done", XINBIAO_FILE);
	
}


/*******************************************************************************************
下面为 首长功能
*******************************************************************************************/

signed char Record_Leader_CurIndex(){
	unsigned char i;
	for(i=0;i<MAX_USER;i++){
		if(leader[i].id==0)return i;//找到空白
	}
	return -1;

}
char Record_Leader_Scanf(int id){
	unsigned char i;

	for(i=0;i<MAX_USER;i++){
		if(leader[i].id==id)break;
		if(leader[i].id==0)break;//找到空白
	}
	if(i==MAX_USER)i=0;               //覆盖第一条
	return i;
}

void Record_Leader_Status(char type){

	unsigned char i;
	unsigned char index;
	
	for(i=1;i<MAX_USER;i++){
		index=Record_Leader_Scanf(UserArr[i-1].id);
		leader[index].id=UserArr[i-1].id;
		if(type==0){//摇匀
			if(sut_check_box.checkstatus[i]){
					leader[index].status|=0x01;
				}
			else  {
				leader[index].status &=~(1<<0);
			}

		}else if(type==1){
			if(sut_check_box.checkstatus[i]){
					leader[index].status|=0x02;
			}
			else {
				leader[index].status &=~(1<<1);
			}
		}else if(type==2){
			if(sut_check_box.checkstatus[i]){
					leader[index].status|=0x04;
			}
			else {
				leader[index].status &=~(1<<2);
			}
		}
	}
	
}

//           ID      cmd   ;
//+POC:8C00 934C0008 00    3B 
//+POC:8C00 72F4000A A0    72F40009 72F40003 72F40002 72F4000872F4000472F4000772F4000672F4000172F40005
void LeaderRecv_handler(char*msg,char type)
{
	char i,len=0;
	char start=0;
	char *Pmsg=0;
	int id=0;
	char data[250];
	char buf[9]={0};

	Pmsg=msg+19;
	len =strlen(Pmsg);
	
	for(i=0;i<len;i+=8){
			memcpy(buf,Pmsg+start,8);
			id=htoi(buf);
			MSG_INFO(1,"id======%08x",id);
			MSG_INFO(1,"LocalUser.ID======%08x",sutPocStatus.LocalUser.ID);
			
			if(id==sutPocStatus.LocalUser.ID){
				sutPocStatus.LeaderCmd|=type;
				MSG_INFO(1,"sutPocStatus.LeaderCmd===%d",sutPocStatus.LeaderCmd);
				if(type==0x04){
					newPara.gpsEnable=1;
					sysIniSave("location");
					GpsTimeUpdate();
				}
				if(sutPocStatus.spk)msgAtSend("AT+POC=0C0000\r\n");//说话状态释放话语权
				break;
			}
			start=i;
	}
	
	if(i==len){
			
		if(type==1&&(sutPocStatus.LeaderCmd&0x01)){
				sutPocStatus.LeaderCmd &=~(1<<0);
		}else if(type==2&&(sutPocStatus.LeaderCmd&0x02)){
				sutPocStatus.LeaderCmd &=~(1<<1);
		}else if(type==4&&(sutPocStatus.LeaderCmd&0x04)){
				sutPocStatus.LeaderCmd &=~(1<<2);
				MSG_INFO(1,"jiechu  GPS");
		}
		
	}
	
}




void LeaderPackSend(char cmd,char type)
{
	char temp[200]={0};
	char buf[10];
	
	int i,j=0;
	int max;

	max=Record_Leader_CurIndex();
	if(max==-1)return;
	snprintf(temp,sizeof(temp),"AT+POC=150000%08x%02x",sutPocStatus.LocalGroup.ID,cmd);
	
	for(i=0;i<max;i++){
		//if(leader[i].status=0){
		if((type==0&&(leader[i].status&0x01))||(type==1&&(leader[i].status&0x02))||(type==2&&(leader[i].status&0x04))){	
			snprintf(buf,sizeof(buf),"%08x",leader[i].id);
			strcat(temp,buf);
			j=1;
		}
	}
	if(j==0){
		if(cmd==YAOYUN)strcat(temp,DISABLE_YAOYUN);
		else if(cmd==YAOBI)strcat(temp,DISABLE_YAOBI);
		else if(cmd==GPSALLON)strcat(temp,DISABLE_GPS);
	}
	msgAtSend(temp);
	msgAtSend("\r\n");
	MSG_INFO(1,temp);
}
//LEADER leader[XINBIAO_NUM];

void ReadLeaderFile()
{
	int fd,i=0;
	fd=LSAPI_FS_Open(LEADER_FILE, LSAPI_FS_O_RDWR,0);
	if(fd<0){
		MSG_ERR(1,"%s open read failed", LEADER_FILE);
		return;
	}
	LSAPI_FS_Seek(fd, 0L, LSAPI_FS_SEEK_SET);
	LSAPI_FS_Read(fd, (unsigned char *)&leader, sizeof(LEADER)*XINBIAO_NUM);
	
}


void WriteLeaderFile()
{
	int fd;
	MSG_INFO(1,"%s save start",LEADER_FILE);
	fd=LSAPI_FS_Open(LEADER_FILE, LSAPI_FS_O_RDWR,0x0);
	if(fd<0){
		MSG_ERR(1,"%s open failed",LEADER_FILE);
		return;
	}
	LSAPI_FS_Seek(fd, 0L, LSAPI_FS_SEEK_SET);
	LSAPI_FS_Write(fd, (unsigned char *)&leader, sizeof(LEADER)*XINBIAO_NUM);
	LSAPI_FS_Close(fd);
	MSG_INFO(1,"%s save done", LEADER_FILE);
	
}