

#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 "driver/PrSTGDefn.h"
#include "matrix/PrVector.h"
#include "matrix/PrMatrix.h"
#include "sampler.h"

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

#define HZ  5000

#define BUFFER_SIZE		20000
#define BUFFER_COL		10	

#define NUM_JOINT	3
#define NUM_JOINT1	6

//#define PRINT
baseControl::baseControl()
{
	int i;
	stg = new ServoToGo(STG_ARM_IRQ, STG_ARM_ADDRESS);

	for(int i=0;i<8;i++)
	{
		angleOld[i] = 0.0;
		velocityOld[i] = 0.0;
	}

	initSystem();

	//init variables
	a0 = new PrVector(NUM_JOINT1);
	a2 = new PrVector(NUM_JOINT1);
	a3 = new PrVector(NUM_JOINT1);
	a0->zero(); a2->zero(); a3->zero();
	qgoal = new PrVector(NUM_JOINT1);
	qdes = new PrVector(NUM_JOINT1);
	dqdes = new PrVector(NUM_JOINT1);
	tfinal = new PrVector(NUM_JOINT1);
	qgoal->zero(); qdes->zero(); dqdes->zero(); tfinal->zero();
	q = new PrVector(NUM_JOINT1);
	dq = new PrVector(NUM_JOINT1);
	q->zero();	dq->zero();

	dataBuffer3 = new PrMatrix(BUFFER_SIZE,BUFFER_COL);
	dataBuffer4 = new PrMatrix(BUFFER_SIZE,BUFFER_COL);
	dataBuffer3->zero();
	dataBuffer4->zero();
	printf("Constructor finished \n");


}

baseControl::~baseControl()
{
	delete stg;
	closeDataFile();
}

void baseControl::initSystem(void)
{
	stg->EncoderInit();
	stg->EncoderLatch();
//	stg->SetEncoderCounts(baseWA1,0);
//	stg->SetEncoderCounts(baseSH1,0);
	initIndex();
//	initLOHETValue[WA1] = stg->GetADC(WA1);
//	initLOHETValue[SH1] = stg->GetADC(EL1);
//	initLOHETValue[EL1] = stg->GetADC(EL1);
	initLOHET[WA1] = (float)(stg->GetADC(WA1))*2.0*5.0/4092.0;
	initLOHET[WA1] = (float)(stg->GetADC(WA1))*2.0*5.0/4092.0;
	initLOHET[WA1] = (float)(stg->GetADC(WA1))*2.0*5.0/4092.0;
	printf("Initial LOHET value: %f \n", initLOHET[WA1]);
}

void baseControl::initIndex(void)
{
	stg->SelectIndexOrExtLatch(0x00);
	stg->EnableCounterLatchOnIndexOrExt(0xff);
	resetIndex();
}

void baseControl::resetIndex(void)
{
	stg->ResetIndexLatches(0x00);
}

int baseControl::hitIndex2(int channel)
{
	int check = 1<<channel;
	int result = stg->GetIndexLatches();
	if(stg->GetIndexLatches() & (1<<channel))
	{
//			printf("index: %d, %x, %d, %d \n", result, 
//							result, check, result & check  );
			return(1);
	}	
	else
		return(0);
}

void baseControl::openDataFile(void)
{
	fdata = fopen("dataSampler.txt","w");
	fdata2 = fopen("dataSampler2.txt","w");
}

void baseControl::closeDataFile(void)
{
	fclose(fdata);
	fclose(fdata2);
}

void baseControl::readPosition(void)
{
	stg->EncoderLatch();
	stg->EncReadAll(temp);

	baseEncoder[WA1].Long = temp[baseWA1].Long;
	// temporary HACK HERE!!!
	// ELBOW CURRENTLY USING baseSH1 CHANNEL. SHOULD REVERT BACK TO baseEL1 LATER!!!
	baseEncoder[EL1].Long = temp[baseSH1].Long;
//	printf("%d \n", baseEncoder[WA1].Long);
}

void baseControl::calcPosition(void)
{
		//all angles are in DEGREES!!!
	// USE THIS FOR WAIST JOINT
	baseOutputAngle[WA1] = (float)(baseEncoder[WA1].Long) * 360.0*(1.0/120.0)/8192.0;
	// USE THIS FOR ELBOW JOINT
//	baseOutputAngle[EL1] = (float)(baseEncoder[EL1].Long) * 360.0*(1.0/50.0)/8192.0;

//	printf("Angle: %f, %d  \n", baseOutputAngle[WA1], baseEncoder[WA1].Long);
#ifdef PRINT
	printf("Angle: %f  \n", baseOutputAngle[WA1]);
#endif
}

void baseControl::displayInfo(void)
{
	static int counter = 0;

	if(counter == 100)
	{
		printf("Angle: %6.2f, LOHET: %5.3f, torque: %5.2f \n", baseOutputAngle[WA1], LOHET[WA1],
						baseSpringTorque[WA1]);
		counter = 0;
	}
	counter++;
}

void baseControl::resetBaseEncoder(void)
{
	stg->SetEncoderCounts(baseWA1,0);	
	printf("Base encoder reset \n");
}

void baseControl::readLOHET(void)
{
	// LOHET voltage
	LOHET[WA1] = (float)(stg->GetADC(WA1))*2.0*5.0/4096.0 - 0*initLOHET[WA1];
	LOHET[SH1] = (float)(stg->GetADC(SH1))*2.0*5.0/4096.0 - 0*initLOHET[SH1];
	LOHET[EL1] = (float)(stg->GetADC(EL1))*2.0*5.0/4096.0 - 0*initLOHET[EL1];

#ifdef PRINT
	printf("LOHET: %f \n", LOHET[WA1]);
#endif
//	printf("LOHET: %f, %f, %f \n", LOHET[WA1], LOHET[SH1], LOHET[EL1]);
}

void baseControl::calcTorque(void)
{
	float m[3] = {27.3, 0.0, 5.12};
	float b[3] = {0.00, 0.0, 0.0};
	//WA1 torque
	baseSpringTorque[WA1] = m[WA1]*LOHET[WA1] + b[WA1];
	//EL1 torque
	baseSpringTorque[EL1] = m[EL1]*LOHET[EL1] + b[EL1];

}

void baseControl::saveInfo(void)
{
	static long int index = 0;

	if(index < BUFFER_SIZE)
	{
		dataBuffer3->elementAt(index,0) = LOHET[EL1];
//		dataBuffer3->elementAt(index,1) = LOHET[SH1];
//		dataBuffer3->elementAt(index,2) = LOHET[EL1];
		index++;
		bufferIndex = index;
	}

}

void baseControl::saveInfo2(void)
{
	static long int index = 0;

	if(index < BUFFER_SIZE)
	{
		dataBuffer4->elementAt(index,0) = LOHET[EL1];
//		dataBuffer4->elementAt(index,1) = LOHET[SH1];
//		dataBuffer4->elementAt(index,2) = LOHET[EL1];
		index++;
		bufferIndex2 = index;
	}

}

void baseControl::saveBuffer(void)
{
	int i;
	openDataFile();
	PrTimer bufferTimer(2000.0);

	printf("Saving data buffer \n");
	bufferTimer.start();
	for(i=0;i<bufferIndex;i++)
	{
//		fprintf(fdata,"%6.3f %6.3f %6.3f\n",
//						dataBuffer3->elementAt(i,0), dataBuffer3->elementAt(i,1),
//						dataBuffer3->elementAt(i,2));
		fprintf(fdata,"%6.3f\n",
						dataBuffer3->elementAt(i,0));
		bufferTimer.wait();
	}
	bufferTimer.start();
	for(i=0;i<bufferIndex2;i++)
	{
//		fprintf(fdata,"%6.3f %6.3f %6.3f\n",
//						dataBuffer3->elementAt(i,0), dataBuffer3->elementAt(i,1),
//						dataBuffer3->elementAt(i,2));
		fprintf(fdata2,"%6.3f\n",
						dataBuffer4->elementAt(i,0));
		bufferTimer.wait();
	}

	closeDataFile();

	printf("Saving completed \n");
}

#define limitDAC	800
void baseControl::baseMotorControl(float runTime)
{
	float driveFreq = 0.5;
	float driveAmplitude[3] = {25.0, 5.0, 20.0};
	float Kp[3] = {250.0, 20.0, 250.0};

	baseOutputAngleDes[WA1] = driveAmplitude[WA1]*sin(driveFreq*runTime);

	baseCommand[WA1] = (short)(Kp[WA1]*(baseOutputAngleDes[WA1] - baseOutputAngle[WA1]));

	if (baseCommand[WA1] > limitDAC)
			baseCommand[WA1] = limitDAC;
	else if (baseCommand[WA1] < -limitDAC)
			baseCommand[WA1] = -limitDAC;
	stg->RawDAC(6, baseCommand[WA1]);

#ifdef PRINT
	printf("Des: %f, At: %f, control: %d \n", baseOutputAngleDes[WA1], baseOutputAngle[WA1],
				baseCommand[WA1]);
#endif

}

#define ELDAC	300
void baseControl::moveBaseShaft(int direction)
{
	// +- 150 for WA motor
	// +- 400 for EL motor
	if(direction == 0)
		stg->RawDAC(6,ELDAC);
	else if(direction == 1)
		stg->RawDAC(6,-ELDAC);
}

#define KDAC	10.0/4096.0	// DAC gain, V/count
#define KAMP	2.0		// amplifier gain, A/V
#define KTORQUE	0.0727	// motor torque constant, N.m./A
#define limitDAC2	4000
void baseControl::SEAcontrol(float torqueDes)
{
	static float x = 0.0, xold = 0.0, y = 0.0, yold = 0.0;
	
	// digital PD compensator with low-pass filter
	// x is input to the compensator, y is output the output
	baseTorqueCommand[WA1] = torqueDes;
	x = baseTorqueCommand[WA1] - baseSpringTorque[WA1]; 
	// x is the difference between desired and spring torque

#if 0
	// USING STANDARD PD CONTROLLER
//	float a1 = 1.375, a2 = 1.307, b1 = 0.5983;	// wc = 40Hz
//	float a1 = 1.404, a2 = 1.3738, b1 = 0.6395;	// wc = 35Hz
//	float a1 = 1.051, a2 = 0.9973, b1 = 0.6395;	// wc = 35Hz
//	float a1 = 0.891, a2 = 0.8703, b1 = 0.7285;	// wc = 35Hz
	float a1 = 0.667, a2 = 0.6409, b1 = 0.8277;	// wc = 15Hz
	float Kp = 1.0;
//	y = b1*yold + a1*x - a2*xold;
	y = b1*yold + Kp*(a1*x - a2*xold);
	// y is the motor torque command
	xold = x;
	yold = y;
#endif

#if 0
	//USING P+PSEUDO-D CONTROLLER
//	float Kp = 0.8*0.6, a1 = 2.0, b1 = 0.5983;
//	float Kp = 0.4375, a1 = 4.852, b1 = 0.02961;
//	float Kp = 0.276, a1 = 2.701, b1 = 0.2283;
//	float Kp = 0.127, a1 = 0.6344, b1 = 0.6828;
	float Kp = 0.127, a1 = 0.54081, b1 = 0.6395;	// wc = 35Hz
	static float y2, y2old = 0.0, x1;
	x1 = baseSpringTorque[WA1];	// x1 is the measured spring torque
	static float x1old = x1;	// x1old is measured spring torque at the prev. tick

	y2 = b1*y2old + a1*(x1 - x1old);	//y2 is the derivate term for the measured torque
	Kp = 0.2;
	y = Kp*x - y2;	// x is torque error

	y2old = y2;
	x1old = x1;
#endif

#if 0
	// ELBOW CONTROLLER
	//USING P+PSEUDO-D CONTROLLER+LAG
	float Kp = 0.2, a1 = 0.54081, b1 = 0.6395;	// wc = 35Hz
	static float y2, y2old = 0.0, x1, x3, y3;
	x1 = baseSpringTorque[WA1];	// x1 is the measured spring torque

	Kp = 0.16;	// 0.16 for EL
	x3 = Kp*x;			// P term, Kp*(torque error)		
	// x3 is torque error * Kp
	static float x1old = x1;	// x1old is measured spring torque at the prev. tick
	static float y3old = 0.0, x3old = 0.0;
	
	float a2 = 1.006, b2 = 0.9987, c2 = 0.9869;
	// P + lag term
	// input to this is (torque error * Kp), x3
	y3 = b2*y3old + a2*(x3 - c2*x3old);	// lag term
	y3old = y3;	x3old = x3;

	y2 = b1*y2old + a1*(x1 - x1old);	//y2 is the derivate term for the measured torque

	y = y3 - y2;	// (P+lag) - (derivative) term 

	y2old = y2;
	x1old = x1;

#endif

#if 1
	// WAIST CONTROLLER
	//USING P+PSEUDO-D CONTROLLER+LAG
//	float Kp = 0.2, a1 = 0.59489, b1 = 0.6395;	// wc = 35Hz
	float Kp = 0.07, a1 = 0.3122, b1 = 0.7285;	// wc = 35Hz
	static float y2, y2old = 0.0, x1, x3, y3;
	x1 = baseSpringTorque[WA1];	// x1 is the measured spring torque
//	printf("x1 1: %f ", x1);
	Kp = 0.07;	// 0.16 FOR el
	x3 = Kp*x;			// P term, Kp*(torque error)		
	// x3 is torque error * Kp
	static float x1old = x1;	// x1old is measured spring torque at the prev. tick
	static float y3old = 0.0, x3old = 0.0;
	
	float a2 = 1.0054, b2 = 0.9988, c2 = 0.9881;
	// P + lag term
	// input to this is (torque error * Kp), x3
	y3 = b2*y3old + a2*(x3 - c2*x3old);	// lag term
	y3old = y3;	x3old = x3;

	x1 = baseOutputAngle[WA1]*152.0/57.3;
//	printf("x1 2: %f \n", x1);
	y2 = b1*y2old + a1*(x1 - x1old);	//y2 is the derivate term for the measured torque

	y = y3 - y2;	// (P+lag) - (derivative) term 

	y2old = y2;
	x1old = x1;

	// trying 60-Hz low-pass filter here on the output command
	// to knock-off high frequency content (>100Hz) contents from torque command
//	float count, d1 = 0.4524, d2 = 0.2738, d3 = 0.2738;	// coeff. for 60Hz low pass filter
//	static float countOld = 0;

	// count is DAC count command to motor
//	count = d1*countOld + d2*y + d3*yold;

//	countOld = count;

	
//	yold = y;

//	baseCommand[WA1] = (short)(count/(KDAC*KAMP*KTORQUE));
#endif


	// calculate the corresponding DAC count to produce the desired MOTOR torque
	baseCommand[WA1] = (short)(y/(KDAC*KAMP*KTORQUE));

#if 0
	// try feedforward friction comp
	if (baseCommand[WA1] > 0)
			baseCommand[WA1] += 200;
	else if(baseCommand[WA1] < 0)
			baseCommand[WA1] -= 200;

	if (baseCommand[WA1] > limitDAC2)
			baseCommand[WA1] = limitDAC2;
	else if (baseCommand[WA1] < -limitDAC2)
			baseCommand[WA1] = -limitDAC2;
#endif

#if 0
	static int counter = 0;
	if (counter == 100)
	{
//			printf("y: %f, y2: %f \n", y, y2);
			printf("des: %5.3f, at: %5.3f, %5.3f, com: %d \n", torqueDes, baseSpringTorque[WA1],
							LOHET[WA1],
						   	baseCommand[WA1]);	
			counter = 0;
	}
	counter++;
#endif
	// write to DAC
	stg->RawDAC(6, baseCommand[WA1]);

}

#define slopeTime	1.0
void baseControl::torqueTrajectory(float torqueDes, float runTime)
{
	if(runTime < slopeTime)
			baseTorqueCommand[WA1] = runTime * (torqueDes/slopeTime);
	else
			baseTorqueCommand[WA1] = torqueDes;

//	if((runTime > 1.0) && (runTime < 3.00)
//		   torqueDesTraj = 4.0;
//	else if ((runTime => 3.00) && (runTime < 5.00))
//		torqueDesTraj = 0.0;

//	printf("time: %f, com: %f \n", runTime, baseTorqueCommand[WA1]);
}
#if 1

void main(void)
{
	baseControl *base = new baseControl();
	int choice, misTick;
	PrTimer timer(HZ);
	static float time = 0.0;
	float torqueCommand;

	pid_t pid = getpid();
	setprio(pid, 63);
	printf("Priority set to %d \n", PR_PRIORITY_MAX);

	base->resetIndex();

	printf("Fast Sampler Test Program \n");

	printf("Program running... \n");

	timer.start();

	static int counter = 0;

	while(!tcischars(1))
	{
		base->readLOHET();
		base->saveInfo();
		counter++;

		if(counter == 10)
		{
				base->saveInfo2();
				counter = 0;
		}

		misTick = timer.wait();
		if(misTick > 0)
				printf("Missed %d \n", misTick);
	}

	base->saveBuffer();

}


#endif
