#include <sys/io.h>
#include <stdio.h>
#include <unistd.h> /* needed for ioperm() */
#include <pthread.h>
#include <termios.h>
#include <errno.h>
#include <math.h>
#include <sys/mman.h>
#include <native/task.h>
#include <native/timer.h>
#include <native/sem.h>


#define DATA 0x378
#define STATUS 0x379
#define CONTROL 0x37A

#define CCONVERT(x) x^0xB

#define BIDIR 0x20

#define PWM1_E CCONVERT(0) /*0xB*/
#define PWM2_E CCONVERT(1) /*0xA*/
#define PWM3_E CCONVERT(2) /*0x9*/
#define AD_CHS CCONVERT(3) /*0x8*/
#define AD_E CCONVERT(4) /*0xF*/
#define ALLOFF CCONVERT(15) /*0x4*/

//AD0 connected to /strobe = 1
//AD1 connected to /linefeed = 2
//AD2 connected to init = 4
//AD3 connected to /select = 8

#define WAITTIME 128000
#define STEP 1.0
#define MINFRQ 2
#define MAXFRQ 100
#define MAXAMP 128

void* read_kb(void* dummy);

float frq=15.0;
unsigned int amp=20;
RT_TASK pwmTask;

#define writeLptVal(a,d) 	outb(d, DATA); \
				outb(a, CONTROL); \
				outb(ALLOFF, CONTROL)
				

void doPwm(void *arg)
{
	float x;
	unsigned char dutyCycle[3]={0,0,0};
	
	rt_task_set_periodic(NULL, TM_NOW, WAITTIME);
	
	
	while(1) {
		if (frq>0.0) {
			x+=2*M_PI*frq*(float)WAITTIME/1000000000.0;
			if (x>2.0*M_PI) x-=2.0*M_PI;
			
			dutyCycle[0]=amp*(sin(x))+128;
			dutyCycle[1]=amp*(sin(x+2*M_PI/3.0))+128;
			dutyCycle[2]=amp*(sin(x+4*M_PI/3.0))+128;
		}
		
		writeLptVal(PWM1_E, dutyCycle[0]);
		writeLptVal(PWM2_E, dutyCycle[1]);
		writeLptVal(PWM3_E, dutyCycle[2]);

		rt_task_wait_period(NULL);
	}
}


int main()
{
	pthread_t reader;
	
	mlockall(MCL_CURRENT|MCL_FUTURE); //Disable swapping
	printf("Setting permissions for registers %X, %X, %X\n", DATA, STATUS, CONTROL);
	if (ioperm(DATA,3,1)) {
		printf("Sorry, you were not able to gain access to the ports\n");
		printf("You must be root to run this program\n");
		return(1);
	}
	
	printf("Creating reader thread\n");
	pthread_create(&reader, 0, read_kb, 0);
	printf("Creating real-time PWM-thread\n");
	rt_task_create(&pwmTask, "PWM", 0, 99, 0);
	rt_task_start(&pwmTask, &doPwm, NULL);
	
	pause();
	
	return(0);
}

#define MKFLAG(which) \
static int io_tio_set_flag_##which(int fd, int val, int on, int *old) \
{ struct termios tio; \
    if (tcgetattr(fd,&tio)) return -1; \
	if (old) *old=(tio.which & (val)); \
    if (on) tio.which |= (val); \
    else tio.which &= ~(val); \
    if (tcsetattr(fd,TCSADRAIN,&tio)) return -1; \
    return 0; \
} \
static int io_tio_get_flag_##which(int fd, int bit, int *value) \
{ struct termios tio; \
    if (tcgetattr(fd,&tio)) return -1; \
	*value=(tio.which & (bit)); \
    return 0; \
}
MKFLAG(c_lflag)

static int 
		read_character_now_without_echo(int fd, char *c)
{
	int old_ICANON,old_ECHO;
	int restoreflag=0;
	int e=0;
	int retcode=-1;
	/* some devices do not support terminal flags. Notably:
	* /dev/disk|floppy
	* /dev/zero
	* ...
	 * This function is able to handle them:
	*/
	if (-1==io_tio_set_flag_c_lflag(fd,ICANON,0,&old_ICANON)) {
		if (errno!=ENOTTY 
				  && errno!=EINVAL) { /* LINUX /dev/random lossage */
			e=errno;
			perror("Turnoff of ICANON failed");
			errno=e;
			return -1;
				  }
	} else
		restoreflag=1;
		if (restoreflag && -1==io_tio_set_flag_c_lflag(fd,ECHO,0,&old_ECHO)) {
			e=errno;
			perror("Turnoff of ICANON failed");
		} else {
			if (restoreflag) restoreflag++;
			while (1) {
				retcode=read(fd,c,1);
				if (retcode==1 || retcode==0) break;
				if (errno==EINTR) continue;
				break;
			}
		}
	/* we do not check for errors anymore: If we were able to change
		* a flag before and are unable to do that afterwards then we
		* are in unsolvable trouble anyway.
		* btw, it _could_ happen, in case revoke() is summoned upon
		* us. Oh, well.
	*/
		/* set echo to old value */
		if (restoreflag==2)
			io_tio_set_flag_c_lflag(fd,ECHO,old_ECHO,NULL);
		/* back to canonical input mode (line by line) */
		if (restoreflag)
			io_tio_set_flag_c_lflag(0,ICANON,old_ICANON,NULL);
		errno=e;
		return retcode;
}

void* read_kb(void* dummy)
{
	char c;
	
	while(1) {
		read_character_now_without_echo(0, &c);
		if (c=='+' && frq<MAXFRQ)
			frq+=STEP;
		if (c=='-' && frq>MINFRQ)
			frq-=STEP;
		if (c=='2') frq=2;
		if (c=='0') amp=0;
		if (c=='_' && amp>0) amp--;
		if (c=='*' && amp<MAXAMP) amp++;
		if (c=='q')
			frq=0;
		printf("Frequency now %.1f Hz, Amplitude %d\n", frq, amp);
	}
	return 0;
}