#include <kd_sfrs.h>

unsigned int instant_ant; // Momento en el que ha llegado la HSI anterior
unsigned int t2_ovf_hsi;  // Contador de overflows
unsigned int vel_inst;    // Velocidad instantánea, en km/h

// 2=2, K=0.15, PI=3.14. Per tant,
#define DOS_K_PI 1

static unsigned int auxiliar;

void inits_hsi() 
{
	
	/* Deshabilitem les interrupcions i les PTS */ 
	asm {
		dpts;
		di;
	}
	
	wsr = 0x00;
	int_mask |= 0x04;	// HSIData Available, cal configurar IOC1.7 
       	
	wsr = 0x0f;
	auxiliar = ioc0;
	
	wsr = 0x00;
	auxiliar |= 0x01;
	ioc0 = auxiliar;	// activo hsi0

    	wsr = 0x0f;
	auxiliar = ioc1;
	
	wsr = 0x00;
	auxiliar &= 0x7f;
	ioc1 = auxiliar;	// La Hsi Data Available (HSIINT) es generada quan 
				// es carrega el Holding Register
	wsr = 0x0f;
	auxiliar = hsi_mode;
	
	wsr = 0x00;
	auxiliar |= 0x01;	// cada transició positiva HSI.0 provocarà interrupció.
       	hsi_mode = auxiliar;

	instant_ant=0;
	t2_ovf_hsi=0;
	vel_inst=0;

	asm { ei; } 
}	



void RSI_Hsi()
{
	unsigned int instant;  // Valor del Timer2 (en tics) cuando llega la HSI
	signed int ha_passat;  // 'signed' para contabilizar bien en caso de overflow
			       // Ej con 2 díg: sube de 45 a "15" (en realidad 115), num_ovf=1
			       // El total será 1*100 + (15-45) = 70
	extern int vel_disp;

	// Comptar els numero de tics que arriven
    asm { di; } 
	// Leer hsi_time para cargar el siguiente valor
	wsr = 0x0;
	instant = hsi_time;

	ha_passat=instant-instant_ant;
	
	// Ara tenim el número de tics que han passats,
	// juntament amb el número d'overflows que hi ha hagut.
	// 20 OVFs del T2 == 1 segon.
	// Per tant, 50 ms per OVF.
	// 
	// 20*2^16 = 1310720 tics = 1 segon
	// Per tant, 1310 tics per ms.
	// 
	
	// mspertic = num_ovf*50+ha_passat/1310
	// Aquest és el nombre de mseg que ha trigat el tic: mseg/tic
	// El seu invers, tics/mseg, és la freqüència
	// En tics/seg, f=1000/mspertic
	// metrperseg=2*k*pi*f
	// kmperh=metrperseg*3600/1000
	//
	// En total queda: kmperh=3600/1000 * 2*k*pi * 1000/mspertic
	// Simplificat: kmperh= 3600/mspertic * 2*k*pi
	
	vel_inst=t2_ovf_hsi*50+ha_passat/1310; // mspertic
	vel_inst=3600/vel_inst; // ...
	vel_inst*=DOS_K_PI; // kmperhora
	

	//   En el bucle principal ya se cogerá esta velocidad instantánea (cada segundo),
	// y se hará la media cada 10.


	// Preparar-nos pel següent tic
	instant_ant=instant; // (encara que s'ignora el que ha trigat en executar-se la RSI)
	t2_ovf_hsi=0;
	asm { ei; }
}