/*
*********************************************************************************************************
*                                       IAR Development Kits
*                                              on the
*
*                                    Nano130
*
* Filename      : adc_calculate.c
* Version       : V1.00
* Programmer(s) : Qian Xianghong
*********************************************************************************************************
*/

/*
*********************************************************************************************************
*                                             INCLUDE FILES
*********************************************************************************************************
*/

#include  "includes.h"

// 介质介电常数
const float e_Src[] = 
{
	1.62,	// LNG
	1.48,	// O2
	1.44,	// N2
	1.52,	// AR
	1.60,	// CO2
	1.23,	// H2（介电常数暂未知）
	1.10,	// CNG（介电常数暂未知）
	1.41,	// He（介电常数暂未知）
	1.60,	// C2H4（介电常数暂未知）
	1.75,	// LPG（介电常数暂未知）
	2.5,	// NH3（介电常数暂未知）
	1.5,	// PENT（戊烷，介电常数暂未知）
	1.5,	// POPO（聚醚多元醇，介电常数暂未知）
};

// 传感器规格参数
const Sensor_Model Adc_Sensor_Tables[1] =
{
	// 真空电容常数, 非线性段长度, 底座厚度（忽略）, 底部非线性段长度
//	{0.0802607,  45,  0, 45}, // 圆管电容,k = 2π*e0 / ln(8 / 4), (e0 = 0.008854187817)
	{0.0667859,  45,  0, 45}, // 圆管电容, 内管外部涂氟0.2mm
};

// 储罐理论参数
Bottle_Data Theoretical_Param;

// 余弦函数，只处理[0, PI/2]区间
#define my_cos(angle)	cos(angle)

// 反余弦函数，只处理[0, 1]区间
#define my_acos(c) acos(c)

// 正弦函数，只处理[0, PI/2]区间
#define my_sin(angle) sin(angle)

// 反正弦函数，只处理[0, 1]区间
#define my_asin(c) asin(c)

// 计算椭球体容积
u32 Calculate_Ellipsoid_Space(u16 d)
{
	float r = ((float) d) / 100 / 2; // 半径, 单位mm->dm
	return (u32) (2 * PI * r * r * r / 3); // 单位L
}	

// 计算圆柱体积
u32 Calculate_Cylinder_Space(u16 d, u16 L)
{
	float r = (float) d / 200; // 转换成分米
	float l = (float) L / 100;
	return (u32)(PI * r * r * l);	// 单位L
}

// 计算球体积
u32 Calculate_Spherical_Space(u16 d)
{
	float r = (float) d / 200; // 转换成分米
	return (u32)(4 * PI * r * r * r / 3);	// 单位L
}

// 根据圆柱体内液位高度与半径之比计算体积比
float Calculate_Cylinder_Rate0(float hr)
{
	float b;
	float vr;
	u8 reverse = 0;

	if(hr > 1)	 // 超过中线，计算顶部容积，再用总容积来减
	{
		hr = 2 - hr;
		reverse = 1;
	}
		
	b = my_acos(1 - hr);
	vr = (b - my_cos(PI / 2 - b) * (1 - hr)) / PI;
	if(!reverse)
	{
		return vr;
	}
	return 1 - vr;
}

// 根据圆柱体内液位高度计算体积比
float Calculate_Cylinder_Rate(u16 d, float h)
{
	float hr = ((float) h) / d * 2; // 高度与半径之比
	return Calculate_Cylinder_Rate0(hr);
}	

// 根据椭球体内液位高度计算体积比
float Calculate_Ellipsoid_Rate(u16 d, float h)
{
	float hr;
	float vr;
	u8 reverse = 0;
	
	hr = ((float) h) / d * 2; // 高度与半径之比
	if(hr > 1)	 // 超过中线，计算顶部容积，再用总容积来减
	{
		hr = 2 - hr;
		reverse = 1;
	}
		
	vr = hr * hr * (3 - hr) / 4;
	if(!reverse)
	{
		return vr;
	}
	return 1 - vr;
}	

// 根据液位高度与直径，计算空间（无扣除）
// 卧罐
u32 Calculate_Space(u16 d, float h, u32 v)
{
	u32 ve = Calculate_Ellipsoid_Space(d);
	u32 vc = v - ve;
	float re = Calculate_Ellipsoid_Rate(d, h);
	float rc = Calculate_Cylinder_Rate(d, h);
	
	return (u32) (ve * re + vc * rc);
}	

// 根据液位高度与直径之比，计算剩余液量（有扣除）
// v0: 留底液量
// 卧罐
u32 Calculate_Volume(u16 d, float h, u32 v, u16 v0)
{
	// 计算空间容积
	u32 space = Calculate_Space(d, h, v);
		
	// 留底
	if(space < v0)
	{
		return 0;
	}
	return space - v0;		
}	

// 根据液位高度，计算空间（无扣除）
// 立罐
u32 Calculate_Space_Stand(u16 d, u16 L, float H)
{
	float r = (float) d / 200; // 转为dm，体积单位为L
	float l = (float) L / 100;
	float h = (float) H / 100;

	if(h <= r / 2)
	{
		// 只有底下椭球部分
		return 2 * PI * r * h * h - 4 * PI * h * h * h / 3;
	}
	if(h <= r / 2 + l)
	{
		// 底下椭球+圆柱部分
		h -= r/2;
		return PI * r * r * r / 3 + PI * r * r * h;
	}	
	if(h <= r / 2 + l + r / 2)
	{
		// 上下椭球+中间圆柱部分
		h -= r / 2 + l;
		return PI * r * r * r / 3 + PI * r * r * l + PI * r * r * h - 4 * PI * h * h * h / 3;
	}	

	// 高度超过有效范围
	return 2 * PI * r * r * r / 3 + PI * r * r * l;
}	

// 根据液位高度，计算剩余液量（有扣除）
// v0: 留底液量
// 立罐
u32 Calculate_Volume_Stand(u16 d, u16 L, float H, u16 v0)
{
	// 计算空间容积
	u32 space = Calculate_Space_Stand(d, L, H);
		
	// 留底
	if(space < v0)
	{
		return 0;
	}
	return space - v0;		
}	

// 根据液位高度，计算空间（无扣除）
// 球罐
u32 Calculate_Space_Spherical(u16 d, float H)
{
	float r = (float) d / 200; // 转为dm，体积单位为L
	float h = (float) H / 100;

	if(h <= r)
	{
		// 高度未超过一半
		return PI * r * h * h - PI * h * h * h / 3;
	}
	if(h <= r + r)
	{
		// 高度超过一半
		h = r + r - h;
		return 4 * PI * r * r * r / 3 - PI * r * h * h + PI * h * h * h / 3;
	}	

	// 高度超过有效范围
	return 4 * PI * r * r * r / 3;
}	

// 根据液位高度，计算剩余液量（有扣除）
// v0: 留底液量
// 球罐
u32 Calculate_Volume_Spherical(u16 d, float H, u16 v0)
{
	// 计算空间容积
	u32 space = Calculate_Space_Spherical(d, H);
		
	// 留底
	if(space < v0)
	{
		return 0;
	}
	return space - v0;		
}	

// 计算底部不可测部分高度
s16 Calculate_Base_Height(s16 d, s16 base_d, s16 base_len)
{
	float r = ((float) d) / 2 ;
	float base_r = ((float) base_d) / 2;
	float b = my_asin(base_r / r);
	b = my_cos(b);
	return (s16) (r * (1 - b)) + base_len;
}

// 计算理论参数		
u8 Calculate_Theoretical_Params(u8 bottle_type, u16 d, u16 L, uint8_t chargePct, float density)
{
	u8 vresv_percent = 0;// 留底百分比，固定为0
	
	u16 h0, h1, zero_h;
	u16 zero_v, v0;
	
	Theoretical_Param.LSrc_k = 1000 / 9.8 / density;
	if(bottle_type == BOTTLE_TYPE_SPHERICAL)
		Theoretical_Param.v = Calculate_Spherical_Space(d);
	else
		Theoretical_Param.v = Calculate_Ellipsoid_Space(d) + Calculate_Cylinder_Space(d, L);
	
	// 计算底部不可测高度
	Theoretical_Param.nl_len = Calculate_Base_Height(d, Adc_Sensor_Tables[0].base_d, Adc_Sensor_Tables[0].base_len); 
	
	// 计算留底液量及高度, 最多留一半
	if(vresv_percent == 0)
	{
		Theoretical_Param.zero_height = 0;
		Theoretical_Param.v0 = 0;
	}
	else
	{
		h0 = 0;
		if(bottle_type == BOTTLE_TYPE_STAND)
		{
			h1 = d / 4 + L / 2; // 立罐
		}	
		else
		{
			h1 = d / 2;	// 卧罐、球罐
		}
		v0 = (u16) (Theoretical_Param.v * (vresv_percent * 0.01)); // 目标留底液量
		while(1)
		{
			zero_h = (h0 + h1) / 2;
			if(bottle_type == BOTTLE_TYPE_LYING)
			{
				zero_v = Calculate_Volume(d, zero_h, Theoretical_Param.v, 0);
			}
			else if(bottle_type == BOTTLE_TYPE_STAND)
			{
				zero_v = Calculate_Volume_Stand(d, L, zero_h, 0);
			}
			else
			{
				zero_v = Calculate_Volume_Spherical(d, zero_h, 0);
			}
			if(zero_v == v0)
			{
				break;
			}
			else if(zero_v > v0)
			{
				if(zero_h == h0)
				{
					break;
				}
				h1 = zero_h - 1;
			}	
			else
			{
				if(zero_h == h1)
				{
					break;
				}
				h0 = zero_h + 1;
			}	
		}
		Theoretical_Param.zero_height = zero_h;
		Theoretical_Param.v0 = zero_v;
	}
	
	// 有效容积
	Theoretical_Param.ve = (Theoretical_Param.v - Theoretical_Param.v0) * chargePct / 100;
	
	return 1;
}

// 判断采集数据范围是否合法
// 输入：采集值、校零值、校满值
// 返回：传感器状态字节
u8 ADC_Validate(u32 adc, u32 zero, u32 full)
{
	u32 temp;
	
	// 如果是降序，交换最小最大ADC值
	if(zero > full)
	{
		temp = zero;
		zero = full;
		full = temp;
	}
	
	// 低于校准点的1/3，或低于量程范围的1/3，未连接
	if(adc < zero / 3 || adc + (full - zero) / 3 < zero)
		return SENSOR_STATUS_NOCONNECT;
		
	// 低于校准点的1/2，或低于量程范围的1/4，下溢出
	if(adc < zero / 2 || adc + (full - zero) / 4 < zero)
		return SENSOR_STATUS_UNDERFLOW;

	// 高于量程范围的1/4，上溢出
	if(adc > full + (full - zero) / 4)
		return SENSOR_STATUS_OVERFLOW;	

	return SENSOR_STATUS_NORMAL;
}

// 计算采集数据
// 输入：adc，校准值，量程
// 输出：经计算得到的值
float ADC_Calculate(u32 adc, u32 zero, u32 full, s32 low, s32 high)
{
	if(zero > full)
	{
		if(adc <= full)
			return high; 	// 量程最大值
		if(adc >= zero)
			return low;		// 量程最小值
		return low + ((float) (zero - adc)) / (zero - full) * (high - low);
	}

	if(adc <= zero)
		return low; 	// 量程最小值
	if(adc >= full)
		return high;	// 量程最大值
	return low + ((float) (adc - zero)) / (full - zero) * (high - low);
}

// 冒泡排序，用于数量较少的排序法
void sort(u32 numbs[], s8 cnt)
{
	s8 i, j, k;
	u32 temp;

	for(i = 0; i < cnt - 1; i++)
	{
		// 查找最小值
		k = i;
		temp = numbs[k];
		for(j = i + 1; j < cnt; j++)
		{
			if(numbs[j] < temp)
			{
				k = j;
				temp = numbs[k];
			}	
		}	
		// 交换
		if(k != i)
		{
			numbs[k] = numbs[i];
			numbs[i] = temp;
		}	
	}	
}

// KPa转换成mmH2O
float KPa2mmH2O(float KPa)
{
	return KPa * 101.9716213;
}
	
// 差压转换为液位高度
float Diff2Level(float dp, u8 bottle_type, u16 d, u16 L) // 单位KPa、mm
{
	float h = Theoretical_Param.LSrc_k * dp;
	u16 max_h;

	// 不能超过罐子高度
	if(bottle_type == BOTTLE_TYPE_STAND)
	{
		// 立罐最大高度
		max_h = L + d / 2;
		if(h > max_h)
			h = max_h;
	}
	else
	{
		// 卧罐、球罐最大高度
		if(h > d)
			h = d;
	}	
		
	return h;
}

// 高度转换为体积
u32 Level2Vol(float h, u8 bottle_type, u16 d, u16 L) // 单位mm、L
{
	u32 v;
	
	if(bottle_type == BOTTLE_TYPE_LYING)
	{
		// 卧罐
		v = Calculate_Volume(d, h, Theoretical_Param.v, Theoretical_Param.v0);
	}	
	else if(bottle_type == BOTTLE_TYPE_STAND)
	{
		// 立罐
		v = Calculate_Volume_Stand(d, L, h, Theoretical_Param.v0);
	}
	else
	{
		// 球罐
		v = Calculate_Volume_Spherical(d, h, Theoretical_Param.v0);
	}
		
	if(v > Theoretical_Param.v - Theoretical_Param.v0)
		v = Theoretical_Param.v - Theoretical_Param.v0;
		
	return v;
}
	
// 体积转换为质量
u32 Vol2Quantity(float v, float density) // 单位L、kg
{
	return density * v;
}

// 质量转换为体积
u32 Quantity2Vol(float quantity, float density) // 单位kg、L
{
	u32 v = quantity / density;

	if(v > Theoretical_Param.v - Theoretical_Param.v0)
		v = Theoretical_Param.v - Theoretical_Param.v0;
	
	return v;

}