/*

    this program use old code used in another experiment
    
    "int" is used for the ADC and DAC data although it should be unsigned, 
    it should be not a problem at all as this data are limited to 12bits...



*/

#include <aduc7020.h>
unsigned int uiPLLTST = 0;
volatile unsigned int ucTest = 0;



void delay (int length)
{
	while (length >=0)
    	length--;
}


// conversion of the read value into it corresponding 12bits integer
int ADCtoDAT(unsigned long ADC)
{
	return (ADC&0xFFF0000)>>16;
}

unsigned long DATtoADC(int DAT)
{
	unsigned long ADC;
	ADC=DAT;
	return ADC<<16;
}



int Read_Digital(int n)
{	
    return ((GP0DAT&0x000000FF)>>n)&0x1;
}

void Write_Digital(int n, int state)
{	
    if(state==1)
	  GP1DAT=(0x00000001<<(n+16))|GP1DAT;
	else 
		GP1DAT=~((0x00000001<<(n+16))|(~GP1DAT));
}


void ADCpoweron(int time)
{
	ADCCON = 0x620;	 			// power-on the ADC
	while (time >=0)	  		// wait for ADC to be fully powered on
    time--;
}


// use DAC0
// it operates a finite number of iteration
// -> to adapte in the while loop
void Lock_min_fringe(int*pV, int step, int N_delay)
{
	int flag, cnt;
	unsigned long temp1,temp2;
//    unsigned long V_th; // threshold
	unsigned long V;
		
	flag = 1;
//    V_th=DATtoADC(threshold);
	V = *pV;//DATtoADC(2000);
	temp1 = ADCDAT;
	cnt = 0;
	
	while(cnt<100){
		//if(Read_Digital(6)==0||temp1<V_th)	break;
		//hold();
		//Write_Digital(0,1);
		V = V+flag*step;
		if(V>4096) // in case it is out of range restarting fro, the middle should converge
            V = 2048;
		//V = Bound_cir(V);
		DAC0DAT = DATtoADC(V);
		delay(N_delay);	    // if the system do not respond immediately
		temp2 = ADCDAT;
		if(temp2>temp1){    // to invert in order to lock the maximum
			flag = -1*flag;	
		}	
		temp1 = temp2;
		cnt++;
	}
	//Write_Digital(0,0);
	*pV = V;
}



void Lock_power(int*pV, int step, int N_delay)
{
	int cnt,g;
	unsigned long temp;
//    unsigned long V_th; // threshold
	unsigned long V,V0;
		
	g = 1;
//    V_th=DATtoADC(threshold);
	V = *pV;//DATtoADC(2000);
	temp = ADCtoDAT(ADCDAT);
	cnt = 0;
	// we first check the targeted power on ADC1
	ADCCP = 0x01;
	V0 = ADCtoDAT(ADCDAT);

	// now we only read the PD on ADC0
	ADCCP = 0x00;

	
	while(cnt<100){
		//if(Read_Digital(6)==0||temp1<V_th)	break;
		//hold();
		//Write_Digital(0,1);
		temp = ADCtoDAT(ADCDAT);
		V = V-(temp-V0)*g;
		//if(V>4096) // in case it is out of range restarting fro, the middle should converge
        //    V = 2048;
		//V = Bound_cir(V);
		DAC1DAT = DATtoADC(V);
		delay(N_delay);	    // if the system do not respond immediately
		cnt++;
	}
	//Write_Digital(0,0);
	*pV = V;
}



void test_blink(void)
{
	short V = -1;
	
	DAC1CON = 0x12;	   // AGND-ARef range 0x12 2.5V
	


	 // configures GPIO to flash LED P4.2
	GP4DAT = 0x04000000;			// P4.2 configured as an output. LED is turned on	

	GP0CON = 0x00000000;	    // setting IO
	GP0DAT = 0x00000000;        // group P0.x as input
	DAC1DAT = DATtoADC(V); // output voltage on DAC0
	//GP4DAT ^= 0x00040000;		// Complement P4.2

	while (1){
		V = -V;
		if(V>4096)
			V = 0;
		DAC1DAT = DATtoADC(V); // output voltage on DAC0
	
		if (Read_Digital(0)==1)
			GP4DAT ^= 0x00040000;		// Complement P4.2
		delay(500000);
	}				
}

void test_blink2(void)
{
	short V = -1;
	short state = 1;
	
	DAC1CON = 0x12;	   // AGND-ARef range 0x12 2.5V
	
	GP1CON = 0x00000000;    // IO initialization
	GP1DAT = 0xFF000000;	// set P1.n as digital output
	
	 // configures GPIO to flash LED P4.2
	
	//GP0CON = 0x00000000;	    // setting IO
	//GP0DAT = 0x04000000;			// P4.2 configured as an output. LED is turned on	
	//GP0DAT = 0x00000000;        // group P0.x as input
	DAC1DAT = DATtoADC(V); // output voltage on DAC0
	//GP4DAT ^= 0x00040000;		// Complement P4.2

	while (1){
		V = -V;
		state=-state;
		if(V>4096)
			V = 0;
		DAC1DAT = DATtoADC(V); // output voltage on DAC0
	
		//if (Read_Digital(0)==1)
		//	GP4DAT ^= 0x00040000;		// Complement P4.2
		if(state>0)
			Write_Digital(4, 1);
		else
			Write_Digital(4, 0);


		//delay(500000);
	}
				
}

void test_clock(void)
{
	//short V = -1;
	short state = 1;
	
	//DAC1CON = 0x12;	   // AGND-ARef range 0x12 2.5V
	
	GP1CON = 0x00000000;    // IO initialization
	GP1DAT = 0xFF000000;	// set P1.n as digital output
	
	 // configures GPIO to flash LED P4.2
	
	//GP0CON = 0x00000000;	    // setting IO
	//GP0DAT = 0x04000000;			// P4.2 configured as an output. LED is turned on	
	//GP0DAT = 0x00000000;        // group P0.x as input
	//DAC1DAT = DATtoADC(V); // output voltage on DAC0
	//GP4DAT ^= 0x00040000;		// Complement P4.2

	while (1){
		//V = -V;
		state=-state;
		//if(V>4096)
		//	V = 0;
		//DAC1DAT = DATtoADC(V); // output voltage on DAC0
	
		//if (Read_Digital(0)==1)
		//	GP4DAT ^= 0x00040000;		// Complement P4.2
		if(state>0)
			Write_Digital(4, 1);
		else
			Write_Digital(4, 0);


		//delay(500000);
	}
				
}

void test_clock2(void)
{
	short state = 1;
	int bit =0x00000001<<(20);
	
	POWKEY1 = 0x01;
	POWCON = 0x00;		   				// 41.78MHz
	POWKEY2 = 0xF4;

   	GP1CON = 0x00000000;    // IO initialization
	GP1DAT = 0xFF000000;	// set P1.n as digital output
	
	
	while (1){
		GP1DAT^=bit;
	}
				
}




void ramp(void)
{
	int Vout=0;
	int step=0;

	// ADC&DAC setting
	ADCpoweron(20000); // power on ADC
	REFCON = 0x01;	   // internal 2.5V reference
	DAC2CON = 0x12;	   // AGND-ARef range 0x12 2.5V
	//DAC1CON = 0x12;	   // AGND-ARef range 0x12 2.5V
	ADCCP = 0x00;	   // conversion on ADC0
	ADCCON = 0x6E4;    // continuous conversion


	while(1){
		if(Vout>=4095)
			step=-1;
		if(Vout<=0)
			step=1;
		Vout=Vout+step;
		DAC2DAT = DATtoADC(Vout); // output voltage on DAC2
	}
}



void init_digital(void)
{
    GP1CON = 0x00000000;    // IO initialization
	GP1DAT = 0xFF000000;	// set P1.n as digital output
	GP0CON = 0x00000000;	// IO initialization
	GP0DAT = 0x00000000;	// set P0.n as digital input
}




#define SWEEP_IN 0
#define CLOCK_IN 4
#define SWEEP_OUT 0

void synchronizer(void)
{
	
	short new_sweep_in=0 ;
	short new_clock_in=0 ;
	short last_sweep_in=0;
	short last_clock_in=0;
	short armed = 0;

	init_digital();


//	DAC1CON = 0x12;	   // AGND-ARef range 0x12 2.5V


	while (1){

        new_sweep_in  = Read_Digital(SWEEP_IN);
        new_clock_in  = Read_Digital(CLOCK_IN);

		if(new_sweep_in==1 && last_sweep_in==0) // edge detection
			armed = 1;

        if(new_clock_in==1 && last_clock_in==0 // edge detection
           && armed==1)                       // + case for fireing output
        {
            Write_Digital(SWEEP_OUT,1);
            armed = 0;
        }

        if(new_clock_in==0 && last_clock_in==1) // edge detection
            Write_Digital(SWEEP_OUT,0);

		last_sweep_in = new_sweep_in;  // memory of the previous state
        last_clock_in = new_clock_in;  // memory of the previous state
	}
}


void synchronizer2(void)
{
	
	short new_sweep_in=0 ;
	short new_clock_in=0 ;
	short last_sweep_in=0;
	short last_clock_in=0;
//	short armed = 0;

	init_digital();


//	DAC1CON = 0x12;	   // AGND-ARef range 0x12 2.5V


	while (1){

        new_sweep_in  = Read_Digital(SWEEP_IN);
        
		if(new_sweep_in==1 && last_sweep_in==0) // edge detection
		{	
			while(1){
				new_clock_in  = Read_Digital(CLOCK_IN);
	        	if(new_clock_in==1 && last_clock_in==0){ // edge detection
	            	Write_Digital(SWEEP_OUT,1);
					break;
				}
				last_clock_in = new_clock_in;  // memory of the previous state
	        }
			while(1){
        		new_clock_in  = Read_Digital(CLOCK_IN);
	        	if(new_clock_in==0 && last_clock_in==1){ // edge detection
            		Write_Digital(SWEEP_OUT,0);
					break;
				}
				last_clock_in = new_clock_in;  // memory of the previous state
	        }
		}

		last_sweep_in = new_sweep_in;  // memory of the previous state
        
	}
}









// Bound the output value of DAC
// when V reaches the limit of DAC range, it will be shifted to the center
/*unsigned long Bound(unsigned long V)
{
    if (V > 0xFCE0000 || V < 0x320000)
		return 0x8000000;
	else
		return V;
} */


int Bound(int V)
{
    if (V > 4000)
		return 500;
	else if(V < 100)
		return 3500;
	else
		return V;
}



#define SH_in     0	  //  high-> lock, low -> hold
#define LH_mode	  3    // high-> lock on max, low -> lock on min
#define SH_disab  7   // disable the sampler & holder function


// for the high mode the SH input has no effect
// 
void lock_EOM(void)
{
	// define variables
	int N_step, N_delay, flag, N, sum, k;
	int Vout, Vin1, Vin2;

	// ADC&DAC setting
	ADCpoweron(20000); // power on ADC
	REFCON = 0x01;	   // internal 2.5V reference
	DAC0CON = 0x12;	   // AGND-ARef range 0x12 2.5V
	ADCCP = 0x00;	   // conversion on ADC0
	ADCCON = 0x6E4;    // continuous conversion

	// IO setting
	init_digital();

	// locking parameters initialization
	N_step = 10;   //  step size
	N_delay = 100; // wait for certain time
	flag = 1;      // indicator for searching direction
	N = 1000;      // number for averaging measurement
	Vin1 = 0;      // initialize the voltage of first step

	// main loop for the locking
	while(1){
		///Sweep();
		// switch between sweep mode and locking mode;
		// note that the sweep mode is just for the convenience of the experiment,
		// for locking the phase, it is not necessary.
		///Write_Digital(0,1); // Output a high level digital output on pin P1.0 for indicating the locking mode.

		if(Read_Digital(SH_disab)==1 || Read_Digital(SH_in)==1){
			Vout = Vout + flag * N_step; // calculate the voltage for next step
			Vout = Bound(Vout); // limit the range of V
			DAC0DAT = DATtoADC(Vout); // output voltage on DAC0
			delay(N_delay); //wait for a certain time for the response of PZT

        	// input average over N samples in order to filter out highest frequencies noise
			sum = 0; // initialization
			for(k = 1; k <= N; k++){
				while(!ADCSTA){}	// wait for the end of ADC conversion
				sum += ADCtoDAT(ADCDAT); // read voltage from ADC0
			}
			Vin2 = sum/N;	// calculate average value for the voltage of second step

			if(Vin2 < Vin1 && Read_Digital(LH_mode)==1) flag = -1 * flag; // change maximum searching direction if V2 < V1
			else if(Vin2 > Vin1 && Read_Digital(LH_mode)==0) flag = -1 * flag; // change minimum searching direction if V2 > V1

			Vin1 = Vin2; // update the voltage of first step

		}
		//else -> it's hold for low locking
	}

}


void lock_EOM2(void)
{
	// define variables
	int N_step, N_delay, flag0,flag1, N, sum, k;
	int Vout0, Vin0a, Vin0b;
	int Vout1, Vin1a, Vin1b;

	// ADC&DAC setting
	ADCpoweron(20000); // power on ADC
	REFCON = 0x01;	   // internal 2.5V reference
	DAC0CON = 0x12;	   // AGND-ARef range 0x12 2.5V
	DAC1CON = 0x12;	   // AGND-ARef range 0x12 2.5V
	ADCCP = 0x00;	   // conversion on ADC0
	ADCCON = 0x6E4;    // continuous conversion

	// IO setting
	init_digital();

	// locking parameters initialization
	N_step = 20;   //  step size
	N_delay = 10; // wait for certain time
	flag0 = 1;      // indicator for searching direction
	flag1 = 1;      // indicator for searching direction
	N = 200;      // number for averaging measurement
	Vin0a = 0;      // initialize the voltage of first step
	Vin1a = 0;      // initialize the voltage of first step

	// main loop for the locking
	while(1){
		///Sweep();
		// switch between sweep mode and locking mode;
		// note that the sweep mode is just for the convenience of the experiment,
		// for locking the phase, it is not necessary.
		///Write_Digital(0,1); // Output a high level digital output on pin P1.0 for indicating the locking mode.

		if(Read_Digital(SH_disab)==1 || Read_Digital(SH_in)==1){
			Vout0 = Vout0 + flag0 * N_step; // calculate the voltage for next step
			Vout0 = Bound(Vout0); // limit the range of V
			DAC0DAT = DATtoADC(Vout0); // output voltage on DAC0
			
			Vout1 = Vout1 + flag1 * N_step; // calculate the voltage for next step
			Vout1 = Bound(Vout1); // limit the range of V
			DAC1DAT = DATtoADC(Vout1); // output voltage on DAC0
			
			
			delay(N_delay); //wait for a certain time for the response of PZT

        	// input average over N samples in order to filter out highest frequencies noise
			sum = 0; // initialization
			ADCCP = 0x00;	   // conversion on ADC0
	
			for(k = 1; k <= N; k++){
				while(!ADCSTA){}	// wait for the end of ADC conversion
				sum += ADCtoDAT(ADCDAT); // read voltage from ADC0
			}
			Vin0b = sum/N;	// calculate average value for the voltage of second step

			if(Vin0b < Vin0a && Read_Digital(LH_mode)==1) flag0 = -1 * flag0; // change maximum searching direction if V2 < V1
			else if(Vin0b > Vin0a && Read_Digital(LH_mode)==0) flag0 = -1 * flag0; // change minimum searching direction if V2 > V1

			Vin0a = Vin0b; // update the voltage of first step


			
			// input average over N samples in order to filter out highest frequencies noise
			sum = 0; // initialization
			ADCCP = 0x01;	   // conversion on ADC1			

			for(k = 1; k <= N; k++){
				while(!ADCSTA){}	// wait for the end of ADC conversion
				sum += ADCtoDAT(ADCDAT); // read voltage from ADC0
			}
			Vin1b = sum/N;	// calculate average value for the voltage of second step

			if(Vin1b < Vin1a && Read_Digital(LH_mode)==1) flag1 = -1 * flag1; // change maximum searching direction if V2 < V1
			else if(Vin1b > Vin1a && Read_Digital(LH_mode)==0) flag1 = -1 * flag1; // change minimum searching direction if V2 > V1

			Vin1a = Vin1b; // update the voltage of first step


		}
		//else -> it's hold for low locking
	}

}


void Copy(void)
{
	// define variables
	int Vout, Vin;
	
	// ADC&DAC setting
	ADCpoweron(20000); // power on ADC
	REFCON = 0x01;	   // internal 2.5V reference
	DAC2CON = 0x12;	   // AGND-ARef range 0x12 2.5V
	//DAC1CON = 0x12;	   // AGND-ARef range 0x12 2.5V
	ADCCP = 0x03;	   // conversion on ADC4
	ADCCON = 0x6E4;    // continuous conversion

	// IO setting
	init_digital();

	
	// main loop for the locking
	while(1){
		
		while(!ADCSTA){}	// wait for the end of ADC conversion
		Vin = ADCtoDAT(ADCDAT); // read voltage from ADC4
		Vout = Vin;
		DAC2DAT = DATtoADC(Vout); // output voltage on DAC2
			
		//else -> it's hold for low locking
	}

}



void Copy_S_and_H(void)
{
	// define variables
	int Vout, Vin;
	
	// ADC&DAC setting
	ADCpoweron(20000); // power on ADC
	REFCON = 0x01;	   // internal 2.5V reference
	DAC2CON = 0x12;	   // AGND-ARef range 0x12 2.5V
	//DAC1CON = 0x12;	   // AGND-ARef range 0x12 2.5V
	ADCCP = 0x03;	   // conversion on ADC4
	ADCCON = 0x6E4;    // continuous conversion

	// IO setting
	init_digital();

	
	// main loop for the locking
	while(1){
		
		if(Read_Digital(6)==1){
			while(!ADCSTA){}	// wait for the end of ADC conversion
			Vin = ADCtoDAT(ADCDAT); // read voltage from ADC4
			Vout = Vin;
			DAC2DAT = DATtoADC(Vout); // output voltage on DAC2
		}	
		//else -> it's hold for low locking
	}

}

void Copy_S_and_Hcnt(void)
{
	// define variables
	int Vout, Vin, k, sum, Ncount;
	k=0;
	sum =0;
	Ncount=100;

	// ADC&DAC setting
	ADCpoweron(20000); // power on ADC
	REFCON = 0x01;	   // internal 2.5V reference
	DAC2CON = 0x12;	   // AGND-ARef range 0x12 2.5V
	//DAC1CON = 0x12;	   // AGND-ARef range 0x12 2.5V
	ADCCP = 0x03;	   // conversion on ADC4
	ADCCON = 0x6E4;    // continuous conversion

	// IO setting
	init_digital();

	
	// main loop for the locking
	while(1){
		
		if(Read_Digital(6)==1){
			while(!ADCSTA){}	// wait for the end of ADC conversion
			Vin = ADCtoDAT(ADCDAT); // read voltage from ADC4
			sum+=Vin;
			k++;
		}
		
		if(k>=Ncount){
			k=0;
			Vout = sum/Ncount;
			DAC2DAT = DATtoADC(Vout); // output voltage on DAC2
			sum=0;
		}	
		//else -> it's hold for low locking
	}

}


void ramp_with_TTLin(void)
{
	int Vout=0;
	int step=0;

	// ADC&DAC setting
	ADCpoweron(20000); // power on ADC
	REFCON = 0x01;	   // internal 2.5V reference
	DAC2CON = 0x12;	   // AGND-ARef range 0x12 2.5V
	//DAC1CON = 0x12;	   // AGND-ARef range 0x12 2.5V
	ADCCP = 0x00;	   // conversion on ADC0
	ADCCON = 0x6E4;    // continuous conversion

	// IO setting
	init_digital();


	while(1){
		if(Vout>=4095)
			step=-1;
		if(Vout<=0)
			step=1;
		Vout=Vout+step;
		if(Read_Digital(3)==1){
			DAC2DAT = DATtoADC(Vout); // output voltage on DAC0
		}

	}
}




#define SH_in0     0	  //  high-> lock, low -> hold
#define SH_in1     1	  //  high-> lock, low -> hold
#define SH_disab  7   // disable the sampler & holder function


void lock_StabPulse(void)
{
	// define variables
	int N_step, N_delay, cnt0,cnt1, N, sum0, sum1;
	int Vout0, Vin0;
	int Vout1, Vin1;
	int Vset0, Vset1;
	int Vmean;
	int MeasureTTLin = 6;
	int ModeTTLin = 3;
	int bit =0x00000001<<(20);
	
	/*POWKEY1 = 0x01;
	POWCON = 0x00;		   				// 41.78MHz
	POWKEY2 = 0xF4;
	 */

	GP1CON = 0x00000000;    // IO initialization
	GP1DAT = 0xFF000000;	// set P1.n as digital output


	// ADC&DAC setting
	ADCpoweron(20000); // power on ADC
	REFCON = 0x01;	   // internal 2.5V reference
	//DAC0CON = 0x12;	   // AGND-ARef range 0x12 2.5V
	DAC2CON = 0x12;	   // AGND-ARef range 0x12 2.5V
	ADCCP = 0x03;	   // conversion on ADC0
	ADCCON = 0x6E4;    // continuous conversion

	// IO setting
	init_digital();

	// locking parameters initialization
	N_step = 5;   //  step size
	N_delay = 10; // wait for certain time
	cnt0 = 0;      // indicator for searching direction
	cnt1 = 0;      // indicator for searching direction
	N = 100;      // number for averaging measurement
	Vin0 = 0;      // initialize the voltage of first step
	Vin1 = 0;      // initialize the voltage of first step
	Vmean = 2000;

	// main loop for the locking
	while(1){
		
		///Write_Digital(0,1); // Output a high level digital output on pin P1.0 for indicating the locking mode.

		//GP1DAT^=bit;
	
		// for each loop we check if there is something to measure
		if(Read_Digital(MeasureTTLin)==1){
				
			ADCCP = 0x03;	   // conversion on ADC0
	
			if (cnt0==0)
				sum0 = 0; // initialization	of the measurement

			while(!ADCSTA){}	// wait for the end of ADC conversion
			sum0 += ADCtoDAT(ADCDAT); // read voltage from ADC0
			cnt0++;
			if(cnt0>=N)
				Vin0 = sum0/N;	// calculate average value
		}

		//the same for the other chanel
		/*if(Read_Digital(SH_in1)==1){
			ADCCP = 0x01;	   // conversion on ADC1			

			if (cnt1==0)
				sum1 = 0; // initialization	of the measurement

			while(!ADCSTA){}	// wait for the end of ADC conversion
			sum1 += ADCtoDAT(ADCDAT); // read voltage from ADC0
			cnt1++;
			if(cnt1==N)
				Vin1 = sum1/N;	// calculate average value
		}*/


		// if we are in the learning mode
		// we set the outputs to the average voltage
		// and save the current input level as the set point of the next locking enable
		if(Read_Digital(ModeTTLin)==0){
			if(cnt0>=N)
				Vset0 = Vin0;
			//if(cnt1>=N)
			//	Vset1 = Vin1;
			Vout0 = Vmean;
			//Vout1 = Vmean;
		} 

		// if we are in the locking mode and each time we have a complete measurement
		if(Read_Digital(ModeTTLin)==1 && cnt0>=N){
			if(Vset0<Vin0)
				Vout0 = Vout0 - N_step; // calculate the voltage for next step
			if(Vset0>Vin0)
				Vout0 = Vout0 + N_step; // calculate the voltage for next step
		}

		//the same for the second chanel
		/*if(Read_Digital(SH_disab)==0 && cnt1==N){
			if(Vset1<Vin1)
				Vout1 = Vout1 + N_step; // calculate the voltage for next step
			if(Vset1>Vin1)
				Vout1 = Vout1 - N_step; // calculate the voltage for next step
		} */

		if(cnt0>=N)
			cnt0=0;

		//those line could also be inserted in the if conditions and thus not set every loops
		DAC2DAT = DATtoADC(Vout0); // output voltage on DAC0
		//DAC1DAT = DATtoADC(Vout1); // output voltage on DAC1
						
		//delay(N_delay); //wait for a certain time.. maybe not nessecary

		
		//else -> it's hold for low locking
	}

}

void lock_StabPulse3(void)
{
	// define variables
	int N_step, N_delay, cnt0,cnt1, N, sum0;
	int Vout0, Vin0;
	int Vout1, Vin1;
	int Vset0, Vset1;
	int Vmean;
	int MeasureTTLin = 6;
	int ModeTTLin = 3;
	int bit =0x00000001<<(19);
	
	POWKEY1 = 0x01;
	POWCON = 0x00;		   				// 41.78MHz
	POWKEY2 = 0xF4;
	 

	GP1CON = 0x00000000;    // IO initialization
	GP1DAT = 0xFF000000;	// set P1.n as digital output


	// ADC&DAC setting
	ADCpoweron(20000); // power on ADC
	REFCON = 0x01;	   // internal 2.5V reference
	//DAC0CON = 0x12;	   // AGND-ARef range 0x12 2.5V
	DAC2CON = 0x12;	   // AGND-ARef range 0x12 2.5V
	ADCCP = 0x03;	   // conversion on ADC0
	ADCCON = 0x3E4;    // continuous conversion

	// IO setting
	init_digital();

	// locking parameters initialization
	N_step = 5;   //  step size
	N_delay = 10; // wait for certain time
	cnt0 = 0;      // indicator for searching direction
	cnt1 = 0;      // indicator for searching direction
	N = 100;      // number for averaging measurement
	Vin0 = 0;      // initialize the voltage of first step
	Vin1 = 0;      // initialize the voltage of first step
	Vmean = 2000;

	// main loop for the locking
	while(1){
		
		///Write_Digital(0,1); // Output a high level digital output on pin P1.0 for indicating the locking mode.

		GP1DAT^=bit;
	
		// for each loop we check if there is something to measure
		if((((GP0DAT&0x000000FF)>>6)&0x1)==1){
				
			//ADCCP = 0x03;	   // conversion on ADC0
	
			if (cnt0==0)
				sum0 = 0; // initialization	of the measurement

			while(!ADCSTA){}	// wait for the end of ADC conversion
			sum0 += ADCtoDAT(ADCDAT); // read voltage from ADC0
			cnt0++;
			if(cnt0>=N)
				Vin0 = sum0/N;	// calculate average value
		}
	


		// if we are in the learning mode
		// we set the outputs to the average voltage
		// and save the current input level as the set point of the next locking enable
		if((((GP0DAT&0x000000FF)>>3)&0x1)==0){
			if(cnt0>=N){
				Vset0 = Vin0;
				cnt0=0;
			}
			Vout0 = Vmean;
		} 

		// if we are in the locking mode and each time we have a complete measurement
		/*if((((GP0DAT&0x000000FF)>>3)&0x1)==1 && cnt0>=N){
			cnt0=0;
			if(Vset0<Vin0)
				Vout0 = Vout0 - N_step; // calculate the voltage for next step
			if(Vset0>Vin0)
				Vout0 = Vout0 + N_step; // calculate the voltage for next step
		}*/

		if((((GP0DAT&0x000000FF)>>3)&0x1)==1 && cnt0>=N){
			cnt0=0;
			Vout0 = Vout0 + (Vset0-Vin0)/2;
		}


		//those line could also be inserted in the if conditions and thus not set every loops
		DAC2DAT = DATtoADC(Vout0); // output voltage on DAC0
		
	}
}



void lock_StabPulse4(void)
{
	// define variables
	int N_step, N_delay, cnt0,cnt1, N, sum0, sum1;
	int Vout0, Vin0;
	unsigned long int Vout1, Vin1;
	int Vset0, Vset1;
	int Vmean;
	int MeasureTTLin = 6;
	int ModeTTLin = 3;
	int bit =0x00000001<<(19);
	int step_max = 100;
	int step;
	
	POWKEY1 = 0x01;
	POWCON = 0x00;		   				// 41.78MHz
	POWKEY2 = 0xF4;
	 

	GP1CON = 0x00000000;    // IO initialization
	GP1DAT = 0xFF000000;	// set P1.n as digital output


	// ADC&DAC setting
	ADCpoweron(20000); // power on ADC
	REFCON = 0x01;	   // internal 2.5V reference
	//DAC0CON = 0x12;	   // AGND-ARef range 0x12 2.5V
	DAC2CON = 0x12;	   // AGND-ARef range 0x12 2.5V
	ADCCP = 0x03;	   // conversion on ADC0
	ADCCON = 0x3E4;    // continuous conversion

	// IO setting
	init_digital();

	// locking parameters initialization
	N_step = 5;   //  step size
	N_delay = 10; // wait for certain time
	cnt0 = 0;      // indicator for searching direction
	cnt1 = 0;      // indicator for searching direction
	N = 50;      // number for averaging measurement
	Vin0 = 0;      // initialize the voltage of first step
	Vin1 = 0;      // initialize the voltage of first step
	Vmean = 2000;

	// main loop for the locking
	while(1){
		
		///Write_Digital(0,1); // Output a high level digital output on pin P1.0 for indicating the locking mode.

		//GP1DAT^=bit;
	
		// for each loop we check if there is something to measure
		if((((GP0DAT&0x000000FF)>>6)&0x1)==1){
				
			//ADCCP = 0x03;	   // conversion on ADC0
	
			if (cnt0==0)
				sum0 = 0; // initialization	of the measurement
			
			GP1DAT^=bit;
			while(!ADCSTA){}	// wait for the end of ADC conversion
			GP1DAT^=bit;
			if((((GP0DAT&0x000000FF)>>6)&0x1)==1){
				sum0 += ADCtoDAT(ADCDAT); // read voltage from ADC0
				cnt0++;
			}
			if(cnt0>=N)
				Vin0 = sum0/N;	// calculate average value
		}

		else if((((GP0DAT&0x000000FF)>>3)&0x1)==1){
			if(cnt0>=N){
				cnt0=0;
				step = (Vset0-Vin0)/4;
				if (step>step_max)
					step = step_max;
				else if (step<-step_max)
					step = -step_max;
				Vout0 = Vout0 + step;
			}
		}

		// if we are in the learning mode
		// we set the outputs to the average voltage
		// and save the current input level as the set point of the next locking enable
		else{
			if(cnt0>=N){
				Vset0 = Vin0;
				cnt0=0;
			}
			Vout0 = Vmean;
		}


		//those line could also be inserted in the if conditions and thus not set every loops
		DAC2DAT = DATtoADC(Vout0); // output voltage on DAC0
		
	}
}




 void lock_StabPulse2(void)
{
	// define variables
	int N_step, N_delay, cnt0,cnt1, N, sum0, sum1;
	int Vout0, Vin0;
	int Vout1, Vin1;
	int Vset0, Vset1;
	int Vmean;
	int MeasureTTLin = 6;
	int ModeTTLin = 3;
	int bit =0x00000001<<(20);
	
	POWKEY1 = 0x01;
	POWCON = 0x00;		   				// 41.78MHz
	POWKEY2 = 0xF4;
	

	GP1CON = 0x00000000;    // IO initialization
	GP1DAT = 0xFF000000;	// set P1.n as digital output


	// ADC&DAC setting
	ADCpoweron(20000); // power on ADC
	REFCON = 0x01;	   // internal 2.5V reference
	//DAC0CON = 0x12;	   // AGND-ARef range 0x12 2.5V
	DAC2CON = 0x12;	   // AGND-ARef range 0x12 2.5V
	ADCCP = 0x03;	   // conversion on ADC0
	ADCCON = 0x6E4;    // continuous conversion

	// IO setting
	init_digital();

	// locking parameters initialization
	N_step = 5;   //  step size
	N_delay = 10; // wait for certain time
	cnt0 = 0;      // indicator for searching direction
	cnt1 = 0;      // indicator for searching direction
	N = 100;      // number for averaging measurement
	Vin0 = 0;      // initialize the voltage of first step
	Vin1 = 0;      // initialize the voltage of first step
	Vmean = 2000;


	// main loop for the locking
	while(1){
		
		///Write_Digital(0,1); // Output a high level digital output on pin P1.0 for indicating the locking mode.

		GP1DAT^=bit;
	
		// for each loop we check if there is something to measure
		if(((GP0DAT&0x000000FF)>>6)&0x1==1){
				
			ADCCP = 0x03;	   // conversion on ADC0
	
			if (cnt0==0)
				sum0 = 0; // initialization	of the measurement

			while(!ADCSTA){}	// wait for the end of ADC conversion
			sum0 += ADCtoDAT(ADCDAT); // read voltage from ADC0
			cnt0++;
			if(cnt0>=N)
				Vin0 = sum0/N;	// calculate average value
		}		


		// if we are in the learning mode
		// we set the outputs to the average voltage
		// and save the current input level as the set point of the next locking enable
		if(((GP0DAT&0x000000FF)>>3)&0x1==0){
			if(cnt0>=N){
				Vset0 = Vin0;
				cnt0=0;
			}
			//if(cnt1>=N)
			//	Vset1 = Vin1;
			Vout0 = Vmean;
			//Vout1 = Vmean;
		}

		// if we are in the locking mode and each time we have a complete measurement
		if(((GP0DAT&0x000000FF)>>3)&0x1==1 && cnt0>=N){
			cnt0=0;
			if(Vset0<Vin0)
				Vout0 = Vout0 - N_step; // calculate the voltage for next step
			if(Vset0>Vin0)
				Vout0 = Vout0 + N_step; // calculate the voltage for next step
		}

		

		//those line could also be inserted in the if conditions and thus not set every loops
		DAC2DAT = DATtoADC(Vout0); // output voltage on DAC0
		
		
		//else -> it's hold for low locking
	}

}






void lock_EOM_and_AOM(void)
{
    int step, N_delay, V_EOM;//, V_AOM;
	//unsigned long y_max, y_min;
	
	ADCpoweron(20000);			// power on ADC	
	ADCCP = 0x00;				// conversion on ADC0
	DAC0CON = 0x12;				// AGND-ARef range 0x12 2.5V
	DAC1CON = 0x12;	
	REFCON = 0x01;				// internal 2.5V reference
    ADCCON = 0x6E4;             // continuous conversion
	GP1CON = 0x00000000;	    // setting IO
	GP1DAT = 0xFF000000;	    // group P1.x as output
	GP0CON = 0x00000000;	    // setting IO
	GP0DAT = 0x00000000;        // group P0.x as input
	
	step=5;
	N_delay=500;
	V_EOM = 2048;
//	V_AOM = 0;
	
	while(1){ // infinite loop
		
		// wait for the trigger of calibration
		if(Read_Digital(6)==1){
            Write_Digital(3,1);// LED on
            GP4DAT = 0x04000000;
            
            // calibration of the zero of the EOM
            Lock_min_fringe(&V_EOM,step, N_delay);
            
            // calibration of the power with the AOM
            
            
		}
		Write_Digital(3,0);// LED off
		GP4DAT ^= 0x00040000;
		
	}
}


int main(void)
{ 
    //test_blink();
	//Copy();
	//Copy_S_and_Hcnt();
	//ramp();
	//ramp_with_TTLin();
	//lock_StabPulse();
	//lock_EOM2();
	//synchronizer2();
	
	lock_StabPulse4();
	
	//test_clock2();

	return 0;
}