#include "stdafx.h"
#include "Slot_Ctrl.h"
#include "USB_Com.h"


void semafor_init(semafors *init){ //Initialize semafors stucture
	init->mode = 0;
	init->pocet = 0;
	for(int i = 0; i<6; i++)
	{
		init->semafor[i].delay = 0;
		init->semafor[i].sec_color[RED]=10;
		init->semafor[i].sec_color[YELLOW]=10;
		init->semafor[i].sec_color[GREEN]=10;
		init->semafor[i].sec_color[ORANGE]=10;
		init->semafor[i].active = 0;
	}
}

int semafor_add(semafors *choice){ //Add a defalut valued semafor
	int neo;
	if(choice->pocet>=6)
		return 0;

	neo = ++choice->pocet;
	choice->semafor[neo].active = 1;
	choice->semafor[neo].delay = 0;
	choice->semafor[neo].state = 0;
	choice->semafor[neo].sec_color[GREEN] = 0;
	choice->semafor[neo].sec_color[YELLOW] = 0;
	choice->semafor[neo].sec_color[RED] = 0;
	choice->semafor[neo].sec_color[ORANGE] = 0;
	return 1;
}

int semafor_mode(semafors *choice, unsigned char mode){
	choice->mode = mode;
	choice->semafor[0].stage = 0;
	choice->semafor[1].stage = 0;
	choice->semafor[2].stage = 0;
	choice->semafor[3].stage = 0;
	choice->semafor[4].stage = 0;
	choice->semafor[5].stage = 0;
	return 1;
}

int semafor_remove(semafors *choice, unsigned int No){
	choice->semafor[No].active = 0;
	return 1;
}

int semafor_times(semafor *choice, unsigned int g , unsigned int y , unsigned int r, unsigned int o, unsigned int delay)
{
	choice->sec_color[GREEN] = g;
	choice->sec_color[YELLOW] = y;
	choice->sec_color[RED] = r;
	choice->sec_color[ORANGE] = o;
	choice->delay = delay;
	return 1;
}

int semafor_state(semafor *choice, UINT8 state){
	choice->state = state;
	return 1;
}

int semafor_cycle(semafors *choice){
	for(int i = 0;i<6;i+=2){
		if(choice->semafor[i].active == 1){
			switch(choice->semafor[i].stage){
			/*case 0:
				if(choice->semafor[i].delay>=choice->semafor[i].sec_color[GREEN])

					choice->semafor[i].time = 0;
				choice->semafor[i].stage = 1;
				choice->semafor[i].targetTime = choice->semafor[i].time+choice->semafor[i].sec_color[GREEN];*/
			case 1:
				choice->semafor[i].state= GREEN1;
				choice->semafor[i+1].state= RED2;
				if(++choice->semafor[i].time>=(choice->semafor[i].targetTime)){
					choice->semafor[i].stage++;
					choice->semafor[i].targetTime = choice->semafor[i].time+choice->semafor[i].sec_color[YELLOW];
				}
				break;

			case 2:
				choice->semafor[i].state= YELLOW1;
				choice->semafor[i+1].state= RED2;
				if(++choice->semafor[i].time>=(choice->semafor[i].targetTime)){
					choice->semafor[i].stage++;
					choice->semafor[i].targetTime = choice->semafor[i].time+choice->semafor[i+1].sec_color[ORANGE];
				}
				break;

			case 3:
				choice->semafor[i].state= RED1;
				choice->semafor[i+1].state= ORANGE2;
				if(++choice->semafor[i].time>=(choice->semafor[i].targetTime)){
					choice->semafor[i].stage++;
					choice->semafor[i].targetTime = choice->semafor[i].time+choice->semafor[i+1].sec_color[GREEN];
				}
				break;

			case 4:
				choice->semafor[i].state= RED1;
				choice->semafor[i+1].state= GREEN2;
				if(++choice->semafor[i].time>=(choice->semafor[i].targetTime)){
					choice->semafor[i].stage++;
					choice->semafor[i].targetTime = choice->semafor[i].time+choice->semafor[i+1].sec_color[YELLOW];
				}
				break;

			case 5:
				choice->semafor[i].state= RED1;
				choice->semafor[i+1].state= YELLOW2;
				if(++choice->semafor[i].time>=(choice->semafor[i].targetTime)){
					choice->semafor[i].stage++;
					choice->semafor[i].targetTime = choice->semafor[i].time+choice->semafor[i].sec_color[ORANGE];
				}
				break;

			case 6:
				choice->semafor[i].state= ORANGE1;
				choice->semafor[i+1].state= RED2;
				if(++choice->semafor[i].time>=(choice->semafor[i].targetTime)){
					choice->semafor[i].stage=1;
					choice->semafor[i].time=0;
					choice->semafor[i].targetTime = choice->semafor[i].time+choice->semafor[i].sec_color[GREEN];
				}
				break;
			}
		}
	}

	return 1;
}
int delay2stage(semafors *semaf){
	int status;
	unsigned int tempdelay;
	status = semaf->semafor[0].sec_color[GREEN];
	status = semaf->semafor[0].sec_color[YELLOW];
	status = semaf->semafor[0].sec_color[RED];
	status = semaf->semafor[0].sec_color[ORANGE];
	for(int i = 0;i<6;i+=2){
		tempdelay = semaf->semafor[i].delay;
		if(semaf->semafor[i].active == 1){
			if(tempdelay<semaf->semafor[i].sec_color[GREEN]){
				semaf->semafor[i].time = tempdelay;
				semaf->semafor[i].targetTime = semaf->semafor[i].sec_color[GREEN];
				semaf->semafor[i].stage = 1;
			}
			status = tempdelay;
			if(tempdelay>=semaf->semafor[i].sec_color[GREEN]&&tempdelay<(semaf->semafor[i].sec_color[GREEN]+semaf->semafor[i].sec_color[YELLOW])){
				tempdelay-=semaf->semafor[i].sec_color[GREEN];
				semaf->semafor[i].time = tempdelay;
				semaf->semafor[i].targetTime = semaf->semafor[i].sec_color[YELLOW];
				semaf->semafor[i].stage = 2;
			}

			tempdelay-=semaf->semafor[i].sec_color[GREEN];
			status = tempdelay;
			if(tempdelay>=semaf->semafor[i].sec_color[YELLOW]&&tempdelay<(semaf->semafor[i].sec_color[YELLOW]+semaf->semafor[i].sec_color[ORANGE])){
				tempdelay-=semaf->semafor[i].sec_color[YELLOW];
				semaf->semafor[i].time = tempdelay;
				semaf->semafor[i].targetTime = semaf->semafor[i].sec_color[ORANGE];
				semaf->semafor[i].stage = 3;
			}

			tempdelay-=semaf->semafor[i].sec_color[YELLOW];
			status = tempdelay;
			if(tempdelay>=semaf->semafor[i].sec_color[ORANGE]&&tempdelay<(semaf->semafor[i].sec_color[ORANGE]+semaf->semafor[i].sec_color[GREEN])){
				tempdelay-=semaf->semafor[i].sec_color[ORANGE];
				semaf->semafor[i].time = tempdelay;
				semaf->semafor[i].targetTime = semaf->semafor[i].sec_color[GREEN];
				semaf->semafor[i].stage = 4;
			}

			tempdelay-=semaf->semafor[i].sec_color[ORANGE];
			status = tempdelay;
			if(tempdelay>=semaf->semafor[i].sec_color[GREEN]&&tempdelay<(semaf->semafor[i].sec_color[GREEN]+semaf->semafor[i].sec_color[YELLOW])){
				tempdelay-=semaf->semafor[i].sec_color[GREEN];
				semaf->semafor[i].time = tempdelay;
				semaf->semafor[i].targetTime = semaf->semafor[i].sec_color[YELLOW];
				semaf->semafor[i].stage = 5;
			}

			tempdelay-=semaf->semafor[i].sec_color[GREEN];
			status = tempdelay;
			if(tempdelay>=semaf->semafor[i].sec_color[YELLOW]&&tempdelay<(semaf->semafor[i].sec_color[YELLOW]+semaf->semafor[i].sec_color[ORANGE])){
				tempdelay-=semaf->semafor[i].sec_color[YELLOW];
				semaf->semafor[i].time = tempdelay;
				semaf->semafor[i].targetTime = semaf->semafor[i].sec_color[ORANGE];
				semaf->semafor[i].stage = 6;
			}

		}
	}
	return 1;
}
UINT8 *semaforMsg1(semafors *all){
	static UINT8 stav[2]={0};
	stav[0]=0;
	stav[1]=0;
	for(int i = 0; i<4;i++){
		switch(all->semafor[i].state){
		case RED:
			stav[0]|=RED_BITS(i);
			break;
		case YELLOW:
			stav[0]|=YELLOW_BITS(i);
			break;
		case GREEN:
			stav[0]|=GREEN_BITS(i);
			break;
		case ORANGE:
			stav[0]|=ORANGE_BITS(i);
			break;
		}
	}
	for(int i = 0; i<2;i++){
		switch(all->semafor[i+4].state){
		case RED:
			stav[1]|=RED_BITS(i);
			break;
		case YELLOW:
			stav[1]|=YELLOW_BITS(i);
			break;
		case GREEN:
			stav[1]|=GREEN_BITS(i);
			break;
		case ORANGE:
			stav[1]|=ORANGE_BITS(i);
			break;
		}
	}
	return stav;
}

UINT8 *semaforMsg2(semafors *all){
	static UINT8 pocet[2]={0};
	pocet[0]=0;
	pocet[1]=0;
	pocet[0] = all->pocet;
	return pocet;

}

char *semafordata(semafors *all){
	static char pocet[9]={'\0'};
	pocet[0]=USB_SEMAF_STATE;
	pocet[1] = all->pocet;
	for(int i = 0; i<all->pocet; i++){
	pocet[i+2] = all->semafor[i].state;
	}
	pocet[8]='\0';
	return pocet;
}

void dpot_init(digpot *init){
	init->aktivni = 0;
	init->pocet = 1;
	init->value[0] = 0;
	init->value[1] = 0;
	init->value[2] = 0;
}

int dpot_changeone(digpot *syst, int dpotNo, int value){
	syst->aktivni = dpotNo;
	syst->value[dpotNo]=value;
	return 1;
}

int dpot_changeall(digpot *stav, int value1, int value2, int value3){
	stav->value[0]=value1;
	stav->value[1]=value2;
	stav->value[2]=value3;
	return 1;
}
char *dpotData(digpot *data){
	static char buff[9] = {'\0'};
	int help;
	buff[0] = USB_DPOT_TEMP;
	buff[1] = data->pocet;
	buff[2] = data->aktivni;
	buff[3] = data->value[0];
	buff[4] = data->value[1];
	buff[5] = data->value[2];
	buff[6] = '\0';
	return buff;

}

int dpot_stop(digpot *stav){
	stav->value[0]=0;
	stav->value[1]=0;
	stav->value[2]=0;
	return 1;
}

UINT8 dpot_speed2pos(int value, bool safe){
	UINT8 pos;
	if(safe == true){
		if(value==0)
			pos=0;
		else if(value==1)
			pos=7;
		else if(value==2)
			pos=12;
		else if(value==3)
			pos=16;
		else if(value==4)
			pos=20;
		else if(value==5)
			pos=24;
		else if(value==6)
			pos=27;
		else if(value==7)
			pos=31;
		else if(value==8)
			pos=35;
		else if(value==9)
			pos=39;
		else if(value==10)
			pos=43;
		else if(value==11)
			pos=47;
		else if(value==12)
			pos=50;
		else if(value==13)
			pos=54;
		else if(value==14)
			pos=99;
	}
	if(safe==false){
		if(value==0)
			pos=0;
		else if(value==1)
			pos=20;
		else if(value==2)
			pos=24;
		else if(value==3)
			pos=27;
		else if(value==4)
			pos=31;
		else if(value==5)
			pos=35;
	}
	return pos;
}

UINT8 *oneDpotMsg(UINT8 dpotNo, UINT8 speed){
	static UINT8 filled[2];
	filled[0] =dpotNo;
	filled[1] =speed;
	return filled;
}

UINT8 *twoDpotMsg(UINT8 speed1, UINT8 speed2){
	static UINT8 filled[2];
	filled[0] = speed1;
	filled[1] = speed2;
	return filled;
}