/*	for(i=0;i<CCDWidth*CCDHeight;i++)
	{
		if(CCDData[i]==0&&CCDData[i-1]==255)
			j=i;
		if(CCDData[i]==0&&CCDData[i+1]==255)
			k=i;
		m=j/32;
		j=j%32;
		k=k%32;
		mid1=(j+k)/2+0.5;
		preserved[m]=(unsigned short)mid1;
	}
	linek=(preserved[79]-preserved[0])/79;
	lineb=preserved[0];
	dkd=sqrt(linek*linek+1);
	for(i=0;i<80;i++)
	{
		offset+=(preserved[i]-linek*i-lineb)/dkd;
	}*/

	

#include "Judgement.H"
#include <math.h>

unsigned short PWM;
#define maxpwm 18000
int count =0;
int tempmid=0;
unsigned short frespeed=0;
unsigned short prespeed=0;
unsigned short freservospeed=0;
unsigned short presservopeed=0;
unsigned short preserved[80]={0};
int mortorPwm[16]={16000,13000,11000,9000,8000,6000,6000,6000,6000,6000,8000,6000,4000,6000,4000,3000};
extern "C" _declspec(dllexport)
int speedPid(int temppwm,int frespeed,int Speed,int prespeed)
{
	double retVal=10.9*Speed+17.02*(temppwm-10.9*Speed)+9*10.9*(Speed-frespeed)+10.9*(frespeed-prespeed);
	if (retVal>65535)
		retVal=65535;
	if (retVal<0)
		retVal=0;
	return retVal;
	return retVal;
}
int servoPid(int temppwm,int frespeed,int prespeed)
{
	double retVal=temppwm+0.8*(temppwm-frespeed)+0.32*(frespeed-prespeed);
		if (retVal>65535)
		retVal=65535;
	if (retVal<0)
		retVal=0;
	return retVal;
}
void Judge( unsigned short Speed,
			unsigned char* SensorData, unsigned short SensorCount,
			unsigned char* CCDData, int CCDWidth, int CCDHeight,
			unsigned short* MotorPWM, unsigned short* SteerPWM)
{
	/*PWM += 1;
	*MotorPWM = 0;
	*SteerPWM = 16384;*/
	int i=0;
	int j=0;
	int k=0;
	int m=0;
	double mid=0;
	int loop=0;
	double avgpwm=0;
	double interlapce=0;
	double angle=0;
	double temppwm;
	double linek=0;
	double lineb=0;
	double dkd=0;
	double offset=0;
		for(i=0;i<CCDWidth*CCDHeight;i++)
	{
		if(CCDData[i]==0&&CCDData[i-1]==255)
			j=i;
		if(CCDData[i]==0&&CCDData[i+1]==255)
			k=i;
		m=j/32;
		j=j%32;
		k=k%32;
		mid=(j+k)/2+0.5;
		preserved[m]=(unsigned short)mid;
	}
	linek=(preserved[79]-preserved[0])/79;
	lineb=preserved[0];
	dkd=sqrt(linek*linek+1);
	for(i=0;i<80;i++)
	{
		offset+=(preserved[i]-linek*i-lineb)/dkd;
	}
	for(i=CCDWidth*CCDHeight;i>0;i--)
	{
		if(CCDData[i]==0&&CCDData[i-1]==255)
			j=i;
		if(CCDData[i]==0&&CCDData[i+1]==255)
			k=i;
	}
	m=j/32;
	j=j%32;
	k=k%32;
	mid=(int)((j+k)/2);

	if(j==0)
	{
		/*if(frePWM[count]>0)
			temppwm=32768*(1-frePWM[count]/(3.14/6));
		else
			temppwm=32768*(1+frePWM[count]/(3.14/6));

		*SteerPWM=temppwm;
		*MotorPWM=500;*/
		return;
	}
	*MotorPWM=18000;
	if((int)mid==CCDWidth/2)
	{
		prespeed=Speed;
		frespeed=Speed;
		*MotorPWM=speedPid(18000,frespeed,Speed,prespeed);
		*SteerPWM=32768;
	}
	else
	{
		
		interlapce=(CCDWidth/2-mid);
		
		temppwm=32768+65535/32*interlapce;
		if(temppwm>65535)
			temppwm=65535;
		else if(temppwm<0)
			temppwm=0;
		
		*SteerPWM=servoPid(temppwm,freservospeed,presservopeed);
		presservopeed=freservospeed;
		freservospeed=*SteerPWM;
		
		int interspeed=0;
		if(interlapce>0)
			interspeed=16-interlapce;
		else
			interspeed=16+interlapce;
		temppwm=mortorPwm[abs(interlapce)];
		
		*MotorPWM=speedPid(temppwm,frespeed,Speed,prespeed);
		prespeed=frespeed;
		frespeed=Speed;
	}
}

extern "C" _declspec(dllexport)
void Reset()
{
	PWM = 0;
}