#include <sys/types.h>
#include <stdlib.h>
#include <signal.h>
#include <unistd.h>
#include <fcntl.h>
#include <string.h>
#include <time.h>
#include <ctype.h>
#include <math.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include "dio.h"
#define TRUE 1
#define MAP_ADDRESS (31*0x100000)  /* 1 mb less than total ram */
#define BYTE unsigned char
#define WORD unsigned short
#define LONG unsigned long
#define i1 get_i(2,8) 
#define i2 get_i(2,9) 
#define i3 get_i(2,10) 
#define i4 get_i(2,11) 
#define i5 get_i(2,12) 
#define i6 get_i(2,13) 
#define i7 get_i(2,14) 
#define i8 get_i(2,15) 
#define i9 get_i(2,0) 
#define i10 get_i(2,1) 
#define i11 get_i(2,2) 
#define i12 get_i(2,3) 
#define i13 get_i(2,4) 
#define i14 get_i(2,5) 
#define i15 get_i(2,6) 
#define i16 get_i(2,7) 
#define i17 get_i(1,8) 
#define i18 get_i(1,9) 
#define i19 get_i(1,10) 
#define i20 get_i(1,11) 
#define i21 get_i(1,12) 
#define i22 get_i(1,13) 
#define i23 get_i(1,14) 
#define i24 get_i(1,15) 
#define q1 set_o(0,7) 
#define q2 set_o(0,6) 
#define q3 set_o(0,5) 
#define q4 set_o(0,4) 
#define q5 set_o(0,3) 
#define q6 set_o(0,2) 
#define q7 set_o(0,1) 
#define q8 set_o(0,0) 
#define q9 set_o(0,15) 
#define q10 set_o(0,14) 
#define q11 set_o(0,13) 
#define q12 set_o(0,12) 
#define q13 set_o(0,11) 
#define q14 set_o(0,10) 
#define q15 set_o(0,9) 
#define q16 set_o(0,8) 
#define q17 set_o(1,7) 
#define q18 set_o(1,6) 
#define q19 set_o(1,5) 
#define q20 set_o(1,4) 
#define q21 set_o(1,3) 
#define q22 set_o(1,2) 
#define q23 set_o(1,1) 
#define q24 set_o(1,0)
#define cq1 clr_o(0,7) 
#define cq2 clr_o(0,6) 
#define cq3 clr_o(0,5) 
#define cq4 clr_o(0,4) 
#define cq5 clr_o(0,3) 
#define cq6 clr_o(0,2) 
#define cq7 clr_o(0,1) 
#define cq8 clr_o(0,0) 
#define cq9 clr_o(0,15) 
#define cq10 clr_o(0,14) 
#define cq11 clr_o(0,13) 
#define cq12 clr_o(0,12) 
#define cq13 clr_o(0,11) 
#define cq14 clr_o(0,10) 
#define cq15 clr_o(0,9) 
#define cq16 clr_o(0,8) 
#define cq17 clr_o(1,7) 
#define cq18 clr_o(1,6) 
#define cq19 clr_o(1,5) 
#define cq20 clr_o(1,4) 
#define cq21 clr_o(1,3) 
#define cq22 clr_o(1,2) 
#define cq23 clr_o(1,1) 
#define cq24 clr_o(1,0)
 
char *DevName = "/dev/dio48H_1A";
char DevNameIO[20];
int  Mode     = 0;
int  Status;
int  mfd;
BYTE  bReg;

int fd_1A, fd_1B, fd_1C;
int fd_2A, fd_2B, fd_2C;
unsigned short value;

typedef struct
{
    unsigned short     reg[512];
    float              alg[512];
    int		       flags;
} LMAP;

LMAP *mapp;

/* Forward Declarations */

void setup_io(void);
void close_io(void);
void read_io(void);
void write_io(void);
void solve(void);
int  get_i(short reg,short bit);
void set_o(short reg,short bit);
void clr_o(short reg,short bit);


main(int argc,char *argv[])
{

    /* Let's do the nasty stuff first */
    /* We are using 1mb of ram excluded from the linux mm as shared memory */
    /* By typedef'ing our struct and mmaping it at MAP_ADDRESS we in effect */
    /* allocate it in the shared memory translated into user space. If this */
    /* seems like FM to you, don't feel bad, I had to read the kernel sources */
    /* to figure out where we actually allocate anything. hint, the compiler  */
    /* did it */

    /* Note, I should check if there is a way to get and drop perms for */
    /* /dev/mem without running as root or chmod'ing /dev/mem. I don't  */
    /* think ioperm() covers this. (security) */

    if(( mfd = open("/dev/mem",O_RDWR)) < 0)
    {
        perror("dev/mem open failed");
        exit(1);
    }
    mapp = (LMAP *) mmap( 0,sizeof(LMAP),PROT_READ | PROT_WRITE,MAP_SHARED,mfd,MAP_ADDRESS);
    if(MAP_FAILED == mmap)
    {
        perror("mmap failed");
        exit(1);
    }
    close(mfd);

    /* We should now have a shared memory map with no fuss. The code above */
    /* should be copied into all the map users. The declarations will go   */
    /* into a common header file for abstraction.                          */


    setup_io();
    mapp->flags = 0;

    while( TRUE )
    {
        while(mapp->flags != 0) ;
        mapp->flags = 1;
        read_io();
	solve();   
        write_io();
        mapp->flags = 0;
        usleep(1000);
    }
}
void setup_io(void)
{
    strcpy(DevNameIO, "/dev/dio48H_1A");
    if ((fd_1A = open(DevNameIO, O_RDWR )) < 0) 
    {
        perror("DevNameIO");
        printf("error opening device %s\n", DevNameIO);
        exit(2);
    }
    strcpy(DevNameIO, "/dev/dio48H_1B");
    if ((fd_1B = open(DevNameIO, O_RDWR )) < 0) 
    {
        perror(DevNameIO);
        printf("error opening device %s\n", DevNameIO);
        exit(2);
    }
    strcpy(DevNameIO, "/dev/dio48H_1C");
    if ((fd_1C = open(DevNameIO, O_RDWR )) < 0) 
    {
        perror(DevNameIO);
        printf("error opening device %s\n", DevNameIO);
        exit(2);
    }

    strcpy(DevNameIO, "/dev/dio48H_2A");
    if ((fd_2A = open(DevNameIO, O_RDWR )) < 0) 
    {
        perror("DevNameIO");
        printf("error opening device %s\n", DevNameIO);
        exit(2);
    }
    strcpy(DevNameIO, "/dev/dio48H_2B");
    if ((fd_2B = open(DevNameIO, O_RDWR )) < 0) 
    {
        perror(DevNameIO);
        printf("error opening device %s\n", DevNameIO);
        exit(2);
    }
    strcpy(DevNameIO, "/dev/dio48H_2C");
    if ((fd_2C = open(DevNameIO, O_RDWR )) < 0) 
    {
        perror(DevNameIO);
        printf("error opening device %s\n", DevNameIO);
        exit(2);
    }
    ioctl(fd_1A, DIO_SET_DIRECTION, PORT_INPUT);
    ioctl(fd_1B, DIO_SET_DIRECTION, PORT_INPUT);
    ioctl(fd_1C, DIO_SET_DIRECTION, LOW_PORT_INPUT);
    ioctl(fd_1C, DIO_SET_DIRECTION, HIGH_PORT_INPUT);
    ioctl(fd_2A, DIO_SET_DIRECTION, PORT_OUTPUT);
    ioctl(fd_2B, DIO_SET_DIRECTION, PORT_OUTPUT);
    ioctl(fd_2C, DIO_SET_DIRECTION, LOW_PORT_OUTPUT);
    ioctl(fd_2C, DIO_SET_DIRECTION, HIGH_PORT_OUTPUT);
}
void close_io(void)
{
    close(fd_1A);
    close(fd_1B);
    close(fd_1C);
    close(fd_2A);
    close(fd_2B);
    close(fd_2C);
}
void write_io(void)
{
    BYTE buf[3];
    /* mapp->reg[0] = ~ mapp->reg[0]; */ /* toggle for testing */
    buf[0] = mapp->reg[0] &  0x00ff;
    buf[1] = mapp->reg[0] >> 8;
    buf[2] = mapp->reg[1] &  0x00ff;
    write(fd_2A,&buf[0],1) ;
    write(fd_2B,&buf[1],1) ;
    write(fd_2C,&buf[2],1) ;
}
void read_io(void)
{
    WORD buf = 0;
    WORD shft = 0;
    read(fd_1A,&shft,1) ;
    shft <<= 8;
    buf = mapp->reg[1] & 0x00ff ;
    mapp->reg[1] = shft | buf;
    shft = 0;
    buf = 0 ;
    read(fd_1B,&buf,1) ;
    read(fd_1C,&shft,1) ;
    shft <<= 8 ;
    shft |= buf;
    mapp->reg[2] = shft;
}
void solve(void)
{
    if(i21 || i23 ) q23; else cq23;
    if(i22 || i24 ) q24; else cq24;
}
int get_i(short reg,short bit)
{
    if(mapp->reg[reg] & (0x01 << bit)) return(1);
    else return(0);
}
void set_o(short reg,short bit)
{
    mapp->reg[reg] |= (0x01 << bit);
}
void clr_o(short reg,short bit)
{
    mapp->reg[reg] &= ~(0x01 << bit);
}
