/***********************************
 * (C) 2007-2008 by Tomasz bla Fortuna <bla@thera.be>.
 * License: GPL3+ (See Docs/LICENSE)
 *
 * Desc: Program controlling inverted-pendulum-balance robot.
 ********************/

extern "C" {
#include <stdio.h>
#include <string.h>
#include <avr/eeprom.h>
#include <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include <avr/sleep.h>
}

/*
 * Program is splitted into .cc files, but it's compiled as a whole.
 * It was meant to decrease size of compiled program by enabling
 * whole-project optimizations.
 */
#include "Global.cc"
#include "Config.cc"

#include "Direction.cc"
#include "RunAvg.cc"

#include "KalmanSimple.cc"
#include "Angle.cc"
#include "Serial.cc"
/* #include "Debug.cc" */

#include "Motor.cc"
#include "Encoder.cc"

/* #include "Testcase.cc" */
#include "Control.cc"


namespace MainLoop {
	const Bool DoTheBalancing = true;

	volatile static UChar Tick, Update, InitWait=25;

	/* This is a main filtering/balancing function */
/*	ISR(TIMER1_OVF_vect)  Overflow interrupt */
	ISR(TIMER1_COMPA_vect) /* Compare match */
	{
		Tick++;
		/*** Update Filters ***/
		Angle::Filter(Tick); /* This damn function takes about 5100*8 cycles */

		if ((Tick-2) % 5 == 0) { /* >50 times per second before calculating accel pos. */
			/* TODO: Check if it ends before the next interrupt occurs */
			if (DoTheBalancing) {
				/* Wait until angle measurement stabilizes */
				if (InitWait)
					InitWait--;
				else {
					/* Calculate destination angle */
					Control::Steer();
					/* Drive motors to achieve this angle */
					Control::Balance();
				}
			}
			Update++;
		}
	}

	/* Sends debug data over serial */
	static inline void SensorDebug(void) __attribute__((noreturn));
	static inline void SensorDebug(void)
	{
		static Char *Buf;

		static Int Gyro;
		static Int Accel;
		static Long AngleAccelS;

		static Int Count;

		static Long KalmanBias, KalmanAngle;

		for (;;) {
			Angle::Wait();

			Gyro = Angle::GyroS;
			Accel = Angle::Accel;

			AngleAccelS = Angle::AngleAccelS;

			/* Kalman test */
			KalmanBias = Kalman::GetBias();
			KalmanAngle = Angle::Angle;

			Buf = Serial::BufClaim();
			Count = sprintf(Buf, "%4d A: %4ld P/I/D %4ld %4ld %4ld POW %3ld M PI/PP %2ld %2ld %2ld DA %4ld\n",
					Encoder::Position, KalmanAngle,
					Control::A_P_tmp/Control::KADiv, Control::A_I_tmp/Control::KADiv,
					Control::A_D_tmp/Control::KADiv, Control::Answer, 
					Control::M_P_tmp/Control::KMDiv, Control::M_I_tmp/Control::KMDiv,
					Control::M_D_tmp/Control::KMDiv, Control::DestAngle);

			Serial::Bulk(Buf, Count);
		}

	}


	static inline void Start(void) __attribute__((noreturn));
	static inline void Start(void)
	{
		/* Debug */
		Cfg::Init();
		Serial::Init();
		Angle::Init();
		Motor::Init();
		Encoder::Init();
		sei();
		set_sleep_mode(SLEEP_MODE_IDLE);

		Motor::Speed(Motor::Both, 0);
		Motor::Dir(Motor::Both, Dir::Stop);

		Util::SDelay(1); /* Stabilize hardware */
	
		/*** Enable 16 bit Timer ***/
		
		/* COM1* = 0 <=> Not output pin toggling;
		 * WGM11, WGM10 = 0 <=> CTC mode */
		TCCR1A = 0;

		/* WGM12 enables Clear Timer on Compare Match (CTC) mode */
		TCCR1B = (1<<WGM12);

		/* Set prescaler */
//		TCCR1B |= (1<<CS12) | (1<<CS10);	/* /1024 prescaler */
//		TCCR1B |= (1<<CS12);	       		/* /256 prescaler */
//		TCCR1B |= (1<<CS10) | (1<<CS11);	/* /64 prescaler */
		TCCR1B |= (1<<CS11);			/* /8 prescaler */

		/* Turn on interrupt */
		TIMSK1 = (1<<OCIE1A); /* TOIE1 <- overflow interrupt, 
				       * OCIE1A <- match with OC1A interrupt*/

		/* Clock = 20Mhz, Prescaler/8, timer increment 2500000
		 * times per second we want to update filter 600 times per second
		 * 20*10^6 / 8 / x = 600;
		 * x = 4167
		 * From this interrupt we will call balacing 50 times per second
		 * So every 12 interrupts
		 */
//		OCR1A = 4166;
		OCR1A = 7000;
		
		Util::Signal::Set(Util::Signal::BLUE);
		SensorDebug();
	}

}

//int main(void) __attribute__((naked));
int main(void)
{
	Util::Signal::Init();

/* TEST CASES */
//	Testcase::Encoder();
//	Testcase::Motor();
//	Testcase::AccelFast();
//	Testcase::Angle();

	MainLoop::Start();

	for (;;);
	return 0;
}