/**
  ******************************************************************************
  * @file    LBS_Transmit.c 
  * @author  
  * @version 
  * @date    
  * @brief   
  ******************************************************************************
  * @attention
  *
  *
  ******************************************************************************
  */

/* Includes ------------------------------------------------------------------*/
#include "stm32f10x.h"
#include "global.h"
#include <math.h>
#include <stdint.h>


// PI定义
#define M_PI          3.1415926535897932384626433832795

// 常量定义
#define EARTH_RADIUS  6378245.0               // 地球长半径
#define EE            0.00669342162296594323  // 偏心率平方


/**
 * @brief 检查坐标是否在中国大陆以外
 * @return 1:境外, 0:境内
 */
static int is_out_of_china(double lon, double lat)
{
	if (lon < 72.004 || lon > 137.8347)
	{
		return 1;
	}
	if (lat < 0.8293 || lat > 55.8271)
	{
		return 1;
	}
	
	return 0;
}

/**
 * @brief 纬度转换辅助函数
 */
static double transform_lat(double x, double y)
{
	double ret = -100.0 + 2.0 * x + 3.0 * y + 0.2 * y * y + 0.1 * x * y + 0.2 * sqrt(fabs(x));
	ret += (20.0 * sin(6.0 * x * M_PI) + 20.0 * sin(2.0 * x * M_PI)) * 2.0 / 3.0;
	ret += (20.0 * sin(y * M_PI) + 40.0 * sin(y / 3.0 * M_PI)) * 2.0 / 3.0;
	ret += (160.0 * sin(y / 12.0 * M_PI) + 320 * sin(y * M_PI / 30.0)) * 2.0 / 3.0;
	return ret;
}

/**
 * @brief 经度转换辅助函数
 */
static double transform_lon(double x, double y)
{
	double ret = 300.0 + x + 2.0 * y + 0.1 * x * x + 0.1 * x * y + 0.1 * sqrt(fabs(x));
	ret += (20.0 * sin(6.0 * x * M_PI) + 20.0 * sin(2.0 * x * M_PI)) * 2.0 / 3.0;
	ret += (20.0 * sin(x * M_PI) + 40.0 * sin(x / 3.0 * M_PI)) * 2.0 / 3.0;
	ret += (150.0 * sin(x / 12.0 * M_PI) + 300.0 * sin(x / 30.0 * M_PI)) * 2.0 / 3.0;
	return ret;
}

/**
 * @brief GCJ-02转WGS84坐标系
 * @param gcj_lon GCJ-02经度
 * @param gcj_lat GCJ-02纬度
 * @param wgs_lon WGS84经度输出指针
 * @param wgs_lat WGS84纬度输出指针
 * @note  迭代7次，精度约0.1-0.5米
 */
void gcj02_to_wgs84(double gcj_lon, double gcj_lat, double *wgs_lon, double *wgs_lat)
{
	//如果坐标不在中国大陆，直接返回原坐标
	if(is_out_of_china(gcj_lon, gcj_lat))
	{
		*wgs_lon = gcj_lon;
		*wgs_lat = gcj_lat;
		return;
	}
	
	//使用迭代法进行转换（7次迭代足够精确）
	double d_lon = 0.0, d_lat = 0.0;
	double tmp_lon = gcj_lon, tmp_lat = gcj_lat;
	
	for(int i = 0; i < 7; i++)
	{
		//计算当前WGS84坐标转GCJ02的偏移
		double delta_lat = transform_lat(tmp_lon - 105.0, tmp_lat - 35.0);
		double delta_lon = transform_lon(tmp_lon - 105.0, tmp_lat - 35.0);
		
		double rad_lat = tmp_lat * M_PI / 180.0;
		double magic = sin(rad_lat);
		magic = 1 - EE * magic * magic;
		double sqrt_magic = sqrt(magic);
		
		delta_lat = (delta_lat * 180.0) / ((EARTH_RADIUS * (1 - EE)) / (magic * sqrt_magic) * M_PI);
		delta_lon = (delta_lon * 180.0) / (EARTH_RADIUS / sqrt_magic * cos(rad_lat) * M_PI);
		
		// 计算与目标GCJ02坐标的差值
		d_lat = gcj_lat - (tmp_lat + delta_lat);
		d_lon = gcj_lon - (tmp_lon + delta_lon);
		
		// 更新WGS84坐标估计
		tmp_lat += d_lat;
		tmp_lon += d_lon;
	}
	
	*wgs_lon = tmp_lon;
	*wgs_lat = tmp_lat;
}