/**
 * \brief
 * Computatations of position, index, speed.
 *
 */

#include "comp.h"
#include "pmsm_state.h"
#include "rp_spi.h"

/**
 * \brief
 * Substact initial position.
 */
void substractOffset(struct rpi_in* data, struct rpi_in* offset){
	data->pozice=data->pozice_raw-offset->pozice_raw;
	return;
}

/*
 * \brief
 * Multiplication of 11 bit
 * Zaporne vysledky prvede na nulu.
 */
uint16_t mult_cap(int32_t s,int d){
	int j;
	int res=0;
	for(j=0;j!=11;j++){
		/* multiplicate as if maximum sinus value was unity */
		res+=(!(s & 0x10000000))*(((1 << j) & s)>>j)*(d>>(10-j));
	}
	return res;
}


/**
 * \brief
 * Computation of distance to index.
 *
 * K dispozici je 12-bit index, to umoznuje ulozit 4096 ruznych bodu
 * Je nutne vyjadrit 1999 bodu proti i posmeru h.r. od indexu -
 * 	to je 3999 bodu
 * 	=>12 bitu je dostacujicich, pokud nikdy nedojde ke ztrate
 * 		signalu indexu
 */
void comIndDist(struct rpi_state* this){
	uint16_t pos = 0x0FFF & this->spi_dat->pozice_raw;
	uint16_t dist;
	uint16_t index = this->spi_dat->index_position;

	if (index<1999){		/*index e<0,1998> */
		if (pos<index){			/*pozice e<0,index-1> */
			/*proti smeru h.r. od indexu*/
			dist=pos+2000-index;
		}else if (pos<=index+1999){	/*pozice e<index,index+1999> */
			/*po smeru h.r. od indexu*/
			dist=pos-index;
		}else if (pos<index+2096){	/*pozice e<index+2000,index+2095> */
			goto index_lost;
		}else{				/*pozice e<index+2096,4095> */
			/*proti smeru h.r. od indexu - podtecena pozice*/
			dist=pos-index-2096;
		}
	}else if (index<=2096){		/*index e<1999,2096>*/
		if (pos<index-1999){		/*pozice e<0,index-2000> */
			goto index_lost;
		}else if (pos<index){		/*pozice e<index-1999,index-1> */
			/*proti smeru h.r. od indexu*/
			dist=pos+2000-index;
		}else if (pos<=index+1999){	/*pozice e<index,index+1999> */
			/*po smeru h.r. od indexu*/
			dist=pos-index;
		}else {				/*pozice e<index+2000,4095> */
			goto index_lost;
		}
	}else{				/*index e<2097,4095> */
		if (pos<=index-2097){		/*pozice e<0,index-2097> */
			/*po smeru h.r. od indexu - pretecena pozice*/
			dist=4096+pos-index;
		}else if (pos<index-1999){	/*pozice e<index-2096,index-2000> */
			goto index_lost;
		}else if (pos<index){		/*pozice e<index-1999,index-1> */
			/*proti smeru h.r. od indexu*/
			dist=pos+2000-index;
		}else{ 				/*pozice e<index,4095> */
			/*po smeru h.r. od indexu*/
			dist=pos-index;
		}
	}

	this->index_dist = dist;
	return;

	index_lost:
		setIndexLost(this);
		return;
}


/*
 * \brief
 * Computate speed.
 */
void compSpeed(struct rpi_state* this){
	signed long int spd;
	spd=this->spi_dat->pozice-this->old_pos[this->tf_count%OLD_POS_NUM];
	this->speed=(int32_t)spd;
}