#include <wiringPi.h>
#include <softPwm.h>

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

#include <fcntl.h>
#include <unistd.h>

#include <pthread.h>

#define RANGE	100
#define MAX_PWM 1000
#define MIN_PWM 0

char str[40] = {0};
char *token;
char *in_pid;
float *myPid;
int out_pwm;
int slope;

int i;

char onState = 1;
char playState = 0;
char updateState = 0;

int pid(float d, float t, float *st_pid);
float *setup_pid(char *in_pid);

/*
 * Pthread for read node.js
 */
void *state_read (void *arg)
{
	while (1) {
		read(0, &str, 40);
		token = strtok(str, ":");
		if (strcmp("pidState", token) == 0) {
			in_pid = strtok(NULL, "");
			myPid = setup_pid(in_pid);
			printf("in_pid %s\n", in_pid);
			//updateState = 1;
		} else if (strcmp("playState", token) == 0) {
			token = strtok(NULL, "");
			printf("Token %s\n", token);
			fflush(stdout);
			if (strcmp("false", token) == 0)
				playState = 0;
			else
				playState = 1;
		} else if (strcmp("onState", token) == 0) {
			token = strtok(NULL, "");
			if (strcmp("false", token) == 0)
				onState = 0;
			else
				onState = 1;
		}
		printf("variable %s:\n", str);
		fflush(stdout);
		sleep(1);

		memset(str, 0, sizeof str);
	}
}

/*
 * Declarate PID function
 */
//int pid(float d, float t, float *st_pid);
//float *setup_pid(char *in_pid);


int main (void)
{
	unsigned long int dt;
	float dist;
	float t_init = 0, t_final = 0, dt_loop = 0;
	unsigned int t0;
	myPid = (float[]){0, 0, 0, 0, 0};

	float A[4] = {1.38995, -0.47366, 1, 0};
	float B[2] = {2.0, 0.0};
	float C[2] = {0.64182, 0.50032};
	float X_k[2] = {0};
	float X_k1[2] = {0};

	pthread_t thread;

	/*system("insmod /home/pi/TFM/pwm_kernel.ko");
	delay(1000);
	system("mknod /dev/chardev c 246 0");
	int fp = open("/dev/chardev", O_RDONLY);*/

	pthread_create(&thread, NULL, state_read, NULL);

	wiringPiSetup();

	/*
	 * Set-up Sensor (Software) & Motor (Hardware) PWM
	 */
	softPwmCreate(3, 1, RANGE);
	pinMode(1, PWM_OUTPUT);
	pwmSetMode(PWM_MODE_MS);
	pwmSetRange(70000);
	pwmSetClock(6);
	pwmWrite(1, 2800);

	delay(3000);

	pwmWrite(1, MIN_PWM); //Motor warm-up

	printf("playApt: 1\n"); //Enable PlayPause buttom
	fflush(stdout);

	t0 = millis();

	delay(10);

	for (i = 0; i < 5; ++i) {
		printf("Point %d: %f\n", i, *(myPid + i));
		fflush(stdout);
		delay(10);
	}

	while (onState) {

		while ((!playState) & onState) {
			pwmWrite(1, MIN_PWM);
			t0 = millis();
			delay(1000);
			t_init = t0;
			myPid[4] = 0; //Update integral term
			printf("Esperando:\n");
			fflush(stdout);
		}

		//read(fp, &dt, sizeof(dt)); //Read kernel module

		//X_k1[0] = A[0]*X_k[0] + A[1]*X_k[1] + B[0]*myPid[0];
		//X_k1[1] = A[2]*X_k[0] + A[3]*X_k[1] + B[1]*myPid[0];
		X_k1[0] = A[0]*X_k[0] + A[1]*X_k[1] + B[0]*out_pwm/1000;
		X_k1[1] = A[2]*X_k[0] + A[3]*X_k[1] + B[1]*out_pwm/1000;
		dist = C[0]*X_k[0] + C[1]*X_k[1];
		//dist = B[0]*myPid[0];
		X_k[0] = X_k1[0];
		X_k[1] = X_k1[1];

		t_final = (millis() - t0)/1e3;
		dt_loop = t_final - t_init;
		t_init = t_final;

		out_pwm = pid(dist, dt_loop, myPid); //Calculate PID out

		pwmWrite(1, out_pwm);

		//printf("msr: %0.2f, %0.2f, %d\n", dist, t_init, out_pwm);
		printf("msr: %0.2f, %0.2f, %f\n", dist, t_init, B[0]*myPid[0]);
		fflush(stdout);

		delay(200);
	}

	printf("playApt: 0\n"); //Enable PlayPause buttom
	fflush(stdout);

	slope = (out_pwm - MIN_PWM)/50;
	for (i = 0; i < 50; ++i) {
		pwmWrite(1, out_pwm - i*slope);
		delay(50);
	}
	pwmWrite(1, 0);

	/*if (close(fp) < 0) {
		printf("Error: not remove chardev\n");
		fflush(stdout);
	} else {
		printf("Chardev removed\n");
		fflush(stdout);
	}

	system("rm /dev/chardev");
	system("rmmod pwm_kernel");*/

	return 0;
}

/*
 * Set-Up PID function
 */
float *setup_pid(char *in_pid) {
	static float temp[5];
	char *token;
	int k = 0;

	token = strtok(in_pid, ",");
	while (token != NULL) {
		temp[k] = atof(token);
		token = strtok(NULL, ",");
		++k;
	}

	return temp;
}

/*
 * PID function
 */
int pid(float d, float dt, float *st_pid) {
	int h = 50;
	float er;
	float u_p;
	float u_i;
	int out_pwm;

	er = st_pid[0] - d;
	u_p = er*st_pid[1];
	u_i = st_pid[4] + st_pid[2]*er*dt;

	out_pwm = MIN_PWM + (u_p + u_i)*(MAX_PWM - MIN_PWM)/h;

	if (out_pwm < MIN_PWM) {
		//st_pid[4] = u_i + (MIN_PWM - out_pwm)/5;
		st_pid[4] = u_i;
		out_pwm = MIN_PWM;
	}
	else if (out_pwm > MAX_PWM) {
		//st_pid[4] = u_i + (MAX_PWM - out_pwm)/5;
		st_pid[4] = u_i;
		out_pwm = MAX_PWM;
	}
	else
		st_pid[4] = u_i;

	return out_pwm;
}