/***********************************
 * (C) 2007-2008 by Tomasz bla Fortuna <bla@thera.be>.
 * License: GPLv3+ (See Docs/LICENSE)
 *
 * Desc: Analog -> Digital converter;
 * measurements of accel and gyro + some small low-pass filtering
 *
 * Parts of code are not used here now. Like the gyro integrator.
 ********************/

#include <avr/pgmspace.h>


namespace Angle {
	/* Raw data read from ADC, normalized but unfiltered */
	static volatile Int Gyro, Accel;

	/* Low-pass filtered unnormalized gyro data */
	static volatile Int GyroS;

	/* Angle is the output from Kalman filter. */
	static volatile Long Angle;

	/* Angle calculated from accel output + Smoothed version */
	static volatile Long AngleAccel, AngleAccelS; 

	/* Shows if we have gathered any new data */
	static volatile UShort Counter;

	/*** ADC Input selection ***/
	enum Select { GYRO, ACCEL };
	volatile static Select Selected;
	
	/*** Gyro data integration and filtering ***/
	static volatile Long Integrator;
	const volatile Int IntegratorDivider = 575;
	static inline void Integrate()
	{
		/* Measurement is in Gyro */
		static Int Val3, Val2, Val1;
		static Int Step;
		/* Runge-Kutta */
		Step = (Val3 + 2*Val2 + 2*Val1 + Gyro) / 6;

		Val3 = Val2;
		Val2 = Val1;
		Val1 = Gyro;

		Integrator += Step;

		/* Range calibration */
		Angle = Integrator / IntegratorDivider;
	}

	static inline void FilterAccel(void)
	{
		static Int Val3, Val2, Val1;
		AngleAccelS = (2*Val3 + 2*Val2 + 2*Val1 + AngleAccel) / 7;
		Val3 = Val2;
		Val2 = Val1;
		Val1 = AngleAccel;
	}

	static inline void FilterGyro(void)
	{
		static Int Val3, Val2, Val1;
		GyroS = (Val3 + Val2 + Val1 + Gyro) / 5;
		Val3 = Val2;
		Val2 = Val1;
		Val1 = Gyro;
	}

	/* Accel->Angle lookup table */
	static const prog_int16_t Ac2Angle[] PROGMEM = {
		0, 7, 15, 22, 30, 37, 45, 53, 60, 68, 75, 83, 90, 98, 106,
		113, 121, 128, 136, 144, 151, 159, 166, 174, 182, 189, 197,
		205, 212, 220, 227, 235, 243, 250, 258, 266, 273, 281, 289,
		296, 304, 312, 319, 327, 335, 342, 350, 358, 366, 373, 381,
		389, 396, 404, 412, 420, 427, 435, 443, 451, 459, 466, 474,
		482, 490, 498, 506, 513, 521, 529, 537, 545, 553, 561, 569,
		577, 585, 593, 601, 608, 616, 624, 632, 641, 649, 657, 665,
		673, 681, 689, 697, 705, 713, 721, 730, 738, 746, 754, 762,
		771, 779, 787, 795, 804, 812, 820, 829, 837, 845, 854, 862,
		871, 879, 888, 896, 905, 913, 922, 930, 939, 947, 956, 965,
		973, 982, 991, 1000, 1008, 1017, 1026, 1035, 1044, 1052,
		1061, 1070, 1079, 1088, 1097, 1106, 1115, 1124, 1134, 1143,
		1152, 1161, 1170, 1180, 1189, 1198, 1208, 1217, 1227, 1236,
		1246, 1255, 1265, 1274, 1284, 1294, 1304, 1313, 1323, 1333,
		1343, 1353, 1363, 1373, 1383, 1393, 1403, 1414, 1424, 1434,
		1445, 1455, 1466, 1476, 1487, 1497, 1508, 1519, 1530, 1541,
		1552, 1563, 1574, 1585, 1596, 1608, 1619, 1631, 1642, 1654, 
		1666, 1677, 1689, 1701, 1714, 1726, 1738, 1750, 1763, 1776,
		1788, 1801, 1814, 1827, 1840, 1854, 1867, 1881, 1895, 1909,
		1923, 1937, 1951, 1966, 1981, 1996, 2011, 2027, 2042, 2058,
		2074, 2091, 2107, 2124, 2142, 2159, 2177, 2196, 2214, 2233,
		2253, 2273, 2294, 2315, 2337, 2360, 2383, 2408, 2433, 2460,
		2488, 2517, 2548, 2582, 2618, 2659, 2705, 2759, 2829
	};

	/* Calculate our angle from accel using the table */
	static const int ACCEL_CENTER = 461;
	static inline int AccelToAngle(const int A)
	{
		static int Mark;
		static int Lookup;
		if (A >= ACCEL_CENTER) {
			Lookup = A - ACCEL_CENTER;
			Mark = 1;
		} else {
			Lookup = ACCEL_CENTER - A;
			Mark = -1;
		}
		if (Lookup > (int)sizeof(Ac2Angle))
			Lookup = sizeof(Ac2Angle) - 1;

		return Mark * (int)pgm_read_word_near(Ac2Angle + Lookup);
	}

	/*** ADC Multiplexer switcher ***/
	static inline void Select(const Select Device)
	{
		/* External 2.56V voltage reference
		 * PA0 - one accel axis (0)
		 * PA1 - second accel axis (MUX0)
		 * PA2 - accel temp sensor (MUX1)
		 * PA3 - gyro (MUX0 | MUX1)
		 */

		Selected = Device;
		switch (Device) {
		case GYRO:
			/* External reference + Gyro input */
			ADMUX = (1<<MUX0) | (1<<MUX1);
			break;

		case ACCEL:
			/* External reference + Accel input */
			ADMUX = 0;
			break;
		}
	}

	volatile char IN; /* DEBUG VALUE */
	ISR(ADC_vect)
	{
		if (IN == 1) {
			Util::Signal::Set(Util::Signal::RED);
			return;
		}
		IN = 1;
		switch (Selected) {
		case GYRO:
			/* Select new reference as fast as possible */
			Select(ACCEL);

			/* On ref = 5V, zero point = 483; ref=2.52, neutral=487 */
			Gyro = ADC;
			ADCSRA |= (1<<ADSC); /* Start the next conversion now */
			FilterGyro();
			Gyro -= 485;

/*			Integrate(); */

			break;

		case ACCEL:
			Select(GYRO);
			Accel = ADC;
			ADCSRA |= (1<<ADSC); /* Start the next conversion now */

			/* Convert measurement into the angle
			 * using the same unit as gyroscope */
			AngleAccel = AccelToAngle(Accel);
			Accel -= ACCEL_CENTER;

			FilterAccel();

			break;
		}
		Counter++;
		IN = 0;
	}

	static inline void Filter(Int Tick)
	{
		/* Apply a Kalman filter returning a real Angle value */
		/* 100 degree/s range matching 0-1024 range
		 * ~ 1.74533 in rads range
		 * x / 1024.0 * 1.74533 = x * .00170442382812500000
		 *
		 * more accurate version in notes!
		 */

		/* Gyro output converted to rad/s */
		const float AngularRate = 0.00172293114708226 * (float)GyroS;
		Kalman::Update(AngularRate, 0.0028);
		if (Tick % 5 == 0) {
			Kalman::Predict(AngleAccelS / 3000.0 * (3.14159/2.0) );
		}
		Angle = Kalman::GetAngle() - 463 + 240 + 14;
	}

	/* true - internal reference, false - external reference */
	const Bool INTERNAL_REF = false;
	const Bool INTERNAL_VCC = false; /* VCC or 2.56V? */
	/* true - Free running mode */
	const Bool FREE_RUNNING = false;

	static inline void Init(void)
	{
		Select(GYRO);

		/* Turn off pull-ups if enabled! */
		DDRA = 0x00;
		PORTA = 0x00;

#if 0
		if (INTERNAL_REF) {
			if (INTERNAL_VCC)
				ADMUX |= (1<<REFS0); /* Internal ref = Vcc */
			else 
				ADMUX |= (1<<REFS0) | (1<<REFS1); /* Internal ref = 2.56V */
		} else {
			/* External reference */
		}
#endif
		/* Prescaler settings (ADPS210=111) = 20Mhz/128 = 156 250 Hz
		 * With (ADPS210=110) we can get /64, which means
		 * less than 10 bits of resolution but higher sampling
		 * rate. Normal conversion takes 13 ADC clock cycles to
		 * finish so with 156Khz clock we get one measurement per 0.0000832 s
		 * So 12019 measurements per second. Enough.
		 *
		 * Interrupt enable, Interrupt Enable */
		ADCSRA = (1<<ADPS2) | (1<<ADPS1) | (1<<ADPS0) | (1<<ADIE);

		/* Free running mode */
		if (FREE_RUNNING) {
			ADCSRA |= (1<<ADATE);		/* Enable auto triggering */
			ADCSRB = 0;
		}

		/* Start conversion! */
		ADCSRA |= (1<<ADEN) | (1<<ADSC);

		/* set_sleep_mode(SLEEP_MODE_ADC); */
	}

	static inline void Wait(void)
	{
		/* FIXME: Go to sleep instead of looping */

		/* Each conversion takes at most 104us,
		 * that is 832 instructions
		 */

		/* Don't go to sleep if we are sending something via UART */
		// sleep_mode();

		Counter = 0;
		while (!Counter);
	}

} /* End of namespace */