#include <stdlib.h>
#include <stdio.h>
#include <string.h>

#include "cmd_proc.h"
#include "logs.h"

/*
 * UDP_CAPABLE definuje, zda je pouzeitelna moznost cteni
 * referencnich informaci z jineho zarizeni po protokolu
 * UDP. Jde zatim jen o testovaci rezim
 */
#define UDP_CAPABLE_X

#ifdef UDP_CAPABLE
#include "udp_cli.h"
#include "misc.h"
#include <pthread.h>
#endif


#define PRUM_PROUD 	2061
#define PRUM_SOUC	6183

static char doPrint = 1;

#ifdef UDP_CAPABLE
static struct remote_pos_st remote_pos={
		.stop=1
};
#endif

/*
 * zastavi cteni po UDP
 */
static inline void stop_udp_SEM(){
	#ifdef UDP_CAPABLE
	remote_pos.stop=1;
	#endif
}



/*
 * \brief
 * Help
 */
static void printHelp(){
	doPrint=0;
	puts("start - Pripravi rizeni, zapne enable bity pwm.");
	puts("stop - Vypne komutaci, pwm a rizeni.");
	puts("0 - Vypne komutaci, pwm a rizeni.");
	puts("ga:[hodnota] - Zapne rizeni na zvolenou absolutni pozici.");
	puts("duty:[hodnota] - Nastavi pevnou sirku plneni.");
	puts("spd:[hodnota] - Zapne rizeni na danou rychlost.");
	puts("log - Spusti nebo ulozi logovani.");
	puts("ao:[hodnota] - Prenastavi alpha offset.");

	#ifdef UDP_CAPABLE
	puts("udp_pos:[ip] - Bude ridit pozici na hodnotu vzdaleneho zarizeni");
	puts("udp_spd:[ip] - Bude ridit rychlost na hodnotu vzdaleneho zarizeni");
	#endif

	puts("print - Zapne nebo vypne pravidelne vypisovani hodnot.");
	puts("help - Vypne vypisovani hodnot a zobrazi tuto napovedu.");
	puts("exit - Bezpecne ukonci program.");
}


/*
 * \brief
 * V prikazech je hodnota od zneni prikazu delena dvojteckou
 * tato funkce dvojtecku nahradi mezerou
 */
static void delCol(char * txt){
        unsigned i=0;
        while(txt[i]!='\0'){
                if (txt[i]==':') txt[i]=' ';
                i++;
        }
}

/*
 * Nastavi enable bity na pwm,
 * zapne komutaci
 */
static void start(struct rpi_state* state){
	sem_wait(&state->thd_par_sem);
	state->test=0x70;	/*konfiguracni byte*/
	sem_post(&state->thd_par_sem);
}

/*
 * \brief
 * Zastavi komutaci, vypne pwm
 */
static void stop(struct rpi_state* state){
	stop_udp_SEM();
	sem_wait(&state->thd_par_sem);
	setCommutationOff(state);
	setRegulationOff(state);
	state->duty=0;
	state->pwm1=0;
	state->pwm2=0;
	state->pwm3=0;
	sem_post(&state->thd_par_sem);
}

/*
 * \brief
 * Nastavi pevnou sirku plneni
 */
static void dutySet(struct rpi_state* state, int duty){
	stop_udp_SEM();
	sem_wait(&state->thd_par_sem);
	if (duty>MAX_DUTY) duty=MAX_DUTY;
	if (duty<-MAX_DUTY) duty=-MAX_DUTY;/*paranoia*/
	state->duty=duty;
	setRegulationOff(state);
	setCommutationOn(state);
	sem_post(&state->thd_par_sem);
}

/*
 * \brief
 * Zapne rizeni na zvolenou polohu vztazenou k pozici pri startu
 */
static void goAbsolute(struct rpi_state* state, int pos){
	stop_udp_SEM();
	sem_wait(&state->thd_par_sem);
	setRegulationPos(state);
	setCommutationOn(state);
	state->desired_pos=pos;
	sem_post(&state->thd_par_sem);
}

/*
 * \brief
 * Zapne nebo vypne pravidelne vypisovani hodnot.
 */
static void changePrint(){
	doPrint=!doPrint;
}

/*
 * \brief
 * Bezpecne ukonci program.
 */
static void exitApp(struct rpi_state* state){
	stop(state);
	/* Note: atexit() is set before*/
	exit(0);
}
/*
 * \brief
 * Set speed.
 */
static void setSpeed(struct rpi_state* state, int speed){
	if (speed>MAX_SPEED) speed=MAX_SPEED;
	if (speed<-MAX_SPEED) speed=-MAX_SPEED;/*paranoia*/
	stop_udp_SEM();
	sem_wait(&state->thd_par_sem);
	setRegulationSpeed(state);
	setCommutationOn(state);
	state->desired_spd=speed;
	sem_post(&state->thd_par_sem);
}

/*
 * \brief
 * Set alpha offset.
 */
static void setAlphaOff(struct rpi_state* state, int offset){
	if (offset<0) offset*=-1;
	offset%=1000;
	sem_wait(&state->thd_par_sem);
	state->alpha_offset=(unsigned short)offset;
	sem_post(&state->thd_par_sem);
}

/**
 * \brief
 * SetLog
 * if logs are being logged, save them
 * if they are not, start log them
 */
static void setLogSEM(struct rpi_state* state){
	sem_wait(&state->thd_par_sem);
	/* ulozim logy a vypnu zachytavani*/
	if (state->log_col_count){
		state->doLogs=0;
		sem_post(&state->thd_par_sem);
		saveLogs(state);
	/* spustim zachytavani logu */
	}else{
		logInit(state);
		sem_post(&state->thd_par_sem);
	}
}

#ifdef UDP_CAPABLE
static void  startUdpPos(struct rpi_state* state, char *ip){
	pthread_t thread_id=pthread_self();
	if (!remote_pos.stop) return; /*dont be reentrant*/
	setCommutationOn(state);
	setRegulationPos(state);
	remote_pos.stop=0;
	remote_pos.ip=ip;
	remote_pos.factor=-5;
	remote_pos.semaphore=&state->thd_par_sem;
	remote_pos.rem_pos=&state->desired_pos;
	create_rt_task(&thread_id,48,start_reading_remote_position,&remote_pos);
}

/*FIXME thers no max speed limit check!!! hard debug mode!! be careful*/
static void  startUdpSpd(struct rpi_state* state, char *ip){
	pthread_t thread_id=pthread_self();
	if (!remote_pos.stop) return; /*dont be reentrant*/
	setCommutationOn(state);
	setRegulationSpeed(state);
	remote_pos.stop=0;
	remote_pos.ip=ip;
	remote_pos.factor=-1;
	remote_pos.semaphore=&state->thd_par_sem;
	remote_pos.rem_pos=&state->desired_spd;
	create_rt_task(&thread_id,48,start_reading_remote_position,&remote_pos);
}
#endif
/**
 * \brief
 * Commands detection.
 */
void poll_cmd(struct rpi_state* state){
	unsigned int tmp;
        char buff[50];
        char cmd[30];
        char cmd2[30];
        int val;

        while(1){
                scanf("%49s",buff);
                delCol(buff);
                sscanf(buff,"%s %d",cmd,&val);
                sscanf(buff,"%s %s",cmd,cmd2);


                if (!strcmp(cmd,"start")){
                	start(state);
                }else if (!strcmp(cmd,"0")){
                	stop(state);
                }else if (!strcmp(cmd,"stop")){
                        stop(state);
                }else if (!strcmp(cmd,"ga")){
                	goAbsolute(state,val);
                }else if (!strcmp(cmd,"duty")){
                        dutySet(state,val);
                }else if (!strcmp(cmd,"help")){
                        printHelp();
                }else if (!strcmp(cmd,"print")){
                        changePrint();
                }else if (!strcmp(cmd,"exit")){
                        exitApp(state);
                }else if (!strcmp(cmd,"spd")){
                        setSpeed(state, val);
                }else if (!strcmp(cmd,"log")){
                        setLogSEM(state);
                }else if (!strcmp(cmd,"ao")){
                	setAlphaOff(state,val);
                }
		#ifdef UDP_CAPABLE
                else if (!strcmp(cmd,"udp_pos")){
                	startUdpPos(state,cmd2);
                }else if (!strcmp(cmd,"udp_spd")){
                	startUdpSpd(state,cmd2);

                }
		#endif

        }

}

/*
 * pocita procentualni odchylku od prumerneho proudu
 */
static float diff_p(float value){
	return ((float)value-PRUM_PROUD)*100/PRUM_PROUD;
}
/*
 * pocita procentualni odchylku od prumerneho souctu proudu
 */
static float diff_s(float value){
	return ((float)value-PRUM_SOUC)*100/PRUM_SOUC;
}

/*
 * tiskne potrebna data
 */
void printData(struct rpi_state* state){
	if (!doPrint) return;

	struct rpi_in data_p;
	struct rpi_state s;	/*state*/
	float cur0, cur1, cur2;
	int i;
	/* copy the data */
	sem_wait(&state->thd_par_sem);
	data_p = *state->spi_dat;
	s=*state;
	sem_post(&state->thd_par_sem);

	if (data_p.adc_m_count){
		cur0=data_p.ch0/data_p.adc_m_count;
		cur1=data_p.ch1/data_p.adc_m_count;
		cur2=data_p.ch2/data_p.adc_m_count;
	}
	for (i = 0; i < 16; i++) {
			if (!(i % 6))
				puts("");
			printf("%.2X ", data_p.debug_rx[i]);
	}
	puts("");
	printf("\npozice=%ld\n",data_p.pozice);
	printf("rychlost=%d\n",s.speed);
	printf("chtena pozice=%d\n",s.desired_pos);
	printf("chtena rychlost=%d\n",s.desired_spd);
	printf("alpha offset=%hu\n",s.alpha_offset);
	printf("transfer count=%u\n",s.tf_count);
	printf("raw_pozice=%u\n",data_p.pozice_raw);
	printf("raw_pozice last12=%u\n",(data_p.pozice_raw&0x0FFF));
	printf("index position=%u\n",data_p.index_position);
	printf("distance to index=%u\n",s.index_dist);
	printf("hal1=%d, hal2=%d, hal3=%d\n",data_p.hal1,data_p.hal2,data_p.hal3);
	printf("en1=%d, en2=%d, en3=%d (Last sent)\n",!!(0x40&s.test),!!(0x20&s.test),!!(0x10&s.test));
	printf("shdn1=%d, shdn2=%d, shdn3=%d (L.s.)\n",!!(0x08&s.test),!!(0x04&s.test),!!(0x02&s.test));
	printf("  PWM1=%u   PWM2=%u   PWM3=%u\n",s.pwm1,s.pwm2,s.pwm3);
	printf("T_PWM1=%u T_PWM2=%u T_PWM3=%u\n",s.t_pwm1,s.t_pwm2, s.t_pwm3);
	printf("Pocet namerenych proudu=%u\n",data_p.adc_m_count);
	printf("(pwm1) (ch1)=%d (avg=%4.0f) (%2.2f%%)\n",data_p.ch1,cur1,diff_p(cur1));
	printf("(pwm2) (ch2)=%d (avg=%4.0f)(%2.2f%%)\n",data_p.ch2,cur2,diff_p(cur2));
	printf("(pwm3) (ch0)=%d (avg=%4.0f)(%2.2f%%)\n",data_p.ch0,cur0,diff_p(cur0));
	printf("soucet prumeru=%5.0f (%2.2f%%)\n",cur0+cur1+cur2,diff_s(cur0+cur1+cur2));
	printf("duty=%d\n",s.duty);
	if (s.index_ok) printf("index ok\n");
	if (s.commutate) printf("commutation in progress\n");
	if (s.doLogs) printf("logujeme\n");
	if (s.error) printf("error pri maloc logs!! \n");

}