#include <sys/time.h>
#include <signal.h>
#include <rt_ipc.h>
#include <stddef.h>
#include <stdio.h>
#include <sys/types.h>
#include <stdlib.h>
#include <unistd.h>
#include <ctype.h>
#include <string.h>
#include <time.h>

#define SPLL_MAIN_LOOP 0
#define SPLL_HELPER_LOOP 1
#define SPLL_KP 0
#define SPLL_KI 1


void error(char *msg)
{
	perror(msg);
	exit(0);
}

void printHelp(char *name)
{
	printf("WRS SoftPLL tune\n\n");
	printf("This tool enables the user to change the proportional and integral constants\n");
	printf("of the SoftPLL helper and main loops.\n\n");
	printf("Usage: %s -s/-g main/helper -c kp/ki -v value\n",name);
	exit(0);
}

void sigintHandler(int sig_num)
{
	exit(0);
}

int main(int argc, char *argv[])
{
	int c = 0;
	char s_loop[7];
	char s_constant[3];
	int set = -1, get = -1;
	int loop=-1, constant=-1;
	int value = -1;

	int check_kp=-1, check_ki=-1;

	while ((c = getopt(argc, argv, "hs:g:c:v:")) != -1)
		switch (c)
		{
		case 'h':
			printHelp(argv[0]);
			break;
		case 's':
			set = 1;
			strcpy(s_loop, optarg);
			break;
		case 'g':
			get = 1;
			strcpy(s_loop, optarg);
			break;
		case 'c':
			strcpy(s_constant, optarg);
			break;
		case 'v':
			value = atoi(optarg);
			break;
		case '?':
			if (optopt == 's' || optopt == 'g')
				fprintf(stderr, "Option -%c requires \"main\" or \"helper\" loop to be defined.\n", optopt);
			else if (optopt == 'c')
				fprintf(stderr, "Option -%c requires \"kp\" or \"ki\" to be defined.\n", optopt);
			else if (optopt == 'v')
				fprintf(stderr, "Option -%c requires the value of the constant to be provided.", optopt);
			else if (isprint(optopt))
			{
				fprintf(stderr, "Unknown option '-%c'.\n", optopt);
				printHelp(argv[0]);
			}
			else
			{
				fprintf(stderr, "Unknown option character '\\x%x'.\n", optopt);
				printHelp(argv[0]);
			}
			return 1;
		default:
			abort();
		}

	if (!strcmp(s_loop, "helper"))
	{
		loop = SPLL_HELPER_LOOP;
	}
	else if (!strcmp(s_loop, "main"))
	{
		loop = SPLL_MAIN_LOOP;
	}
	else
	{
		fprintf(stderr, "'main' or 'helper' option must be provided.\n");
		printHelp(argv[0]);
	}

	if (get != 1)
	{
		if (!strcmp(s_constant, "kp"))
		{
			constant = SPLL_KP;
		}
		else if (!strcmp(s_constant, "ki"))
		{
			constant = SPLL_KI;
		}
		else
		{
			fprintf(stderr, "'kp' or 'ki' option must be provided.\n");
			printHelp(argv[0]);
		}
	}

	if( rts_connect(NULL) < 0 ){
		printf("Can't connect to the RT subsys\n");
	}

	if (set == 1)
	{
		rts_spll_set_constants(loop,constant,value);

		do{
			check_kp = rts_spll_get_constants(loop,SPLL_KP,0);
			check_ki = rts_spll_get_constants(loop,SPLL_KI,0);
		}while(check_ki == -1 || check_kp == -1);
		usleep(100);

		printf("%s constants: kp=%d ki=%d\n",s_loop,check_kp,check_ki);
		return 0;

	}else if (get == 1)
	{

		do{
			check_kp = rts_spll_get_constants(loop,SPLL_KP,0);
			check_ki = rts_spll_get_constants(loop,SPLL_KI,0);
		}while(check_ki == -1 || check_kp == -1);

		usleep(100);

		printf("%s constants: kp=%d ki=%d\n",s_loop,check_kp,check_ki);
		return 0;
	}

	return -1;
	
}
