// Test reading encoder



#include <stdio.h>

#include <stdlib.h>

#include <iostream.h>

#include <math.h>

#include <termios.h>

#include <sys/sched.h>

#include <sys/types.h>

#include <unistd.h>



#include "driver/PrTimer.h"

//#include "/local/pr/include/driver/ServoToGo.h"

#include "driver/ServoToGo.h"

#include "driver/PrSTGDefn.h"





#define DELAY_1MS		7159		// 1 ms delay if timer crystal is 7.159 MHz



#define HZ  500



//unsigned short GetADC(unsigned char iChannel);

unsigned char hitIndex (unsigned char channel);



ServoToGo *stg = new ServoToGo(STG_ARM_IRQ, STG_ARM_ADDRESS);	// new

//ServoToGo *stg = new ServoToGo(2, 0x320);	// old



void main (void)

{

	int i,k; 

	PrTimer timer(HZ);

	static int counter = 0;

	int misTick;

	clock_t curTime, prevTime;

	long int elapsedTime;

	FILE *f2;

	double time1, time2, deltaTime;

	static long int loopCounter = 0;



	STG_LONGBYTE e[8];

	static STG_LONGBYTE ep[8];



	pid_t pid = getpid();

//	setprio(pid,63);

	setprio(0,63);



	printf("Priority set to: %d \n", getprio(pid));

	f2 = fopen("encdata.txt","w");

	timer.start();

	stg->EncoderInit(); stg->EncoderLatch(); stg->EncReadAll(e);

	stg->CalADC();

	stg->ResetIndexLatches(0x00);

	stg->Timer2Delay(DELAY_1MS);



	for(i=0;i<8;i++)

		ep[i].Long = e[i].Long;



	printf("\n");

#if 0

	static float time = 0.0;

	float dacOut;

	while(!tcischars(1))

	{	

		stg->EncoderLatch();

		stg->EncReadAll(e);

		printf("[6] %d \r", e[6].Long);

//		dacOut = 250.0*cos(time);	

//		time += 1.2/HZ;

//		stg->RawDAC(6,(short)(dacOut));

//		printf("dac: %f \n", dacOut);

//		stg->RawDAC(2,500);

		stg->RawDAC(6,-170);

//		printf("testing DAC \n");



		timer.wait();

	}

	exit(1);

#endif





	printf("(1) Encoder test \n");

	printf("(2) AD test \n");

	printf("(3) Test Index \n");



	int mode;

	unsigned char answer;



	cin >> answer;



	switch(answer)

	{

		case '1':

			printf("Init to zero? (1) yes (2) no \n");

			cin >> answer;



			if (answer == '1')

			{

				for(k=0; k<8; k++)

					stg->SetEncoderCounts(k, 0);

			}



			printf("spin encoder around \n \n");



			for(k=0; k<8; k++)

				printf("     [%d] ",k);



			printf("\n");





			while(!tcischars(1))

			{ 

				stg->EncoderLatch();

				stg->EncReadAll(e);

			

			//	if(counter == int(HZ))

				if(counter == 20)

				{	

					printf(" %8d %8d %8d %8d %8d %8d %8d %8d \r",

						e[0].Long,e[1].Long,e[2].Long,e[3].Long,

						e[4].Long,e[5].Long,e[6].Long,e[7].Long);

					counter = 0;

				}

				counter++;



				misTick = timer.wait();

				if(misTick != 0)

						printf("loop: %8d, missed %d ticks \n", loopCounter, misTick);



				time1 = timer.timeGlobal();

				deltaTime = time1 - time2;

				time2 = time1;

				loopCounter++;				

//				printf("%f \n", timer.timeGlobal());

//				curTime = clock();

//				elapsedTime = (curTime - prevTime)/CLOCKS_PER_SEC;

//				prevTime = curTime;

	//			fprintf(f2,"%8d %lu %d %d \n", e[4].Long, elapsedTime, curTime, prevTime);

//				fprintf(f2,"%8d %8d %f \n", loopCounter, e[4].Long, deltaTime);

			}



			break;



		case '2':

			printf("Displaying A/D data on screen \n \n");



			short data[8];

			float v[8];

			int i;



			while(!tcischars(1))

			{

				for(i=0;i<8;i++)

				{

					data[i] = stg->GetADC(i);

					v[i] = float(data[i])*10.0/4096.0;

				}

//				printf("%5d %5d %5d %5d %5d %5d %5d %5d \r", 

//					data[0], data[1], data[2], data[3], 

//					data[4], data[5], data[6], data[7]);



	//			printf("%5d %5d %5d %5d %5d %5d %5d %5d\n", 

	//				data[0], data[1], data[2],data[3], data[4], data[5], data[6], data[7]);



				printf("%8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f \n",

						v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7]);

				timer.wait();

			}



			break;



		case '3':

			unsigned char CHANNEL = 3;	// 4 is joint motor enc; 5 is joint enc

			printf("Testing index pulse \n");

			unsigned char selectbit = 0xff &~ (1<<CHANNEL);

	//		timer.start();



			static int  counter = 0;

			stg->SelectIndexOrExtLatch(selectbit);

			stg->ResetIndexLatches(0x00);

			while(!tcischars(1))

			{

					if(counter == 20)

					{

						stg->EncoderLatch();

						stg->EncReadAll(e);

//						printf("e: %8d\r", e[CHANNEL]); 

						counter = 0;

					}

					counter++;

				if(hitIndex(CHANNEL))

				{

					stg->EncoderLatch();

					stg->EncReadAll(e);

					printf("index, e: %8d, %8d, %8d \n", e[CHANNEL], 

							e[CHANNEL].Long-ep[CHANNEL].Long, ep[CHANNEL].Long);

					stg->ResetIndexLatches(0x00);

					ep[CHANNEL] = e[CHANNEL];

				}

	//				timer.wait();

			}



	}



 	fclose(f2); 





}



unsigned char hitIndex (unsigned char channel)

{

	if(stg->GetIndexLatches() && (1<<channel))

	{

		return(1);

	}

	else

		return(0);

}



			



