/*  program to run a LEGO clock
	(c) Chris Lomont Feb 2002
	Designed to run on LegOS 2.5
*/

#include <conio.h>

#include <dsensor.h>
#include <dmotor.h>
#include <dsound.h>
#include <dbutton.h>
#include <dkey.h>

// ************* rotation stuff
#define ROTSENSOR		SENSOR_3
#define ROT_READ		ROTATION_3


void run_clock(void)
	{
	// every 15/2 secs, run 16*40/8 = 80 counts through sensor
	int msec = 7500; // these two determine update frequency
	int counts = 80; 

	msec = 15000; // 15 secs gives.... 
	counts = 160; // 16*40/4 = 160 counts

	unsigned long last_time, cur_time;   // system time reads
	unsigned long last_count, cur_count; // counts from sensor

	motor_a_dir(off);
	motor_a_speed(MAX_SPEED);

	last_time  = sys_time;
	cur_count = last_count = ROT_READ;
	while (1)
		{
		cur_time = sys_time;
		msleep(msec - (cur_time - last_time)); // 7.5 secs
		last_time = sys_time; // get time

		motor_a_dir(fwd); 

		while (last_count - cur_count < counts)
			cur_count = ROT_READ;

		motor_a_dir(off);

		lcd_int(cur_count); // show what's up

		last_count -= counts; // causes errors to be kept track of next loop

		if (last_count < 2000) // getting small, bump all up
			{
			last_count += counts*50;
			cur_count  += counts*50;
			ds_rotation_set(&ROTSENSOR,last_count); // set to here
			}
		}
	} // run_clock

// main driver
int main(int argc, char *argv[]) 
	{
	unsigned long last_time, cur_time;   // system time reads
	unsigned long last_count, cur_count; // counts from sensor
	int error = 0;                       // how many counts off we are from last reading

	int speed = (MAX_SPEED+MIN_SPEED)/2; // start at average
	
	// turn on sensor, set initial value
	ds_active(&ROTSENSOR);      // enable sensor
	ds_rotation_on(&ROTSENSOR); // start OS functions
	ds_rotation_set(&ROTSENSOR,5000); // start here for fun


	// gearing of clock is: 40 rotations of motor gives 60 seconds
	// each rotation of motor gives 16 counts to sensor (counts down in my construction)
	// so 16*40 = 640 counts per minute, or 64/6 = 32/3 counts per sec (32 counts every 3 secs)
	// todo - allow switch for count down/up

	// first of all , we set the clock (todo)
	// set_clock();

	// init vars and turn on motors
	last_time  = sys_time;
	last_count = ROT_READ;
	motor_a_speed(speed);
	motor_a_dir(fwd);

	run_clock(); // todo - testing 15 sec clock

#if 0 // todo - merge
	// and clock away
	while (1) 
		{ // loop forever (until key pressed asking for function - todo)

		// update rotation counter data, and current internal time
		cur_time  = sys_time;
		cur_count = ROT_READ;

		// todo - do more often than this, use float to estimate error, but keep total error exact
		if (cur_time - last_time >= 3000) // 3 secs have passed, update
			{
			// better have 32 rotations here
			last_time = cur_time;
			error = error+ ((int)(last_count - cur_count) - 32); // accumulate error (counts down)
			last_count = cur_count;

			if (last_count < 2000) // bump back up
				{
				ds_rotation_set(&ROTSENSOR,5000); // start here for fun
				last_count = 5000;
				}

			// check speed, double motor or halve it as necessary  - todo - double good value? alpha val?
			if (error < 0)
				{
				speed = speed*2;
				if (speed == 0) speed = 1; // at least this
				motor_a_speed(speed);
				}
			if (error > 0)
				{
				speed = speed/2; // may make speed 0
				motor_a_speed(speed);
				}
	
			lcd_int(error); // show error
			}

		// sleep 1 sec - then do again
		msleep(1000);

		} // infinite loop
#endif // todo

	} // main


// end - ClockI.c