#include "global.th";
#include "sensors.th";

#define ENCWORKER 5
EncoderProcess works[ENCWORKER];
string vector[4];
char portVector[SENSORS];
int calibrate_mode = 0;


void encoder_init(){
	for (int i = 0; i< SENSORS;i++){
		portVector[i]=-1;
	}
	int intnum = ((int)(config.EncoderCfg.encoderPort/4));
	for (char i = 0; i < ENCWORKER; i++){
		works[i].Enabled = false;
		works[i].config = &config.EncoderCfg;
		works[i].State = false;
		works[i].Count = config.EncoderCfg.encoderInterval/2;
		works[i].WorkPort=0;
		string ports = config.EncoderCfg.workedPorts;
		if (i<len(ports)){
			works[i].WorkPort = config.EncoderCfg.workedPorts[i];
			portVector[works[i].WorkPort] = i;
			works[i].Enabled = true;
			vector[intnum-1]+=chr(i);
		}
	}
#if PLATFORM_ID != SIMULATOR
	io.intnum = intnum-1;
	io.intenabled = YES;
#endif

}

bool getEncEnabled(unsigned char port,bool value){
	if (port == config.EncoderCfg.encoderPort)
		return false;
	if (portVector[port]==-1)
		return true;
	
	if (works[portVector[port]].Enabled && works[portVector[port]].State == false && value){
		long interval = works[portVector[port]].config->encoderInterval;
		long diff = interval /5;
		
		if (works[portVector[port]].Count >= (interval/2 - diff) && works[portVector[port]].Count <= (interval/2+diff)){
			works[portVector[port]].State = true;
		}
		return works[portVector[port]].State;
	}
	else{
		return false;
	}
}

void on_io_int(unsigned char linestate){
	string workers = vector[linestate-1];
	for (int i = 0; i < len(workers);i++){
		if (works[i].Enabled){
			works[i].Count++;
			if (works[i].Count>works[i].config->encoderInterval)
			{
				works[i].Count = 0;
				works[i].State = false;
			}
			d("count ["+str(i)+"] :"+str(works[i].Count));
		}
	}
	
}
/// Блок калибровки 

void enc_calibrate_mode(int mode){
	switch (mode){
		case 0: 	// Остановка калибровки
			calibrate_mode = 0;
			break;
		case 1:		// Калибровка интервала бутылок
			calibrate_mode = 1;
			break;
		case 2:		// Калибровка интервала датчиков
			calibrate_mode = 2;
			break;
			
		default:
			calibrate_mode = 0;
			d("Установка режима калибровки энкодера - неверный режим: "+ str(mode));
		break;
	}

}


int enc_calibrate_result(){

	return 0;
}