/*

LegWay

Self-balancing robot

Note : 
- L'unita di misura del tempo sono ms
- Non capisco come funzioni il MotorSpeedArray[32]

*/

int MotorSpeedArray[32] =  {1,1,1,1,1,1,1,1, // motor speed 0-7 = forward, 0 = fast, 7 = slow
							3,3,3,3,3,3,3,3, // motor speed 8 = stop
							2,2,2,2,2,2,2,2, // motor speed 9-16 = reverse, 9 = slow, 16 = full
							0,0,0,0,0,0,0,0}; // 24 = float

#include <conio.h>
#include <unistd.h>
#include <dsensor.h>
#include <dmotor.h>
#include <rom/system.h>
#include <dsound.h>
#include <dbutton.h>

int main(int argc, char *argv[])
{
	int L1;
	int MotorA = 0;

	int MotorSetting;

	int L3;
	int MotorC = 0; // sembra essere una sorta di offset

	int MotorRunningValue;

	long CheckTime;		// ogni quanto tempo viene ristabilito l'equilibrio (l'angolo, usando i motori)
	long AdjustTime;	// ogni quanto tempo viene riaggiustato l'angolo
	long FallTime;		// Da quanto tempo sta cadendo? last time < full speed

	int CenterPoint1;
	int CenterPoint3;

	int EnableSpinner = 0;

	ds_active(&SENSOR_1);

	motor_a_dir(3); //bisogna guardare come funzionano i motori in brickos per capire ste 4 linee.
	motor_c_dir(3);
	motor_a_speed(255);
	motor_c_speed(255);

	L1 = 0;
	L3 = 0;
	MotorSetting = 8; // entrambi i motori fermi

	msleep(500);  // per il warmup dei sensori

	CenterPoint1 = LIGHT_1; // prende il valore di luce corrente come punto di equilibrio iniziale quindi bisogna mettere il robo circa dritto all'inizio

	dsound_system(DSOUND_BEEP);

	while(PRESSED(dbutton(),BUTTON_RUN))
	{
		EnableSpinner = 1;
	}


	FallTime = get_system_up_time();
	CheckTime = 0;
	AdjustTime = 0;

	while(FallTime + 1000 > get_system_up_time()) //fino a quando i motori non sono al massimo per 1 sec.
	{

		if (get_system_up_time() > AdjustTime) // ricalcolo il punto di eq
		{
			CenterPoint1 = (CenterPoint1 + MotorSetting - 8);
			AdjustTime = get_system_up_time() + 50; // ogni 50 ms
		}

		if (get_system_up_time()>CheckTime)
		{

			L1 = LIGHT_1;

			MotorSetting = (CenterPoint1 - L1) / 2 + 8; // la divisione smorza

			if (MotorSetting < 0) MotorSetting = 0;
			if (MotorSetting > 16) MotorSetting = 16;

			CheckTime = get_system_up_time() + 1;
		}

		if (MotorSetting > 0 && MotorSetting < 16) FallTime = get_system_up_time();

		MotorRunningValue = (get_system_up_time() & 7);
		motor_a_dir(MotorSpeedArray[MotorSetting + MotorRunningValue - MotorA]);
		motor_c_dir(MotorSpeedArray[MotorSetting + MotorRunningValue - MotorC]);

		if (((get_system_up_time() >> 8) & 2) == 0) cputs("LEG");
		else cputs(" WAY"); // solo per scriv sul display
	}
	motor_a_dir(3);  // stop after fall time expires
	motor_c_dir(3);
	ds_passive(&SENSOR_1);
	ds_passive(&SENSOR_3);
	return 0;
}