/---------------------------------------------------------------------------
#include "stdafx.h"
#include "piezo.h"

#pragma hdrstop

//+++++ regulace

double UpdatePID(SPid *pid, double error, double position)
{
  double pTerm,dTerm,iTerm;

  pTerm = pid->pGain * error;   
  // calculate the proportional term
  // calculate the integral state with appropriate limiting
  pid->iState += error;
  /*if(pid->iState > pid->iMax) 
	pid->iState = pid->iMax;
  else if(pid->iState < pid->iMin) 
	pid->iState = pid->iMin;*/
  iTerm = pid->iGain * pid->iState;  // calculate the integral term
  dTerm = pid->dGain * (position - pid->dState);
  pid->dState = position;
/* if(FILE *f = fopen("log.txt","a")){
			fprintf(f,"%lf,%lf,%lf,%lf\n",iTerm,dTerm,pTerm,position);
			fclose(f);
 }*/
  return pTerm + iTerm - dTerm;
}

//----- regulace

// ++++++ detekce maxima

void add(SMax *data,double amp,double faze)
{
	for(int x = 0; x < NOR - 1; x++){
		data->tmp[x] = data->tmp[x + 1];
		data->tmpfz[x] = data->tmpfz[x + 1];
	}
	data->tmp[NOR - 1] = amp;
	data->tmpfz[NOR - 1] = faze;
	if(data->pocethodnot < NOR) 
		data->pocethodnot++;
	return;
}

double regreseA(SMax *data)
{
	double	Sxy = 0,  // suma x*y
			Sx = 0,   // suma x
			Sy = 0,	  // suma y
			Sx2 = 0;  // suma x^2

	for(int x = 0; x < NOR; x++){
		Sxy += (data->tmp[x] * (x + 1));
		Sx  += (x + 1);
		Sy  += data->tmp[x];
		Sx2 += pow(x,2.);
	}
	data->regA = (NOR * Sxy - Sx * Sy)/(NOR * Sx2 - pow(Sx,2)); // linearni regrese
	return data->regA;
}

double regreseF(SMax *data)
{
	double	Sxy = 0,  // suma x*y
			Sx = 0,   // suma x
			Sy = 0,	  // suma y
			Sx2 = 0;  // suma x^2

	for(int x = 0; x < NOR; x++){
		Sxy += (data->tmpfz[x] * (x + 1));
		Sx  += (x + 1);
		Sy  += data->tmpfz[x];
		Sx2 += pow(x,2.);
	}
	data->regF = (NOR * Sxy - Sx * Sy)/(NOR * Sx2 - pow(Sx,2)); // linearni regrese
	return data->regF;
}


int UpdateAmax(SMax *data,double amp,double faze)
{	
	int ret = 0;
	double smernice = 0;
	add(data,amp,faze);
	if(data->pocethodnot == NOR){
		smernice = regreseA(data);
		/*if(FILE *f = fopen("log.txt","a")){
			fprintf(f,"%lf,%lf\n",smernice,amp);
			fclose(f);
			} */
		if((smernice < 0) && (data->tmpA > 0)){
			if(abs(data->koef) == 1){
				data->koef *= (-1 * 0.1); // jemne doladeni
			}
			else { 
				ret = 1;
				data->koef = 1;
			}
		}else
		if((smernice < 0)){
			data->koef *= -1;
		}
		data->pocethodnot = 0;
		data->tmpA = smernice;
	}

    // vypocet prebehu faze	

	double uhel = atan((data->tmpfz[NOR-2] - data->tmpfz[NOR-3])) + atan((data->tmpfz[NOR-2] - data->tmpfz[NOR-1]));
	if(uhel > 0.8)		
		data->cntFF++;
	/*if((faze > 2.2) && (data->sph == 0)){
		data->cntFF++;
		data->sph = 1;
	};
	if((faze < 2.2) && (data->sph == 1))
		data->sph = 0;
*/
/*	if(FILE *f = fopen("log.txt","a")){
		fprintf(f,"%ld;%lf;%lf;%lf\n",data->cntFF,faze,amp,uhel);
		fclose(f);
	} */
	return ret;
}


//---- detekce maxima


//---------------------------------------------------------------------------
HANDLE cport=NULL;

//---------------------------------------------------------------------------
// otevrit COM port, pro win XP a COM vyssi nez 9 uvadet jmena ve formatu "\\\\.\\COM10"
//---------------------------------------------------------------------------
int COMopen(HANDLE *cporth, LPCWSTR cportname)
{
	*cporth=CreateFile(cportname,GENERIC_READ|GENERIC_WRITE,0,NULL,OPEN_EXISTING,0,NULL);
	if(*cporth==INVALID_HANDLE_VALUE)
		return(1);
	return(0);
}
//---------------------------------------------------------------------------
// zavrit COM port
//---------------------------------------------------------------------------
void COMclose(HANDLE *cporth)
{
	CloseHandle(*cporth);
	*cporth=NULL;
}
//---------------------------------------------------------------------------
// nastavit velikost RX, TX bufferu
//---------------------------------------------------------------------------
BOOL COMsetBuf(HANDLE cporth, int inlen, int outlen)
{
	return(SetupComm(cporth,inlen,outlen));
}
//---------------------------------------------------------------------------
// nastavit format ramce (baudrate a pocet stopbitu)
//---------------------------------------------------------------------------
void COMsetFormat(HANDLE cporth, int baud, int stopbit)
{
	DCB dcb;
	GetCommState(cporth,&dcb);
	dcb.fBinary=true;
	dcb.fAbortOnError=false;
	dcb.BaudRate=baud;
	dcb.StopBits=stopbit;
	dcb.Parity=NOPARITY;
	dcb.ByteSize=8;
	SetCommState(cporth,&dcb);
}
//---------------------------------------------------------------------------
// nastavit timeouty (rxtime/txtime v ms)
//---------------------------------------------------------------------------
void COMsetTimeouts(HANDLE cporth, int rxinterval, int rxtime, int rxmult, int txtime, int txmult)
{
	COMMTIMEOUTS timeouts;
	timeouts.ReadIntervalTimeout=rxinterval;
	timeouts.ReadTotalTimeoutMultiplier=rxmult;
	timeouts.ReadTotalTimeoutConstant=rxtime;
	timeouts.WriteTotalTimeoutMultiplier=txmult;
	timeouts.WriteTotalTimeoutConstant=txtime;
	SetCommTimeouts(cporth,&timeouts);
}
//---------------------------------------------------------------------------
// vymazat vstupni buffer
//---------------------------------------------------------------------------
void COMflushInput(HANDLE cporth)
{
	PurgeComm(cporth,PURGE_RXCLEAR);
}
//---------------------------------------------------------------------------
// odeslat byte (vraci pocet zapsanych bytu)
//---------------------------------------------------------------------------
unsigned long COMtxByte(HANDLE cporth, byte byte)
{
	unsigned long written;
	WriteFile(cporth,&byte,1,&written,NULL);
	return(written);
}
//---------------------------------------------------------------------------
// cist byte (vraci pocet prectenych bytu)
//---------------------------------------------------------------------------
unsigned long COMrxByte(HANDLE cporth, byte *byte)
{
	unsigned long read;
	ReadFile(cporth,byte,1,&read,NULL);
	return(read);
}
//---------------------------------------------------------------------------
// odeslat data (vraci pocet skutecne zapsanych bytu)
//---------------------------------------------------------------------------
unsigned long COMtxData(HANDLE cporth, void *data, unsigned long datalen)
{
	unsigned long written;
	WriteFile(cporth,data,datalen,&written,NULL);
	return(written);
}
//---------------------------------------------------------------------------
// cist data (vraci pocet skutecne nactenych bytu)
//---------------------------------------------------------------------------
unsigned long COMrxData(HANDLE cporth, void *data, unsigned long datalen)
{
	unsigned long read;
	WriteFile(cporth,data,datalen,&read,NULL);
	return(read);
}

BOOL initComport(LPCWSTR comport){
	if(cport == NULL){
		if(COMopen(&cport,comport)!=0)
		{
			//puts("Can't open selected COM port!\n");
			//MessageBox(NULL, TEXT("nelze otevøít COM port") , TEXT("chyba"), MB_OK | MB_ICONINFORMATION);
			return(1);
		}
		// tend nastavit velikost bufferu (je to jen doporuceni a PC to muze ignorovat!!!)
		COMsetBuf(cport,1024,256);
		// ted nastavit baudrate a pocet stopbitu
		COMsetFormat(cport,9600,ONESTOPBIT);
		// ted nastavit timeouty (tohle je dost dulezity!)
		COMsetTimeouts(cport,0,100,1,100,1);
	//	outCom(65535,65535); // kratkodobe nastaveni pieza na plne napeti kvuli hysterezi
	//	Sleep(10);
		outCom(0,0); // nastaveni na stredni napeti aby byla mozna regulace +-
		Sleep(10);
		return(0);
	}
	else return(1);
}
void outCom(unsigned int M ,unsigned int N){
	if(cport != NULL){
		M = M + 32768;
		N = N + 32768;
		int znak_A,znak_B,znak_C,znak_D;
		znak_B = M % 256;
		znak_D = N % 256;
		znak_A = ((M - znak_B) / 256);
		znak_C = ((N - znak_D) / 256);

		COMflushInput(cport);                  // flushnout RX buffer
		COMtxByte(cport,znak_A);                  
		COMtxByte(cport,znak_B);                  
		COMtxByte(cport,znak_C);                  
		COMtxByte(cport,znak_D);                  
		COMtxByte(cport,(znak_A ^ znak_B ^ znak_C ^ znak_D));
	}
}


void closeComport(){
	if(cport != NULL) COMclose(&cport);
}






/* #pragma argsused
int main(int argc, char* argv[])
{
	byte rxb;

	// nejdriv otevrit COM port
	if(COMopen(&cport,"COM1")!=0)
	{
		printf("Can't open selected COM port!\n");
		getch();
		return(1);
	}

	// tend nastavit velikost bufferu (je to jen doporuceni a PC to muze ignorovat!!!)
	COMsetBuf(cport,1024,256);
	// ted nastavit baudrate a pocet stopbitu
	COMsetFormat(cport,115200,ONESTOPBIT);
	// ted nastavit timeouty (tohle je dost dulezity!)
	COMsetTimeouts(cport,0,100,1,100,1);


	while(1)
	{
		rxb=(byte)getch();                     // cist klavesu
		if(rxb==27) break;										 // kdyz esc, tak ukoncit
		COMflushInput(cport);                  // flushnout RX buffer
		COMtxByte(cport,rxb);                  // odeslat tu klavesu
		if(COMrxByte(cport,&rxb)!=1)
			printf("nothnig recieved ...\n");    // nebylo nic prijato
		else
			printf("\'%c\' - %d\n",rxb,(int)rxb);    // byl prijat byte, vypsat
	}

	// ted ho zase zavrit
	COMclose(&cport);

	return 0;
}
*/