/**
 * \file main_pmsm.c
 * \author Martin Prudek
 * \brief Mainfile pro pmsm control.
 */

#ifndef NULL
#define NULL (void*) 0
#endif /*NULL*/


#include <stdlib.h> 	/*exit*/
#include <signal.h> 	/*signal handler Ctrl+C*/
#include <stdio.h> 	/*printf*/
#include <sched.h>	/*sheduler*/
#include <unistd.h>	/*usleep*/
#include <pthread.h>	/*threads*/
#include <time.h>	/*nanosleep*/
#include <string.h>

#include "rpin.h"	/*gpclk*/
#include "rp_spi.h"	/*spi*/
#include "misc.h" 	/*structure for priorities*/
#include "pmsm_state.h"
#include "cmd_proc.h"
#include "controllers.h"
#include "commutators.h"
#include "comp.h"
#include "logs.h"

#define PRIOR_KERN	50
#define PRIOR_HIGH	49
#define PRIOR_LOW	20

#define THREAD_SHARED 	0
#define INIT_VALUE	1 	/*init value for semaphor*/


#define NSEC_PER_SEC    (1000000000) /* The number of nsecs per sec. */


struct rpi_in data;
struct rpi_state rps={
	//.MAX_DUTY=170,
	.spi_dat=&data,
	.test=0,
	.pwm1=0,.pwm2=0, .pwm3=0,
	.pwm1=0, .t_pwm2=0, .t_pwm3=0,
	.commutate=0,
	.duty=0,			/* duty cycle of pwm */
	.index_dist=0,		/* distance to index position */
	.index_ok=0,
	.tf_count=0,		/*number of transfer*/
	.desired_pos=0,		/* desired position */
	.pos_reg_ena=0,
	.desired_spd=0,
	.spd_reg_ena=0,
	.old_pos={0},
	.spd_err_sum=0,
	.log_col_count=0,	/* pocet radku zaznamu */
	.log_col=0,
	.doLogs=0,
	.alpha_offset=960,
	.main_commutator=zero_commutator,	/* komutace vypnuta */
	.main_controller=zero_controller	/*rizeni vypnuto */
};


/**
 * \brief Initilizes GPCLK.
 */
int clk_init(){
	initialise(); /*namapovani gpio*/
	initClock(PLLD_500_MHZ, 10, 0);
	gpioSetMode(4, FSEL_ALT0);
	return 0;
}

/**
 * \brief Signal handler pro Ctrl+C
 */
void appl_stop(){
	uint8_t tx[16];
	sem_wait(&rps.thd_par_sem);

	memset(tx,0,16*sizeof(int));
	rps.pwm1=0;
	rps.pwm2=0;
	rps.pwm3=0;
	spi_read(&rps);

	spi_disable();
	termClock(0);
	freeLogs(&rps);
	/*muzeme zavrit semafor*/
	sem_destroy(&rps.thd_par_sem);
        printf("\nprogram bezpecne ukoncen\n");
}

/**
 * Funkce pravidelne vypisuje posledni zjistenou pozici lokalniho motoru
 */
void * pos_monitor(void* param){
	while(1){
		printData(&rps);
		usleep(1000000); 	/*1 Hz*/
	}
	return (void*)0;
}

/*
 * \brief
 * Feedback loop.
 */
void * control_loop(void* param){
	int i;
	struct rpi_in pocatek;
	struct rpi_state poc={
		.spi_dat=&pocatek,
		.test=0,
		.pwm1=0, .pwm1=0, .pwm3=0
	};
	struct timespec t;
	int interval = 1000000; /* 1ms ~ 1kHz*/
	char first=1;
	uint16_t last_index;				/*we have index up-to date*/
	spi_read(&poc);		/*pocatecni informace*/
	clock_gettime(CLOCK_MONOTONIC ,&t);
		while(1){
			/* wait until next shot */
			clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL);
			sem_wait(&rps.thd_par_sem);		/*---take semaphore---*/

			/*old positions*/
			rps.old_pos[rps.tf_count%OLD_POS_NUM]=rps.spi_dat->pozice;
			spi_read(&rps);				/*exchange data*/
			rps.tf_count++;
			substractOffset(&data,poc.spi_dat);	/*subtract initiate postion */
			compSpeed(&rps);			/*spocita rychlost*/

			if (!rps.index_ok){
				if (first){
					last_index=data.index_position;
					first=0;
				}else if (last_index!=data.index_position){
					setIndexOK(&rps);
					comIndDist(&rps);	/*vypocet vzdalenosti indexu*/
				}
			}else{ /*index je v poradku*/
				comIndDist(&rps);		/*vypocet vzdalenosti indexu*/
			}

			/* pocitame sirku plneni podle potreb rizeni*/
			rps.main_controller(&rps);

			/* sirku plneni prepocteme na jednotlive pwm */
			rps.main_commutator(&rps);

			/*zalogujeme hodnoty*/
			if (rps.doLogs && !(rps.tf_count%LOG_PERIOD)){
				makeLog(&rps);
			}

			sem_post(&rps.thd_par_sem);		/*--post semaphore---*/

			/* calculate next shot */
			t.tv_nsec += interval;

			while (t.tv_nsec >= NSEC_PER_SEC) {
				t.tv_nsec -= NSEC_PER_SEC;
				t.tv_sec++;
			}

		}
}

/**
 * \brief Main function.
 */
int main(){
	pthread_t base_thread_id;
	clk_init(); 		/* inicializace gpio hodin */
	spi_init(); 		/* iniicializace spi*/

	/*semafor pro detekci zpracovani parametru vlaken*/
	sem_init(&rps.thd_par_sem,THREAD_SHARED,INIT_VALUE);
	setup_environment();

	base_thread_id=pthread_self();

	/*main control loop*/
	create_rt_task(&base_thread_id,PRIOR_HIGH,control_loop,NULL);

	/*monitor of current state*/
	create_rt_task(&base_thread_id,PRIOR_LOW,pos_monitor,NULL);

	/*wait for commands*/
	poll_cmd(&rps);

	return 0;
}